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 distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int qsize)
{
Triangle last_tri1 = node->tri_indices1[node->last_tri_id1];
Triangle last_tri2 = node->tri_indices2[node->last_tri_id2];
Vec3f last_tri1_points[3];
Vec3f last_tri2_points[3];
last_tri1_points[0] = node->vertices1[last_tri1[0]];
last_tri1_points[1] = node->vertices1[last_tri1[1]];
last_tri1_points[2] = node->vertices1[last_tri1[2]];
last_tri2_points[0] = node->vertices2[last_tri2[0]];
last_tri2_points[1] = node->vertices2[last_tri2[1]];
last_tri2_points[2] = node->vertices2[last_tri2[2]];
Vec3f last_tri_P, last_tri_Q;
node->min_distance = TriangleDistance::triDistance(last_tri1_points[0], last_tri1_points[1], last_tri1_points[2],
last_tri2_points[0], last_tri2_points[1], last_tri2_points[2],
node->R, node->T, last_tri_P, last_tri_Q);
node->p1 = last_tri_P;
node->p2 = matTransMulVec(node->R, last_tri_Q - node->T);
if(qsize <= 2)
distanceRecurse(node, 0, 0, front_list);
else
distanceQueueRecurse(node, 0, 0, front_list, qsize);
Vec3f u = node->p2 - node->T;
node->p2 = matTransMulVec(node->R, u);
}
void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize)
{
if(qsize <= 2)
distanceRecurse(node, 0, 0, front_list);
else
distanceQueueRecurse(node, 0, 0, front_list, qsize);
}
void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize)
{
node->preprocess();
if(qsize <= 2)
distanceRecurse(node, 0, 0, front_list);
else
distanceQueueRecurse(node, 0, 0, front_list, qsize);
node->postprocess();
}
int conservativeAdvancement(const CollisionGeometry* o1,
const MotionBase* motion1,
const CollisionGeometry* o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
if(request.num_max_contacts == 0)
{
std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl;
return 0;
}
const OBJECT_TYPE object_type1 = o1->getObjectType();
const NODE_TYPE node_type1 = o1->getNodeType();
const OBJECT_TYPE object_type2 = o2->getObjectType();
const NODE_TYPE node_type2 = o2->getNodeType();
if(object_type1 != OT_BVH || object_type2 != OT_BVH)
return 0;
if(node_type1 != BV_RSS || node_type2 != BV_RSS)
return 0;
const BVHModel<BV>* model1 = static_cast<const BVHModel<BV>*>(o1);
const BVHModel<BV>* model2 = static_cast<const BVHModel<BV>*>(o2);
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
CollisionNode cnode;
if(!initialize(cnode, *model1, tf1, *model2, tf2, request, result))
std::cout << "initialize error" << std::endl;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), cnode.R, cnode.T);
cnode.enable_statistics = false;
cnode.request = request;
collide(&cnode);
int b = result.numContacts();
if(b > 0)
{
toc = 0;
// std::cout << "zero collide" << std::endl;
return b;
}
ConservativeAdvancementNode node;
initialize(node, *model1, tf1, *model2, tf2);
node.motion1 = motion1;
node.motion2 = motion2;
do
{
Matrix3f R1_t, R2_t;
Vec3f T1_t, T2_t;
node.motion1->getCurrentTransform(R1_t, T1_t);
node.motion2->getCurrentTransform(R2_t, T2_t);
// compute the transformation from 1 to 2
relativeTransform(R1_t, T1_t, R2_t, T2_t, node.R, node.T);
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
// std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
}
while(1);
toc = node.toc;
if(node.toc < 1)
return 1;
return 0;
}
bool conservativeAdvancement(const BVHModel<BV>& o1,
const MotionBase* motion1,
const BVHModel<BV>& o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
if(collide(&o1, tf1, &o2, tf2, request, result))
{
toc = 0;
return true;
}
BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
MeshConservativeAdvancementTraversalNode<BV> node;
node.motion1 = motion1;
node.motion2 = motion2;
do
{
// repeatedly update mesh to global coordinate, so time consuming
initialize(node, *o1_tmp, tf1, *o2_tmp, tf2);
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
// std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
}
while(1);
delete o1_tmp;
delete o2_tmp;
toc = node.toc;
if(node.toc < 1)
return true;
return false;
}
bool conservativeAdvancementShapeMeshOriented(const S& o1,
const MotionBase* motion1,
const BVHModel<BV>& o2,
const MotionBase* motion2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
if(collide(&o1, tf1, &o2, tf2, request, result))
{
toc = 0;
return true;
}
ConservativeAdvancementOrientedNode node;
initialize(node, o1, tf1, o2, tf2, nsolver);
node.motion1 = motion1;
node.motion2 = motion2;
do
{
node.motion1->getCurrentTransform(tf1);
node.motion2->getCurrentTransform(tf2);
node.tf1 = tf1;
node.tf2 = tf2;
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
}
while(1);
toc = node.toc;
if(node.toc < 1)
return true;
return false;
}
bool conservativeAdvancement(const S& o1,
const MotionBase* motion1,
const BVHModel<BV>& o2,
const MotionBase* motion2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
if(collide(&o1, tf1, &o2, tf2, request, result))
{
toc = 0;
return true;
}
BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
ShapeMeshConservativeAdvancementTraversalNode<S, BV, NarrowPhaseSolver> node;
node.motion1 = motion1;
node.motion2 = motion2;
do
{
// initialize update mesh to global coordinate, so time consuming
initialize(node, o1, tf1, *o2_tmp, tf2, nsolver);
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
}
while(1);
delete o2_tmp;
toc = node.toc;
if(node.toc < 1)
return true;
return false;
}
bool conservativeAdvancement(const S1& o1,
const MotionBase* motion1,
const S2& o2,
const MotionBase* motion2,
const NarrowPhaseSolver* solver,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
if(collide(&o1, tf1, &o2, tf2, request, result))
{
toc = 0;
return true;
}
ShapeConservativeAdvancementTraversalNode<S1, S2, NarrowPhaseSolver> node;
initialize(node, o1, tf1, o2, tf2, solver);
node.motion1 = motion1;
node.motion2 = motion2;
do
{
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
node.tf1 = tf1;
node.tf2 = tf2;
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
// std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
}
while(1);
toc = node.toc;
if(node.toc < 1)
return true;
return false;
}
bool conservativeAdvancementMeshOriented(const BVHModel<BV>& o1,
const MotionBase* motion1,
const BVHModel<BV>& o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
if(collide(&o1, tf1, &o2, tf2, request, result))
{
toc = 0;
return true;
}
ConservativeAdvancementOrientedNode node;
initialize(node, o1, tf1, o2, tf2);
node.motion1 = motion1;
node.motion2 = motion2;
do
{
node.motion1->getCurrentTransform(tf1);
node.motion2->getCurrentTransform(tf2);
// compute the transformation from 1 to 2
Transform3f tf;
relativeTransform(tf1, tf2, tf);
node.R = tf.getRotation();
node.T = tf.getTranslation();
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
// std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
}
while(1);
toc = node.toc;
if(node.toc < 1)
return true;
return false;
}
