void distanceRecurse(DistanceTraversalNodeBase* node, int bv_node1_id, int bv_node2_id, BVHFrontList* front_list) { bool is_first_node_leaf = node->isFirstNodeLeaf(bv_node1_id); bool is_second_node_leaf = node->isSecondNodeLeaf(bv_node2_id); if(is_first_node_leaf && is_second_node_leaf) { updateFrontList(front_list, bv_node1_id, bv_node2_id); node->leafTesting(bv_node1_id, bv_node2_id); return; } // BVNodes distance pairs int a1, a2, c1, c2; if(node->firstOverSecond(bv_node1_id, bv_node2_id)) { a1 = node->getFirstLeftChild(bv_node1_id); a2 = bv_node2_id; c1 = node->getFirstRightChild(bv_node1_id); c2 = bv_node2_id; } else { a1 = bv_node1_id; a2 = node->getSecondLeftChild(bv_node2_id); c1 = bv_node1_id; c2 = node->getSecondRightChild(bv_node2_id); } FCL_REAL distance_a = node->BVTesting(a1, a2); FCL_REAL distance_c = node->BVTesting(c1, c2); if(distance_c < distance_a) { if(!node->canStop(distance_c)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); if(!node->canStop(distance_a)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); } else { if(!node->canStop(distance_a)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); if(!node->canStop(distance_c)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); } }
void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 && l2) { updateFrontList(front_list, b1, b2); node->leafTesting(b1, b2); return; } int a1, a2, c1, c2; if(node->firstOverSecond(b1, b2)) { a1 = node->getFirstLeftChild(b1); a2 = b2; c1 = node->getFirstRightChild(b1); c2 = b2; } else { a1 = b1; a2 = node->getSecondLeftChild(b2); c1 = b1; c2 = node->getSecondRightChild(b2); } FCL_REAL d1 = node->BVTesting(a1, a2); FCL_REAL d2 = node->BVTesting(c1, c2); if(d2 < d1) { if(!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); if(!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); } else { if(!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); if(!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); } }
void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 && l2) { updateFrontList(front_list, b1, b2); if(node->BVTesting(b1, b2)) return; node->leafTesting(b1, b2); return; } if(node->BVTesting(b1, b2)) { updateFrontList(front_list, b1, b2); return; } if(node->firstOverSecond(b1, b2)) { int c1 = node->getFirstLeftChild(b1); int c2 = node->getFirstRightChild(b1); collisionRecurse(node, c1, b2, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; collisionRecurse(node, c2, b2, front_list); } else { int c1 = node->getSecondLeftChild(b2); int c2 = node->getSecondRightChild(b2); collisionRecurse(node, b1, c1, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; collisionRecurse(node, b1, c2, front_list); } }
void collisionRecurse(CollisionTraversalNodeBase* node, int bv_node1_id, int bv_node2_id, BVHFrontList* front_list) { bool is_first_node_leaf = node->isFirstNodeLeaf(bv_node1_id); bool is_second_node_leaf = node->isSecondNodeLeaf(bv_node2_id); if(is_first_node_leaf && is_second_node_leaf) { updateFrontList(front_list, bv_node1_id, bv_node2_id); if(node->BVTesting(bv_node1_id, bv_node2_id)) return; node->leafTesting(bv_node1_id, bv_node2_id); return; } if(node->BVTesting(bv_node1_id, bv_node2_id)) { updateFrontList(front_list, bv_node1_id, bv_node2_id); return; } if(node->firstOverSecond(bv_node1_id, bv_node2_id)) { int left_child = node->getFirstLeftChild(bv_node1_id); collisionRecurse(node, left_child, bv_node2_id, front_list); // early stop is disabled if front_list is used if(node->canStop() && !front_list) return; int right_child = node->getFirstRightChild(bv_node1_id); collisionRecurse(node, right_child, bv_node2_id, front_list); } else { int left_child = node->getSecondLeftChild(bv_node2_id); collisionRecurse(node, bv_node1_id, left_child, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; int right_child = node->getSecondRightChild(bv_node2_id); collisionRecurse(node, bv_node1_id, right_child, front_list); } }
void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 && l2) { updateFrontList(front_list, b1, b2); if(node->BVTesting(b1, b2, R, T)) return; node->leafTesting(b1, b2, R, T); return; } if(node->BVTesting(b1, b2, R, T)) { updateFrontList(front_list, b1, b2); return; } Vec3f temp; if(node->firstOverSecond(b1, b2)) { int c1 = node->getFirstLeftChild(b1); int c2 = node->getFirstRightChild(b1); const OBB& bv1 = node->model1->getBV(c1).bv; Matrix3f Rc(R.transposeTimes(bv1.axis[0]), R.transposeTimes(bv1.axis[1]), R.transposeTimes(bv1.axis[2])); temp = T - bv1.To; Vec3f Tc(temp.dot(bv1.axis[0]), temp.dot(bv1.axis[1]), temp.dot(bv1.axis[2])); collisionRecurse(node, c1, b2, Rc, Tc, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; const OBB& bv2 = node->model1->getBV(c2).bv; Rc = Matrix3f(R.transposeTimes(bv2.axis[0]), R.transposeTimes(bv2.axis[1]), R.transposeTimes(bv2.axis[2])); temp = T - bv2.To; Tc.setValue(temp.dot(bv2.axis[0]), temp.dot(bv2.axis[1]), temp.dot(bv2.axis[2])); collisionRecurse(node, c2, b2, Rc, Tc, front_list); } else { int c1 = node->getSecondLeftChild(b2); int c2 = node->getSecondRightChild(b2); const OBB& bv1 = node->model2->getBV(c1).bv; Matrix3f Rc; temp = R * bv1.axis[0]; Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; temp = R * bv1.axis[1]; Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; temp = R * bv1.axis[2]; Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; Vec3f Tc = R * bv1.To + T; collisionRecurse(node, b1, c1, Rc, Tc, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; const OBB& bv2 = node->model2->getBV(c2).bv; temp = R * bv2.axis[0]; Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; temp = R * bv2.axis[1]; Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; temp = R * bv2.axis[2]; Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; Tc = R * bv2.To + T; collisionRecurse(node, b1, c2, Rc, Tc, front_list); } }
void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize) { BVTQ bvtq; bvtq.qsize = qsize; BVT min_test; min_test.b1 = b1; min_test.b2 = b2; while(1) { bool l1 = node->isFirstNodeLeaf(min_test.b1); bool l2 = node->isSecondNodeLeaf(min_test.b2); if(l1 && l2) { updateFrontList(front_list, min_test.b1, min_test.b2); node->leafTesting(min_test.b1, min_test.b2); } else if(bvtq.full()) { // queue should not get two more tests, recur distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize); } else { // queue capacity is not full yet BVT bvt1, bvt2; if(node->firstOverSecond(min_test.b1, min_test.b2)) { int c1 = node->getFirstLeftChild(min_test.b1); int c2 = node->getFirstRightChild(min_test.b1); bvt1.b1 = c1; bvt1.b2 = min_test.b2; bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); bvt2.b1 = c2; bvt2.b2 = min_test.b2; bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); } else { int c1 = node->getSecondLeftChild(min_test.b2); int c2 = node->getSecondRightChild(min_test.b2); bvt1.b1 = min_test.b1; bvt1.b2 = c1; bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); bvt2.b1 = min_test.b1; bvt2.b2 = c2; bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); } bvtq.push(bvt1); bvtq.push(bvt2); } if(bvtq.empty()) break; else { min_test = bvtq.top(); bvtq.pop(); if(node->canStop(min_test.d)) { updateFrontList(front_list, min_test.b1, min_test.b2); break; } } } }
void distanceQueueRecurse(DistanceTraversalNodeBase* node, int bv_node1_id, int bv_node2_id, BVHFrontList* front_list, int qsize) { BVTQ bvtq; bvtq.qsize = qsize; BVT min_test; min_test.bv_node1_id = bv_node1_id; min_test.bv_node2_id = bv_node2_id; FCL_REAL whole_distance_fraction = node->whole_distance_ / 100.0; while(1) { bool l1 = node->isFirstNodeLeaf(min_test.bv_node1_id); bool l2 = node->isSecondNodeLeaf(min_test.bv_node2_id); if(l1 && l2) { updateFrontList(front_list, min_test.bv_node1_id, min_test.bv_node2_id); node->leafTesting(min_test.bv_node1_id, min_test.bv_node2_id); } else if(bvtq.full()) { // queue should not get two more tests, recur distanceQueueRecurse(node, min_test.bv_node1_id, min_test.bv_node2_id, front_list, qsize); } else { // queue capacity is not full yet BVT bvt1, bvt2; if(node->firstOverSecond(min_test.bv_node1_id, min_test.bv_node2_id)) { int c1 = node->getFirstLeftChild(min_test.bv_node1_id); int c2 = node->getFirstRightChild(min_test.bv_node1_id); bvt1.bv_node1_id = c1; bvt1.bv_node2_id = min_test.bv_node2_id; bvt1.d = node->BVTesting(bvt1.bv_node1_id, bvt1.bv_node2_id); bvt2.bv_node1_id = c2; bvt2.bv_node2_id = min_test.bv_node2_id; bvt2.d = node->BVTesting(bvt2.bv_node1_id, bvt2.bv_node2_id); } else { int c1 = node->getSecondLeftChild(min_test.bv_node2_id); int c2 = node->getSecondRightChild(min_test.bv_node2_id); bvt1.bv_node1_id = min_test.bv_node1_id; bvt1.bv_node2_id = c1; bvt1.d = node->BVTesting(bvt1.bv_node1_id, bvt1.bv_node2_id); bvt2.bv_node1_id = min_test.bv_node1_id; bvt2.bv_node2_id = c2; bvt2.d = node->BVTesting(bvt2.bv_node1_id, bvt2.bv_node2_id); } bvt1.depth = min_test.depth + 1; bvt2.depth = min_test.depth + 1; bvtq.push(bvt1); bvtq.push(bvt2); } if(bvtq.empty()) break; else { min_test = bvtq.top(); bvtq.pop(); if (node->canStop(min_test.d) ) { updateFrontList(front_list, min_test.bv_node1_id, min_test.bv_node2_id); break; } if ( min_test.d >= node->can_stop_distance_ || (min_test.depth <= 2 && min_test.d >= whole_distance_fraction) ) { node->result->update(min_test.d, node->o1, node->o2, min_test.bv_node1_id, min_test.bv_node2_id); updateFrontList(front_list, min_test.bv_node1_id, min_test.bv_node2_id); break; } } } }