25 #ifndef __POLY_GRID_PARTITION_H 26 #define __POLY_GRID_PARTITION_H 28 #include <geometry/seg.h> 29 #include <geometry/shape_line_chain.h> 30 #include <geometry/shape_rect.h> 34 #include <unordered_map> 54 using EDGE_LIST = std::vector<int>;
57 inline void hash_combine( std::size_t& seed,
const T& v )
60 seed ^= hasher( v ) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
65 bool operator()(
const SEG& a,
const SEG& b )
const 67 return (a.A == b.A && a.B == b.B) || (a.A == b.B && a.B == b.A);
73 std::size_t operator()(
const SEG& a )
const 77 return a.A.x + a.B.x + a.A.y + a.B.y;
85 int px = rescale( p.x, m_bbox.GetWidth(), m_gridSize ) + m_bbox.GetPosition().x;
86 int py = rescale( p.y, m_bbox.GetHeight(), m_gridSize ) + m_bbox.GetPosition().y;
91 void stupid_test()
const 93 for(
int i = 0; i < 16;i++)
94 assert( poly2gridX(grid2polyX(i)) == i);
97 int grid2polyX(
int x )
const 99 return rescale( x, m_bbox.GetWidth(), m_gridSize ) + m_bbox.GetPosition().x;
102 int grid2polyY(
int y )
const 104 return rescale( y, m_bbox.GetHeight(), m_gridSize ) + m_bbox.GetPosition().y;
109 int px = rescale( p.x - m_bbox.GetPosition().x, m_gridSize, m_bbox.GetWidth() );
110 int py = rescale( p.y - m_bbox.GetPosition().y, m_gridSize, m_bbox.GetHeight() );
115 if( px >= m_gridSize )
121 if( py >= m_gridSize )
127 int poly2gridX(
int x )
const 129 int px = rescale( x - m_bbox.GetPosition().x, m_gridSize, m_bbox.GetWidth() );
134 if( px >= m_gridSize )
140 int poly2gridY(
int y )
const 142 int py = rescale( y - m_bbox.GetPosition().y, m_gridSize, m_bbox.GetHeight() );
147 if( py >= m_gridSize )
155 m_outline = aPolyOutline;
160 m_bbox = m_outline.
BBox();
161 m_gridSize = gridSize;
163 m_outline.SetClosed(
true );
165 m_grid.reserve( gridSize * gridSize );
167 for(
int y = 0; y < gridSize; y++ )
169 for(
int x = 0; x < gridSize; x++ )
171 m_grid.push_back( EDGE_LIST() );
178 m_flags.reserve( m_outline.SegmentCount() );
180 std::unordered_map<SEG, int, segHash, segsEqual> edgeSet;
182 for(
int i = 0; i<m_outline.SegmentCount(); i++ )
184 SEG edge = m_outline.CSegment( i );
186 if( edgeSet.find( edge ) == edgeSet.end() )
196 for(
int i = 0; i<m_outline.SegmentCount(); i++ )
198 auto edge = m_outline.CSegment( i );
199 auto dir = edge.B - edge.A;
202 if( edgeSet[edge] == 1 )
204 if( dir.Dot( ref_h ) < 0 )
208 else if( dir.Dot( ref_h ) > 0 )
215 m_flags.push_back( flags );
217 std::set<int> indices;
219 indices.insert( m_gridSize * poly2gridY( edge.A.y ) + poly2gridX( edge.A.x ) );
220 indices.insert( m_gridSize * poly2gridY( edge.B.y ) + poly2gridX( edge.B.x ) );
222 if( edge.A.x > edge.B.x )
223 std::swap( edge.A, edge.B );
225 dir = edge.B - edge.A;
229 int gx0 = poly2gridX( edge.A.x );
230 int gx1 = poly2gridX( edge.B.x );
232 for(
int x = gx0; x <= gx1; x++ )
234 int px = grid2polyX( x );
235 int py = ( edge.A.y + rescale( dir.y, px - edge.A.x, dir.x ) );
236 int yy = poly2gridY( py );
238 indices.insert( m_gridSize * yy + x );
240 indices.insert( m_gridSize * yy + x - 1 );
245 if( edge.A.y > edge.B.y )
246 std::swap( edge.A, edge.B );
248 dir = edge.B - edge.A;
252 int gy0 = poly2gridY( edge.A.y );
253 int gy1 = poly2gridY( edge.B.y );
255 for(
int y = gy0; y <= gy1; y++ )
257 int py = grid2polyY( y );
258 int px = ( edge.A.x + rescale( dir.x, py - edge.A.y, dir.y ) );
259 int xx = poly2gridX( px );
261 indices.insert( m_gridSize * y + xx );
263 indices.insert( m_gridSize * (y - 1) + xx );
267 for(
auto idx : indices )
268 m_grid[idx].push_back( i );
274 bool inRange(
int v1,
int v2,
int x )
const 278 return x >= v1 && x <= v2;
281 return x >= v2 && x <= v1;
299 void scanCell( SCAN_STATE& state,
const EDGE_LIST& cell,
const VECTOR2I& aP,
int cx,
int cy )
const 301 int cx0 = grid2polyX(cx);
302 int cx1 = grid2polyX(cx + 1);
304 for(
auto index : cell )
306 const SEG& edge = m_outline.CSegment( index );
309 if( m_flags[index] == 0 )
311 if ( aP.y == edge.A.y && inRange( edge.A.x, edge.B.x, aP.x ) )
313 state.nearest = index;
321 if( inRange( edge.A.y, edge.B.y, aP.y ) )
325 if( edge.A.y == aP.y )
329 else if( edge.B.y == aP.y )
335 x0 = edge.A.x + rescale( ( edge.B.x - edge.A.x ), (aP.y - edge.A.y), (edge.B.y - edge.A.y ) );
338 if( x0 < cx0 || x0 > cx1 )
347 if( state.nearest_prev < 0 || state.nearest != index )
349 state.dist_prev = state.dist_max;
350 state.nearest_prev = state.nearest;
353 state.nearest = index;
358 if( dist != 0 && std::abs( dist ) <= std::abs( state.dist_max ) )
360 if( state.nearest_prev < 0 || state.nearest != index )
362 state.dist_prev = state.dist_max;
363 state.nearest_prev = state.nearest;
366 state.dist_max = dist;
367 state.nearest = index;
377 build( aPolyOutline, gridSize );
380 int containsPoint(
const VECTOR2I& aP )
382 const auto gridPoint = poly2grid( aP );
384 if( !m_bbox.Contains( aP ) )
388 const EDGE_LIST& cell = m_grid[ m_gridSize * gridPoint.y + gridPoint.x ];
390 scanCell( state, cell, aP, gridPoint.x, gridPoint.y );
392 if( state.nearest < 0 )
394 state = SCAN_STATE();
396 for(
int d = 1; d <= m_gridSize; d++ )
398 int xl = gridPoint.x - d;
399 int xh = gridPoint.x + d;
403 const EDGE_LIST& cell2 = m_grid[ m_gridSize * gridPoint.y + xl ];
404 scanCell( state, cell2, aP, xl, gridPoint.y );
406 if( state.nearest >= 0 )
410 if( xh < m_gridSize )
412 const EDGE_LIST& cell2 = m_grid[ m_gridSize * gridPoint.y + xh ];
413 scanCell( state, cell2, aP, xh, gridPoint.y );
415 if( state.nearest >= 0 )
421 if( state.nearest < 0 )
424 if( state.dist_max == 0 )
427 if( state.nearest_prev >= 0 && state.dist_max == state.dist_prev )
429 int d = std::abs( state.nearest_prev - state.nearest );
431 if( (d == 1) && ( (m_flags[state.nearest_prev] & m_flags[state.nearest]) == 0 ) )
441 if( state.dist_max > 0 )
443 return m_flags[state.nearest] & LEAD_H ? 1 : 0;
447 return m_flags[state.nearest] & TRAIL_H ? 1 : 0;
451 bool checkClearance(
const VECTOR2I& aP,
int aClearance )
453 int gx0 = poly2gridX( aP.x - aClearance - 1);
454 int gx1 = poly2gridX( aP.x + aClearance + 1);
455 int gy0 = poly2gridY( aP.y - aClearance - 1);
456 int gy1 = poly2gridY( aP.y + aClearance + 1);
458 using ecoord = VECTOR2I::extended_type;
460 ecoord dist = (ecoord) aClearance * aClearance;
462 for (
int gx = gx0; gx <= gx1; gx++ )
464 for (
int gy = gy0; gy <= gy1; gy++ )
466 const auto& cell = m_grid [ m_gridSize * gy + gx];
467 for (
auto index : cell )
469 const auto& seg = m_outline.CSegment(index);
471 if ( seg.SquaredDistance(aP) <= dist )
483 int ContainsPoint(
const VECTOR2I& aP,
int aClearance = 0 )
485 if( containsPoint(aP) )
489 return checkClearance ( aP, aClearance );
494 const BOX2I& BBox()
const 503 std::vector<int> m_flags;
504 std::vector<EDGE_LIST> m_grid;
const BOX2I BBox(int aClearance=0) const override
Function BBox()
Definition: shape_line_chain.h:264
Class SHAPE_LINE_CHAIN.
Definition: shape_line_chain.h:47
Class POLY_GRID_PARTITION.
Definition: poly_grid_partition.h:43