| 11 | { |
| 12 | |
| 13 | MeshProjectionResult findProjectionSubtree( const Vector3f & pt, const MeshPart & mp, const AABBTree & tree, float upDistLimitSq, const AffineXf3f * xf, float loDistLimitSq, |
| 14 | const FacePredicate & validFaces, const std::function<bool(const MeshProjectionResult&)> & validProjections ) |
| 15 | { |
| 16 | MeshProjectionResult res; |
| 17 | res.distSq = upDistLimitSq; |
| 18 | if ( tree.nodes().empty() ) |
| 19 | return res; |
| 20 | |
| 21 | struct SubTask |
| 22 | { |
| 23 | NoInitNodeId n; |
| 24 | float distSq; |
| 25 | }; |
| 26 | InplaceStack<SubTask, 32> subtasks; |
| 27 | |
| 28 | auto addSubTask = [&]( const SubTask & s ) |
| 29 | { |
| 30 | if ( s.distSq < res.distSq ) |
| 31 | subtasks.push( s ); |
| 32 | }; |
| 33 | |
| 34 | auto getSubTask = [&]( NodeId n ) |
| 35 | { |
| 36 | const auto & box = tree.nodes()[n].box; |
| 37 | float distSq = xf ? transformed( box, *xf ).getDistanceSq( pt ) : box.getDistanceSq( pt ); |
| 38 | return SubTask { n, distSq }; |
| 39 | }; |
| 40 | |
| 41 | addSubTask( getSubTask( tree.rootNodeId() ) ); |
| 42 | |
| 43 | while ( !subtasks.empty() ) |
| 44 | { |
| 45 | const auto s = subtasks.top(); |
| 46 | subtasks.pop(); |
| 47 | const auto & node = tree[s.n]; |
| 48 | if ( s.distSq >= res.distSq ) |
| 49 | continue; |
| 50 | |
| 51 | if ( node.leaf() ) |
| 52 | { |
| 53 | const auto face = node.leafId(); |
| 54 | if ( validFaces && !validFaces( face ) ) |
| 55 | continue; |
| 56 | if ( mp.region && !mp.region->test( face ) ) |
| 57 | continue; |
| 58 | Vector3f a, b, c; |
| 59 | mp.mesh.getTriPoints( face, a, b, c ); |
| 60 | if ( xf ) |
| 61 | { |
| 62 | a = (*xf)( a ); |
| 63 | b = (*xf)( b ); |
| 64 | c = (*xf)( c ); |
| 65 | } |
| 66 | |
| 67 | // compute the closest point in double-precision, because float might be not enough |
| 68 | const auto [projD, baryD] = closestPointInTriangle( Vector3d( pt ), Vector3d( a ), Vector3d( b ), Vector3d( c ) ); |
| 69 | const Vector3f proj( projD ); |
| 70 | const MeshProjectionResult candidate |
no test coverage detected