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
75 return a.A.x + a.B.x + a.A.y + a.B.y;
81 int px = rescale( p.x, m_bbox.GetWidth(), m_gridSize ) + m_bbox.GetPosition().x;
82 int py = rescale( p.y, m_bbox.GetHeight(), m_gridSize ) + m_bbox.GetPosition().y;
87 void stupid_test()
const
89 for(
int i = 0; i < 16;i++)
90 assert( poly2gridX(grid2polyX(i)) == i);
93 int grid2polyX(
int x )
const
95 return rescale( x, m_bbox.GetWidth(), m_gridSize ) + m_bbox.GetPosition().x;
98 int grid2polyY(
int y )
const
100 return rescale( y, m_bbox.GetHeight(), m_gridSize ) + m_bbox.GetPosition().y;
105 int px = rescale( p.x - m_bbox.GetPosition().x, m_gridSize, m_bbox.GetWidth() );
106 int py = rescale( p.y - m_bbox.GetPosition().y, m_gridSize, m_bbox.GetHeight() );
111 if( px >= m_gridSize )
117 if( py >= m_gridSize )
123 int poly2gridX(
int x )
const
125 int px = rescale( x - m_bbox.GetPosition().x, m_gridSize, m_bbox.GetWidth() );
130 if( px >= m_gridSize )
136 int poly2gridY(
int y )
const
138 int py = rescale( y - m_bbox.GetPosition().y, m_gridSize, m_bbox.GetHeight() );
143 if( py >= m_gridSize )
151 m_outline = aPolyOutline;
156 m_bbox = m_outline.
BBox();
157 m_gridSize = gridSize;
161 m_grid.reserve( gridSize * gridSize );
163 for(
int y = 0; y < gridSize; y++ )
165 for(
int x = 0; x < gridSize; x++ )
167 m_grid.push_back( EDGE_LIST() );
176 std::unordered_map<SEG, int, segHash, segsEqual> edgeSet;
182 if( edgeSet.find( edge ) == edgeSet.end() )
194 auto edge = m_outline.
CSegment( i );
195 auto dir = edge.B - edge.A;
198 if( edgeSet[edge] == 1 )
200 if( dir.Dot( ref_h ) < 0 )
204 else if( dir.Dot( ref_h ) > 0 )
211 m_flags.push_back( flags );
213 std::set<int> indices;
215 indices.insert( m_gridSize * poly2gridY( edge.A.y ) + poly2gridX( edge.A.x ) );
216 indices.insert( m_gridSize * poly2gridY( edge.B.y ) + poly2gridX( edge.B.x ) );
218 if( edge.A.x > edge.B.x )
219 std::swap( edge.A, edge.B );
221 dir = edge.B - edge.A;
225 int gx0 = poly2gridX( edge.A.x );
226 int gx1 = poly2gridX( edge.B.x );
228 for(
int x = gx0; x <= gx1; x++ )
230 int px = grid2polyX( x );
231 int py = ( edge.A.y + rescale( dir.y, px - edge.A.x, dir.x ) );
232 int yy = poly2gridY( py );
234 indices.insert( m_gridSize * yy + x );
236 indices.insert( m_gridSize * yy + x - 1 );
241 if( edge.A.y > edge.B.y )
242 std::swap( edge.A, edge.B );
244 dir = edge.B - edge.A;
248 int gy0 = poly2gridY( edge.A.y );
249 int gy1 = poly2gridY( edge.B.y );
251 for(
int y = gy0; y <= gy1; y++ )
253 int py = grid2polyY( y );
254 int px = ( edge.A.x + rescale( dir.x, py - edge.A.y, dir.y ) );
255 int xx = poly2gridX( px );
257 indices.insert( m_gridSize * y + xx );
259 indices.insert( m_gridSize * (y - 1) + xx );
263 for(
auto idx : indices )
264 m_grid[idx].push_back( i );
270 bool inRange(
int v1,
int v2,
int x )
const
274 return x >= v1 && x <= v2;
277 return x >= v2 && x <= v1;
296 void scanCell( SCAN_STATE& state,
const EDGE_LIST& cell,
const VECTOR2I& aP,
int cx,
int cy )
const
298 int cx0 = grid2polyX(cx);
299 int cx1 = grid2polyX(cx + 1);
301 for(
auto index : cell )
306 if( m_flags[index] == 0 )
308 if ( aP.y == edge.A.y && inRange( edge.A.x, edge.B.x, aP.x ) )
310 state.nearest = index;
318 if( inRange( edge.A.y, edge.B.y, aP.y ) )
322 if( edge.A.y == aP.y )
326 else if( edge.B.y == aP.y )
332 x0 = edge.A.x + rescale( ( edge.B.x - edge.A.x ), (aP.y - edge.A.y), (edge.B.y - edge.A.y ) );
335 if( x0 < cx0 || x0 > cx1 )
344 if( state.nearest_prev < 0 || state.nearest != index )
346 state.dist_prev = state.dist_max;
347 state.nearest_prev = state.nearest;
350 state.nearest = index;
355 if( dist != 0 && std::abs( dist ) <= std::abs( state.dist_max ) )
357 if( state.nearest_prev < 0 || state.nearest != index )
359 state.dist_prev = state.dist_max;
360 state.nearest_prev = state.nearest;
363 state.dist_max = dist;
364 state.nearest = index;
374 build( aPolyOutline, gridSize );
377 int containsPoint(
const VECTOR2I& aP )
379 const auto gridPoint = poly2grid( aP );
385 const EDGE_LIST& cell = m_grid[ m_gridSize * gridPoint.y + gridPoint.x ];
387 scanCell( state, cell, aP, gridPoint.x, gridPoint.y );
389 if( state.nearest < 0 )
391 state = SCAN_STATE();
393 for(
int d = 1; d <= m_gridSize; d++ )
395 int xl = gridPoint.x - d;
396 int xh = gridPoint.x + d;
400 const EDGE_LIST& cell2 = m_grid[ m_gridSize * gridPoint.y + xl ];
401 scanCell( state, cell2, aP, xl, gridPoint.y );
403 if( state.nearest >= 0 )
407 if( xh < m_gridSize )
409 const EDGE_LIST& cell2 = m_grid[ m_gridSize * gridPoint.y + xh ];
410 scanCell( state, cell2, aP, xh, gridPoint.y );
412 if( state.nearest >= 0 )
418 if( state.nearest < 0 )
421 if( state.dist_max == 0 )
424 if( state.nearest_prev >= 0 && state.dist_max == state.dist_prev )
426 int d = std::abs( state.nearest_prev - state.nearest );
428 if( (d == 1) && ( (m_flags[state.nearest_prev] & m_flags[state.nearest]) == 0 ) )
438 if( state.dist_max > 0 )
440 return m_flags[state.nearest] & LEAD_H ? 1 : 0;
444 return m_flags[state.nearest] & TRAIL_H ? 1 : 0;
448 bool checkClearance(
const VECTOR2I& aP,
int aClearance )
450 int gx0 = poly2gridX( aP.x - aClearance - 1);
451 int gx1 = poly2gridX( aP.x + aClearance + 1);
452 int gy0 = poly2gridY( aP.y - aClearance - 1);
453 int gy1 = poly2gridY( aP.y + aClearance + 1);
455 using ecoord = VECTOR2I::extended_type;
457 ecoord dist = (ecoord) aClearance * aClearance;
459 for (
int gx = gx0; gx <= gx1; gx++ )
461 for (
int gy = gy0; gy <= gy1; gy++ )
463 const auto& cell = m_grid [ m_gridSize * gy + gx];
464 for (
auto index : cell )
466 const auto& seg = m_outline.
CSegment(index);
468 if ( seg.SquaredDistance(aP) <= dist )
480 int ContainsPoint(
const VECTOR2I& aP,
int aClearance = 0 )
482 if( containsPoint(aP) )
486 return checkClearance ( aP, aClearance );
491 const BOX2I& BBox()
const
500 std::vector<int> m_flags;
501 std::vector<EDGE_LIST> m_grid;