From f9cf8bae0d77b23c2c9eeb9d76ee6720a29f1c10 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Sun, 29 Jul 2012 08:43:20 +0000
Subject: [PATCH] fix inconsistency bug in cost computation
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@146 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/broad_phase_collision.h | 44 +++++++++++++++----
trunk/fcl/include/fcl/collision_object.h | 23 +++++++---
trunk/fcl/include/fcl/matrix_3f.h | 8 ++++
trunk/fcl/include/fcl/octree.h | 8 ++--
trunk/fcl/include/fcl/traversal_node_octree.h | 26 +++++------
trunk/fcl/include/fcl/vec_3f.h | 8 ++++
trunk/fcl/src/broad_phase_collision.cpp | 30 ++++++++-----
trunk/fcl/src/collision_func_matrix.cpp | 30 ++++++-------
trunk/fcl/src/geometric_shapes_utility.cpp | 32 +++++++-------
9 files changed, 136 insertions(+), 73 deletions(-)
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index c74bc4d1..3935a71c 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -1287,6 +1287,8 @@ public:
int& tree_topdown_level;
int tree_init_level;
+ bool octree_as_geometry;
+
DynamicAABBTreeCollisionManager() : tree_topdown_balance_threshold(dtree.bu_threshold),
tree_topdown_level(dtree.topdown_level)
@@ -1297,6 +1299,8 @@ public:
tree_topdown_level = 0;
tree_init_level = 0;
setup_ = false;
+
+ octree_as_geometry = false;
}
/** \brief add objects to the manager */
@@ -1342,8 +1346,13 @@ public:
{
case GEOM_OCTREE:
{
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ if(!octree_as_geometry)
+ {
+ 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;
default:
@@ -1360,8 +1369,13 @@ public:
{
case GEOM_OCTREE:
{
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
+ if(!octree_as_geometry)
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
+ }
+ else
+ distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
}
break;
default:
@@ -1457,6 +1471,8 @@ public:
int& tree_topdown_balance_threshold;
int& tree_topdown_level;
int tree_init_level;
+
+ bool octree_as_geometry;
DynamicAABBTreeCollisionManager2() : tree_topdown_balance_threshold(dtree.bu_threshold),
tree_topdown_level(dtree.topdown_level)
@@ -1467,6 +1483,8 @@ public:
tree_topdown_level = 0;
tree_init_level = 0;
setup_ = false;
+
+ octree_as_geometry = false;
}
/** \brief add objects to the manager */
@@ -1512,8 +1530,13 @@ public:
{
case GEOM_OCTREE:
{
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ if(!octree_as_geometry)
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
+ }
+ else
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
}
break;
default:
@@ -1530,8 +1553,13 @@ public:
{
case GEOM_OCTREE:
{
- const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
- distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
+ if(!octree_as_geometry)
+ {
+ const OcTree* octree = static_cast<const OcTree*>(obj->getCollisionGeometry());
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
+ }
+ else
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
}
break;
default:
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index b4d0b52b..e2415791 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -242,16 +242,27 @@ public:
FCL_REAL getCostDensity() const
{
- if(cgeom)
- return cgeom->cost_density;
- else
- return 0;
+ return cgeom->cost_density;
}
void setCostDensity(FCL_REAL c)
{
- if(cgeom)
- cgeom->cost_density = c;
+ cgeom->cost_density = c;
+ }
+
+ bool isOccupied() const
+ {
+ return cgeom->isOccupied();
+ }
+
+ bool isFree() const
+ {
+ return cgeom->isFree();
+ }
+
+ bool isUncertain() const
+ {
+ return cgeom->isUncertain();
}
protected:
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
index e42af05c..763e4268 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -409,6 +409,14 @@ typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename
typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
//typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
+static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
+{
+ o << "[(" << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << ")("
+ << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << ")("
+ << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << ")]";
+ return o;
+}
+
}
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index 338a5401..7960606e 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -102,9 +102,9 @@ public:
tree->updateNode(octomap::point3d(x, y, z), occupied);
}
- inline std::vector<boost::array<FCL_REAL, 4> > toBoxes() const
+ inline std::vector<boost::array<FCL_REAL, 6> > toBoxes() const
{
- std::vector<boost::array<FCL_REAL, 4> > boxes;
+ std::vector<boost::array<FCL_REAL, 6> > boxes;
boxes.reserve(tree->size() / 2);
for(octomap::OcTree::iterator it = tree->begin(tree->getTreeDepth()), end = tree->end();
it != end;
@@ -116,8 +116,10 @@ public:
FCL_REAL x = it.getX();
FCL_REAL y = it.getY();
FCL_REAL z = it.getZ();
+ FCL_REAL c = (*it).getOccupancy();
+ FCL_REAL t = tree->getOccupancyThres();
- boost::array<FCL_REAL, 4> box = {{x, y, z, size}};
+ boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
boxes.push_back(box);
}
}
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index 3748dfa4..81852e66 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -281,7 +281,7 @@ private:
{
if(!root1->hasChildren())
{
- if(tree1->isNodeOccupied(root1)) // occupied area
+ if(tree1->isNodeOccupied(root1) && s.isOccupied()) // occupied area
{
OBB obb1;
convertBV(bv1, tf1, obb1);
@@ -329,7 +329,7 @@ private:
}
else return false;
}
- else if(tree1->isNodeUncertain(root1) && crequest->enable_cost) // uncertain area
+ else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost) // uncertain area
{
OBB obb1;
convertBV(bv1, tf1, obb1);
@@ -357,10 +357,10 @@ private:
}
/// stop when 1) bounding boxes of two objects not overlap; OR
- /// 2) the node is free; OR
- /// 2) the node is uncertain AND cost is not required
- if(tree1->isNodeFree(root1)) return false;
- else if(tree1->isNodeUncertain(root1) && !crequest->enable_cost) return false;
+ /// 2) at least of one the nodes is free; OR
+ /// 2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required
+ if(tree1->isNodeFree(root1) || s.isFree()) return false;
+ else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost) return false;
else
{
OBB obb1;
@@ -477,7 +477,7 @@ private:
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
{
- if(tree1->isNodeOccupied(root1))
+ if(tree1->isNodeOccupied(root1) && tree2->isOccupied())
{
OBB obb1, obb2;
convertBV(bv1, tf1, obb1);
@@ -533,7 +533,7 @@ private:
else
return false;
}
- else if(tree1->isNodeUncertain(root1) && crequest->enable_cost) // uncertain area
+ else if(!tree1->isNodeFree(root1) && !tree2->isFree() && crequest->enable_cost) // uncertain area
{
OBB obb1, obb2;
convertBV(bv1, tf1, obb1);
@@ -568,10 +568,10 @@ private:
}
/// stop when 1) bounding boxes of two objects not overlap; OR
- /// 2) the node is free; OR
- /// 2) the node is uncertain AND cost is not required
- if(tree1->isNodeFree(root1)) return false;
- else if(tree1->isNodeUncertain(root1) && !crequest->enable_cost) return false;
+ /// 2) at least one of the nodes is free; OR
+ /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required
+ if(tree1->isNodeFree(root1) || tree2->isFree()) return false;
+ else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()) && !crequest->enable_cost) return false;
else
{
OBB obb1, obb2;
@@ -774,7 +774,7 @@ private:
/// stop when 1) bounding boxes of two objects not overlap; OR
/// 2) at least one of the nodes is free; OR
- /// 2) (two uncertain nodes OR one node occupied one node uncertain) AND not cost required
+ /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required
if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false;
else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false;
else
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 471e3f20..9573fe99 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -58,6 +58,7 @@ public:
Vec3fX() {}
Vec3fX(const Vec3fX& other) : data(other.data) {}
Vec3fX(U x, U y, U z) : data(x, y, z) {}
+ Vec3fX(U x) : data(x) {}
Vec3fX(const T& data_) : data(data_) {}
inline U operator [] (size_t i) const { return data[i]; }
@@ -208,6 +209,13 @@ typedef Vec3fX<details::Vec3Data<FCL_REAL> > Vec3f;
//typedef Vec3fX<details::sse_meta_f4> Vec3f;
+static inline std::ostream& operator << (std::ostream& o, const Vec3f& v)
+{
+ o << "(" << v[0] << " " << v[1] << " " << v[2] << ")";
+ return o;
+}
+
+
}
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 0b65397f..e4c55af3 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -59,7 +59,7 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
collide(o1, o2, request, result);
- if( (result.isCollision()) && (result.numContacts() >= request.num_max_contacts))
+ if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts))
cdata->done = true;
return cdata->done;
@@ -2338,7 +2338,9 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root1->isLeaf() && !root2->hasChildren())
{
- if(!tree2->isNodeFree(root2))
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
OBB obb1, obb2;
convertBV(root1->bv, SimpleTransform(), obb1);
@@ -2353,8 +2355,8 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
box->cost_density = root2->getOccupancy();
box->threshold_occupied = tree2->getOccupancyThres();
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
}
else return false;
}
@@ -2396,7 +2398,9 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root1->isLeaf() && !root2->hasChildren())
{
- if(!tree2->isNodeFree(root2))
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+
+ if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
const AABB& root2_bv_t = translate(root2_bv, tf2);
if(root1->bv.overlap(root2_bv_t))
@@ -2408,8 +2412,8 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
box->cost_density = root2->getOccupancy();
box->threshold_occupied = tree2->getOccupancyThres();
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
}
else return false;
}
@@ -2949,7 +2953,8 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
{
- if(!tree2->isNodeFree(root2))
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
+ if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
OBB obb1, obb2;
convertBV(root1->bv, SimpleTransform(), obb1);
@@ -2964,8 +2969,8 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
box->cost_density = root2->getOccupancy();
box->threshold_occupied = tree2->getOccupancyThres();
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
}
else return false;
}
@@ -3009,6 +3014,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
{
+ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
if(!tree2->isNodeFree(root2))
{
const AABB& root_bv_t = translate(root2_bv, tf2);
@@ -3021,8 +3027,8 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
box->cost_density = root2->getOccupancy();
box->threshold_occupied = tree2->getOccupancyThres();
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ CollisionObject obj2(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(obj1, &obj2, cdata);
}
else return false;
}
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 5b88cea6..28e0641e 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -50,7 +50,7 @@ int ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
@@ -69,7 +69,7 @@ int OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
@@ -88,7 +88,7 @@ int OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
@@ -107,7 +107,7 @@ int OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, co
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
@@ -126,7 +126,7 @@ int BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, co
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
@@ -146,7 +146,7 @@ int ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, c
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
@@ -166,7 +166,7 @@ struct BVHShapeCollider
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
@@ -191,7 +191,7 @@ struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver> node;
@@ -213,7 +213,7 @@ struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
@@ -235,7 +235,7 @@ struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
@@ -257,7 +257,7 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
@@ -275,7 +275,7 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
template<typename T_BVH>
int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshCollisionTraversalNode<T_BVH> node;
@@ -298,7 +298,7 @@ int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const Co
template<>
int BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshCollisionTraversalNodeOBB node;
@@ -314,7 +314,7 @@ int BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, con
template<>
int BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshCollisionTraversalNodeOBBRSS node;
@@ -331,7 +331,7 @@ int BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1,
template<>
int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(request.num_max_contacts <= result.numContacts()) return 0;
+ if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return 0;
size_t num_contacts_old = result.numContacts();
MeshCollisionTraversalNodekIOS node;
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 7ef0596b..a7a035cb 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -226,8 +226,9 @@ void computeBV<AABB, Box>(const Box& s, const SimpleTransform& tf, AABB& bv)
FCL_REAL y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2]));
FCL_REAL z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2]));
- bv.max_ = T + Vec3f(x_range, y_range, z_range);
- bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
+ Vec3f v_delta(x_range, y_range, z_range);
+ bv.max_ = T + v_delta;
+ bv.min_ = T - v_delta;
}
template<>
@@ -235,8 +236,9 @@ void computeBV<AABB, Sphere>(const Sphere& s, const SimpleTransform& tf, AABB& b
{
const Vec3f& T = tf.getTranslation();
- bv.max_ = T + Vec3f(s.radius, s.radius, s.radius);
- bv.min_ = T + Vec3f(-s.radius, -s.radius, -s.radius);
+ Vec3f v_delta(s.radius);
+ bv.max_ = T + v_delta;
+ bv.min_ = T - v_delta;
}
template<>
@@ -249,8 +251,9 @@ void computeBV<AABB, Capsule>(const Capsule& s, const SimpleTransform& tf, AABB&
FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius;
FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius;
- bv.max_ = T + Vec3f(x_range, y_range, z_range);
- bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
+ Vec3f v_delta(x_range, y_range, z_range);
+ bv.max_ = T + v_delta;
+ bv.min_ = T - v_delta;
}
template<>
@@ -263,8 +266,9 @@ void computeBV<AABB, Cone>(const Cone& s, const SimpleTransform& tf, AABB& bv)
FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
- bv.max_ = T + Vec3f(x_range, y_range, z_range);
- bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
+ Vec3f v_delta(x_range, y_range, z_range);
+ bv.max_ = T + v_delta;
+ bv.min_ = T - v_delta;
}
template<>
@@ -277,8 +281,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, AAB
FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
- bv.max_ = T + Vec3f(x_range, y_range, z_range);
- bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
+ Vec3f v_delta(x_range, y_range, z_range);
+ bv.max_ = T + v_delta;
+ bv.min_ = T - v_delta;
}
template<>
@@ -300,12 +305,7 @@ void computeBV<AABB, Convex>(const Convex& s, const SimpleTransform& tf, AABB& b
template<>
void computeBV<AABB, Triangle2>(const Triangle2& s, const SimpleTransform& tf, AABB& bv)
{
- AABB bv_;
- bv_ += tf.transform(s.a);
- bv_ += tf.transform(s.b);
- bv_ += tf.transform(s.c);
-
- bv = bv_;
+ bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
}
template<>
--
GitLab