void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
{
BVHFrontList::iterator front_iter;
BVHFrontList append;
for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter)
{
int b1 = front_iter->left;
int b2 = front_iter->right;
bool l1 = node->isFirstNodeLeaf(b1);
bool l2 = node->isSecondNodeLeaf(b2);
if(l1 & l2)
{
front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again.
collisionRecurse(node, b1, b2, &append);
}
else
{
if(!node->BVTesting(b1, b2))
{
front_iter->valid = false;
if(node->firstOverSecond(b1, b2))
{
int c1 = node->getFirstLeftChild(b1);
int c2 = node->getFirstRightChild(b2);
collisionRecurse(node, c1, b2, front_list);
collisionRecurse(node, c2, b2, front_list);
}
else
{
int c1 = node->getSecondLeftChild(b2);
int c2 = node->getSecondRightChild(b2);
collisionRecurse(node, b1, c1, front_list);
collisionRecurse(node, b1, c2, front_list);
}
}
}
}
// clean the old front list (remove invalid node)
for(front_iter = front_list->begin(); front_iter != front_list->end();)
{
if(!front_iter->valid)
front_iter = front_list->erase(front_iter);
else
++front_iter;
}
for(front_iter = append.begin(); front_iter != append.end(); ++front_iter)
{
front_list->push_back(*front_iter);
}
}
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 collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
{
if(front_list && front_list->size() > 0)
{
propagateBVHFrontListCollisionRecurse(node, front_list);
}
else
{
collisionRecurse(node, 0, 0, front_list);
}
}
void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list)
{
if(front_list && front_list->size() > 0)
{
propagateBVHFrontListCollisionRecurse(node, front_list);
}
else
{
collisionRecurse(node, 0, 0, node->R, node->T, front_list);
}
}
void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list)
{
FCL_REAL sqrDistLowerBound = 0;
if(front_list && front_list->size() > 0)
{
propagateBVHFrontListCollisionRecurse(node, front_list, sqrDistLowerBound);
}
else
{
collisionRecurse(node, 0, 0, node->R, node->T, front_list);
}
}
void collide(CollisionTraversalNodeBase* node, FCL_REAL& sqrDistLowerBound,
BVHFrontList* front_list)
{
if(front_list && front_list->size() > 0)
{
propagateBVHFrontListCollisionRecurse(node, front_list, sqrDistLowerBound);
}
else
{
collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
}
}
/** Recurse function for self collision
* Make sure node is set correctly so that the first and second tree are the same
*/
void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list)
{
bool l = node->isFirstNodeLeaf(b);
if(l) return;
int c1 = node->getFirstLeftChild(b);
int c2 = node->getFirstRightChild(b);
selfCollisionRecurse(node, c1, front_list);
if(node->canStop() && !front_list) return;
selfCollisionRecurse(node, c2, front_list);
if(node->canStop() && !front_list) return;
collisionRecurse(node, c1, c2, front_list);
}
/** Recurse function for self collision
* Make sure node is set correctly so that the first and second tree are the same
*/
void selfCollisionRecurse(CollisionTraversalNodeBase* node, int bv_node_id, BVHFrontList* front_list)
{
bool is_first_node_leaf = node->isFirstNodeLeaf(bv_node_id);
if(is_first_node_leaf) return;
int left_child = node->getFirstLeftChild(bv_node_id);
int right_child = node->getFirstRightChild(bv_node_id);
selfCollisionRecurse(node, left_child, front_list);
if(node->canStop() && !front_list) return;
selfCollisionRecurse(node, right_child, front_list);
if(node->canStop() && !front_list) return;
collisionRecurse(node, left_child, right_child, front_list);
}
void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list)
{
if(front_list && front_list->size() > 0)
{
propagateBVHFrontListCollisionRecurse(node, front_list);
}
else
{
Matrix3f Rtemp, R;
Vec3f Ttemp, T;
Rtemp = node->R * node->model2->getBV(0).getOrientation();
R = node->model1->getBV(0).getOrientation().transposeTimes(Rtemp);
Ttemp = node->R * node->model2->getBV(0).getCenter() + node->T;
Ttemp -= node->model1->getBV(0).getCenter();
T = node->model1->getBV(0).getOrientation().transposeTimes(Ttemp);
collisionRecurse(node, 0, 0, R, T, 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);
}
}
