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  * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
6  *
7  * This program is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU General Public License
9  * as published by the Free Software Foundation; either version 2
10  * of the License, or (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, you may find one here:
19  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
20  * or you may search the http://www.gnu.org website for the version 2 license,
21  * or you may write to the Free Software Foundation, Inc.,
22  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
23  */
24 
25 #include <math/vector2d.h>
26 #include <math.h>
27 
28 #include <geometry/shape.h>
30 #include <geometry/shape_circle.h>
31 #include <geometry/shape_rect.h>
32 #include <geometry/shape_segment.h>
33 #include <geometry/shape_convex.h>
34 
36 
37 static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_CIRCLE& aB, int aClearance,
38  bool aNeedMTV, VECTOR2I& aMTV )
39 {
40  ecoord min_dist = aClearance + aA.GetRadius() + aB.GetRadius();
41  ecoord min_dist_sq = min_dist * min_dist;
42 
43  const VECTOR2I delta = aB.GetCenter() - aA.GetCenter();
44 
45  ecoord dist_sq = delta.SquaredEuclideanNorm();
46 
47  if( dist_sq >= min_dist_sq )
48  return false;
49 
50  if( aNeedMTV )
51  aMTV = delta.Resize( min_dist - sqrt( dist_sq ) + 3 ); // fixme: apparent rounding error
52 
53  return true;
54 }
55 
56 
57 static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance,
58  bool aNeedMTV, VECTOR2I& aMTV )
59 {
60  const VECTOR2I c = aB.GetCenter();
61  const VECTOR2I p0 = aA.GetPosition();
62  const VECTOR2I size = aA.GetSize();
63  const int r = aB.GetRadius();
64  const int min_dist = aClearance + r;
65 
66  const VECTOR2I vts[] =
67  {
68  VECTOR2I( p0.x, p0.y ),
69  VECTOR2I( p0.x, p0.y + size.y ),
70  VECTOR2I( p0.x + size.x, p0.y + size.y ),
71  VECTOR2I( p0.x + size.x, p0.y ),
72  VECTOR2I( p0.x, p0.y )
73  };
74 
75  int nearest_seg_dist = INT_MAX;
76  VECTOR2I nearest;
77 
78  bool inside = c.x >= p0.x && c.x <= ( p0.x + size.x )
79  && c.y >= p0.y && c.y <= ( p0.y + size.y );
80 
81 
82  if( !aNeedMTV && inside )
83  return true;
84 
85  for( int i = 0; i < 4; i++ )
86  {
87  const SEG seg( vts[i], vts[i + 1] );
88 
89  VECTOR2I pn = seg.NearestPoint( c );
90 
91  int d = ( pn - c ).EuclideanNorm();
92 
93  if( ( d < min_dist ) && !aNeedMTV )
94  return true;
95 
96  if( d < nearest_seg_dist )
97  {
98  nearest = pn;
99  nearest_seg_dist = d;
100  }
101  }
102 
103  if( nearest_seg_dist >= min_dist && !inside )
104  return false;
105 
106  VECTOR2I delta = c - nearest;
107 
108  if( !aNeedMTV )
109  return true;
110 
111 
112  if( inside )
113  aMTV = -delta.Resize( abs( min_dist + 1 + nearest_seg_dist ) + 1 );
114  else
115  aMTV = delta.Resize( abs( min_dist + 1 - nearest_seg_dist ) + 1 );
116 
117 
118  return true;
119 }
120 
121 
122 static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance )
123 {
124  VECTOR2I f( 0, 0 );
125 
126  const VECTOR2I c = aA.GetCenter();
127  const VECTOR2I nearest = aB.NearestPoint( c );
128 
129  const int r = aA.GetRadius();
130 
131  int dist = ( nearest - c ).EuclideanNorm();
132  int min_dist = aClearance + r;
133 
134  if( dist < min_dist )
135  {
136  for( int corr = 0; corr < 5; corr++ )
137  {
138  f = ( aA.GetCenter() - nearest ).Resize( min_dist - dist + corr );
139 
140  if( aB.Distance( c + f ) >= min_dist )
141  break;
142  }
143  }
144 
145  return f;
146 }
147 
148 
149 static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_LINE_CHAIN& aB, int aClearance,
150  bool aNeedMTV, VECTOR2I& aMTV )
151 {
152  bool found = false;
153 
154  for( int s = 0; s < aB.SegmentCount(); s++ )
155  {
156  if( aA.Collide( aB.CSegment( s ), aClearance ) )
157  {
158  found = true;
159  break;
160  }
161  }
162 
163  if( !aNeedMTV || !found )
164  return found;
165 
166  SHAPE_CIRCLE cmoved( aA );
167  VECTOR2I f_total( 0, 0 );
168 
169  for( int s = 0; s < aB.SegmentCount(); s++ )
170  {
171  VECTOR2I f = pushoutForce( cmoved, aB.CSegment( s ), aClearance );
172  cmoved.SetCenter( cmoved.GetCenter() + f );
173  f_total += f;
174  }
175 
176  aMTV = f_total;
177  return found;
178 }
179 
180 
181 static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_CONVEX& aB, int aClearance,
182  bool aNeedMTV, VECTOR2I& aMTV )
183 {
184  bool found;
185  const SHAPE_LINE_CHAIN& lc( aB.Vertices() );
186 
187  found = lc.Distance( aA.GetCenter() ) <= aClearance + aA.GetRadius();
188 
189  if( !aNeedMTV || !found )
190  return found;
191 
192  SHAPE_CIRCLE cmoved( aA );
193  VECTOR2I f_total( 0, 0 );
194 
195  for( int s = 0; s < lc.SegmentCount(); s++ )
196  {
197  VECTOR2I f = pushoutForce( cmoved, lc.CSegment( s ), aClearance );
198  cmoved.SetCenter( cmoved.GetCenter() + f );
199  f_total += f;
200  }
201 
202  aMTV = f_total;
203  return found;
204 }
205 
206 
207 static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_SEGMENT& aSeg, int aClearance,
208  bool aNeedMTV, VECTOR2I& aMTV )
209 {
210  bool col = aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2);
211 
212  if( col && aNeedMTV )
213  {
214  aMTV = -pushoutForce( aA, aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2);
215  }
216  return col;
217 }
218 
219 
220 static inline bool Collide( const SHAPE_LINE_CHAIN& aA, const SHAPE_LINE_CHAIN& aB, int aClearance,
221  bool aNeedMTV, VECTOR2I& aMTV )
222 {
223  for( int i = 0; i < aB.SegmentCount(); i++ )
224  if( aA.Collide( aB.CSegment( i ), aClearance ) )
225  return true;
226 
227  return false;
228 }
229 
230 
231 static inline bool Collide( const SHAPE_LINE_CHAIN& aA, const SHAPE_CONVEX& aB, int aClearance,
232  bool aNeedMTV, VECTOR2I& aMTV )
233 {
234  return Collide( aA, aB.Vertices(), aClearance, aNeedMTV, aMTV );
235 }
236 
237 
238 static inline bool Collide( const SHAPE_CONVEX& aA, const SHAPE_CONVEX& aB, int aClearance,
239  bool aNeedMTV, VECTOR2I& aMTV )
240 {
241  return Collide( aA.Vertices(), aB.Vertices(), aClearance, aNeedMTV, aMTV );
242 }
243 
244 
245 static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_LINE_CHAIN& aB, int aClearance,
246  bool aNeedMTV, VECTOR2I& aMTV )
247 {
248  for( int s = 0; s < aB.SegmentCount(); s++ )
249  {
250  SEG seg = aB.CSegment( s );
251 
252  if( aA.Collide( seg, aClearance ) )
253  return true;
254  }
255 
256  return false;
257 }
258 
259 
260 static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CONVEX& aB, int aClearance,
261  bool aNeedMTV, VECTOR2I& aMTV )
262 {
263  return Collide( aA, aB.Vertices(), aClearance, aNeedMTV, aMTV );
264 }
265 
266 
267 static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_SEGMENT& aSeg, int aClearance,
268  bool aNeedMTV, VECTOR2I& aMTV )
269 {
270  return aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2 );
271 }
272 
273 
274 static inline bool Collide( const SHAPE_SEGMENT& aA, const SHAPE_SEGMENT& aB, int aClearance,
275  bool aNeedMTV, VECTOR2I& aMTV )
276 {
277  return aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2 );
278 }
279 
280 
281 static inline bool Collide( const SHAPE_LINE_CHAIN& aA, const SHAPE_SEGMENT& aB, int aClearance,
282  bool aNeedMTV, VECTOR2I& aMTV )
283 {
284  if( aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2 ) )
285  return true;
286 
287  return false;
288 }
289 
290 
291 static inline bool Collide( const SHAPE_CONVEX& aA, const SHAPE_SEGMENT& aB, int aClearance,
292  bool aNeedMTV, VECTOR2I& aMTV )
293 {
294  return Collide( aA.Vertices(), aB, aClearance, aNeedMTV, aMTV );
295 }
296 
297 static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_RECT& aB, int aClearance,
298  bool aNeedMTV, VECTOR2I& aMTV )
299 {
300  return Collide( aA.Outline(), aB.Outline(), aClearance, aNeedMTV, aMTV );
301 }
302 
303 
304 template<class ShapeAType, class ShapeBType>
305 inline bool CollCase( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV )
306 {
307  return Collide (*static_cast<const ShapeAType*>( aA ),
308  *static_cast<const ShapeBType*>( aB ),
309  aClearance, aNeedMTV, aMTV);
310 }
311 
312 template<class ShapeAType, class ShapeBType>
313 inline bool CollCaseReversed ( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV )
314 {
315  bool rv = Collide (*static_cast<const ShapeBType*>( aB ),
316  *static_cast<const ShapeAType*>( aA ),
317  aClearance, aNeedMTV, aMTV);
318  if(rv && aNeedMTV)
319  aMTV = -aMTV;
320  return rv;
321 }
322 
323 
324 bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV )
325 {
326  switch( aA->Type() )
327  {
328  case SH_RECT:
329  switch( aB->Type() )
330  {
331  case SH_RECT:
332  return CollCase<SHAPE_RECT, SHAPE_RECT>( aA, aB, aClearance, aNeedMTV, aMTV );
333 
334  case SH_CIRCLE:
335  return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );
336 
337  case SH_LINE_CHAIN:
338  return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );
339 
340  case SH_SEGMENT:
341  return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
342 
343  case SH_CONVEX:
344  return CollCase<SHAPE_RECT, SHAPE_CONVEX>( aA, aB, aClearance, aNeedMTV, aMTV );
345 
346  default:
347  break;
348  }
349  break;
350 
351  case SH_CIRCLE:
352  switch( aB->Type() )
353  {
354  case SH_RECT:
355  return CollCaseReversed<SHAPE_CIRCLE, SHAPE_RECT>( aA, aB, aClearance, aNeedMTV, aMTV );
356 
357  case SH_CIRCLE:
358  return CollCase<SHAPE_CIRCLE, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );
359 
360  case SH_LINE_CHAIN:
361  return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );
362 
363  case SH_SEGMENT:
364  return CollCase<SHAPE_CIRCLE, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
365 
366  case SH_CONVEX:
367  return CollCase<SHAPE_CIRCLE, SHAPE_CONVEX>( aA, aB, aClearance, aNeedMTV, aMTV );
368 
369  default:
370  break;
371  }
372  break;
373 
374  case SH_LINE_CHAIN:
375  switch( aB->Type() )
376  {
377  case SH_RECT:
378  return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );
379 
380  case SH_CIRCLE:
381  return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );
382 
383  case SH_LINE_CHAIN:
384  return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );
385 
386  case SH_SEGMENT:
387  return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
388 
389  case SH_CONVEX:
390  return CollCase<SHAPE_LINE_CHAIN, SHAPE_CONVEX>( aA, aB, aClearance, aNeedMTV, aMTV );
391 
392  default:
393  break;
394  }
395  break;
396 
397  case SH_SEGMENT:
398  switch( aB->Type() )
399  {
400  case SH_RECT:
401  return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );
402 
403  case SH_CIRCLE:
404  return CollCaseReversed<SHAPE_SEGMENT, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );
405 
406  case SH_LINE_CHAIN:
407  return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );
408 
409  case SH_SEGMENT:
410  return CollCase<SHAPE_SEGMENT, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
411 
412  case SH_CONVEX:
413  return CollCase<SHAPE_CONVEX, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );
414 
415  default:
416  break;
417  }
418  break;
419 
420  case SH_CONVEX:
421  switch( aB->Type() )
422  {
423  case SH_RECT:
424  return CollCase<SHAPE_RECT, SHAPE_CONVEX>( aB, aA, aClearance, aNeedMTV, aMTV );
425 
426  case SH_CIRCLE:
427  return CollCase<SHAPE_CIRCLE, SHAPE_CONVEX>( aB, aA, aClearance, aNeedMTV, aMTV );
428 
429  case SH_LINE_CHAIN:
430  return CollCase<SHAPE_LINE_CHAIN, SHAPE_CONVEX>( aB, aA, aClearance, aNeedMTV, aMTV );
431 
432  case SH_SEGMENT:
433  return CollCase<SHAPE_CONVEX, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
434 
435  case SH_CONVEX:
436  return CollCase<SHAPE_CONVEX, SHAPE_CONVEX>( aA, aB, aClearance, aNeedMTV, aMTV );
437 
438  default:
439  break;
440  }
441  break;
442 
443  default:
444  break;
445  }
446 
447  bool unsupported_collision = true;
448  (void) unsupported_collision; // make gcc quiet
449 
450  assert( unsupported_collision == false );
451 
452  return false;
453 }
454 
455 
456 bool SHAPE::Collide( const SHAPE* aShape, int aClerance, VECTOR2I& aMTV ) const
457 {
458  return CollideShapes( this, aShape, aClerance, true, aMTV );
459 }
460 
461 
462 bool SHAPE::Collide( const SHAPE* aShape, int aClerance ) const
463 {
464  VECTOR2I dummy;
465 
466  return CollideShapes( this, aShape, aClerance, false, dummy );
467 }
468 
469 
470 bool SHAPE_RECT::Collide( const SEG& aSeg, int aClearance ) const
471 {
472  //VECTOR2I pmin = VECTOR2I( std::min( aSeg.a.x, aSeg.b.x ), std::min( aSeg.a.y, aSeg.b.y ) );
473  //VECTOR2I pmax = VECTOR2I( std::max( aSeg.a.x, aSeg.b.x ), std::max( aSeg.a.y, aSeg.b.y ));
474  //BOX2I r( pmin, VECTOR2I( pmax.x - pmin.x, pmax.y - pmin.y ) );
475 
476  //if( BBox( 0 ).SquaredDistance( r ) > aClearance * aClearance )
477  // return false;
478 
479  if( BBox( 0 ).Contains( aSeg.A ) || BBox( 0 ).Contains( aSeg.B ) )
480  return true;
481 
482  VECTOR2I vts[] = { VECTOR2I( m_p0.x, m_p0.y ),
483  VECTOR2I( m_p0.x, m_p0.y + m_h ),
484  VECTOR2I( m_p0.x + m_w, m_p0.y + m_h ),
485  VECTOR2I( m_p0.x + m_w, m_p0.y ),
486  VECTOR2I( m_p0.x, m_p0.y ) };
487 
488  for( int i = 0; i < 4; i++ )
489  {
490  SEG s( vts[i], vts[i + 1], i );
491 
492  if( s.Distance( aSeg ) < aClearance )
493  return true;
494  }
495 
496  return false;
497 }
double EuclideanNorm(const wxPoint &vector)
Euclidean norm of a 2D vector.
Definition: trigo.h:112
const SHAPE_LINE_CHAIN Outline() const
Definition: shape_rect.h:145
VECTOR2_TRAITS< int >::extended_type extended_type
Definition: vector2d.h:77
void SetCenter(const VECTOR2I &aCenter)
Definition: shape_circle.h:74
bool Collide(const SEG &aSeg, int aClearance=0) const override
Function Collide()
Definition: shape_segment.h:55
const VECTOR2I GetCenter() const
Definition: shape_circle.h:84
VECTOR2< T > Resize(T aNewLength) const
Function Resize returns a vector of the same direction, but length specified in aNewLength.
Definition: vector2d.h:387
bool Collide(const SEG &aSeg, int aClearance=0) const override
Function Collide()
int GetRadius() const
Definition: shape_circle.h:79
static const int dist[10][10]
Definition: dist.cpp:57
Class SHAPE_CONVEX.
Definition: shape_convex.h:42
VECTOR2< int > VECTOR2I
Definition: vector2d.h:589
#define abs(a)
Definition: auxiliary.h:84
static const int delta[8][2]
Definition: solve.cpp:112
bool Collide(const VECTOR2I &aP, int aClearance=0) const override
Function Collide()
const SEG CSegment(int aIndex) const
Function CSegment()
const SHAPE_LINE_CHAIN & Vertices() const
Function Vertices()
Definition: shape_convex.h:139
VECTOR2I m_p0
Top-left corner
Definition: shape_rect.h:159
bool CollCaseReversed(const SHAPE *aA, const SHAPE *aB, int aClearance, bool aNeedMTV, VECTOR2I &aMTV)
VECTOR2I::extended_type ecoord
const VECTOR2I & GetPosition() const
Function GetPosition()
Definition: shape_rect.h:100
int GetWidth() const
Definition: shape_segment.h:80
int m_h
Height
Definition: shape_rect.h:165
Class SHAPE.
Definition: shape.h:57
int m_w
Width
Definition: shape_rect.h:162
static VECTOR2I pushoutForce(const SHAPE_CIRCLE &aA, const SEG &aB, int aClearance)
line chain (polyline)
Definition: shape.h:46
virtual bool Collide(const VECTOR2I &aP, int aClearance=0) const
Function Collide()
Definition: shape.h:106
const VECTOR2I NearestPoint(const VECTOR2I &aP) const
Function NearestPoint()
Definition: seg.h:354
bool Collide(const SEG &aSeg, int aClearance=0) const override
Function Collide()
Definition: shape_circle.h:62
Definition: seg.h:36
extended_type SquaredEuclideanNorm() const
Function Squared Euclidean Norm computes the squared euclidean norm of the vector, which is defined as (x ** 2 + y ** 2).
Definition: vector2d.h:301
static bool Collide(const SHAPE_CIRCLE &aA, const SHAPE_CIRCLE &aB, int aClearance, bool aNeedMTV, VECTOR2I &aMTV)
const SEG & GetSeg() const
Definition: shape_segment.h:70
static LIB_PART * dummy()
Used when a LIB_PART is not found in library to draw a dummy shape This component is a 400 mils squar...
SHAPE_TYPE Type() const
Function Type()
Definition: shape.h:82
Class SHAPE_LINE_CHAIN.
const BOX2I BBox(int aClearance=0) const override
Function BBox()
Definition: shape_rect.h:73
VECTOR2I A
Definition: seg.h:46
line segment
Definition: shape.h:45
Definition: shape.h:43
int Distance(const VECTOR2I &aP, bool aOutlineOnly=false) const
Function Distance()
circle
Definition: shape.h:47
int Distance(const SEG &aSeg) const
Function Distance()
Definition: seg.h:185
const VECTOR2I GetSize() const
Function GetSize()
Definition: shape_rect.h:110
int SegmentCount() const
Function SegmentCount()
bool CollideShapes(const SHAPE *aA, const SHAPE *aB, int aClearance, bool aNeedMTV, VECTOR2I &aMTV)
bool CollCase(const SHAPE *aA, const SHAPE *aB, int aClearance, bool aNeedMTV, VECTOR2I &aMTV)
axis-aligned rectangle
Definition: shape.h:44
VECTOR2I B
Definition: seg.h:47