static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
if(request.enable_cost && request.use_approximate_cost)
{
CollisionRequest no_cost_request(request);
no_cost_request.enable_cost = false;
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result);
FCL_REAL sqrDistance;
fcl::collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
Box box;
Transform3f box_tf;
constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
box.cost_density = obj1->cost_density;
box.threshold_occupied = obj1->threshold_occupied;
box.threshold_free = obj1->threshold_free;
CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
ShapeShapeCollide<Box, T_SH>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
}
else
{
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
FCL_REAL sqrDistance;
fcl::collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
}
return result.numContacts();
}
std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
if (request.enable_distance_lower_bound) {
DistanceResult distanceResult;
DistanceRequest distanceRequest (request.enable_contact);
FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, NarrowPhaseSolver>
(o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
if (distance <= 0) {
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
contact.normal = (p2-p1)/(p2-p1).length ();
result.addContact (contact);
return 1;
}
result.distance_lower_bound = distance;
return 0;
}
ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
if(request.enable_cached_gjk_guess)
{
nsolver->enableCachedGuess(true);
nsolver->setCachedGuess(request.cached_gjk_guess);
}
else
{
nsolver->enableCachedGuess(true);
}
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
if(request.enable_cached_gjk_guess)
result.cached_gjk_guess = nsolver->getCachedGuess();
return result.numContacts();
}
std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OrientedMeshCollisionTraversalNode node (request.enable_distance_lower_bound);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
initialize(node, *obj1, tf1, *obj2, tf2, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
return result.numContacts();
}
int conservativeAdvancement(const CollisionGeometry* o1,
const MotionBase* motion1,
const CollisionGeometry* o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
typedef ConservativeAdvancement<BV, ConservativeAdvancementNode, CollisionNode>
ConservativeAdvancementType;
boost::shared_ptr<ConservativeAdvancementType> advancement =
boost::make_shared<ConservativeAdvancementType>(o1, motion1, o2, motion2);
ContinuousCollisionRequest continuous_request;
ContinuousCollisionResult continuous_result;
continuous_request.assign(request);
advancement->collide(continuous_request, continuous_result);
result = continuous_result;
toc = continuous_result.getTimeOfContact();
return static_cast<int>(result.numContacts() );
}
std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
return result.numContacts();
}
std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
if(request.enable_cost && request.use_approximate_cost)
{
CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree
no_cost_request.enable_cost = false; // disable cost computation
MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
Box box;
Transform3f box_tf;
constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
box.cost_density = obj1->cost_density;
box.threshold_occupied = obj1->threshold_occupied;
box.threshold_free = obj1->threshold_free;
CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
ShapeOcTreeCollide<Box, NarrowPhaseSolver>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
}
else
{
MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
}
return result.numContacts();
}
std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
MeshCollisionTraversalNode<T_BVH> node (request.enable_distance_lower_bound);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
Transform3f tf2_tmp = tf2;
initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
FCL_REAL sqrDistance;
fcl::collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
delete obj2_tmp;
return result.numContacts();
}
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 CollisionRequest::isSatisfied(const CollisionResult& result) const
{
return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts());
}
