KiCad PCB EDA Suite
Loading...
Searching...
No Matches
shape_collisions.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 * Copyright (C) 2015-2022 KiCad Developers, see AUTHORS.txt for contributors.
6 * @author Tomasz Wlostowski <[email protected]>
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 <cmath>
27#include <limits>
28
29#include <geometry/seg.h> // for SEG
30#include <geometry/shape.h>
31#include <geometry/shape_arc.h>
34#include <geometry/shape_rect.h>
38#include <math/vector2d.h>
39
41
42
43static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_CIRCLE& aB, int aClearance,
44 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
45{
46 ecoord min_dist = aClearance + aA.GetRadius() + aB.GetRadius();
47 ecoord min_dist_sq = min_dist * min_dist;
48
49 const VECTOR2I delta = aB.GetCenter() - aA.GetCenter();
50 ecoord dist_sq = delta.SquaredEuclideanNorm();
51
52 if( dist_sq == 0 || dist_sq < min_dist_sq )
53 {
54 if( aActual )
55 *aActual = std::max( 0, (int) sqrt( dist_sq ) - aA.GetRadius() - aB.GetRadius() );
56
57 if( aLocation )
58 *aLocation = ( aA.GetCenter() + aB.GetCenter() ) / 2;
59
60 if( aMTV )
61 *aMTV = delta.Resize( min_dist - sqrt( dist_sq ) + 3 ); // fixme: apparent rounding error
62
63 return true;
64 }
65
66 return false;
67}
68
69
70static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance,
71 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
72{
73 const VECTOR2I c = aB.GetCenter();
74 const VECTOR2I p0 = aA.GetPosition();
75 const VECTOR2I size = aA.GetSize();
76 const int r = aB.GetRadius();
77 const int min_dist = aClearance + r;
78 const ecoord min_dist_sq = SEG::Square( min_dist );
79
80 const VECTOR2I vts[] =
81 {
82 VECTOR2I( p0.x, p0.y ),
83 VECTOR2I( p0.x, p0.y + size.y ),
84 VECTOR2I( p0.x + size.x, p0.y + size.y ),
85 VECTOR2I( p0.x + size.x, p0.y ),
86 VECTOR2I( p0.x, p0.y )
87 };
88
89 ecoord nearest_side_dist_sq = VECTOR2I::ECOORD_MAX;
90 VECTOR2I nearest;
91
92 bool inside = c.x >= p0.x && c.x <= ( p0.x + size.x )
93 && c.y >= p0.y && c.y <= ( p0.y + size.y );
94
95 // If we're not looking for MTV or actual, short-circuit once we find a hard collision
96 if( inside && !aActual && !aLocation && !aMTV )
97 return true;
98
99 for( int i = 0; i < 4; i++ )
100 {
101 const SEG side( vts[i], vts[ i + 1] );
102
103 VECTOR2I pn = side.NearestPoint( c );
104 ecoord side_dist_sq = ( pn - c ).SquaredEuclideanNorm();
105
106 if( side_dist_sq < nearest_side_dist_sq )
107 {
108 nearest = pn;
109 nearest_side_dist_sq = side_dist_sq;
110
111 if( aMTV )
112 continue;
113
114 if( nearest_side_dist_sq == 0 )
115 break;
116
117 // If we're not looking for aActual then any collision will do
118 if( nearest_side_dist_sq < min_dist_sq && !aActual )
119 break;
120 }
121 }
122
123 if( inside || nearest_side_dist_sq == 0 || nearest_side_dist_sq < min_dist_sq )
124 {
125 if( aLocation )
126 *aLocation = nearest;
127
128 if( aActual )
129 *aActual = std::max( 0, (int) sqrt( nearest_side_dist_sq ) - r );
130
131 if( aMTV )
132 {
133 VECTOR2I delta = c - nearest;
134
135 if( inside )
136 *aMTV = -delta.Resize( abs( min_dist + 1 + sqrt( nearest_side_dist_sq ) ) + 1 );
137 else
138 *aMTV = delta.Resize( abs( min_dist + 1 - sqrt( nearest_side_dist_sq ) ) + 1 );
139 }
140
141 return true;
142 }
143
144 return false;
145}
146
147
148static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance )
149{
150 VECTOR2I f( 0, 0 );
151
152 const VECTOR2I c = aA.GetCenter();
153 const VECTOR2I nearest = aB.NearestPoint( c );
154
155 const int r = aA.GetRadius();
156
157 int dist = ( nearest - c ).EuclideanNorm();
158 int min_dist = aClearance + r;
159
160 if( dist < min_dist )
161 {
162 for( int corr = 0; corr < 5; corr++ )
163 {
164 f = ( aA.GetCenter() - nearest ).Resize( min_dist - dist + corr );
165
166 if( aB.Distance( c + f ) >= min_dist )
167 break;
168 }
169 }
170
171 return f;
172}
173
174
175static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_LINE_CHAIN_BASE& aB,
176 int aClearance, int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
177{
178 int closest_dist = std::numeric_limits<int>::max();
179 int closest_mtv_dist = std::numeric_limits<int>::max();
180 VECTOR2I nearest;
181 int closest_mtv_seg = -1;
182
183 if( aB.IsClosed() && aB.PointInside( aA.GetCenter() ) )
184 {
185 nearest = aA.GetCenter();
186 closest_dist = 0;
187
188 if( aMTV )
189 {
190 for( size_t s = 0; s < aB.GetSegmentCount(); s++ )
191 {
192 int dist = aB.GetSegment(s).Distance( aA.GetCenter() );
193
194 if( dist < closest_mtv_dist )
195 {
196 closest_mtv_dist = dist;
197 closest_mtv_seg = s;
198 }
199 }
200 }
201
202 }
203 else
204 {
205 for( size_t s = 0; s < aB.GetSegmentCount(); s++ )
206 {
207 int collision_dist = 0;
208 VECTOR2I pn;
209
210 if( aA.Collide( aB.GetSegment( s ), aClearance,
211 aActual || aLocation ? &collision_dist : nullptr,
212 aLocation ? &pn : nullptr ) )
213 {
214 if( collision_dist < closest_dist )
215 {
216 nearest = pn;
217 closest_dist = collision_dist;
218 }
219
220 if( closest_dist == 0 )
221 break;
222
223 // If we're not looking for aActual then any collision will do
224 if( !aActual )
225 break;
226 }
227 }
228 }
229
230 if( closest_dist == 0 || closest_dist < aClearance )
231 {
232 if( aLocation )
233 *aLocation = nearest;
234
235 if( aActual )
236 *aActual = closest_dist;
237
238 if( aMTV )
239 {
240 SHAPE_CIRCLE cmoved( aA );
241 VECTOR2I f_total( 0, 0 );
242
243 VECTOR2I f;
244
245 if (closest_mtv_seg >= 0)
246 {
247 SEG cs = aB.GetSegment( closest_mtv_seg );
248 VECTOR2I np = cs.NearestPoint( aA.GetCenter() );
249 f = ( np - aA.GetCenter() ) + ( np - aA.GetCenter() ).Resize( aA.GetRadius() );
250 }
251
252 cmoved.SetCenter( cmoved.GetCenter() + f );
253 f_total += f;
254
255 for( size_t s = 0; s < aB.GetSegmentCount(); s++ )
256 {
257 f = pushoutForce( cmoved, aB.GetSegment( s ), aClearance );
258 cmoved.SetCenter( cmoved.GetCenter() + f );
259 f_total += f;
260 }
261
262 *aMTV = f_total;
263 }
264
265 return true;
266 }
267
268 return false;
269}
270
271
272static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_SEGMENT& aSeg, int aClearance,
273 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
274{
275 if( aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2, aActual, aLocation ) )
276 {
277 if( aMTV )
278 *aMTV = -pushoutForce( aA, aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2);
279
280 if( aActual )
281 *aActual = std::max( 0, *aActual - aSeg.GetWidth() / 2 );
282
283 return true;
284 }
285
286 return false;
287}
288
289
290static inline bool Collide( const SHAPE_LINE_CHAIN_BASE& aA, const SHAPE_LINE_CHAIN_BASE& aB,
291 int aClearance, int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
292{
293 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
294 aA.TypeName(),
295 aB.TypeName() ) );
296
297 int closest_dist = std::numeric_limits<int>::max();
298 VECTOR2I nearest;
299
300 if( aB.IsClosed() && aA.GetPointCount() > 0 && aB.PointInside( aA.GetPoint( 0 ) ) )
301 {
302 closest_dist = 0;
303 nearest = aA.GetPoint( 0 );
304 }
305 else
306 {
307 for( size_t i = 0; i < aB.GetSegmentCount(); i++ )
308 {
309 int collision_dist = 0;
310 VECTOR2I pn;
311
312 if( aB.Type() == SH_LINE_CHAIN )
313 {
314 const SHAPE_LINE_CHAIN* aB_line_chain = static_cast<const SHAPE_LINE_CHAIN*>( &aB );
315
316 // ignore arcs - we will collide these separately
317 if( aB_line_chain->IsArcSegment( i ) )
318 continue;
319 }
320
321 if( aA.Collide( aB.GetSegment( i ), aClearance,
322 aActual || aLocation ? &collision_dist : nullptr,
323 aLocation ? &pn : nullptr ) )
324 {
325 if( collision_dist < closest_dist )
326 {
327 nearest = pn;
328 closest_dist = collision_dist;
329 }
330
331 if( closest_dist == 0 )
332 break;
333
334 // If we're not looking for aActual then any collision will do
335 if( !aActual )
336 break;
337 }
338 }
339
340 if( aB.Type() == SH_LINE_CHAIN )
341 {
342 const SHAPE_LINE_CHAIN* aB_line_chain = static_cast<const SHAPE_LINE_CHAIN*>( &aB );
343
344 for( size_t i = 0; i < aB_line_chain->ArcCount(); i++ )
345 {
346 const SHAPE_ARC& arc = aB_line_chain->Arc( i );
347
348 // The arcs in the chain should have zero width
349 wxASSERT_MSG( arc.GetWidth() == 0, wxT( "Invalid arc width - should be zero" ) );
350
351 if( arc.Collide( &aA, aClearance, aActual, aLocation ) )
352 return true;
353 }
354 }
355 }
356
357 if( closest_dist == 0 || closest_dist < aClearance )
358 {
359 if( aLocation )
360 *aLocation = nearest;
361
362 if( aActual )
363 *aActual = closest_dist;
364
365 return true;
366 }
367
368 return false;
369}
370
371
372static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_LINE_CHAIN_BASE& aB, int aClearance,
373 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
374{
375 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
376 aA.TypeName(),
377 aB.TypeName() ) );
378
379 int closest_dist = std::numeric_limits<int>::max();
380 VECTOR2I nearest;
381
382 if( aB.IsClosed() && aB.PointInside( aA.Centre() ) )
383 {
384 nearest = aA.Centre();
385 closest_dist = 0;
386 }
387 else
388 {
389 for( size_t s = 0; s < aB.GetSegmentCount(); s++ )
390 {
391 int collision_dist = 0;
392 VECTOR2I pn;
393
394 if( aA.Collide( aB.GetSegment( s ), aClearance,
395 aActual || aLocation ? &collision_dist : nullptr,
396 aLocation ? &pn : nullptr ) )
397 {
398 if( collision_dist < closest_dist )
399 {
400 nearest = pn;
401 closest_dist = collision_dist;
402 }
403
404 if( closest_dist == 0 )
405 break;
406
407 // If we're not looking for aActual then any collision will do
408 if( !aActual )
409 break;
410 }
411 }
412 }
413
414 if( closest_dist == 0 || closest_dist < aClearance )
415 {
416 if( aLocation )
417 *aLocation = nearest;
418
419 if( aActual )
420 *aActual = closest_dist;
421
422 return true;
423 }
424
425 return false;
426}
427
428
429static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_SEGMENT& aB, int aClearance,
430 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
431{
432 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
433 aA.TypeName(),
434 aB.TypeName() ) );
435
436 bool rv = aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2, aActual, aLocation );
437
438 if( aActual )
439 *aActual = std::max( 0, *aActual - aB.GetWidth() / 2 );
440
441 return rv;
442}
443
444
445static inline bool Collide( const SHAPE_SEGMENT& aA, const SHAPE_SEGMENT& aB, int aClearance,
446 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
447{
448 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
449 aA.TypeName(),
450 aB.TypeName() ) );
451
452 bool rv = aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2, aActual, aLocation );
453
454 if( aActual )
455 *aActual = std::max( 0, *aActual - aB.GetWidth() / 2 );
456
457 return rv;
458}
459
460
461static inline bool Collide( const SHAPE_LINE_CHAIN_BASE& aA, const SHAPE_SEGMENT& aB,
462 int aClearance, int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
463{
464 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
465 aA.TypeName(),
466 aB.TypeName() ) );
467
468 bool rv = aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2, aActual, aLocation );
469
470 if( aActual )
471 *aActual = std::max( 0, *aActual - aB.GetWidth() / 2 );
472
473 return rv;
474}
475
476
477static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_RECT& aB, int aClearance,
478 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
479{
480 if( aClearance || aActual || aLocation || aMTV )
481 {
482 return Collide( aA.Outline(), aB.Outline(), aClearance, aActual, aLocation, aMTV );
483 }
484 else
485 {
486 BOX2I bboxa = aA.BBox();
487 BOX2I bboxb = aB.BBox();
488
489 return bboxa.Intersects( bboxb );
490 }
491}
492
493
494static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_RECT& aB, int aClearance,
495 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
496{
497 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
498 aA.TypeName(),
499 aB.TypeName() ) );
500
501 const SHAPE_LINE_CHAIN lc( aA );
502
503 bool rv = Collide( lc, aB.Outline(), aClearance + aA.GetWidth() / 2, aActual, aLocation, aMTV );
504
505 if( rv && aActual )
506 *aActual = std::max( 0, *aActual - aA.GetWidth() / 2 );
507
508 return rv;
509}
510
511
512static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_CIRCLE& aB, int aClearance,
513 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
514{
515 const SHAPE_LINE_CHAIN lc( aA );
516
517 bool rv = Collide( aB, lc, aClearance + aA.GetWidth() / 2, aActual, aLocation, aMTV );
518
519 if( rv && aActual )
520 *aActual = std::max( 0, *aActual - aA.GetWidth() / 2 );
521
522 return rv;
523}
524
525
526static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_LINE_CHAIN& aB, int aClearance,
527 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
528{
529 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
530 aA.TypeName(),
531 aB.TypeName() ) );
532
533 int closest_dist = std::numeric_limits<int>::max();
534 VECTOR2I nearest;
535
536 if( aB.IsClosed() && aB.PointInside( aA.GetP0() ) )
537 {
538 closest_dist = 0;
539 nearest = aA.GetP0();
540 }
541 else
542 {
543 for( size_t i = 0; i < aB.GetSegmentCount(); i++ )
544 {
545 int collision_dist = 0;
546 VECTOR2I pn;
547
548 // ignore arcs - we will collide these separately
549 if( aB.IsArcSegment( i ) )
550 continue;
551
552 if( aA.Collide( aB.GetSegment( i ), aClearance,
553 aActual || aLocation ? &collision_dist : nullptr,
554 aLocation ? &pn : nullptr ) )
555 {
556 if( collision_dist < closest_dist )
557 {
558 nearest = pn;
559 closest_dist = collision_dist;
560 }
561
562 if( closest_dist == 0 )
563 break;
564
565 // If we're not looking for aActual then any collision will do
566 if( !aActual )
567 break;
568 }
569 }
570
571 for( size_t i = 0; i < aB.ArcCount(); i++ )
572 {
573 const SHAPE_ARC& arc = aB.Arc( i );
574
575 // The arcs in the chain should have zero width
576 wxASSERT_MSG( arc.GetWidth() == 0, wxT( "Invalid arc width - should be zero" ) );
577
578 if( arc.Collide( &aA, aClearance, aActual, aLocation ) )
579 return true;
580 }
581 }
582
583 if( closest_dist == 0 || closest_dist < aClearance )
584 {
585 if( aLocation )
586 *aLocation = nearest;
587
588 if( aActual )
589 *aActual = closest_dist;
590
591 return true;
592 }
593
594 return false;
595}
596
597
598static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_SEGMENT& aB, int aClearance,
599 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
600{
601 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
602 aA.TypeName(),
603 aB.TypeName() ) );
604
605 const SHAPE_LINE_CHAIN lc( aA );
606
607 bool rv = Collide( lc, aB, aClearance + aA.GetWidth() / 2, aActual, aLocation, aMTV );
608
609 if( rv && aActual )
610 *aActual = std::max( 0, *aActual - aA.GetWidth() / 2 );
611
612 return rv;
613}
614
615
616static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_LINE_CHAIN_BASE& aB, int aClearance,
617 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
618{
619 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
620 aA.TypeName(),
621 aB.TypeName() ) );
622
623 int closest_dist = std::numeric_limits<int>::max();
624 VECTOR2I nearest;
625
626 if( aB.IsClosed() && aB.PointInside( aA.GetP0() ) )
627 {
628 closest_dist = 0;
629 nearest = aA.GetP0();
630 }
631 else
632 {
633 for( size_t i = 0; i < aB.GetSegmentCount(); i++ )
634 {
635 int collision_dist = 0;
636 VECTOR2I pn;
637
638 if( aA.Collide( aB.GetSegment( i ), aClearance,
639 aActual || aLocation ? &collision_dist : nullptr,
640 aLocation ? &pn : nullptr ) )
641 {
642 if( collision_dist < closest_dist )
643 {
644 nearest = pn;
645 closest_dist = collision_dist;
646 }
647
648 if( closest_dist == 0 )
649 break;
650
651 // If we're not looking for aActual then any collision will do
652 if( !aActual )
653 break;
654 }
655 }
656 }
657
658 if( closest_dist == 0 || closest_dist < aClearance )
659 {
660 if( aLocation )
661 *aLocation = nearest;
662
663 if( aActual )
664 *aActual = closest_dist;
665
666 return true;
667 }
668
669 return false;
670}
671
672
673static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_ARC& aB, int aClearance,
674 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
675{
676 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
677 aA.TypeName(),
678 aB.TypeName() ) );
679
680 SEG mediatrix( aA.GetCenter(), aB.GetCenter() );
681
682 std::vector<VECTOR2I> ips;
683
684 // Basic case - arcs intersect
685 if( aA.Intersect( aB, &ips ) > 0 )
686 {
687 if( aActual )
688 *aActual = 0;
689
690 if( aLocation )
691 *aLocation = ips[0]; // Pick the first intersection point
692
693 return true;
694 }
695
696 // Arcs don't intersect, build a list of points to check
697 std::vector<VECTOR2I> ptsA;
698 std::vector<VECTOR2I> ptsB;
699
700 bool cocentered = ( mediatrix.A == mediatrix.B );
701
702 // 1: Interior points of both arcs, which are on the line segment between the two centres
703 if( !cocentered )
704 {
705 aA.IntersectLine( mediatrix, &ptsA );
706 aB.IntersectLine( mediatrix, &ptsB );
707 }
708
709 // 2: Check arc end points
710 ptsA.push_back( aA.GetP0() );
711 ptsA.push_back( aA.GetP1() );
712 ptsB.push_back( aB.GetP0() );
713 ptsB.push_back( aB.GetP1() );
714
715 // 3: Endpoint of one and "projected" point on the other, which is on the
716 // line segment through that endpoint and the centre of the other arc
717 aA.IntersectLine( SEG( aB.GetP0(), aA.GetCenter() ), &ptsA );
718 aA.IntersectLine( SEG( aB.GetP1(), aA.GetCenter() ), &ptsA );
719
720 aB.IntersectLine( SEG( aA.GetP0(), aB.GetCenter() ), &ptsB );
721 aB.IntersectLine( SEG( aA.GetP1(), aB.GetCenter() ), &ptsB );
722
723 double minDist = std::numeric_limits<double>::max();
724 SEG minDistSeg;
725 bool rv = false;
726
727 int widths = ( aA.GetWidth() / 2 ) + ( aB.GetWidth() / 2 );
728
729 // @todo performance could be improved by only checking certain points (e.g only check end
730 // points against other end points or their corresponding "projected" points)
731 for( const VECTOR2I& ptA : ptsA )
732 {
733 for( const VECTOR2I& ptB : ptsB )
734 {
735 SEG candidateMinDist( ptA, ptB );
736 int dist = candidateMinDist.Length() - widths;
737
738 if( dist < aClearance )
739 {
740 if( !rv || dist < minDist )
741 {
742 minDist = dist;
743 minDistSeg = candidateMinDist;
744 }
745
746 rv = true;
747 }
748 }
749 }
750
751 if( rv && aActual )
752 *aActual = std::max( 0, minDistSeg.Length() - widths );
753
754 if( rv && aLocation )
755 *aLocation = minDistSeg.Center();
756
757 return rv;
758}
759
760
761template<class T_a, class T_b>
762inline bool CollCase( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
763 VECTOR2I* aLocation, VECTOR2I* aMTV )
764
765{
766 return Collide( *static_cast<const T_a*>( aA ), *static_cast<const T_b*>( aB ),
767 aClearance, aActual, aLocation, aMTV);
768}
769
770
771template<class T_a, class T_b>
772inline bool CollCaseReversed ( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
773 VECTOR2I* aLocation, VECTOR2I* aMTV )
774{
775 bool rv = Collide( *static_cast<const T_b*>( aB ), *static_cast<const T_a*>( aA ),
776 aClearance, aActual, aLocation, aMTV);
777
778 if( rv && aMTV)
779 *aMTV = - *aMTV;
780
781 return rv;
782}
783
784
785static bool collideSingleShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
786 VECTOR2I* aLocation, VECTOR2I* aMTV )
787{
788 if( aA->Type() == SH_POLY_SET )
789 {
790 const SHAPE_POLY_SET* polySetA = static_cast<const SHAPE_POLY_SET*>( aA );
791
792 wxASSERT( !aMTV );
793 return polySetA->Collide( aB, aClearance, aActual, aLocation );
794 }
795 else if( aB->Type() == SH_POLY_SET )
796 {
797 const SHAPE_POLY_SET* polySetB = static_cast<const SHAPE_POLY_SET*>( aB );
798
799 wxASSERT( !aMTV );
800 return polySetB->Collide( aA, aClearance, aActual, aLocation );
801 }
802
803 switch( aA->Type() )
804 {
805 case SH_NULL:
806 return false;
807
808 case SH_RECT:
809 switch( aB->Type() )
810 {
811 case SH_RECT:
812 return CollCase<SHAPE_RECT, SHAPE_RECT>( aA, aB, aClearance, aActual, aLocation, aMTV );
813
814 case SH_CIRCLE:
815 return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
816
817 case SH_LINE_CHAIN:
818 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
819
820 case SH_SEGMENT:
821 return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
822
823 case SH_SIMPLE:
825 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
826
827 case SH_ARC:
828 return CollCaseReversed<SHAPE_RECT, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
829
830 case SH_NULL:
831 return false;
832
833 default:
834 break;
835 }
836 break;
837
838 case SH_CIRCLE:
839 switch( aB->Type() )
840 {
841 case SH_RECT:
842 return CollCaseReversed<SHAPE_CIRCLE, SHAPE_RECT>( aA, aB, aClearance, aActual, aLocation, aMTV );
843
844 case SH_CIRCLE:
845 return CollCase<SHAPE_CIRCLE, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
846
847 case SH_LINE_CHAIN:
848 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
849
850 case SH_SEGMENT:
851 return CollCase<SHAPE_CIRCLE, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
852
853 case SH_SIMPLE:
855 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
856
857 case SH_ARC:
858 return CollCaseReversed<SHAPE_CIRCLE, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
859
860 case SH_NULL:
861 return false;
862
863 default:
864 break;
865 }
866 break;
867
868 case SH_LINE_CHAIN:
869 switch( aB->Type() )
870 {
871 case SH_RECT:
872 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aActual, aLocation, aMTV );
873
874 case SH_CIRCLE:
875 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aActual, aLocation, aMTV );
876
877 case SH_LINE_CHAIN:
878 return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
879
880 case SH_SEGMENT:
881 return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
882
883 case SH_SIMPLE:
885 return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
886
887 case SH_ARC:
888 return CollCaseReversed<SHAPE_LINE_CHAIN, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
889
890 case SH_NULL:
891 return false;
892
893 default:
894 break;
895 }
896 break;
897
898 case SH_SEGMENT:
899 switch( aB->Type() )
900 {
901 case SH_RECT:
902 return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aB, aA, aClearance, aActual, aLocation, aMTV );
903
904 case SH_CIRCLE:
905 return CollCaseReversed<SHAPE_SEGMENT, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
906
907 case SH_LINE_CHAIN:
908 return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aB, aA, aClearance, aActual, aLocation, aMTV );
909
910 case SH_SEGMENT:
911 return CollCase<SHAPE_SEGMENT, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
912
913 case SH_SIMPLE:
915 return CollCase<SHAPE_LINE_CHAIN_BASE, SHAPE_SEGMENT>( aB, aA, aClearance, aActual, aLocation, aMTV );
916
917 case SH_ARC:
918 return CollCaseReversed<SHAPE_SEGMENT, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
919
920 case SH_NULL:
921 return false;
922
923 default:
924 break;
925 }
926 break;
927
928 case SH_SIMPLE:
930 switch( aB->Type() )
931 {
932 case SH_RECT:
933 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN_BASE>( aB, aA, aClearance, aActual, aLocation, aMTV );
934
935 case SH_CIRCLE:
936 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN_BASE>( aB, aA, aClearance, aActual, aLocation, aMTV );
937
938 case SH_LINE_CHAIN:
939 return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN_BASE>( aB, aA, aClearance, aActual, aLocation, aMTV );
940
941 case SH_SEGMENT:
942 return CollCase<SHAPE_LINE_CHAIN_BASE, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
943
944 case SH_SIMPLE:
946 return CollCase<SHAPE_LINE_CHAIN_BASE, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
947
948 case SH_ARC:
949 return CollCaseReversed<SHAPE_LINE_CHAIN_BASE, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
950
951 case SH_NULL:
952 return false;
953
954 default:
955 break;
956 }
957 break;
958
959 case SH_ARC:
960 switch( aB->Type() )
961 {
962 case SH_RECT:
963 return CollCase<SHAPE_ARC, SHAPE_RECT>( aA, aB, aClearance, aActual, aLocation, aMTV );
964
965 case SH_CIRCLE:
966 return CollCase<SHAPE_ARC, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
967
968 case SH_LINE_CHAIN:
969 return CollCase<SHAPE_ARC, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
970
971 case SH_SEGMENT:
972 return CollCase<SHAPE_ARC, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
973
974 case SH_SIMPLE:
976 return CollCase<SHAPE_ARC, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
977
978 case SH_ARC:
979 return CollCase<SHAPE_ARC, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
980
981 case SH_NULL:
982 return false;
983
984 default:
985 break;
986 }
987 break;
988
989 default:
990 break;
991 }
992
993 wxFAIL_MSG( wxString::Format( wxT( "Unsupported collision: %s with %s" ),
994 SHAPE_TYPE_asString( aA->Type() ),
995 SHAPE_TYPE_asString( aB->Type() ) ) );
996
997 return false;
998}
999
1000static bool collideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
1001 VECTOR2I* aLocation, VECTOR2I* aMTV )
1002{
1003 int currentActual = std::numeric_limits<int>::max();
1004 VECTOR2I currentLocation;
1005 VECTOR2I currentMTV(0, 0);
1006 bool colliding = false;
1007
1008 auto canExit =
1009 [&]()
1010 {
1011 if( !colliding )
1012 return false;
1013
1014 if( aActual && currentActual > 0 )
1015 return false;
1016
1017 if( aMTV )
1018 return false;
1019
1020 return true;
1021 };
1022
1023 auto collideCompoundSubshapes =
1024 [&]( const SHAPE* elemA, const SHAPE* elemB, int clearance ) -> bool
1025 {
1026 int actual = 0;
1027 VECTOR2I location;
1028 VECTOR2I mtv;
1029
1030 if( collideSingleShapes( elemA, elemB, clearance,
1031 aActual || aLocation ? &actual : nullptr,
1032 aLocation ? &location : nullptr,
1033 aMTV ? &mtv : nullptr ) )
1034 {
1035 if( actual < currentActual )
1036 {
1037 currentActual = actual;
1038 currentLocation = location;
1039 }
1040
1041 if( aMTV && mtv.SquaredEuclideanNorm() > currentMTV.SquaredEuclideanNorm() )
1042 {
1043 currentMTV = mtv;
1044 }
1045
1046 return true;
1047 }
1048
1049 return false;
1050 };
1051
1052 if( aA->Type() == SH_COMPOUND && aB->Type() == SH_COMPOUND )
1053 {
1054 const SHAPE_COMPOUND* cmpA = static_cast<const SHAPE_COMPOUND*>( aA );
1055 const SHAPE_COMPOUND* cmpB = static_cast<const SHAPE_COMPOUND*>( aB );
1056
1057 for( const SHAPE* elemA : cmpA->Shapes() )
1058 {
1059 for( const SHAPE* elemB : cmpB->Shapes() )
1060 {
1061 if( collideCompoundSubshapes( elemA, elemB, aClearance ) )
1062 {
1063 colliding = true;
1064
1065 if( canExit() )
1066 break;
1067 }
1068 }
1069
1070 if( canExit() )
1071 break;
1072 }
1073 }
1074 else if( aA->Type() == SH_COMPOUND )
1075 {
1076 const SHAPE_COMPOUND* cmpA = static_cast<const SHAPE_COMPOUND*>( aA );
1077
1078 for( const SHAPE* elemA : cmpA->Shapes() )
1079 {
1080 if( collideCompoundSubshapes( elemA, aB, aClearance ) )
1081 {
1082 colliding = true;
1083
1084 if( canExit() )
1085 break;
1086 }
1087 }
1088 }
1089 else if( aB->Type() == SH_COMPOUND )
1090 {
1091 const SHAPE_COMPOUND* cmpB = static_cast<const SHAPE_COMPOUND*>( aB );
1092
1093 for( const SHAPE* elemB : cmpB->Shapes() )
1094 {
1095 if( collideCompoundSubshapes( aA, elemB, aClearance ) )
1096 {
1097 colliding = true;
1098
1099 if( canExit() )
1100 break;
1101 }
1102 }
1103 }
1104 else
1105 {
1106 return collideSingleShapes( aA, aB, aClearance, aActual, aLocation, aMTV );
1107 }
1108
1109 if( colliding )
1110 {
1111 if( aLocation )
1112 *aLocation = currentLocation;
1113
1114 if( aActual )
1115 *aActual = currentActual;
1116
1117 if( aMTV )
1118 *aMTV = currentMTV;
1119 }
1120
1121 return colliding;
1122}
1123
1124
1125bool SHAPE::Collide( const SHAPE* aShape, int aClearance, VECTOR2I* aMTV ) const
1126{
1127 return collideShapes( this, aShape, aClearance, nullptr, nullptr, aMTV );
1128}
1129
1130
1131bool SHAPE::Collide( const SHAPE* aShape, int aClearance, int* aActual, VECTOR2I* aLocation ) const
1132{
1133 return collideShapes( this, aShape, aClearance, aActual, aLocation, nullptr );
1134}
1135
1136
bool Intersects(const BOX2< Vec > &aRect) const
Definition: box2.h:270
Definition: seg.h:42
VECTOR2I A
Definition: seg.h:49
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:269
int Length() const
Return the length (this).
Definition: seg.h:326
static SEG::ecoord Square(int a)
Definition: seg.h:123
VECTOR2I Center() const
Definition: seg.h:362
int Distance(const SEG &aSeg) const
Compute minimum Euclidean distance to segment aSeg.
Definition: seg.cpp:329
int GetWidth() const
Definition: shape_arc.h:157
const VECTOR2I & GetP1() const
Definition: shape_arc.h:113
int IntersectLine(const SEG &aSeg, std::vector< VECTOR2I > *aIpsBuffer) const
Find intersection points between this arc and aSeg, treating aSeg as an infinite line.
Definition: shape_arc.cpp:274
bool Collide(const SEG &aSeg, int aClearance=0, int *aActual=nullptr, VECTOR2I *aLocation=nullptr) const override
Check if the boundary of shape (this) lies closer to the segment aSeg than aClearance,...
Definition: shape_arc.cpp:242
int Intersect(const SHAPE_ARC &aArc, std::vector< VECTOR2I > *aIpsBuffer) const
Find intersection points between this arc and aArc.
Definition: shape_arc.cpp:295
VECTOR2I GetCenter() const
Definition: shape_arc.cpp:434
const VECTOR2I & GetP0() const
Definition: shape_arc.h:112
wxString TypeName() const
Definition: shape.h:103
SHAPE_TYPE Type() const
Return the type of the shape.
Definition: shape.h:98
bool Collide(const SEG &aSeg, int aClearance=0, int *aActual=nullptr, VECTOR2I *aLocation=nullptr) const override
Check if the boundary of shape (this) lies closer to the segment aSeg than aClearance,...
Definition: shape_circle.h:77
int GetRadius() const
Definition: shape_circle.h:108
const VECTOR2I GetCenter() const
Definition: shape_circle.h:113
void SetCenter(const VECTOR2I &aCenter)
Definition: shape_circle.h:103
const std::vector< SHAPE * > & Shapes() const
virtual bool Collide(const VECTOR2I &aP, int aClearance=0, int *aActual=nullptr, VECTOR2I *aLocation=nullptr) const override
Check if point aP lies closer to us than aClearance.
virtual size_t GetPointCount() const =0
virtual size_t GetSegmentCount() const =0
virtual const VECTOR2I GetPoint(int aIndex) const =0
bool PointInside(const VECTOR2I &aPt, int aAccuracy=0, bool aUseBBoxCache=false) const override
Check if point aP lies inside a closed shape.
virtual bool IsClosed() const =0
virtual const SEG GetSegment(int aIndex) const =0
Represent a polyline containing arcs as well as line segments: A chain of connected line and/or arc s...
const SHAPE_ARC & Arc(size_t aArc) const
bool IsClosed() const override
virtual const SEG GetSegment(int aIndex) const override
virtual size_t GetSegmentCount() const override
size_t ArcCount() const
bool IsArcSegment(size_t aSegment) const
Represent a set of closed polygons.
bool Collide(const SHAPE *aShape, int aClearance=0, int *aActual=nullptr, VECTOR2I *aLocation=nullptr) const override
Check if the boundary of shape (this) lies closer to the shape aShape than aClearance,...
bool Collide(const SHAPE *aShape, int aClearance, VECTOR2I *aMTV) const override
Check if the boundary of shape (this) lies closer to the shape aShape than aClearance,...
Definition: shape_rect.h:109
const SHAPE_LINE_CHAIN Outline() const
Definition: shape_rect.h:179
const BOX2I BBox(int aClearance=0) const override
Compute a bounding box of the shape, with a margin of aClearance a collision.
Definition: shape_rect.h:92
const VECTOR2I & GetPosition() const
Definition: shape_rect.h:127
const VECTOR2I GetSize() const
Definition: shape_rect.h:135
const SEG & GetSeg() const
int GetWidth() const
bool Collide(const SHAPE *aShape, int aClearance, VECTOR2I *aMTV) const override
Check if the boundary of shape (this) lies closer to the shape aShape than aClearance,...
Definition: shape_segment.h:69
An abstract shape on 2D plane.
Definition: shape.h:126
virtual bool Collide(const VECTOR2I &aP, int aClearance=0, int *aActual=nullptr, VECTOR2I *aLocation=nullptr) const
Check if the boundary of shape (this) lies closer to the point aP than aClearance,...
Definition: shape.h:181
virtual VECTOR2I Centre() const
Compute a center-of-mass of the shape.
Definition: shape.h:232
extended_type SquaredEuclideanNorm() const
Compute the squared euclidean norm of the vector, which is defined as (x ** 2 + y ** 2).
Definition: vector2d.h:272
static constexpr extended_type ECOORD_MAX
Definition: vector2d.h:75
VECTOR2_TRAITS< int >::extended_type extended_type
Definition: vector2d.h:72
@ SH_POLY_SET
set of polygons (with holes, etc.)
Definition: shape.h:52
@ SH_RECT
axis-aligned rectangle
Definition: shape.h:47
@ SH_CIRCLE
circle
Definition: shape.h:50
@ SH_SIMPLE
simple polygon
Definition: shape.h:51
@ SH_NULL
empty shape (no shape...),
Definition: shape.h:55
@ SH_SEGMENT
line segment
Definition: shape.h:48
@ SH_ARC
circular arc
Definition: shape.h:54
@ SH_POLY_SET_TRIANGLE
a single triangle belonging to a POLY_SET triangulation
Definition: shape.h:56
@ SH_LINE_CHAIN
line chain (polyline)
Definition: shape.h:49
@ SH_COMPOUND
compound shape, consisting of multiple simple shapes
Definition: shape.h:53
static wxString SHAPE_TYPE_asString(SHAPE_TYPE a)
Definition: shape.h:59
static bool collideSingleShapes(const SHAPE *aA, const SHAPE *aB, int aClearance, int *aActual, VECTOR2I *aLocation, VECTOR2I *aMTV)
static bool Collide(const SHAPE_CIRCLE &aA, const SHAPE_CIRCLE &aB, int aClearance, int *aActual, VECTOR2I *aLocation, VECTOR2I *aMTV)
bool CollCaseReversed(const SHAPE *aA, const SHAPE *aB, int aClearance, int *aActual, VECTOR2I *aLocation, VECTOR2I *aMTV)
static VECTOR2I pushoutForce(const SHAPE_CIRCLE &aA, const SEG &aB, int aClearance)
bool CollCase(const SHAPE *aA, const SHAPE *aB, int aClearance, int *aActual, VECTOR2I *aLocation, VECTOR2I *aMTV)
static bool collideShapes(const SHAPE *aA, const SHAPE *aB, int aClearance, int *aActual, VECTOR2I *aLocation, VECTOR2I *aMTV)
VECTOR2I::extended_type ecoord
constexpr int delta
double EuclideanNorm(const VECTOR2I &vector)
Definition: trigo.h:129
VECTOR2< int > VECTOR2I
Definition: vector2d.h:588