| 14 | { |
| 15 | |
| 16 | MeshMeshDistanceResult findDistance( const MeshPart& a, const MeshPart& b, const AffineXf3f* rigidB2A, float upDistLimitSq ) |
| 17 | { |
| 18 | MR_TIMER; |
| 19 | |
| 20 | const AABBTree& aTree = a.mesh.getAABBTree(); |
| 21 | const AABBTree& bTree = b.mesh.getAABBTree(); |
| 22 | |
| 23 | MeshMeshDistanceResult res; |
| 24 | res.distSq = upDistLimitSq; |
| 25 | if ( aTree.nodes().empty() || bTree.nodes().empty() ) |
| 26 | { |
| 27 | assert( false ); |
| 28 | return res; |
| 29 | } |
| 30 | |
| 31 | NodeBitSet aNodes, bNodes; |
| 32 | NodeBitSet* aNodesPtr{nullptr}, *bNodesPtr{nullptr}; |
| 33 | if ( a.region ) |
| 34 | { |
| 35 | aNodes = aTree.getNodesFromLeaves( *a.region ); |
| 36 | aNodesPtr = &aNodes; |
| 37 | } |
| 38 | if ( b.region ) |
| 39 | { |
| 40 | bNodes = bTree.getNodesFromLeaves( *b.region ); |
| 41 | bNodesPtr = &bNodes; |
| 42 | } |
| 43 | |
| 44 | struct SubTask |
| 45 | { |
| 46 | NoInitNodeId a, b; |
| 47 | float distSq; |
| 48 | }; |
| 49 | InplaceStack<SubTask, 128> subtasks; |
| 50 | |
| 51 | auto addSubTask = [&]( const SubTask& s ) |
| 52 | { |
| 53 | if ( s.distSq < res.distSq ) |
| 54 | subtasks.push( s ); |
| 55 | }; |
| 56 | |
| 57 | auto getSubTask = [&]( NodeId a, NodeId b ) |
| 58 | { |
| 59 | float distSq = aTree.nodes()[a].box.getDistanceSq( transformed( bTree.nodes()[b].box, rigidB2A ) ); |
| 60 | return SubTask { a, b, distSq }; |
| 61 | }; |
| 62 | |
| 63 | addSubTask( getSubTask( aTree.rootNodeId(), bTree.rootNodeId() ) ); |
| 64 | |
| 65 | while ( !subtasks.empty() ) |
| 66 | { |
| 67 | const auto s = subtasks.top(); |
| 68 | subtasks.pop(); |
| 69 | if ( aNodesPtr && !aNodes.test( s.a ) ) |
| 70 | continue; |
| 71 | if ( bNodesPtr && !bNodes.test( s.b ) ) |
| 72 | continue; |
| 73 | |