bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
BVH_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
bool exhaustive,
bool enable_contact,
BVH_REAL expand_r)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.model2 = &model2;
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
node.tri_indices2 = model2.tri_indices;
node.uc1.reset(new Uncertainty[model1.num_vertices]);
estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
BVHExpand(model1, node.uc1.get(), expand_r);
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.model2 = &model2;
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
int num_max_contacts, bool exhaustive, bool enable_contact)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
node.tri_indices1 = model1.tri_indices;
node.tri_indices2 = model2.tri_indices;
node.model1 = &model1;
node.model2 = &model2;
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
relativeTransform(model1.getRotation(), model1.getTranslation(), model2.getRotation(), model2.getTranslation(), node.R, node.T);
return true;
}
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 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;
}
