From d7d66c14ff988a49416f0ee891408dd88ae29e2c Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Wed, 8 Aug 2012 01:47:45 +0000
Subject: [PATCH] mesh related cost
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@160 253336fb-580f-4252-a368-f3cef5a2a82b
---
.../broadphase/broadphase_dynamic_AABB_tree.h | 12 +-
trunk/fcl/include/fcl/octree.h | 31 ++-
.../fcl/traversal/traversal_node_octree.h | 177 ++++++++----
.../broadphase_dynamic_AABB_tree.cpp | 85 +++++-
.../broadphase_dynamic_AABB_tree_array.cpp | 87 +++++-
trunk/fcl/src/collision_func_matrix.cpp | 257 ++++++++++++------
trunk/fcl/src/distance_func_matrix.cpp | 93 +++----
7 files changed, 533 insertions(+), 209 deletions(-)
diff --git a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
index 81ebdef3..2e44690d 100644
--- a/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
+++ b/trunk/fcl/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h
@@ -125,12 +125,12 @@ public:
{
case GEOM_OCTREE:
{
- //if(!octree_as_geometry_collide)
- //{
- // const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- // collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
- //}
- //else
+ if(!octree_as_geometry_collide)
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ }
+ else
collisionRecurse(dtree.getRoot(), obj, cdata, callback);
}
break;
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index d4ea5420..75c3c06e 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -54,6 +54,9 @@ class OcTree : public CollisionGeometry
{
private:
boost::shared_ptr<const octomap::OcTree> tree;
+
+ FCL_REAL default_occupancy;
+
public:
/// @brief OcTreeNode must implement the following interfaces:
@@ -63,10 +66,16 @@ public:
typedef octomap::OcTreeNode OcTreeNode;
/// @brief construct octree with a given resolution
- OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<const octomap::OcTree>(new octomap::OcTree(resolution))) {}
+ OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<const octomap::OcTree>(new octomap::OcTree(resolution)))
+ {
+ default_occupancy = tree->getOccupancyThres();
+ }
/// @brief construct octree from octomap
- OcTree(const boost::shared_ptr<const octomap::OcTree>& tree_) : tree(tree_) {}
+ OcTree(const boost::shared_ptr<const octomap::OcTree>& tree_) : tree(tree_)
+ {
+ default_occupancy = tree->getOccupancyThres();
+ }
/// @brief compute the AABB for the octree in its local coordinate system
void computeLocalAABB()
@@ -135,12 +144,28 @@ public:
return boxes;
}
- /// @brief the threshold used to decide whether one node is occupied
+ /// @brief the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold
FCL_REAL getOccupancyThres() const
{
return tree->getOccupancyThres();
}
+ /// @brief the threshold used to decide whether one node is occupied, this is NOT the octree free_threshold
+ FCL_REAL getFreeThres() const
+ {
+ return 0.0;
+ }
+
+ FCL_REAL getDefaultOccupancy() const
+ {
+ return default_occupancy;
+ }
+
+ void setCellDefaultOccupancy(FCL_REAL d)
+ {
+ default_occupancy = d;
+ }
+
/// @brief return object type, it is an octree
OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
diff --git a/trunk/fcl/include/fcl/traversal/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
index 366f25c5..9cc1d50d 100644
--- a/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal/traversal_node_octree.h
@@ -104,36 +104,12 @@ public:
const CollisionRequest& request_,
CollisionResult& result_) const
{
- if(request_.enable_cost && request_.use_approximate_cost)
- {
- CollisionRequest request_no_cost(request_);
- request_no_cost.enable_cost = false;
-
- crequest = &request_no_cost;
- cresult = &result_;
-
- OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
- tree2, 0,
- tf1, tf2);
-
- Box box;
- Transform3f box_tf;
- constructBox(tree2->getBV(0).bv, tf2, box, box_tf);
-
- OcTreeShapeIntersect(tree1, box,
- tf1, box_tf,
- request_,
- result_);
- }
- else
- {
- crequest = &request_;
- cresult = &result_;
+ crequest = &request_;
+ cresult = &result_;
- OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
- tree2, 0,
- tf1, tf2);
- }
+ OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
+ tree2, 0,
+ tf1, tf2);
}
/// @brief distance between octree and mesh
@@ -159,36 +135,12 @@ public:
CollisionResult& result_) const
{
- if(request_.enable_cost && request_.use_approximate_cost)
- {
- CollisionRequest request_no_cost(request_);
- request_no_cost.enable_cost = false;
-
- crequest = &request_no_cost;
- cresult = &result_;
-
- OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
- tree1, 0,
- tf2, tf1);
-
- Box box;
- Transform3f box_tf;
- constructBox(tree1->getBV(0).bv, tf1, box, box_tf);
-
- ShapeOcTreeIntersect(box, tree2,
- box_tf, tf2,
- request_,
- result_);
- }
- else
- {
- crequest = &request_;
- cresult = &result_;
+ crequest = &request_;
+ cresult = &result_;
- OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
- tree1, 0,
- tf2, tf1);
- }
+ OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
+ tree1, 0,
+ tf2, tf1);
}
/// @brief distance between mesh and octree
@@ -338,7 +290,7 @@ private:
{
OBB obb1;
convertBV(bv1, tf1, obb1);
- // if(obb1.overlap(obb2))
+ if(obb1.overlap(obb2))
{
Box box;
Transform3f box_tf;
@@ -827,7 +779,92 @@ private:
const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
const Transform3f& tf1, const Transform3f& tf2) const
{
- if(!root1->hasChildren() && !root2->hasChildren())
+ if(!root1 && !root2)
+ {
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(bv2, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box box1, box2;
+ Transform3f box1_tf, box2_tf;
+ constructBox(bv1, tf1, box1, box1_tf);
+ constructBox(bv2, tf2, box2, box2_tf);
+
+ AABB overlap_part;
+ AABB aabb1, aabb2;
+ computeBV<AABB, Box>(box1, box1_tf, aabb1);
+ computeBV<AABB, Box>(box2, box2_tf, aabb2);
+ aabb1.overlap(aabb2, overlap_part);
+ cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->getOccupancyThres()), crequest->num_max_cost_sources);
+ }
+
+ return false;
+ }
+ else if(!root1 && root2)
+ {
+ if(root2->hasChildren())
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(bv2, i, child_bv);
+ if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2))
+ return true;
+ }
+ else
+ {
+ AABB child_bv;
+ computeChildBV(bv2, i, child_bv);
+ if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, tf1, tf2))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2))
+ return true;
+ }
+
+ return false;
+ }
+ else if(root1 && !root2)
+ {
+ if(root1->hasChildren())
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root1->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root1->getChild(i);
+ AABB child_bv;
+ computeChildBV(bv1, i, child_bv);
+ if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2))
+ return true;
+ }
+ else
+ {
+ AABB child_bv;
+ computeChildBV(bv1, i, child_bv);
+ if(OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, tf1, tf2))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2))
+ return true;
+ }
+
+ return false;
+ }
+ else if(!root1->hasChildren() && !root2->hasChildren())
{
if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area
{
@@ -935,6 +972,16 @@ private:
tf1, tf2))
return true;
}
+ else if(!tree2->isNodeFree(root2) && crequest->enable_cost)
+ {
+ AABB child_bv;
+ computeChildBV(bv1, i, child_bv);
+
+ if(OcTreeIntersectRecurse(tree1, NULL, child_bv,
+ tree2, root2, bv2,
+ tf1, tf2))
+ return true;
+ }
}
}
else
@@ -952,6 +999,16 @@ private:
tf1, tf2))
return true;
}
+ else if(!tree1->isNodeFree(root1) && crequest->enable_cost)
+ {
+ AABB child_bv;
+ computeChildBV(bv2, i, child_bv);
+
+ if(OcTreeIntersectRecurse(tree1, root1, bv1,
+ tree2, NULL, child_bv,
+ tf1, tf2))
+ return true;
+ }
}
}
diff --git a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp
index cb106a5d..1d10bd37 100644
--- a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp
+++ b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree.cpp
@@ -351,7 +351,42 @@ bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root,
bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
{
- if(root1->isLeaf() && !root2->hasChildren())
+ if(!root2)
+ {
+ if(root1->isLeaf())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!obj1->isFree())
+ {
+ OBB obb1, obb2;
+ convertBV(root1->bv, Transform3f(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = tree2->getDefaultOccupancy();
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ }
+ }
+ else
+ {
+ if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+
+ return false;
+ }
+ else if(root1->isLeaf() && !root2->hasChildren())
{
CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
@@ -404,6 +439,13 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
return true;
}
+ else
+ {
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+ if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback))
+ return true;
+ }
}
}
return false;
@@ -411,7 +453,39 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
{
- if(root1->isLeaf() && !root2->hasChildren())
+ if(!root2)
+ {
+ if(root1->isLeaf())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!obj1->isFree())
+ {
+ const AABB& root2_bv_t = translate(root2_bv, tf2);
+ if(root1->bv.overlap(root2_bv_t))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ }
+ }
+ else
+ {
+ if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+
+ return false;
+ }
+ else if(root1->isLeaf() && !root2->hasChildren())
{
CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
@@ -458,6 +532,13 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
return true;
}
+ else
+ {
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+ if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback))
+ return true;
+ }
}
}
return false;
diff --git a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
index 0d21379e..23374d92 100644
--- a/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
+++ b/trunk/fcl/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
@@ -362,7 +362,42 @@ bool DynamicAABBTreeCollisionManager_Array::selfDistanceRecurse(DynamicAABBNode*
bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
{
DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
- if(root1->isLeaf() && !root2->hasChildren())
+ if(!root2)
+ {
+ if(root1->isLeaf())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!obj1->isFree())
+ {
+ OBB obb1, obb2;
+ convertBV(root1->bv, Transform3f(), obb1);
+ convertBV(root2_bv, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = tree2->getDefaultOccupancy();
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ }
+ }
+ else
+ {
+ if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+
+ return false;
+ }
+ else if(root1->isLeaf() && !root2->hasChildren())
{
CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
if(!tree2->isNodeFree(root2) && !obj1->isFree())
@@ -414,6 +449,13 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n
if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
return true;
}
+ else
+ {
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+ if(collisionRecurse_(nodes1, root1_id, tree2, NULL, child_bv, tf2, cdata, callback))
+ return true;
+ }
}
}
@@ -423,10 +465,42 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n
bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
{
DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id;
- if(root1->isLeaf() && !root2->hasChildren())
+ if(!root2)
+ {
+ if(root1->isLeaf())
+ {
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!obj1->isFree())
+ {
+ const AABB& root_bv_t = translate(root2_bv, tf2);
+ if(root1->bv.overlap(root_bv_t))
+ {
+ Box* box = new Box();
+ Transform3f box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+
+ box->cost_density = tree2->getDefaultOccupancy();
+
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
+ }
+ }
+ }
+ else
+ {
+ if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+
+ return false;
+ }
+ else if(root1->isLeaf() && !root2->hasChildren())
{
CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
- if(!tree2->isNodeFree(root2))
+ if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
const AABB& root_bv_t = translate(root2_bv, tf2);
if(root1->bv.overlap(root_bv_t))
@@ -469,6 +543,13 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n
if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
return true;
}
+ else
+ {
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+ if(collisionRecurse_(nodes1, root1_id, tree2, NULL, child_bv, tf2, cdata, callback))
+ return true;
+ }
}
}
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 742ef826..1219eeb0 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -106,13 +106,40 @@ std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
- const OcTree* obj1 = static_cast<const OcTree*>(o1);
- const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ 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
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
- collide(&node);
+ OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
+ collide(&node);
+
+ Box box;
+ Transform3f box_tf;
+ constructBox(obj2->getBV(0).bv, tf2, box, box_tf); // compute the box for BVH's root node
+
+ box.cost_density = obj2->cost_density;
+ box.threshold_occupied = obj2->threshold_occupied;
+ box.threshold_free = obj2->threshold_free;
+
+ CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); // additional cost request, no contacts
+ OcTreeShapeCollide<Box, NarrowPhaseSolver>(o1, tf1, &box, box_tf, nsolver, only_cost_request, result);
+ }
+ else
+ {
+ OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
+ OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
+ collide(&node);
+ }
return result.numContacts();
}
@@ -123,14 +150,41 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1
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);
+ 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);
- collide(&node);
+ initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
+ collide(&node);
+
+ 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);
+ collide(&node);
+ }
return result.numContacts();
}
@@ -162,19 +216,98 @@ struct BVHShapeCollider
{
if(request.isSatisfied(result)) return result.numContacts();
- MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
+ 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::collide(&node);
+
+ 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::collide(&node);
+
+ delete obj1_tmp;
+ }
+
+ return result.numContacts();
+ }
+};
+
+namespace details
+{
+
+template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
+std::size_t orientedBVHShapeCollide(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;
+
+ OrientMeshShapeCollisionTraveralNode 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);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result);
fcl::collide(&node);
+
+ 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
+ {
+ OrientMeshShapeCollisionTraveralNode node;
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
- delete obj1_tmp;
- return result.numContacts();
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
+ fcl::collide(&node);
}
-};
+
+ return result.numContacts();
+}
+
+}
template<typename T_SH, typename NarrowPhaseSolver>
@@ -184,16 +317,7 @@ struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.isSatisfied(result)) return result.numContacts();
-
- MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver> node;
- const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::collide(&node);
-
- return result.numContacts();
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver>, OBB, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -205,16 +329,7 @@ struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.isSatisfied(result)) return result.numContacts();
-
- MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
- const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::collide(&node);
-
- return result.numContacts();
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver>, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -226,16 +341,7 @@ struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.isSatisfied(result)) return result.numContacts();
-
- MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
- const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::collide(&node);
-
- return result.numContacts();
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver>, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -247,16 +353,7 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.isSatisfied(result)) return result.numContacts();
-
- MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
- const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::collide(&node);
-
- return result.numContacts();
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -283,14 +380,16 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
return result.numContacts();
}
-template<>
-std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
+namespace details
+{
+template<typename OrientedMeshCollisionTraversalNode, typename T_BVH>
+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();
- MeshCollisionTraversalNodeOBB node;
- const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
- const BVHModel<OBB>* obj2 = static_cast<const BVHModel<OBB>* >(o2);
+ OrientedMeshCollisionTraversalNode node;
+ 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);
collide(&node);
@@ -298,35 +397,25 @@ std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1,
return result.numContacts();
}
+}
+
template<>
-std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
+std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(request.isSatisfied(result)) return result.numContacts();
-
- MeshCollisionTraversalNodeOBBRSS node;
- const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
- const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, request, result);
- collide(&node);
+ return details::orientedMeshCollide<MeshCollisionTraversalNodeOBB, OBB>(o1, tf1, o2, tf2, request, result);
+}
- return result.numContacts();
+template<>
+std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
+{
+ return details::orientedMeshCollide<MeshCollisionTraversalNodeOBBRSS, OBBRSS>(o1, tf1, o2, tf2, request, result);
}
template<>
std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(request.isSatisfied(result)) return result.numContacts();
-
- MeshCollisionTraversalNodekIOS node;
- const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
- const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, request, result);
- collide(&node);
-
- return result.numContacts();
+ return details::orientedMeshCollide<MeshCollisionTraversalNodekIOS, kIOS>(o1, tf1, o2, tf2, request, result);
}
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index d5a55095..b85401b2 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -161,21 +161,33 @@ struct BVHShapeDistancer
}
};
+namespace details
+{
+
+template<typename OrientedMeshShapeDistanceTraversalNode, typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
+FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
+{
+ if(request.isSatisfied(result)) return result.min_distance;
+ OrientedMeshShapeDistanceTraversalNode node;
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
+ fcl::distance(&node);
+
+ return result.min_distance;
+}
+
+}
+
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
{
static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
- if(request.isSatisfied(result)) return result.min_distance;
- MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
- const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::distance(&node);
-
- return result.min_distance;
+ return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver>, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -186,15 +198,7 @@ struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
- if(request.isSatisfied(result)) return result.min_distance;
- MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
- const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::distance(&node);
-
- return result.min_distance;
+ return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver>, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -204,15 +208,7 @@ struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
- if(request.isSatisfied(result)) return result.min_distance;
- MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
- const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
- fcl::distance(&node);
-
- return result.min_distance;
+ return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -236,14 +232,16 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const
return result.min_distance;
}
-template<>
-FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const DistanceRequest& request, DistanceResult& result)
+namespace details
+{
+template<typename OrientedMeshDistanceTraversalNode, typename T_BVH>
+FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
+ const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- MeshDistanceTraversalNodeRSS node;
- const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
- const BVHModel<RSS>* obj2 = static_cast<const BVHModel<RSS>* >(o2);
+ OrientedMeshDistanceTraversalNode node;
+ 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);
distance(&node);
@@ -251,19 +249,20 @@ FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1, c
return result.min_distance;
}
+}
+
+template<>
+FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
+ const DistanceRequest& request, DistanceResult& result)
+{
+ return details::orientedMeshDistance<MeshDistanceTraversalNodeRSS, RSS>(o1, tf1, o2, tf2, request, result);
+}
+
template<>
FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
- if(request.isSatisfied(result)) return result.min_distance;
- MeshDistanceTraversalNodekIOS node;
- const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
- const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, request, result);
- distance(&node);
-
- return result.min_distance;
+ return details::orientedMeshDistance<MeshDistanceTraversalNodekIOS, kIOS>(o1, tf1, o2, tf2, request, result);
}
@@ -271,15 +270,7 @@ template<>
FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
- if(request.isSatisfied(result)) return result.min_distance;
- MeshDistanceTraversalNodeOBBRSS node;
- const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
- const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, request, result);
- distance(&node);
-
- return result.min_distance;
+ return details::orientedMeshDistance<MeshDistanceTraversalNodeOBBRSS, OBBRSS>(o1, tf1, o2, tf2, request, result);
}
--
GitLab