KiCad PCB EDA Suite
Loading...
Searching...
No Matches
seg.cpp
Go to the documentation of this file.
1/*
2 * This program source code file is part of KiCad, a free EDA CAD application.
3 *
4 * Copyright (C) 2013 CERN
5 * @author Tomasz Wlostowski <[email protected]>
6 * Copyright (C) 2021 KiCad Developers, see AUTHORS.txt for contributors.
7 *
8 * This program is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU General Public License
10 * as published by the Free Software Foundation; either version 2
11 * of the License, or (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public License
19 * along with this program; if not, you may find one here:
20 * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
21 * or you may search the http://www.gnu.org website for the version 2 license,
22 * or you may write to the Free Software Foundation, Inc.,
23 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
24 */
25
26#include <algorithm> // for min
27#include <geometry/seg.h>
28#include <math/util.h> // for rescale
29#include <math/vector2d.h> // for VECTOR2I, VECTOR2
30#include <trigo.h> // for RAD2DEG
31
32template <typename T>
33int sgn( T aVal )
34{
35 return ( T( 0 ) < aVal ) - ( aVal < T( 0 ) );
36}
37
38template <typename T>
39constexpr T sqrt_helper(T x, T lo, T hi)
40{
41 if (lo == hi)
42 return lo;
43
44 const T mid = (lo + hi + 1) / 2;
45 if (x / mid < mid)
46 return sqrt_helper<T>(x, lo, mid - 1);
47 else
48 return sqrt_helper(x, mid, hi);
49}
50
51template <typename T>
52constexpr T ct_sqrt(T x)
53{
54 return sqrt_helper<T>(x, 0, x / 2 + 1);
55}
56
57template <typename T>
58static constexpr T sqrt_max_typed = ct_sqrt( std::numeric_limits<T>::max() );
59
60template <typename T>
61T isqrt(T x)
62{
63 T r = (T) std::sqrt((double) x);
64 T sqrt_max = sqrt_max_typed<T>;
65
66 while (r < sqrt_max && r * r < x)
67 r++;
68 while (r > sqrt_max || r * r > x)
69 r--;
70
71 return r;
72}
73
74
76{
77 if( Intersects( aSeg ) )
78 return 0;
79
80 const VECTOR2I pts[4] =
81 {
82 aSeg.NearestPoint( A ) - A,
83 aSeg.NearestPoint( B ) - B,
84 NearestPoint( aSeg.A ) - aSeg.A,
85 NearestPoint( aSeg.B ) - aSeg.B
86 };
87
89
90 for( int i = 0; i < 4; i++ )
91 m = std::min( m, pts[i].SquaredEuclideanNorm() );
92
93 return m;
94}
95
96
97EDA_ANGLE SEG::Angle( const SEG& aOther ) const
98{
99 EDA_ANGLE thisAngle = EDA_ANGLE( A - B ).Normalize180();
100 EDA_ANGLE otherAngle = EDA_ANGLE( aOther.A - aOther.B ).Normalize180();
101
102 EDA_ANGLE angle = std::abs( ( thisAngle - otherAngle ).Normalize180() );
103
104 return std::min( ANGLE_180 - angle, angle );
105}
106
107
108const VECTOR2I SEG::NearestPoint( const SEG& aSeg ) const
109{
110 if( OPT_VECTOR2I p = Intersect( aSeg ) )
111 return *p;
112
113 const VECTOR2I pts_origin[4] =
114 {
115 aSeg.NearestPoint( A ),
116 aSeg.NearestPoint( B ),
117 NearestPoint( aSeg.A ),
118 NearestPoint( aSeg.B )
119 };
120
121
122 const VECTOR2I* pts_out[4] =
123 {
124 &A,
125 &B,
126 &pts_origin[2],
127 &pts_origin[3]
128 };
129
130 const ecoord pts_dist[4] =
131 {
132 ( pts_origin[0] - A ).SquaredEuclideanNorm(),
133 ( pts_origin[1] - B ).SquaredEuclideanNorm(),
134 ( pts_origin[2] - aSeg.A ).SquaredEuclideanNorm(),
135 ( pts_origin[3] - aSeg.B ).SquaredEuclideanNorm()
136 };
137
138 int min_i = 0;
139
140 for( int i = 0; i < 4; i++ )
141 {
142 if( pts_dist[i] < pts_dist[min_i] )
143 min_i = i;
144 }
145
146 return *pts_out[min_i];
147}
148
149
150bool SEG::NearestPoints( const SEG& aSeg, VECTOR2I& aPtA, VECTOR2I& aPtB, int64_t& aDistSq ) const
151{
152 if( OPT_VECTOR2I p = Intersect( aSeg ) )
153 {
154 aPtA = aPtB = *p;
155 aDistSq = 0;
156
157 return true;
158 }
159
160 const VECTOR2I pts_origin[4] =
161 {
162 aSeg.NearestPoint( A ),
163 aSeg.NearestPoint( B ),
164 NearestPoint( aSeg.A ),
165 NearestPoint( aSeg.B )
166 };
167
168 const VECTOR2I* pts_a_out[4] =
169 {
170 &A,
171 &B,
172 &pts_origin[2],
173 &pts_origin[3]
174 };
175
176 const VECTOR2I* pts_b_out[4] =
177 {
178 &pts_origin[0],
179 &pts_origin[1],
180 &aSeg.A,
181 &aSeg.B
182 };
183
184 const ecoord pts_dist[4] =
185 {
186 ( pts_origin[0] - A ).SquaredEuclideanNorm(),
187 ( pts_origin[1] - B ).SquaredEuclideanNorm(),
188 ( pts_origin[2] - aSeg.A ).SquaredEuclideanNorm(),
189 ( pts_origin[3] - aSeg.B ).SquaredEuclideanNorm()
190 };
191
192 int min_i = 0;
193
194 for( int i = 0; i < 4; i++ )
195 {
196 if( pts_dist[i] < pts_dist[min_i] )
197 min_i = i;
198 }
199
200 aPtA = *pts_a_out[min_i];
201 aPtB = *pts_b_out[min_i];
202 aDistSq = pts_dist[min_i];
203
204 return true;
205}
206
207
208bool SEG::intersects( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines, VECTOR2I* aPt ) const
209{
210 const VECTOR2<ecoord> e = VECTOR2<ecoord>( B ) - A;
211 const VECTOR2<ecoord> f = VECTOR2<ecoord>( aSeg.B ) - aSeg.A;
212 const VECTOR2<ecoord> ac = VECTOR2<ecoord>( aSeg.A ) - A;
213
214 ecoord d = f.Cross( e );
215 ecoord p = f.Cross( ac );
216 ecoord q = e.Cross( ac );
217
218 if( d == 0 )
219 return false;
220
221 if( !aLines && d > 0 && ( q < 0 || q > d || p < 0 || p > d ) )
222 return false;
223
224 if( !aLines && d < 0 && ( q < d || p < d || p > 0 || q > 0 ) )
225 return false;
226
227 if( !aLines && aIgnoreEndpoints && ( q == 0 || q == d ) && ( p == 0 || p == d ) )
228 return false;
229
230 if( aPt )
231 {
232 VECTOR2<ecoord> result( aSeg.A.x + rescale( q, (ecoord) f.x, d ),
233 aSeg.A.y + rescale( q, (ecoord) f.y, d ) );
234
235 if( abs( result.x ) > std::numeric_limits<VECTOR2I::coord_type>::max()
236 || abs( result.y ) > std::numeric_limits<VECTOR2I::coord_type>::max() )
237 {
238 return false;
239 }
240
241 *aPt = VECTOR2I( (int) result.x, (int) result.y );
242 }
243
244 return true;
245}
246
247
248bool SEG::Intersects( const SEG& aSeg ) const
249{
250 return intersects( aSeg );
251}
252
253
254OPT_VECTOR2I SEG::Intersect( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines ) const
255{
256 VECTOR2I ip;
257
258 if( intersects( aSeg, aIgnoreEndpoints, aLines, &ip ) )
259 return ip;
260 else
261 return OPT_VECTOR2I();
262}
263
264
266{
267 VECTOR2I slope( B - A );
268 VECTOR2I endPoint = slope.Perpendicular() + aP;
269
270 return SEG( aP, endPoint );
271}
272
273
274SEG SEG::ParallelSeg( const VECTOR2I& aP ) const
275{
276 VECTOR2I slope( B - A );
277 VECTOR2I endPoint = slope + aP;
278
279 return SEG( aP, endPoint );
280}
281
282
283bool SEG::ccw( const VECTOR2I& aA, const VECTOR2I& aB, const VECTOR2I& aC ) const
284{
285 return (ecoord) ( aC.y - aA.y ) * ( aB.x - aA.x ) > (ecoord) ( aB.y - aA.y ) * ( aC.x - aA.x );
286}
287
288
289bool SEG::Collide( const SEG& aSeg, int aClearance, int* aActual ) const
290{
291 // check for intersection
292 // fixme: move to a method
293 if( ccw( A, aSeg.A, aSeg.B ) != ccw( B, aSeg.A, aSeg.B ) &&
294 ccw( A, B, aSeg.A ) != ccw( A, B, aSeg.B ) )
295 {
296 if( aActual )
297 *aActual = 0;
298
299 return true;
300 }
301
303
304 dist_sq = std::min( dist_sq, SquaredDistance( aSeg.A ) );
305 dist_sq = std::min( dist_sq, SquaredDistance( aSeg.B ) );
306 dist_sq = std::min( dist_sq, aSeg.SquaredDistance( A ) );
307 dist_sq = std::min( dist_sq, aSeg.SquaredDistance( B ) );
308
309 if( dist_sq == 0 || dist_sq < (ecoord) aClearance * aClearance )
310 {
311 if( aActual )
312 *aActual = int( isqrt( dist_sq ) );
313
314 return true;
315 }
316
317 return false;
318}
319
320
321bool SEG::Contains( const VECTOR2I& aP ) const
322{
323 return Distance( aP ) <= 1;
324}
325
326
327const VECTOR2I SEG::NearestPoint( const VECTOR2I& aP ) const
328{
329 // Inlined for performance reasons
330 VECTOR2L d( B.x - A.x, B.y - A.y );
331 ecoord l_squared( d.x * d.x + d.y * d.y );
332
333 if( l_squared == 0 )
334 return A;
335
336 ecoord t = d.Dot( aP - A );
337
338 if( t < 0 )
339 return A;
340 else if( t > l_squared )
341 return B;
342
343 ecoord xp = rescale( t, (ecoord) d.x, l_squared );
344 ecoord yp = rescale( t, (ecoord) d.y, l_squared );
345
346 return VECTOR2<ecoord>( A.x + xp, A.y + yp );
347}
348
349
350const VECTOR2I SEG::ReflectPoint( const VECTOR2I& aP ) const
351{
352 VECTOR2I d = B - A;
353 VECTOR2I::extended_type l_squared = d.Dot( d );
354 VECTOR2I::extended_type t = d.Dot( aP - A );
356
357 if( !l_squared )
358 {
359 c = aP;
360 }
361 else
362 {
363 c.x = A.x + rescale( t, static_cast<VECTOR2I::extended_type>( d.x ), l_squared );
364 c.y = A.y + rescale( t, static_cast<VECTOR2I::extended_type>( d.y ), l_squared );
365 }
366
367 return VECTOR2<ecoord>( 2 * c.x - aP.x, 2 * c.y - aP.y );
368}
369
370
372{
373 VECTOR2I d = B - A;
374 ecoord l_squared = d.Dot( d );
375
376 if( l_squared == 0 )
377 return A;
378
379 ecoord t = d.Dot( aP - A );
380
381 ecoord xp = rescale( t, ecoord{ d.x }, l_squared );
382 ecoord yp = rescale( t, ecoord{ d.y }, l_squared );
383
384 return VECTOR2<ecoord>( A.x + xp, A.y + yp );
385}
386
387
388int SEG::Distance( const SEG& aSeg ) const
389{
390 return int( isqrt( SquaredDistance( aSeg ) ) );
391}
392
393
394int SEG::Distance( const VECTOR2I& aP ) const
395{
396 return int( isqrt( SquaredDistance( aP ) ) );
397}
398
399
400int SEG::LineDistance( const VECTOR2I& aP, bool aDetermineSide ) const
401{
402 ecoord p = ecoord{ A.y } - B.y;
403 ecoord q = ecoord{ B.x } - A.x;
404 ecoord r = -p * A.x - q * A.y;
405 ecoord l = p * p + q * q;
406 ecoord det = p * aP.x + q * aP.y + r;
407 ecoord dist_sq = 0;
408
409 if( l > 0 )
410 {
411 dist_sq = rescale( det, det, l );
412 }
413
414 ecoord dist = isqrt( dist_sq );
415
416 return static_cast<int>( aDetermineSide ? dist : std::abs( dist ) );
417}
418
419
420bool SEG::mutualDistanceSquared( const SEG& aSeg, ecoord& aD1, ecoord& aD2 ) const
421{
422 SEG a( *this );
423 SEG b( aSeg );
424
425 if( a.SquaredLength() < b.SquaredLength() )
426 std::swap(a, b);
427
428 ecoord p = ecoord{ a.A.y } - a.B.y;
429 ecoord q = ecoord{ a.B.x } - a.A.x;
430 ecoord r = -p * a.A.x - q * a.A.y;
431
432 ecoord l = p * p + q * q;
433
434 if( l == 0 )
435 return false;
436
437 ecoord det1 = p * b.A.x + q * b.A.y + r;
438 ecoord det2 = p * b.B.x + q * b.B.y + r;
439
440 ecoord dsq1 = rescale( det1, det1, l );
441 ecoord dsq2 = rescale( det2, det2, l );
442
443 aD1 = sgn( det1 ) * dsq1;
444 aD2 = sgn( det2 ) * dsq2;
445
446 return true;
447}
448
449bool SEG::ApproxCollinear( const SEG& aSeg, int aDistanceThreshold ) const
450{
451 ecoord thresholdSquared = Square( aDistanceThreshold );
452 ecoord d1_sq, d2_sq;
453
454 if( !mutualDistanceSquared( aSeg, d1_sq, d2_sq ) )
455 return false;
456
457 return std::abs( d1_sq ) <= thresholdSquared && std::abs( d2_sq ) <= thresholdSquared;
458}
459
460
461bool SEG::ApproxParallel( const SEG& aSeg, int aDistanceThreshold ) const
462{
463 ecoord thresholdSquared = Square( aDistanceThreshold );
464 ecoord d1_sq, d2_sq;
465
466 if( ! mutualDistanceSquared( aSeg, d1_sq, d2_sq ) )
467 return false;
468
469 return std::abs( d1_sq - d2_sq ) <= thresholdSquared;
470}
471
472
473bool SEG::ApproxPerpendicular( const SEG& aSeg ) const
474{
475 SEG perp = PerpendicularSeg( A );
476
477 return aSeg.ApproxParallel( perp );
478}
EDA_ANGLE Normalize180()
Definition: eda_angle.h:260
Definition: seg.h:42
const VECTOR2I ReflectPoint(const VECTOR2I &aP) const
Reflect a point using this segment as axis.
Definition: seg.cpp:350
VECTOR2I A
Definition: seg.h:49
int LineDistance(const VECTOR2I &aP, bool aDetermineSide=false) const
Return the closest Euclidean distance between point aP and the line defined by the ends of segment (t...
Definition: seg.cpp:400
ecoord SquaredDistance(const SEG &aSeg) const
Definition: seg.cpp:75
bool intersects(const SEG &aSeg, bool aIgnoreEndpoints=false, bool aLines=false, VECTOR2I *aPt=nullptr) const
Definition: seg.cpp:208
VECTOR2I::extended_type ecoord
Definition: seg.h:44
VECTOR2I B
Definition: seg.h:50
const VECTOR2I NearestPoint(const VECTOR2I &aP) const
Compute a point on the segment (this) that is closest to point aP.
Definition: seg.cpp:327
bool Intersects(const SEG &aSeg) const
Definition: seg.cpp:248
OPT_VECTOR2I Intersect(const SEG &aSeg, bool aIgnoreEndpoints=false, bool aLines=false) const
Compute intersection point of segment (this) with segment aSeg.
Definition: seg.cpp:254
static SEG::ecoord Square(int a)
Definition: seg.h:123
bool Collide(const SEG &aSeg, int aClearance, int *aActual=nullptr) const
Definition: seg.cpp:289
bool ApproxParallel(const SEG &aSeg, int aDistanceThreshold=1) const
Definition: seg.cpp:461
SEG()
Create an empty (0, 0) segment.
Definition: seg.h:55
SEG ParallelSeg(const VECTOR2I &aP) const
Compute a segment parallel to this one, passing through point aP.
Definition: seg.cpp:274
ecoord SquaredLength() const
Definition: seg.h:341
bool ApproxPerpendicular(const SEG &aSeg) const
Definition: seg.cpp:473
bool ApproxCollinear(const SEG &aSeg, int aDistanceThreshold=1) const
Definition: seg.cpp:449
bool mutualDistanceSquared(const SEG &aSeg, ecoord &aD1, ecoord &aD2) const
Definition: seg.cpp:420
bool NearestPoints(const SEG &aSeg, VECTOR2I &aPtA, VECTOR2I &aPtB, int64_t &aDistSq) const
Compute closest points between this segment and aSeg.
Definition: seg.cpp:150
int Distance(const SEG &aSeg) const
Compute minimum Euclidean distance to segment aSeg.
Definition: seg.cpp:388
bool ccw(const VECTOR2I &aA, const VECTOR2I &aB, const VECTOR2I &aC) const
Definition: seg.cpp:283
bool Contains(const SEG &aSeg) const
Definition: seg.h:317
VECTOR2I LineProject(const VECTOR2I &aP) const
Compute the perpendicular projection point of aP on a line passing through ends of the segment.
Definition: seg.cpp:371
SEG PerpendicularSeg(const VECTOR2I &aP) const
Compute a segment perpendicular to this one, passing through point aP.
Definition: seg.cpp:265
EDA_ANGLE Angle(const SEG &aOther) const
Determine the smallest angle between two segments.
Definition: seg.cpp:97
static constexpr extended_type ECOORD_MAX
Definition: vector2d.h:75
VECTOR2_TRAITS< T >::extended_type extended_type
Definition: vector2d.h:72
VECTOR2< T > Perpendicular() const
Compute the perpendicular vector.
Definition: vector2d.h:309
extended_type Cross(const VECTOR2< T > &aVector) const
Compute cross product of self with aVector.
Definition: vector2d.h:535
extended_type Dot(const VECTOR2< T > &aVector) const
Compute dot product of self with aVector.
Definition: vector2d.h:543
static constexpr EDA_ANGLE ANGLE_180
Definition: eda_angle.h:405
EDA_ANGLE abs(const EDA_ANGLE &aAngle)
Definition: eda_angle.h:390
constexpr T ct_sqrt(T x)
Definition: seg.cpp:52
constexpr T sqrt_helper(T x, T lo, T hi)
Definition: seg.cpp:39
static constexpr T sqrt_max_typed
Definition: seg.cpp:58
int sgn(T aVal)
Definition: seg.cpp:33
T isqrt(T x)
Definition: seg.cpp:61
std::optional< VECTOR2I > OPT_VECTOR2I
Definition: seg.h:39
T rescale(T aNumerator, T aValue, T aDenominator)
Scale a number (value) by rational (numerator/denominator).
Definition: util.h:165
VECTOR2< int32_t > VECTOR2I
Definition: vector2d.h:673