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 The 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>
62{
63 T sqrt_max = sqrt_max_typed<T>;
64
65 if( x < 0 )
66 return sqrt_max;
67
68 T r = (T) std::sqrt( (double) x );
69
70 while( r < sqrt_max && r * r < x )
71 r++;
72
73 while( r > sqrt_max || r * r > x )
74 r--;
75
76 return r;
77}
78
79
81{
82 if( Intersects( aSeg ) )
83 return 0;
84
85 const VECTOR2I pts[4] =
86 {
87 aSeg.NearestPoint( A ) - A,
88 aSeg.NearestPoint( B ) - B,
89 NearestPoint( aSeg.A ) - aSeg.A,
90 NearestPoint( aSeg.B ) - aSeg.B
91 };
92
94
95 for( int i = 0; i < 4; i++ )
96 m = std::min( m, pts[i].SquaredEuclideanNorm() );
97
98 return m;
99}
100
101
102EDA_ANGLE SEG::Angle( const SEG& aOther ) const
103{
104 EDA_ANGLE thisAngle = EDA_ANGLE( A - B ).Normalize180();
105 EDA_ANGLE otherAngle = EDA_ANGLE( aOther.A - aOther.B ).Normalize180();
106
107 EDA_ANGLE angle = std::abs( ( thisAngle - otherAngle ).Normalize180() );
108
109 return std::min( ANGLE_180 - angle, angle );
110}
111
112
113const VECTOR2I SEG::NearestPoint( const SEG& aSeg ) const
114{
115 if( OPT_VECTOR2I p = Intersect( aSeg ) )
116 return *p;
117
118 const VECTOR2I pts_origin[4] =
119 {
120 aSeg.NearestPoint( A ),
121 aSeg.NearestPoint( B ),
122 NearestPoint( aSeg.A ),
123 NearestPoint( aSeg.B )
124 };
125
126
127 const VECTOR2I* pts_out[4] =
128 {
129 &A,
130 &B,
131 &pts_origin[2],
132 &pts_origin[3]
133 };
134
135 const ecoord pts_dist[4] =
136 {
137 ( pts_origin[0] - A ).SquaredEuclideanNorm(),
138 ( pts_origin[1] - B ).SquaredEuclideanNorm(),
139 ( pts_origin[2] - aSeg.A ).SquaredEuclideanNorm(),
140 ( pts_origin[3] - aSeg.B ).SquaredEuclideanNorm()
141 };
142
143 int min_i = 0;
144
145 for( int i = 0; i < 4; i++ )
146 {
147 if( pts_dist[i] < pts_dist[min_i] )
148 min_i = i;
149 }
150
151 return *pts_out[min_i];
152}
153
154
155bool SEG::NearestPoints( const SEG& aSeg, VECTOR2I& aPtA, VECTOR2I& aPtB, int64_t& aDistSq ) const
156{
157 if( OPT_VECTOR2I p = Intersect( aSeg ) )
158 {
159 aPtA = aPtB = *p;
160 aDistSq = 0;
161
162 return true;
163 }
164
165 const VECTOR2I pts_origin[4] =
166 {
167 aSeg.NearestPoint( A ),
168 aSeg.NearestPoint( B ),
169 NearestPoint( aSeg.A ),
170 NearestPoint( aSeg.B )
171 };
172
173 const VECTOR2I* pts_a_out[4] =
174 {
175 &A,
176 &B,
177 &pts_origin[2],
178 &pts_origin[3]
179 };
180
181 const VECTOR2I* pts_b_out[4] =
182 {
183 &pts_origin[0],
184 &pts_origin[1],
185 &aSeg.A,
186 &aSeg.B
187 };
188
189 const ecoord pts_dist[4] =
190 {
191 ( pts_origin[0] - A ).SquaredEuclideanNorm(),
192 ( pts_origin[1] - B ).SquaredEuclideanNorm(),
193 ( pts_origin[2] - aSeg.A ).SquaredEuclideanNorm(),
194 ( pts_origin[3] - aSeg.B ).SquaredEuclideanNorm()
195 };
196
197 int min_i = 0;
198
199 for( int i = 0; i < 4; i++ )
200 {
201 if( pts_dist[i] < pts_dist[min_i] )
202 min_i = i;
203 }
204
205 aPtA = *pts_a_out[min_i];
206 aPtB = *pts_b_out[min_i];
207 aDistSq = pts_dist[min_i];
208
209 return true;
210}
211
212
213bool SEG::intersects( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines, VECTOR2I* aPt ) const
214{
215 const VECTOR2<ecoord> e = VECTOR2<ecoord>( B.x - A.x, B.y - A.y );
216 const VECTOR2<ecoord> f = VECTOR2<ecoord>( aSeg.B.x - aSeg.A.x, aSeg.B.y - aSeg.A.y );
217 const VECTOR2<ecoord> ac = VECTOR2<ecoord>( aSeg.A.x - A.x, aSeg.A.y - A.y );
218
219 ecoord d = f.Cross( e );
220 ecoord p = f.Cross( ac );
221 ecoord q = e.Cross( ac );
222
223 if( d == 0 )
224 return false;
225
226 if( !aLines && d > 0 && ( q < 0 || q > d || p < 0 || p > d ) )
227 return false;
228
229 if( !aLines && d < 0 && ( q < d || p < d || p > 0 || q > 0 ) )
230 return false;
231
232 if( !aLines && aIgnoreEndpoints && ( q == 0 || q == d ) && ( p == 0 || p == d ) )
233 return false;
234
235 if( aPt )
236 {
237 VECTOR2<ecoord> result( aSeg.A.x + rescale( q, (ecoord) f.x, d ),
238 aSeg.A.y + rescale( q, (ecoord) f.y, d ) );
239
240 if( abs( result.x ) > std::numeric_limits<VECTOR2I::coord_type>::max()
241 || abs( result.y ) > std::numeric_limits<VECTOR2I::coord_type>::max() )
242 {
243 return false;
244 }
245
246 *aPt = VECTOR2I( (int) result.x, (int) result.y );
247 }
248
249 return true;
250}
251
252
253bool SEG::Intersects( const SEG& aSeg ) const
254{
255 return intersects( aSeg );
256}
257
258
259OPT_VECTOR2I SEG::Intersect( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines ) const
260{
261 VECTOR2I ip;
262
263 if( intersects( aSeg, aIgnoreEndpoints, aLines, &ip ) )
264 return ip;
265 else
266 return OPT_VECTOR2I();
267}
268
269
271{
272 VECTOR2I slope( B - A );
273 VECTOR2I endPoint = slope.Perpendicular() + aP;
274
275 return SEG( aP, endPoint );
276}
277
278
279SEG SEG::ParallelSeg( const VECTOR2I& aP ) const
280{
281 VECTOR2I slope( B - A );
282 VECTOR2I endPoint = slope + aP;
283
284 return SEG( aP, endPoint );
285}
286
287
288bool SEG::ccw( const VECTOR2I& aA, const VECTOR2I& aB, const VECTOR2I& aC ) const
289{
290 return (ecoord) ( aC.y - aA.y ) * ( aB.x - aA.x ) > (ecoord) ( aB.y - aA.y ) * ( aC.x - aA.x );
291}
292
293
294bool SEG::Collide( const SEG& aSeg, int aClearance, int* aActual ) const
295{
296 // check for intersection
297 // fixme: move to a method
298 if( ccw( A, aSeg.A, aSeg.B ) != ccw( B, aSeg.A, aSeg.B ) &&
299 ccw( A, B, aSeg.A ) != ccw( A, B, aSeg.B ) )
300 {
301 if( aActual )
302 *aActual = 0;
303
304 return true;
305 }
306
308 ecoord clearance_sq = (ecoord) aClearance * aClearance;
309
310 auto checkCollision =
311 [&]( bool aFinal )
312 {
313 if( dist_sq == 0 )
314 {
315 if( aActual )
316 *aActual = 0;
317
318 return true;
319 }
320 else if( dist_sq < clearance_sq )
321 {
322 if( aActual )
323 {
324 if( aFinal )
325 {
326 *aActual = int( isqrt( dist_sq ) );
327 return true;
328 }
329 else
330 {
331 // We have to keep going to ensure we have the lowest value
332 // for aActual
333 return false;
334 }
335 }
336
337 return true;
338 }
339
340 return false;
341 };
342
343 dist_sq = std::min( dist_sq, SquaredDistance( aSeg.A ) );
344
345 if( checkCollision( false ) )
346 return true;
347
348 dist_sq = std::min( dist_sq, SquaredDistance( aSeg.B ) );
349
350 if( checkCollision( false ) )
351 return true;
352
353 dist_sq = std::min( dist_sq, aSeg.SquaredDistance( A ) );
354
355 if( checkCollision( false ) )
356 return true;
357
358 dist_sq = std::min( dist_sq, aSeg.SquaredDistance( B ) );
359
360 return checkCollision( true );
361}
362
363
364bool SEG::Contains( const VECTOR2I& aP ) const
365{
366 return SquaredDistance( aP ) <= 3;
367}
368
369
370const VECTOR2I SEG::NearestPoint( const VECTOR2I& aP ) const
371{
372 // Inlined for performance reasons
373 VECTOR2L d;
374 d.x = static_cast<int64_t>( B.x ) - A.x;
375 d.y = static_cast<int64_t>( B.y ) - A.y;
376
377 ecoord l_squared( d.x * d.x + d.y * d.y );
378
379 if( l_squared == 0 )
380 return A;
381
382 // Inlined for performance reasons
383 VECTOR2L pa;
384 pa.x = static_cast<int64_t>( aP.x ) - A.x;
385 pa.y = static_cast<int64_t>( aP.y ) - A.y;
386
387 ecoord t = d.Dot( pa );
388
389 if( t < 0 )
390 return A;
391 else if( t > l_squared )
392 return B;
393
394 ecoord xp = rescale( t, (ecoord) d.x, l_squared );
395 ecoord yp = rescale( t, (ecoord) d.y, l_squared );
396
397 return VECTOR2<ecoord>( A.x + xp, A.y + yp );
398}
399
400
401const VECTOR2I SEG::ReflectPoint( const VECTOR2I& aP ) const
402{
403 VECTOR2I d = B - A;
404 VECTOR2I::extended_type l_squared = d.Dot( d );
405 VECTOR2I::extended_type t = d.Dot( aP - A );
407
408 if( !l_squared )
409 {
410 c = aP;
411 }
412 else
413 {
414 c.x = A.x + rescale( t, static_cast<VECTOR2I::extended_type>( d.x ), l_squared );
415 c.y = A.y + rescale( t, static_cast<VECTOR2I::extended_type>( d.y ), l_squared );
416 }
417
418 return VECTOR2<ecoord>( 2 * c.x - aP.x, 2 * c.y - aP.y );
419}
420
421
423{
424 VECTOR2I d = B - A;
425 ecoord l_squared = d.Dot( d );
426
427 if( l_squared == 0 )
428 return A;
429
430 ecoord t = d.Dot( aP - A );
431
432 ecoord xp = rescale( t, ecoord{ d.x }, l_squared );
433 ecoord yp = rescale( t, ecoord{ d.y }, l_squared );
434
435 return VECTOR2<ecoord>( A.x + xp, A.y + yp );
436}
437
438
439int SEG::Distance( const SEG& aSeg ) const
440{
441 return int( isqrt( SquaredDistance( aSeg ) ) );
442}
443
444
445int SEG::Distance( const VECTOR2I& aP ) const
446{
447 return int( isqrt( SquaredDistance( aP ) ) );
448}
449
450
452{
453 // Using the VECTOR2L() reflexive c'tor is a performance hit.
454 // Even sticking these in a lambda still invokes it.
455 VECTOR2L ab, ap;
456 ab.x = static_cast<int64_t>( B.x ) - A.x;
457 ab.y = static_cast<int64_t>( B.y ) - A.y;
458 ap.x = static_cast<int64_t>( aP.x ) - A.x;
459 ap.y = static_cast<int64_t>( aP.y ) - A.y;
460
461 ecoord e = ap.Dot( ab );
462
463 if( e <= 0 )
464 return ap.SquaredEuclideanNorm();
465
467
468 if( e >= f )
469 {
470 VECTOR2L bp;
471 bp.x = static_cast<int64_t>( aP.x ) - B.x;
472 bp.y = static_cast<int64_t>( aP.y ) - B.y;
473 return bp.Dot( bp );
474 }
475
476 const double g = ap.SquaredEuclideanNorm() - ( double( e ) * e ) / f;
477
478 // The only way g can be negative is if there was a rounding error since
479 // e is the projection of aP onto ab and therefore cannot be greater than
480 // the length of ap and f is guaranteed to be greater than e, meaning
481 // e * e / f cannot be greater than ap.SquaredEuclideanNorm()
482 if( g < 0 || g > static_cast<double>( std::numeric_limits<ecoord>::max() ) )
483 return 0;
484
485 return KiROUND<double, ecoord>( g );
486}
487
488
489int SEG::LineDistance( const VECTOR2I& aP, bool aDetermineSide ) const
490{
491 ecoord p = ecoord{ A.y } - B.y;
492 ecoord q = ecoord{ B.x } - A.x;
493 ecoord r = -p * A.x - q * A.y;
494 ecoord l = p * p + q * q;
495 ecoord det = p * aP.x + q * aP.y + r;
496 ecoord dist_sq = 0;
497
498 if( l > 0 )
499 {
500 dist_sq = rescale( det, det, l );
501 }
502
503 ecoord dist = isqrt( dist_sq );
504
505 return static_cast<int>( aDetermineSide ? sgn( det ) * dist : std::abs( dist ) );
506}
507
508
509bool SEG::mutualDistanceSquared( const SEG& aSeg, ecoord& aD1, ecoord& aD2 ) const
510{
511 SEG a( *this );
512 SEG b( aSeg );
513
514 if( a.SquaredLength() < b.SquaredLength() )
515 std::swap(a, b);
516
517 ecoord p = ecoord{ a.A.y } - a.B.y;
518 ecoord q = ecoord{ a.B.x } - a.A.x;
519 ecoord r = -p * a.A.x - q * a.A.y;
520
521 ecoord l = p * p + q * q;
522
523 if( l == 0 )
524 return false;
525
526 ecoord det1 = p * b.A.x + q * b.A.y + r;
527 ecoord det2 = p * b.B.x + q * b.B.y + r;
528
529 ecoord dsq1 = rescale( det1, det1, l );
530 ecoord dsq2 = rescale( det2, det2, l );
531
532 aD1 = sgn( det1 ) * dsq1;
533 aD2 = sgn( det2 ) * dsq2;
534
535 return true;
536}
537
538bool SEG::ApproxCollinear( const SEG& aSeg, int aDistanceThreshold ) const
539{
540 ecoord thresholdSquared = Square( aDistanceThreshold );
541 ecoord d1_sq, d2_sq;
542
543 if( !mutualDistanceSquared( aSeg, d1_sq, d2_sq ) )
544 return false;
545
546 return std::abs( d1_sq ) <= thresholdSquared && std::abs( d2_sq ) <= thresholdSquared;
547}
548
549
550bool SEG::ApproxParallel( const SEG& aSeg, int aDistanceThreshold ) const
551{
552 ecoord thresholdSquared = Square( aDistanceThreshold );
553 ecoord d1_sq, d2_sq;
554
555 if( ! mutualDistanceSquared( aSeg, d1_sq, d2_sq ) )
556 return false;
557
558 return std::abs( d1_sq - d2_sq ) <= thresholdSquared;
559}
560
561
562bool SEG::ApproxPerpendicular( const SEG& aSeg ) const
563{
564 SEG perp = PerpendicularSeg( A );
565
566 return aSeg.ApproxParallel( perp );
567}
EDA_ANGLE Normalize180()
Definition: eda_angle.h:263
Definition: seg.h:42
const VECTOR2I ReflectPoint(const VECTOR2I &aP) const
Reflect a point using this segment as axis.
Definition: seg.cpp:401
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:489
ecoord SquaredDistance(const SEG &aSeg) const
Definition: seg.cpp:80
bool intersects(const SEG &aSeg, bool aIgnoreEndpoints=false, bool aLines=false, VECTOR2I *aPt=nullptr) const
Definition: seg.cpp:213
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:370
bool Intersects(const SEG &aSeg) const
Definition: seg.cpp:253
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:259
static SEG::ecoord Square(int a)
Definition: seg.h:123
bool Collide(const SEG &aSeg, int aClearance, int *aActual=nullptr) const
Definition: seg.cpp:294
bool ApproxParallel(const SEG &aSeg, int aDistanceThreshold=1) const
Definition: seg.cpp:550
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:279
ecoord SquaredLength() const
Definition: seg.h:338
bool ApproxPerpendicular(const SEG &aSeg) const
Definition: seg.cpp:562
bool ApproxCollinear(const SEG &aSeg, int aDistanceThreshold=1) const
Definition: seg.cpp:538
bool mutualDistanceSquared(const SEG &aSeg, ecoord &aD1, ecoord &aD2) const
Definition: seg.cpp:509
bool NearestPoints(const SEG &aSeg, VECTOR2I &aPtA, VECTOR2I &aPtB, int64_t &aDistSq) const
Compute closest points between this segment and aSeg.
Definition: seg.cpp:155
int Distance(const SEG &aSeg) const
Compute minimum Euclidean distance to segment aSeg.
Definition: seg.cpp:439
bool ccw(const VECTOR2I &aA, const VECTOR2I &aB, const VECTOR2I &aC) const
Definition: seg.cpp:288
bool Contains(const SEG &aSeg) const
Definition: seg.h:314
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:422
SEG PerpendicularSeg(const VECTOR2I &aP) const
Compute a segment perpendicular to this one, passing through point aP.
Definition: seg.cpp:270
EDA_ANGLE Angle(const SEG &aOther) const
Determine the smallest angle between two segments.
Definition: seg.cpp:102
constexpr extended_type Cross(const VECTOR2< T > &aVector) const
Compute cross product of self with aVector.
Definition: vector2d.h:546
constexpr extended_type SquaredEuclideanNorm() const
Compute the squared euclidean norm of the vector, which is defined as (x ** 2 + y ** 2).
Definition: vector2d.h:307
static constexpr extended_type ECOORD_MAX
Definition: vector2d.h:76
VECTOR2_TRAITS< T >::extended_type extended_type
Definition: vector2d.h:73
constexpr VECTOR2< T > Perpendicular() const
Compute the perpendicular vector.
Definition: vector2d.h:314
constexpr extended_type Dot(const VECTOR2< T > &aVector) const
Compute dot product of self with aVector.
Definition: vector2d.h:554
static constexpr EDA_ANGLE ANGLE_180
Definition: eda_angle.h:408
EDA_ANGLE abs(const EDA_ANGLE &aAngle)
Definition: eda_angle.h:393
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:153
VECTOR2< int32_t > VECTOR2I
Definition: vector2d.h:695