| 326 | |
| 327 | template<typename V, typename F> |
| 328 | void findEdgesInBallCore( const AABBTreePolyline<V>& tree, const V& center, |
| 329 | float radius, const FoundEdgeCallback<V>& foundCallback, AffineXf<V>* xf, F&& edgeToEndPoints ) |
| 330 | { |
| 331 | if ( !foundCallback ) |
| 332 | { |
| 333 | assert( false ); |
| 334 | return; |
| 335 | } |
| 336 | |
| 337 | if ( tree.nodes().empty() ) |
| 338 | return; |
| 339 | |
| 340 | const auto radiusSq = sqr( radius ); |
| 341 | InplaceStack<NoInitNodeId, 32> subtasks; |
| 342 | |
| 343 | auto addSubTask = [&] ( NodeId n ) |
| 344 | { |
| 345 | const auto & box = tree.nodes()[n].box; |
| 346 | float distSq = xf ? transformed( box, *xf ).getDistanceSq( center ) : box.getDistanceSq( center ); |
| 347 | if ( distSq <= radiusSq ) |
| 348 | subtasks.push( n ); |
| 349 | }; |
| 350 | |
| 351 | addSubTask( tree.rootNodeId() ); |
| 352 | |
| 353 | while ( !subtasks.empty() ) |
| 354 | { |
| 355 | const auto n = subtasks.top(); |
| 356 | subtasks.pop(); |
| 357 | const auto& node = tree[n]; |
| 358 | |
| 359 | if ( node.leaf() ) |
| 360 | { |
| 361 | LineSegm<V> segm; |
| 362 | edgeToEndPoints( node.leafId(), segm.a, segm.b ); |
| 363 | if ( xf ) |
| 364 | { |
| 365 | segm.a = ( *xf )( segm.a ); |
| 366 | segm.b = ( *xf )( segm.b ); |
| 367 | } |
| 368 | auto proj = closestPointOnLineSegm( center, segm ); |
| 369 | |
| 370 | float distSq = ( proj - center ).lengthSq(); |
| 371 | if ( distSq <= radiusSq ) |
| 372 | foundCallback( node.leafId(), proj, distSq ); |
| 373 | continue; |
| 374 | } |
| 375 | |
| 376 | addSubTask( node.r ); // look at right node later |
| 377 | addSubTask( node.l ); // look at left node first |
| 378 | } |
| 379 | } |
| 380 | |
| 381 | void findEdgesInBall( const Polyline2& polyline, const Vector2f& center, float radius, const FoundEdgeCallback2& foundCallback, AffineXf2f* xf ) |
| 382 | { |
no test coverage detected