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 if( aA.IsClosed() && aB.GetPointCount() > 0 && aA.PointInside( aB.GetPoint( 0 ) ) )
306 {
307 closest_dist = 0;
308 nearest = aB.GetPoint( 0 );
309 }
310 else
311 {
312 std::vector<SEG> a_segs;
313 std::vector<SEG> b_segs;
314
315 for( size_t ii = 0; ii < aA.GetSegmentCount(); ii++ )
316 {
317 if( aA.Type() != SH_LINE_CHAIN
318 || !static_cast<const SHAPE_LINE_CHAIN*>( &aA )->IsArcSegment( ii ) )
319 {
320 a_segs.push_back( aA.GetSegment( ii ) );
321 }
322 }
323
324 for( size_t ii = 0; ii < aB.GetSegmentCount(); ii++ )
325 {
326 if( aB.Type() != SH_LINE_CHAIN
327 || !static_cast<const SHAPE_LINE_CHAIN*>( &aB )->IsArcSegment( ii ) )
328 {
329 b_segs.push_back( aB.GetSegment( ii ) );
330 }
331 }
332
333 auto seg_sort = []( const SEG& a, const SEG& b )
334 {
335 return a.A.x < b.A.x || ( a.A.x == b.A.x && a.A.y < b.A.y );
336 };
337
338 std::sort( a_segs.begin(), a_segs.end(), seg_sort );
339 std::sort( b_segs.begin(), b_segs.end(), seg_sort );
340
341 for( const SEG& a_seg : a_segs )
342 {
343 for( const SEG& b_seg : b_segs )
344 {
345 int dist = 0;
346
347 if( a_seg.Collide( b_seg, aClearance, aActual || aLocation ? &dist : nullptr ) )
348 {
349 if( dist < closest_dist )
350 {
351 nearest = a_seg.NearestPoint( b_seg );
352 closest_dist = dist;
353 }
354
355 if( closest_dist == 0 )
356 break;
357
358 // If we're not looking for aActual then any collision will do
359 if( !aActual )
360 break;
361 }
362 }
363 }
364 }
365
366 if( (!aActual && !aLocation ) || closest_dist > 0 )
367 {
368 std::vector<const SHAPE_LINE_CHAIN*> chains = {
369 dynamic_cast<const SHAPE_LINE_CHAIN*>( &aA ),
370 dynamic_cast<const SHAPE_LINE_CHAIN*>( &aB )
371 };
372
373 std::vector<const SHAPE*> shapes = { &aA, &aB };
374
375 for( int ii = 0; ii < 2; ii++ )
376 {
377 const SHAPE_LINE_CHAIN* chain = chains[ii];
378 const SHAPE* other = shapes[( ii + 1 ) % 2];
379
380 if( !chain )
381 continue;
382
383 for( size_t jj = 0; jj < chain->ArcCount(); jj++ )
384 {
385 const SHAPE_ARC& arc = chain->Arc( jj );
386
387 if( arc.Collide( other, aClearance, aActual, aLocation ) )
388 return true;
389 }
390 }
391 }
392
393 if( closest_dist == 0 || closest_dist < aClearance )
394 {
395 if( aLocation )
396 *aLocation = nearest;
397
398 if( aActual )
399 *aActual = closest_dist;
400
401 return true;
402 }
403
404 return false;
405}
406
407
408static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_LINE_CHAIN_BASE& aB, int aClearance,
409 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
410{
411 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
412 aA.TypeName(),
413 aB.TypeName() ) );
414
415 int closest_dist = std::numeric_limits<int>::max();
416 VECTOR2I nearest;
417
418 if( aB.IsClosed() && aB.PointInside( aA.Centre() ) )
419 {
420 nearest = aA.Centre();
421 closest_dist = 0;
422 }
423 else
424 {
425 for( size_t s = 0; s < aB.GetSegmentCount(); s++ )
426 {
427 int collision_dist = 0;
428 VECTOR2I pn;
429
430 if( aA.Collide( aB.GetSegment( s ), aClearance,
431 aActual || aLocation ? &collision_dist : nullptr,
432 aLocation ? &pn : nullptr ) )
433 {
434 if( collision_dist < closest_dist )
435 {
436 nearest = pn;
437 closest_dist = collision_dist;
438 }
439
440 if( closest_dist == 0 )
441 break;
442
443 // If we're not looking for aActual then any collision will do
444 if( !aActual )
445 break;
446 }
447 }
448 }
449
450 if( closest_dist == 0 || closest_dist < aClearance )
451 {
452 if( aLocation )
453 *aLocation = nearest;
454
455 if( aActual )
456 *aActual = closest_dist;
457
458 return true;
459 }
460
461 return false;
462}
463
464
465static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_SEGMENT& aB, int aClearance,
466 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
467{
468 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
469 aA.TypeName(),
470 aB.TypeName() ) );
471
472 bool rv = aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2, aActual, aLocation );
473
474 if( aActual )
475 *aActual = std::max( 0, *aActual - aB.GetWidth() / 2 );
476
477 return rv;
478}
479
480
481static inline bool Collide( const SHAPE_SEGMENT& aA, const SHAPE_SEGMENT& aB, int aClearance,
482 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
483{
484 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
485 aA.TypeName(),
486 aB.TypeName() ) );
487
488 bool rv = aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2, aActual, aLocation );
489
490 if( aActual )
491 *aActual = std::max( 0, *aActual - aB.GetWidth() / 2 );
492
493 return rv;
494}
495
496
497static inline bool Collide( const SHAPE_LINE_CHAIN_BASE& aA, const SHAPE_SEGMENT& aB,
498 int aClearance, int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
499{
500 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
501 aA.TypeName(),
502 aB.TypeName() ) );
503
504 bool rv = aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2, aActual, aLocation );
505
506 if( aActual )
507 *aActual = std::max( 0, *aActual - aB.GetWidth() / 2 );
508
509 return rv;
510}
511
512
513static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_RECT& aB, int aClearance,
514 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
515{
516 if( aClearance || aActual || aLocation || aMTV )
517 {
518 return Collide( aA.Outline(), aB.Outline(), aClearance, aActual, aLocation, aMTV );
519 }
520 else
521 {
522 BOX2I bboxa = aA.BBox();
523 BOX2I bboxb = aB.BBox();
524
525 return bboxa.Intersects( bboxb );
526 }
527}
528
529
530static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_RECT& aB, int aClearance,
531 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
532{
533 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
534 aA.TypeName(),
535 aB.TypeName() ) );
536
537 const SHAPE_LINE_CHAIN lc( aA );
538
539 bool rv = Collide( lc, aB.Outline(), aClearance + aA.GetWidth() / 2, aActual, aLocation, aMTV );
540
541 if( rv && aActual )
542 *aActual = std::max( 0, *aActual - aA.GetWidth() / 2 );
543
544 return rv;
545}
546
547
548static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_CIRCLE& aB, int aClearance,
549 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
550{
551 const SHAPE_LINE_CHAIN lc( aA );
552
553 bool rv = Collide( aB, lc, aClearance + aA.GetWidth() / 2, aActual, aLocation, aMTV );
554
555 if( rv && aActual )
556 *aActual = std::max( 0, *aActual - aA.GetWidth() / 2 );
557
558 return rv;
559}
560
561
562static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_LINE_CHAIN& aB, int aClearance,
563 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
564{
565 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
566 aA.TypeName(),
567 aB.TypeName() ) );
568
569 int closest_dist = std::numeric_limits<int>::max();
570 VECTOR2I nearest;
571
572 if( aB.IsClosed() && aB.PointInside( aA.GetP0() ) )
573 {
574 closest_dist = 0;
575 nearest = aA.GetP0();
576 }
577 else
578 {
579 int collision_dist = 0;
580 VECTOR2I pn;
581
582 for( size_t i = 0; i < aB.GetSegmentCount(); i++ )
583 {
584 // ignore arcs - we will collide these separately
585 if( aB.IsArcSegment( i ) )
586 continue;
587
588 if( aA.Collide( aB.GetSegment( i ), aClearance,
589 aActual || aLocation ? &collision_dist : nullptr,
590 aLocation ? &pn : nullptr ) )
591 {
592 if( collision_dist < closest_dist )
593 {
594 nearest = pn;
595 closest_dist = collision_dist;
596 }
597
598 if( closest_dist == 0 )
599 break;
600
601 // If we're not looking for aActual then any collision will do
602 if( !aActual )
603 break;
604 }
605 }
606
607 for( size_t i = 0; i < aB.ArcCount(); i++ )
608 {
609 const SHAPE_ARC& arc = aB.Arc( i );
610
611 // The arcs in the chain should have zero width
612 wxASSERT_MSG( arc.GetWidth() == 0, wxT( "Invalid arc width - should be zero" ) );
613
614 if( aA.Collide( &arc, aClearance, aActual || aLocation ? &collision_dist : nullptr,
615 aLocation ? &pn : nullptr ) )
616 {
617 if( collision_dist < closest_dist )
618 {
619 nearest = pn;
620 closest_dist = collision_dist;
621 }
622
623 if( closest_dist == 0 )
624 break;
625
626 if( !aActual )
627 break;
628 }
629 }
630 }
631
632 if( closest_dist == 0 || closest_dist < aClearance )
633 {
634 if( aLocation )
635 *aLocation = nearest;
636
637 if( aActual )
638 *aActual = closest_dist;
639
640 return true;
641 }
642
643 return false;
644}
645
646
647static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_SEGMENT& aB, int aClearance,
648 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
649{
650 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
651 aA.TypeName(),
652 aB.TypeName() ) );
653
654 bool rv = aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2, aActual, aLocation );
655
656 if( rv && aActual )
657 *aActual = std::max( 0, *aActual - aB.GetWidth() / 2 );
658
659 return rv;
660}
661
662
663static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_LINE_CHAIN_BASE& aB, int aClearance,
664 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
665{
666 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
667 aA.TypeName(),
668 aB.TypeName() ) );
669
670 int closest_dist = std::numeric_limits<int>::max();
671 VECTOR2I nearest;
672
673 if( aB.IsClosed() && aB.PointInside( aA.GetP0() ) )
674 {
675 closest_dist = 0;
676 nearest = aA.GetP0();
677 }
678 else
679 {
680 for( size_t i = 0; i < aB.GetSegmentCount(); i++ )
681 {
682 int collision_dist = 0;
683 VECTOR2I pn;
684
685 if( aA.Collide( aB.GetSegment( i ), aClearance,
686 aActual || aLocation ? &collision_dist : nullptr,
687 aLocation ? &pn : nullptr ) )
688 {
689 if( collision_dist < closest_dist )
690 {
691 nearest = pn;
692 closest_dist = collision_dist;
693 }
694
695 if( closest_dist == 0 )
696 break;
697
698 // If we're not looking for aActual then any collision will do
699 if( !aActual )
700 break;
701 }
702 }
703 }
704
705 if( closest_dist == 0 || closest_dist < aClearance )
706 {
707 if( aLocation )
708 *aLocation = nearest;
709
710 if( aActual )
711 *aActual = closest_dist;
712
713 return true;
714 }
715
716 return false;
717}
718
719
720static inline bool Collide( const SHAPE_ARC& aA, const SHAPE_ARC& aB, int aClearance,
721 int* aActual, VECTOR2I* aLocation, VECTOR2I* aMTV )
722{
723 wxASSERT_MSG( !aMTV, wxString::Format( wxT( "MTV not implemented for %s : %s collisions" ),
724 aA.TypeName(),
725 aB.TypeName() ) );
726
727 SEG mediatrix( aA.GetCenter(), aB.GetCenter() );
728
729 std::vector<VECTOR2I> ips;
730
731 // Basic case - arcs intersect
732 if( aA.Intersect( aB, &ips ) > 0 )
733 {
734 if( aActual )
735 *aActual = 0;
736
737 if( aLocation )
738 *aLocation = ips[0]; // Pick the first intersection point
739
740 return true;
741 }
742
743 // Arcs don't intersect, build a list of points to check
744 std::vector<VECTOR2I> ptsA;
745 std::vector<VECTOR2I> ptsB;
746
747 bool cocentered = ( mediatrix.A == mediatrix.B );
748
749 // 1: Interior points of both arcs, which are on the line segment between the two centres
750 if( !cocentered )
751 {
752 aA.IntersectLine( mediatrix, &ptsA );
753 aB.IntersectLine( mediatrix, &ptsB );
754 }
755
756 // 2: Check arc end points
757 ptsA.push_back( aA.GetP0() );
758 ptsA.push_back( aA.GetP1() );
759 ptsB.push_back( aB.GetP0() );
760 ptsB.push_back( aB.GetP1() );
761
762 // 3: Endpoint of one and "projected" point on the other, which is on the
763 // line segment through that endpoint and the centre of the other arc
764 aA.IntersectLine( SEG( aB.GetP0(), aA.GetCenter() ), &ptsA );
765 aA.IntersectLine( SEG( aB.GetP1(), aA.GetCenter() ), &ptsA );
766
767 aB.IntersectLine( SEG( aA.GetP0(), aB.GetCenter() ), &ptsB );
768 aB.IntersectLine( SEG( aA.GetP1(), aB.GetCenter() ), &ptsB );
769
770 double minDist = std::numeric_limits<double>::max();
771 SEG minDistSeg;
772 bool rv = false;
773
774 int widths = ( aA.GetWidth() / 2 ) + ( aB.GetWidth() / 2 );
775
776 // @todo performance could be improved by only checking certain points (e.g only check end
777 // points against other end points or their corresponding "projected" points)
778 for( const VECTOR2I& ptA : ptsA )
779 {
780 for( const VECTOR2I& ptB : ptsB )
781 {
782 SEG candidateMinDist( ptA, ptB );
783 int dist = candidateMinDist.Length() - widths;
784
785 if( dist < aClearance )
786 {
787 if( !rv || dist < minDist )
788 {
789 minDist = dist;
790 minDistSeg = candidateMinDist;
791 }
792
793 rv = true;
794 }
795 }
796 }
797
798 if( rv && aActual )
799 *aActual = std::max( 0, minDistSeg.Length() - widths );
800
801 if( rv && aLocation )
802 *aLocation = minDistSeg.Center();
803
804 return rv;
805}
806
807
808template<class T_a, class T_b>
809inline bool CollCase( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
810 VECTOR2I* aLocation, VECTOR2I* aMTV )
811
812{
813 return Collide( *static_cast<const T_a*>( aA ), *static_cast<const T_b*>( aB ),
814 aClearance, aActual, aLocation, aMTV);
815}
816
817
818template<class T_a, class T_b>
819inline bool CollCaseReversed ( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
820 VECTOR2I* aLocation, VECTOR2I* aMTV )
821{
822 bool rv = Collide( *static_cast<const T_b*>( aB ), *static_cast<const T_a*>( aA ),
823 aClearance, aActual, aLocation, aMTV);
824
825 if( rv && aMTV)
826 *aMTV = - *aMTV;
827
828 return rv;
829}
830
831
832static bool collideSingleShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
833 VECTOR2I* aLocation, VECTOR2I* aMTV )
834{
835 if( aA->Type() == SH_POLY_SET )
836 {
837 const SHAPE_POLY_SET* polySetA = static_cast<const SHAPE_POLY_SET*>( aA );
838
839 wxASSERT( !aMTV );
840 return polySetA->Collide( aB, aClearance, aActual, aLocation );
841 }
842 else if( aB->Type() == SH_POLY_SET )
843 {
844 const SHAPE_POLY_SET* polySetB = static_cast<const SHAPE_POLY_SET*>( aB );
845
846 wxASSERT( !aMTV );
847 return polySetB->Collide( aA, aClearance, aActual, aLocation );
848 }
849
850 switch( aA->Type() )
851 {
852 case SH_NULL:
853 return false;
854
855 case SH_RECT:
856 switch( aB->Type() )
857 {
858 case SH_RECT:
859 return CollCase<SHAPE_RECT, SHAPE_RECT>( aA, aB, aClearance, aActual, aLocation, aMTV );
860
861 case SH_CIRCLE:
862 return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
863
864 case SH_LINE_CHAIN:
865 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
866
867 case SH_SEGMENT:
868 return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
869
870 case SH_SIMPLE:
872 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
873
874 case SH_ARC:
875 return CollCaseReversed<SHAPE_RECT, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
876
877 case SH_NULL:
878 return false;
879
880 default:
881 break;
882 }
883 break;
884
885 case SH_CIRCLE:
886 switch( aB->Type() )
887 {
888 case SH_RECT:
889 return CollCaseReversed<SHAPE_CIRCLE, SHAPE_RECT>( aA, aB, aClearance, aActual, aLocation, aMTV );
890
891 case SH_CIRCLE:
892 return CollCase<SHAPE_CIRCLE, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
893
894 case SH_LINE_CHAIN:
895 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
896
897 case SH_SEGMENT:
898 return CollCase<SHAPE_CIRCLE, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
899
900 case SH_SIMPLE:
902 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
903
904 case SH_ARC:
905 return CollCaseReversed<SHAPE_CIRCLE, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
906
907 case SH_NULL:
908 return false;
909
910 default:
911 break;
912 }
913 break;
914
915 case SH_LINE_CHAIN:
916 switch( aB->Type() )
917 {
918 case SH_RECT:
919 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aActual, aLocation, aMTV );
920
921 case SH_CIRCLE:
922 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aActual, aLocation, aMTV );
923
924 case SH_LINE_CHAIN:
925 return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
926
927 case SH_SEGMENT:
928 return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
929
930 case SH_SIMPLE:
932 return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
933
934 case SH_ARC:
935 return CollCaseReversed<SHAPE_LINE_CHAIN, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
936
937 case SH_NULL:
938 return false;
939
940 default:
941 break;
942 }
943 break;
944
945 case SH_SEGMENT:
946 switch( aB->Type() )
947 {
948 case SH_RECT:
949 return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aB, aA, aClearance, aActual, aLocation, aMTV );
950
951 case SH_CIRCLE:
952 return CollCaseReversed<SHAPE_SEGMENT, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
953
954 case SH_LINE_CHAIN:
955 return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aB, aA, aClearance, aActual, aLocation, aMTV );
956
957 case SH_SEGMENT:
958 return CollCase<SHAPE_SEGMENT, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
959
960 case SH_SIMPLE:
962 return CollCase<SHAPE_LINE_CHAIN_BASE, SHAPE_SEGMENT>( aB, aA, aClearance, aActual, aLocation, aMTV );
963
964 case SH_ARC:
965 return CollCaseReversed<SHAPE_SEGMENT, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
966
967 case SH_NULL:
968 return false;
969
970 default:
971 break;
972 }
973 break;
974
975 case SH_SIMPLE:
977 switch( aB->Type() )
978 {
979 case SH_RECT:
980 return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN_BASE>( aB, aA, aClearance, aActual, aLocation, aMTV );
981
982 case SH_CIRCLE:
983 return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN_BASE>( aB, aA, aClearance, aActual, aLocation, aMTV );
984
985 case SH_LINE_CHAIN:
986 return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN_BASE>( aB, aA, aClearance, aActual, aLocation, aMTV );
987
988 case SH_SEGMENT:
989 return CollCase<SHAPE_LINE_CHAIN_BASE, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
990
991 case SH_SIMPLE:
993 return CollCase<SHAPE_LINE_CHAIN_BASE, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
994
995 case SH_ARC:
996 return CollCaseReversed<SHAPE_LINE_CHAIN_BASE, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
997
998 case SH_NULL:
999 return false;
1000
1001 default:
1002 break;
1003 }
1004 break;
1005
1006 case SH_ARC:
1007 switch( aB->Type() )
1008 {
1009 case SH_RECT:
1010 return CollCase<SHAPE_ARC, SHAPE_RECT>( aA, aB, aClearance, aActual, aLocation, aMTV );
1011
1012 case SH_CIRCLE:
1013 return CollCase<SHAPE_ARC, SHAPE_CIRCLE>( aA, aB, aClearance, aActual, aLocation, aMTV );
1014
1015 case SH_LINE_CHAIN:
1016 return CollCase<SHAPE_ARC, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aActual, aLocation, aMTV );
1017
1018 case SH_SEGMENT:
1019 return CollCase<SHAPE_ARC, SHAPE_SEGMENT>( aA, aB, aClearance, aActual, aLocation, aMTV );
1020
1021 case SH_SIMPLE:
1023 return CollCase<SHAPE_ARC, SHAPE_LINE_CHAIN_BASE>( aA, aB, aClearance, aActual, aLocation, aMTV );
1024
1025 case SH_ARC:
1026 return CollCase<SHAPE_ARC, SHAPE_ARC>( aA, aB, aClearance, aActual, aLocation, aMTV );
1027
1028 case SH_NULL:
1029 return false;
1030
1031 default:
1032 break;
1033 }
1034 break;
1035
1036 default:
1037 break;
1038 }
1039
1040 wxFAIL_MSG( wxString::Format( wxT( "Unsupported collision: %s with %s" ),
1041 SHAPE_TYPE_asString( aA->Type() ),
1042 SHAPE_TYPE_asString( aB->Type() ) ) );
1043
1044 return false;
1045}
1046
1047static bool collideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, int* aActual,
1048 VECTOR2I* aLocation, VECTOR2I* aMTV )
1049{
1050 int currentActual = std::numeric_limits<int>::max();
1051 VECTOR2I currentLocation;
1052 VECTOR2I currentMTV(0, 0);
1053 bool colliding = false;
1054
1055 auto canExit =
1056 [&]()
1057 {
1058 if( !colliding )
1059 return false;
1060
1061 if( aActual && currentActual > 0 )
1062 return false;
1063
1064 if( aMTV )
1065 return false;
1066
1067 return true;
1068 };
1069
1070 auto collideCompoundSubshapes =
1071 [&]( const SHAPE* elemA, const SHAPE* elemB, int clearance ) -> bool
1072 {
1073 int actual = 0;
1074 VECTOR2I location;
1075 VECTOR2I mtv;
1076
1077 if( collideSingleShapes( elemA, elemB, clearance,
1078 aActual || aLocation ? &actual : nullptr,
1079 aLocation ? &location : nullptr,
1080 aMTV ? &mtv : nullptr ) )
1081 {
1082 if( actual < currentActual )
1083 {
1084 currentActual = actual;
1085 currentLocation = location;
1086 }
1087
1088 if( aMTV && mtv.SquaredEuclideanNorm() > currentMTV.SquaredEuclideanNorm() )
1089 {
1090 currentMTV = mtv;
1091 }
1092
1093 return true;
1094 }
1095
1096 return false;
1097 };
1098
1099 if( aA->Type() == SH_COMPOUND && aB->Type() == SH_COMPOUND )
1100 {
1101 const SHAPE_COMPOUND* cmpA = static_cast<const SHAPE_COMPOUND*>( aA );
1102 const SHAPE_COMPOUND* cmpB = static_cast<const SHAPE_COMPOUND*>( aB );
1103
1104 for( const SHAPE* elemA : cmpA->Shapes() )
1105 {
1106 for( const SHAPE* elemB : cmpB->Shapes() )
1107 {
1108 if( collideCompoundSubshapes( elemA, elemB, aClearance ) )
1109 {
1110 colliding = true;
1111
1112 if( canExit() )
1113 break;
1114 }
1115 }
1116
1117 if( canExit() )
1118 break;
1119 }
1120 }
1121 else if( aA->Type() == SH_COMPOUND )
1122 {
1123 const SHAPE_COMPOUND* cmpA = static_cast<const SHAPE_COMPOUND*>( aA );
1124
1125 for( const SHAPE* elemA : cmpA->Shapes() )
1126 {
1127 if( collideCompoundSubshapes( elemA, aB, aClearance ) )
1128 {
1129 colliding = true;
1130
1131 if( canExit() )
1132 break;
1133 }
1134 }
1135 }
1136 else if( aB->Type() == SH_COMPOUND )
1137 {
1138 const SHAPE_COMPOUND* cmpB = static_cast<const SHAPE_COMPOUND*>( aB );
1139
1140 for( const SHAPE* elemB : cmpB->Shapes() )
1141 {
1142 if( collideCompoundSubshapes( aA, elemB, aClearance ) )
1143 {
1144 colliding = true;
1145
1146 if( canExit() )
1147 break;
1148 }
1149 }
1150 }
1151 else
1152 {
1153 return collideSingleShapes( aA, aB, aClearance, aActual, aLocation, aMTV );
1154 }
1155
1156 if( colliding )
1157 {
1158 if( aLocation )
1159 *aLocation = currentLocation;
1160
1161 if( aActual )
1162 *aActual = currentActual;
1163
1164 if( aMTV )
1165 *aMTV = currentMTV;
1166 }
1167
1168 return colliding;
1169}
1170
1171
1172bool SHAPE::Collide( const SHAPE* aShape, int aClearance, VECTOR2I* aMTV ) const
1173{
1174 return collideShapes( this, aShape, aClearance, nullptr, nullptr, aMTV );
1175}
1176
1177
1178bool SHAPE::Collide( const SHAPE* aShape, int aClearance, int* aActual, VECTOR2I* aLocation ) const
1179{
1180 return collideShapes( this, aShape, aClearance, aActual, aLocation, nullptr );
1181}
1182
1183
constexpr bool Intersects(const BOX2< Vec > &aRect) const
Definition: box2.h:311
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:327
int Length() const
Return the length (this).
Definition: seg.h:333
static SEG::ecoord Square(int a)
Definition: seg.h:123
VECTOR2I Center() const
Definition: seg.h:369
int Distance(const SEG &aSeg) const
Compute minimum Euclidean distance to segment aSeg.
Definition: seg.cpp:388
int Intersect(const CIRCLE &aArc, std::vector< VECTOR2I > *aIpsBuffer) const
Find intersection points between this arc and a CIRCLE.
Definition: shape_arc.cpp:316
int GetWidth() const
Definition: shape_arc.h:168
const VECTOR2I & GetP1() const
Definition: shape_arc.h:115
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:295
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:244
const VECTOR2I & GetP0() const
Definition: shape_arc.h:114
const VECTOR2I & GetCenter() const
Definition: shape_arc.cpp:523
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:118
const VECTOR2I GetCenter() const
Definition: shape_circle.h:123
void SetCenter(const VECTOR2I &aCenter)
Definition: shape_circle.h:113
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:142
const SHAPE_LINE_CHAIN Outline() const
Definition: shape_rect.h:212
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:102
const VECTOR2I & GetPosition() const
Definition: shape_rect.h:160
const VECTOR2I GetSize() const
Definition: shape_rect.h:168
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
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< int32_t >::extended_type extended_type
Definition: vector2d.h:73
@ 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
VECTOR2< int32_t > VECTOR2I
Definition: vector2d.h:691