| 22 | |
| 23 | template<typename V, typename F, typename B, typename L> |
| 24 | PolylineProjectionResult<V> findProjectionCore( const AABBTreePolyline<V> & tree, float upDistLimitSq, AffineXf<V>* xf, |
| 25 | F && edgeToEndPoints, float loDistLimitSq, B && distSqToBox, L && closestPointsToLineSegm ) |
| 26 | { |
| 27 | PolylineProjectionResult<V> res; |
| 28 | res.distSq = upDistLimitSq; |
| 29 | if ( tree.nodes().empty() ) |
| 30 | return res; |
| 31 | |
| 32 | struct SubTask |
| 33 | { |
| 34 | NoInitNodeId n; |
| 35 | float distSq; |
| 36 | }; |
| 37 | InplaceStack<SubTask, 32> subtasks; |
| 38 | |
| 39 | auto addSubTask = [&] ( const SubTask& s ) |
| 40 | { |
| 41 | if ( s.distSq < res.distSq ) |
| 42 | subtasks.push( s ); |
| 43 | }; |
| 44 | |
| 45 | auto getSubTask = [&] ( NodeId n ) |
| 46 | { |
| 47 | return SubTask { n, distSqToBox( transformed( tree.nodes()[n].box, xf ) ) }; |
| 48 | }; |
| 49 | |
| 50 | addSubTask( getSubTask( tree.rootNodeId() ) ); |
| 51 | |
| 52 | while ( !subtasks.empty() ) |
| 53 | { |
| 54 | const auto s = subtasks.top(); |
| 55 | subtasks.pop(); |
| 56 | const auto& node = tree[s.n]; |
| 57 | if ( s.distSq >= res.distSq ) |
| 58 | continue; |
| 59 | |
| 60 | if ( node.leaf() ) |
| 61 | { |
| 62 | const auto lineId = node.leafId(); |
| 63 | V a, b; |
| 64 | edgeToEndPoints( lineId, a, b ); |
| 65 | if ( xf ) |
| 66 | { |
| 67 | a = ( *xf )( a ); |
| 68 | b = ( *xf )( b ); |
| 69 | } |
| 70 | |
| 71 | const auto closest = closestPointsToLineSegm( LineSegm<V>{ a, b } ); |
| 72 | const float distSq = closest.lengthSq(); |
| 73 | if ( distSq < res.distSq ) |
| 74 | { |
| 75 | res.distSq = distSq; |
| 76 | res.point = closest.b; |
| 77 | res.line = lineId; |
| 78 | if ( distSq <= loDistLimitSq ) |
| 79 | break; |
| 80 | } |
| 81 | continue; |
no test coverage detected