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