KiCad PCB EDA Suite
poly_grid_partition.h
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) 2016-2017 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 #ifndef __POLY_GRID_PARTITION_H
26 #define __POLY_GRID_PARTITION_H
27 
28 
29 #include <algorithm>
30 #include <functional>
31 #include <set>
32 #include <unordered_map>
33 #include <vector>
34 
35 #include <geometry/seg.h>
37 #include <geometry/shape_rect.h>
38 #include <math/util.h>
39 #include <math/vector2d.h>
40 
63 {
64 public:
65  POLY_GRID_PARTITION( const SHAPE_LINE_CHAIN& aPolyOutline, int gridSize )
66  {
67  build( aPolyOutline, gridSize );
68  }
69 
70  int ContainsPoint( const VECTOR2I& aP, int aClearance = 0 ) // const
71  {
72  if( containsPoint(aP) )
73  return 1;
74 
75  if( aClearance > 0 )
76  return checkClearance ( aP, aClearance );
77 
78  return 0;
79  }
80 
81  const BOX2I& BBox() const
82  {
83  return m_bbox;
84  }
85 
86 private:
87  enum HASH_FLAG
88  {
89  LEAD_EDGE = 1,
91  };
92 
93  using EDGE_LIST = std::vector<int>;
94 
95  template <class T>
96  inline void hash_combine( std::size_t& seed, const T& v )
97  {
98  std::hash<T> hasher;
99  seed ^= hasher( v ) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
100  }
101 
102  struct segsEqual
103  {
104  bool operator()( const SEG& a, const SEG& b ) const
105  {
106  return (a.A == b.A && a.B == b.B) || (a.A == b.B && a.B == b.A);
107  }
108  };
109 
110  struct segHash
111  {
112  std::size_t operator()( const SEG& a ) const
113  {
114  return a.A.x + a.B.x + a.A.y + a.B.y;
115  }
116  };
117 
118  int containsPoint( const VECTOR2I& aP, bool debug = false ) const
119  {
120  const auto gridPoint = poly2grid( aP );
121 
122  if( !m_bbox.Contains( aP ) )
123  return 0;
124 
125  SCAN_STATE state;
126  const EDGE_LIST& cell = m_grid[ m_gridSize * gridPoint.y + gridPoint.x ];
127 
128  scanCell( state, cell, aP, gridPoint.x, gridPoint.y );
129 
130  if( state.nearest < 0 )
131  {
132  state = SCAN_STATE();
133 
134  for( int d = 1; d <= m_gridSize; d++ )
135  {
136  int xl = gridPoint.x - d;
137  int xh = gridPoint.x + d;
138 
139  if( xl >= 0 )
140  {
141  const EDGE_LIST& cell2 = m_grid[ m_gridSize * gridPoint.y + xl ];
142  scanCell( state, cell2, aP, xl, gridPoint.y );
143 
144  if( state.nearest >= 0 )
145  break;
146  }
147 
148  if( xh < m_gridSize )
149  {
150  const EDGE_LIST& cell2 = m_grid[ m_gridSize * gridPoint.y + xh ];
151  scanCell( state, cell2, aP, xh, gridPoint.y );
152 
153  if( state.nearest >= 0 )
154  break;
155  }
156  }
157  }
158 
159  #ifdef TOM_EXTRA_VERBOSE
160  printf("Nearest: %d prev: %d dmax %d\n", state.nearest, state.nearest_prev, state.dist_max );
161  #endif
162 
163  if( state.nearest < 0 )
164  return 0;
165 
166  if( state.dist_max == 0 )
167  return 1;
168 
169 
170  // special case for diagonal 'slits', e.g. two segments that partially overlap each other.
171  // Just love handling degeneracy... As I can't find any reliable way of fixing it for the moment,
172  // let's fall back to the good old O(N) point-in-polygon test
173  if( state.nearest_prev >= 0 && state.dist_max == state.dist_prev )
174  {
175  int d = std::abs( state.nearest_prev - state.nearest );
176 
177  if( (d == 1) && ( (m_flags[state.nearest_prev] & m_flags[state.nearest]) == 0 ) )
178  {
179  return m_outline.PointInside( aP );
180  }
181  }
182 
183  if( state.dist_max > 0 )
184  {
185  return m_flags[state.nearest] & LEAD_EDGE ? 1 : 0;
186  }
187  else
188  {
189  return m_flags[state.nearest] & TRAIL_EDGE ? 1 : 0;
190  }
191  }
192 
193  bool checkClearance( const VECTOR2I& aP, int aClearance )
194  {
195  int gx0 = poly2gridX( aP.x - aClearance - 1);
196  int gx1 = poly2gridX( aP.x + aClearance + 1);
197  int gy0 = poly2gridY( aP.y - aClearance - 1);
198  int gy1 = poly2gridY( aP.y + aClearance + 1);
199 
201 
202  ecoord dist = (ecoord) aClearance * aClearance;
203 
204  for ( int gx = gx0; gx <= gx1; gx++ )
205  {
206  for ( int gy = gy0; gy <= gy1; gy++ )
207  {
208  const auto& cell = m_grid [ m_gridSize * gy + gx];
209  for ( auto index : cell )
210  {
211  const auto& seg = m_outline.Segment( index );
212 
213  if ( seg.SquaredDistance(aP) <= dist )
214  return true;
215 
216  }
217  }
218 
219  }
220  return false;
221  }
222 
223  int rescale_trunc( int aNumerator, int aValue, int aDenominator ) const
224  {
225  int64_t numerator = (int64_t) aNumerator * (int64_t) aValue;
226  return numerator / aDenominator;
227  }
228 
229 
230  // convertes grid cell coordinates to the polygon coordinates
231  const VECTOR2I grid2poly( const VECTOR2I& p ) const
232  {
234  int py = rescale_trunc( p.y, m_bbox.GetHeight(), m_gridSize ) + m_bbox.GetPosition().y;
235  return VECTOR2I( px, py );
236  }
237 
238  void stupid_test() const
239  {
240  for(int i = 0; i < 16;i++)
241  assert( poly2gridX(grid2polyX(i)) == i);
242  }
243 
244  int grid2polyX( int x ) const
245  {
247  }
248 
249  int grid2polyY( int y ) const
250  {
252  }
253 
254  const VECTOR2I poly2grid( const VECTOR2I& p ) const
255  {
258 
259  if( px < 0 )
260  px = 0;
261 
262  if( px >= m_gridSize )
263  px = m_gridSize - 1;
264 
265  if( py < 0 )
266  py = 0;
267 
268  if( py >= m_gridSize )
269  py = m_gridSize - 1;
270 
271  return VECTOR2I( px, py );
272  }
273 
274  int poly2gridX( int x ) const
275  {
277 
278  if( px < 0 )
279  px = 0;
280 
281  if( px >= m_gridSize )
282  px = m_gridSize - 1;
283 
284  return px;
285  }
286 
287  int poly2gridY( int y ) const
288  {
290 
291  if( py < 0 )
292  py = 0;
293 
294  if( py >= m_gridSize )
295  py = m_gridSize - 1;
296 
297  return py;
298  }
299 
300  void build( const SHAPE_LINE_CHAIN& aPolyOutline, int gridSize )
301  {
302  m_outline = aPolyOutline;
303 
304  //if (orientation(m_outline) < 0)
305  // m_outline = m_outline.Reverse();
306 
307  m_bbox = m_outline.BBox();
308  m_gridSize = gridSize;
309 
310  m_outline.SetClosed( true );
311 
312  m_grid.reserve( gridSize * gridSize );
313 
314  for( int y = 0; y < gridSize; y++ )
315  {
316  for( int x = 0; x < gridSize; x++ )
317  {
318  m_grid.emplace_back( );
319  }
320  }
321 
322  VECTOR2I ref_v( 0, 1 );
323  VECTOR2I ref_h( 0, 1 );
324 
325  m_flags.reserve( m_outline.SegmentCount() );
326 
327  std::unordered_map<SEG, int, segHash, segsEqual> edgeSet;
328 
329  for( int i = 0; i<m_outline.SegmentCount(); i++ )
330  {
331  SEG edge = m_outline.Segment( i );
332 
333  if( edgeSet.find( edge ) == edgeSet.end() )
334  {
335  edgeSet[edge] = 1;
336  }
337  else
338  {
339  edgeSet[edge]++;
340  }
341  }
342 
343  for( int i = 0; i<m_outline.SegmentCount(); i++ )
344  {
345  auto edge = m_outline.Segment( i );
346  auto dir = edge.B - edge.A;
347  int flags = 0;
348 
349 
350  if ( dir.y == 0 )
351  {
352  flags = 0;
353  }
354  else if( edgeSet[edge] == 1 )
355  {
356  if( dir.Dot( ref_h ) < 0 )
357  {
358  flags |= LEAD_EDGE;
359  }
360  else if( dir.Dot( ref_h ) > 0 )
361  {
362  flags |= TRAIL_EDGE;
363  }
364 
365  }
366 
367  m_flags.push_back( flags );
368 
369  if( edge.A.y == edge.B.y )
370  continue;
371 
372  std::set<int> indices;
373 
374  indices.insert( m_gridSize * poly2gridY( edge.A.y ) + poly2gridX( edge.A.x ) );
375  indices.insert( m_gridSize * poly2gridY( edge.B.y ) + poly2gridX( edge.B.x ) );
376 
377  if( edge.A.x > edge.B.x )
378  std::swap( edge.A, edge.B );
379 
380  dir = edge.B - edge.A;
381 
382  if( dir.x != 0 )
383  {
384  int gx0 = poly2gridX( edge.A.x );
385  int gx1 = poly2gridX( edge.B.x );
386 
387  for( int x = gx0; x <= gx1; x++ )
388  {
389  int px = grid2polyX( x );
390  int py = ( edge.A.y + rescale_trunc( dir.y, px - edge.A.x, dir.x ) );
391  int yy = poly2gridY( py );
392 
393  indices.insert( m_gridSize * yy + x );
394  if( x > 0 )
395  indices.insert( m_gridSize * yy + x - 1 );
396 
397  }
398  }
399 
400  if( edge.A.y > edge.B.y )
401  std::swap( edge.A, edge.B );
402 
403  dir = edge.B - edge.A;
404 
405  if( dir.y != 0 )
406  {
407  int gy0 = poly2gridY( edge.A.y );
408  int gy1 = poly2gridY( edge.B.y );
409 
410  for( int y = gy0; y <= gy1; y++ )
411  {
412  int py = grid2polyY( y );
413  int px = ( edge.A.x + rescale_trunc( dir.x, py - edge.A.y, dir.y ) );
414  int xx = poly2gridX( px );
415 
416  indices.insert( m_gridSize * y + xx );
417  if( y > 0 )
418  indices.insert( m_gridSize * (y - 1) + xx );
419  }
420  }
421 
422  for( auto idx : indices )
423  m_grid[idx].push_back( i );
424  }
425 
426  }
427 
428 
429  bool inRange( int v1, int v2, int x ) const
430  {
431  if( v1 < v2 )
432  {
433  return x >= v1 && x <= v2;
434  }
435 
436  return x >= v2 && x <= v1;
437  }
438 
439  struct SCAN_STATE
440  {
442  {
443  dist_prev = INT_MAX;
444  dist_max = INT_MAX;
445  nearest = -1;
446  nearest_prev = -1;
447  };
448 
449  int dist_prev;
450  int dist_max;
452  int nearest;
453  };
454 
455  void scanCell( SCAN_STATE& state, const EDGE_LIST& cell, const VECTOR2I& aP, int cx, int cy ) const
456  {
457  int cx0 = grid2polyX(cx);
458  int cx1 = grid2polyX(cx + 1);
459 
460  #ifdef TOM_EXTRA_VERBOSE
461  printf("Scan %d %d\n", cx, cy );
462  #endif
463 
464  for( auto index : cell )
465  {
466  const SEG& edge = m_outline.CSegment( index );
467 
468 
469  if( m_flags[index] == 0 )
470  {
471  if ( aP.y == edge.A.y && inRange( edge.A.x, edge.B.x, aP.x ) ) // we belong to the outline
472  {
473  state.nearest = index;
474  state.dist_max = 0;
475  return;
476  } else {
477  continue;
478  }
479  }
480 
481  if( inRange( edge.A.y, edge.B.y, aP.y ) )
482  {
483  #ifdef TOM_EXTRA_VERBOSE
484  printf("Test edge: %d [%d %d %d %d] p %d %d flags %d\n", index, edge.A.x, edge.A.y, edge.B.x, edge.B.y, aP.x, aP.y );
485  #endif
486  int dist = 0;
487  int x0;
488  if( edge.A.y == aP.y )
489  {
490  x0 = edge.A.x;
491  }
492  else if( edge.B.y == aP.y )
493  {
494  x0 = edge.B.x;
495  }
496  else
497  {
498  x0 = edge.A.x + rescale( ( edge.B.x - edge.A.x ), (aP.y - edge.A.y), (edge.B.y - edge.A.y ) );
499  }
500 
501 
502  dist = aP.x - x0;
503 
504  #ifdef TOM_EXTRA_VERBOSE
505  printf(" x0 %d dist %d [%s]\n", x0, dist, x0 < cx0 || x0 > cx1 ? "outside" : "inside" );
506  #endif
507 
508  if( x0 < cx0 || x0 > cx1 )
509  {
510  continue;
511  }
512 
513 
514  if( dist == 0 )
515  {
516  if( state.nearest_prev < 0 || state.nearest != index )
517  {
518  state.dist_prev = state.dist_max;
519  state.nearest_prev = state.nearest;
520  }
521 
522  state.nearest = index;
523  state.dist_max = 0;
524  return;
525  }
526 
527  if( dist != 0 && std::abs( dist ) <= std::abs( state.dist_max ) )
528  {
529  if( state.nearest_prev < 0 || state.nearest != index )
530  {
531  state.dist_prev = state.dist_max;
532  state.nearest_prev = state.nearest;
533  }
534 
535  state.dist_max = dist;
536  state.nearest = index;
537  }
538  }
539  }
540  }
541 
542 private:
546  std::vector<int> m_flags;
547  std::vector<EDGE_LIST> m_grid;
548 };
549 
550 #endif
const VECTOR2I poly2grid(const VECTOR2I &p) const
VECTOR2_TRAITS< T >::extended_type extended_type
Definition: vector2d.h:76
void build(const SHAPE_LINE_CHAIN &aPolyOutline, int gridSize)
void scanCell(SCAN_STATE &state, const EDGE_LIST &cell, const VECTOR2I &aP, int cx, int cy) const
VECTOR2I v2(1, 0)
Test suite for KiCad math code.
SHAPE_LINE_CHAIN m_outline
int ContainsPoint(const VECTOR2I &aP, int aClearance=0)
std::size_t operator()(const SEG &a) const
bool inRange(int v1, int v2, int x) const
int grid2polyY(int y) const
std::vector< int > m_flags
VECTOR2< int > VECTOR2I
Definition: vector2d.h:623
int poly2gridX(int x) const
std::vector< EDGE_LIST > m_grid
void SetClosed(bool aClosed)
Function SetClosed()
const BOX2I BBox(int aClearance=0) const override
Compute a bounding box of the shape, with a margin of aClearance a collision.
const BOX2I & BBox() const
POLY_GRID_PARTITION(const SHAPE_LINE_CHAIN &aPolyOutline, int gridSize)
coord_type GetWidth() const
Definition: box2.h:197
bool Contains(const Vec &aPoint) const
Function Contains.
Definition: box2.h:151
VECTOR2I::extended_type ecoord
int containsPoint(const VECTOR2I &aP, bool debug=false) const
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.
bool operator()(const SEG &a, const SEG &b) const
std::vector< int > EDGE_LIST
int grid2polyX(int x) const
int SegmentCount() const
Function SegmentCount()
const Vec & GetPosition() const
Definition: box2.h:194
Definition: seg.h:41
void hash_combine(std::size_t &seed, const T &v)
SEG Segment(int aIndex)
Function Segment()
const VECTOR2I grid2poly(const VECTOR2I &p) const
const SEG CSegment(int aIndex) const
Function CSegment()
SHAPE_LINE_CHAIN.
VECTOR2I A
Definition: seg.h:49
T rescale(T aNumerator, T aValue, T aDenominator)
Function rescale()
Definition: util.h:95
coord_type GetHeight() const
Definition: box2.h:198
int poly2gridY(int y) const
Class POLY_GRID_PARTITION.
int rescale_trunc(int aNumerator, int aValue, int aDenominator) const
bool checkClearance(const VECTOR2I &aP, int aClearance)
VECTOR2I B
Definition: seg.h:50