1 /*
2 * nearest point routines for D2<SBasis> and Piecewise<D2<SBasis>>
3 *
4 * Authors:
5 *
6 * Marco Cecchetti <mrcekets at gmail.com>
7 *
8 * Copyright 2007-2008 authors
9 *
10 * This library is free software; you can redistribute it and/or
11 * modify it either under the terms of the GNU Lesser General Public
12 * License version 2.1 as published by the Free Software Foundation
13 * (the "LGPL") or, at your option, under the terms of the Mozilla
14 * Public License Version 1.1 (the "MPL"). If you do not alter this
15 * notice, a recipient may use your version of this file under either
16 * the MPL or the LGPL.
17 *
18 * You should have received a copy of the LGPL along with this library
19 * in the file COPYING-LGPL-2.1; if not, write to the Free Software
20 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21 * You should have received a copy of the MPL along with this library
22 * in the file COPYING-MPL-1.1
23 *
24 * The contents of this file are subject to the Mozilla Public License
25 * Version 1.1 (the "License"); you may not use this file except in
26 * compliance with the License. You may obtain a copy of the License at
27 * http://www.mozilla.org/MPL/
28 *
29 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
30 * OF ANY KIND, either express or implied. See the LGPL or the MPL for
31 * the specific language governing rights and limitations.
32 */
35 #include <2geom/nearest-point.h>
36 #include <algorithm>
39 namespace Geom
40 {
42 ////////////////////////////////////////////////////////////////////////////////
43 // D2<SBasis> versions
45 /*
46 * Return the parameter t of a nearest point on the portion of the curve "c",
47 * related to the interval [from, to], to the point "p".
48 * The needed curve derivative "dc" is passed as parameter.
49 * The function return the first nearest point to "p" that is found.
50 */
52 double nearest_point( Point const& p,
53 D2<SBasis> const& c,
54 D2<SBasis> const& dc,
55 double from, double to )
56 {
57 if ( from > to ) std::swap(from, to);
58 if ( from < 0 || to > 1 )
59 {
60 THROW_RANGEERROR("[from,to] interval out of bounds");
61 }
62 if (c.isConstant()) return from;
63 SBasis dd = dot(c - p, dc);
64 std::vector<double> zeros = Geom::roots(dd);
66 double closest = from;
67 double min_dist_sq = L2sq(c(from) - p);
68 double distsq;
69 for ( unsigned int i = 0; i < zeros.size(); ++i )
70 {
71 distsq = L2sq(c(zeros[i]) - p);
72 if ( min_dist_sq > L2sq(c(zeros[i]) - p) )
73 {
74 closest = zeros[i];
75 min_dist_sq = distsq;
76 }
77 }
78 if ( min_dist_sq > L2sq( c(to) - p ) )
79 closest = to;
80 return closest;
82 }
84 /*
85 * Return the parameters t of all the nearest points on the portion of
86 * the curve "c", related to the interval [from, to], to the point "p".
87 * The needed curve derivative "dc" is passed as parameter.
88 */
90 std::vector<double>
91 all_nearest_points( Point const& p,
92 D2<SBasis> const& c,
93 D2<SBasis> const& dc,
94 double from, double to )
95 {
96 std::swap(from, to);
97 if ( from > to ) std::swap(from, to);
98 if ( from < 0 || to > 1 )
99 {
100 THROW_RANGEERROR("[from,to] interval out of bounds");
101 }
103 std::vector<double> result;
104 if (c.isConstant())
105 {
106 result.push_back(from);
107 return result;
108 }
109 SBasis dd = dot(c - p, dc);
111 std::vector<double> zeros = Geom::roots(dd);
112 std::vector<double> candidates;
113 candidates.push_back(from);
114 candidates.insert(candidates.end(), zeros.begin(), zeros.end());
115 candidates.push_back(to);
116 std::vector<double> distsq;
117 distsq.reserve(candidates.size());
118 for ( unsigned int i = 0; i < candidates.size(); ++i )
119 {
120 distsq.push_back( L2sq(c(candidates[i]) - p) );
121 }
122 unsigned int closest = 0;
123 double dsq = distsq[0];
124 for ( unsigned int i = 1; i < candidates.size(); ++i )
125 {
126 if ( dsq > distsq[i] )
127 {
128 closest = i;
129 dsq = distsq[i];
130 }
131 }
132 for ( unsigned int i = 0; i < candidates.size(); ++i )
133 {
134 if( distsq[closest] == distsq[i] )
135 {
136 result.push_back(candidates[i]);
137 }
138 }
139 return result;
140 }
143 ////////////////////////////////////////////////////////////////////////////////
144 // Piecewise< D2<SBasis> > versions
147 double nearest_point( Point const& p,
148 Piecewise< D2<SBasis> > const& c,
149 double from, double to )
150 {
151 if ( from > to ) std::swap(from, to);
152 if ( from < c.cuts[0] || to > c.cuts[c.size()] )
153 {
154 THROW_RANGEERROR("[from,to] interval out of bounds");
155 }
157 unsigned int si = c.segN(from);
158 unsigned int ei = c.segN(to);
159 if ( si == ei )
160 {
161 double nearest=
162 nearest_point(p, c[si], c.segT(from, si), c.segT(to, si));
163 return c.mapToDomain(nearest, si);
164 }
165 double t;
166 double nearest = nearest_point(p, c[si], c.segT(from, si));
167 unsigned int ni = si;
168 double dsq;
169 double mindistsq = distanceSq(p, c[si](nearest));
170 Rect bb(Geom::Point(0,0),Geom::Point(0,0));
171 for ( unsigned int i = si + 1; i < ei; ++i )
172 {
173 bb = *bounds_fast(c[i]);
174 dsq = distanceSq(p, bb);
175 if ( mindistsq <= dsq ) continue;
176 t = nearest_point(p, c[i]);
177 dsq = distanceSq(p, c[i](t));
178 if ( mindistsq > dsq )
179 {
180 nearest = t;
181 ni = i;
182 mindistsq = dsq;
183 }
184 }
185 bb = *bounds_fast(c[ei]);
186 dsq = distanceSq(p, bb);
187 if ( mindistsq > dsq )
188 {
189 t = nearest_point(p, c[ei], 0, c.segT(to, ei));
190 dsq = distanceSq(p, c[ei](t));
191 if ( mindistsq > dsq )
192 {
193 nearest = t;
194 ni = ei;
195 }
196 }
197 return c.mapToDomain(nearest, ni);
198 }
200 std::vector<double>
201 all_nearest_points( Point const& p,
202 Piecewise< D2<SBasis> > const& c,
203 double from, double to )
204 {
205 if ( from > to ) std::swap(from, to);
206 if ( from < c.cuts[0] || to > c.cuts[c.size()] )
207 {
208 THROW_RANGEERROR("[from,to] interval out of bounds");
209 }
211 unsigned int si = c.segN(from);
212 unsigned int ei = c.segN(to);
213 if ( si == ei )
214 {
215 std::vector<double> all_nearest =
216 all_nearest_points(p, c[si], c.segT(from, si), c.segT(to, si));
217 for ( unsigned int i = 0; i < all_nearest.size(); ++i )
218 {
219 all_nearest[i] = c.mapToDomain(all_nearest[i], si);
220 }
221 return all_nearest;
222 }
223 std::vector<double> all_t;
224 std::vector< std::vector<double> > all_np;
225 all_np.push_back( all_nearest_points(p, c[si], c.segT(from, si)) );
226 std::vector<unsigned int> ni;
227 ni.push_back(si);
228 double dsq;
229 double mindistsq = distanceSq( p, c[si](all_np.front().front()) );
230 Rect bb(Geom::Point(0,0),Geom::Point(0,0));
231 for ( unsigned int i = si + 1; i < ei; ++i )
232 {
233 bb = *bounds_fast(c[i]);
234 dsq = distanceSq(p, bb);
235 if ( mindistsq < dsq ) continue;
236 all_t = all_nearest_points(p, c[i]);
237 dsq = distanceSq( p, c[i](all_t.front()) );
238 if ( mindistsq > dsq )
239 {
240 all_np.clear();
241 all_np.push_back(all_t);
242 ni.clear();
243 ni.push_back(i);
244 mindistsq = dsq;
245 }
246 else if ( mindistsq == dsq )
247 {
248 all_np.push_back(all_t);
249 ni.push_back(i);
250 }
251 }
252 bb = *bounds_fast(c[ei]);
253 dsq = distanceSq(p, bb);
254 if ( mindistsq >= dsq )
255 {
256 all_t = all_nearest_points(p, c[ei], 0, c.segT(to, ei));
257 dsq = distanceSq( p, c[ei](all_t.front()) );
258 if ( mindistsq > dsq )
259 {
260 for ( unsigned int i = 0; i < all_t.size(); ++i )
261 {
262 all_t[i] = c.mapToDomain(all_t[i], ei);
263 }
264 return all_t;
265 }
266 else if ( mindistsq == dsq )
267 {
268 all_np.push_back(all_t);
269 ni.push_back(ei);
270 }
271 }
272 std::vector<double> all_nearest;
273 for ( unsigned int i = 0; i < all_np.size(); ++i )
274 {
275 for ( unsigned int j = 0; j < all_np[i].size(); ++j )
276 {
277 all_nearest.push_back( c.mapToDomain(all_np[i][j], ni[i]) );
278 }
279 }
280 all_nearest.erase(std::unique(all_nearest.begin(), all_nearest.end()),
281 all_nearest.end());
282 return all_nearest;
283 }
285 } // end namespace Geom