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;
}
}
}
}
