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