diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index ef2e65998b475c6709330026187569f089d1dbe7..b680d9f24d4989d2aafe61c8037e6eabf19cb933 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -41,7 +41,7 @@ link_directories(${OCTOMAP_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/hierarchy_tree.cpp src/octomap_extension.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/hierarchy_tree.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index 0993f1ccb48d66d957ea180c51c0cf9406c84a2a..ee6eb6e6eac117656b7876fb1cd1111155a3d206 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -63,11 +63,14 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
/** \brief distance function for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now */
bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
+
/** \brief return value is whether can stop now */
typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
+typedef bool (*IsCostEnabledCallBack)(void* cdata);
+
/** \brief Base class for broad phase collision */
class BroadPhaseCollisionManager
{
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index f72af25eab93362f95983b567ade2a0bfa5877b4..0457a500851556e4fcf4268b802d0d3d64d57b87 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -4,6 +4,7 @@
#include "fcl/collision_object.h"
#include "fcl/vec_3f.h"
#include <vector>
+#include <set>
#include <limits>
@@ -70,6 +71,12 @@ struct CostSource
{
}
+ CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_),
+ aabb_max(aabb.max_),
+ cost_density(cost_density_)
+ {
+ }
+
CostSource() {}
bool operator < (const CostSource& other) const
@@ -105,10 +112,20 @@ struct CollisionResult
{
private:
std::vector<Contact> contacts;
- std::vector<CostSource> cost_sources;
+
+ std::set<CostSource> cost_sources;
+
+ const CollisionRequest* request;
+
public:
CollisionResult()
{
+ request = NULL;
+ }
+
+ void setRequest(const CollisionRequest& request_)
+ {
+ request = &request_;
}
inline void addContact(const Contact& c)
@@ -118,7 +135,16 @@ public:
inline void addCostSource(const CostSource& c)
{
- cost_sources.push_back(c);
+ if(request)
+ {
+ cost_sources.insert(c);
+ if(cost_sources.size() > request->num_max_cost_sources)
+ cost_sources.erase(cost_sources.begin());
+ }
+ else
+ {
+ cost_sources.insert(c);
+ }
}
bool isCollision() const
@@ -144,14 +170,6 @@ public:
return contacts.back();
}
- const CostSource& getCostSource(size_t i) const
- {
- if(i < cost_sources.size())
- return cost_sources[i];
- else
- return cost_sources.back();
- }
-
void getContacts(std::vector<Contact>& contacts_)
{
contacts_.resize(contacts.size());
@@ -196,6 +214,10 @@ struct DistanceRequest
struct DistanceResult
{
+private:
+ const DistanceRequest* request;
+
+public:
FCL_REAL min_distance;
@@ -214,6 +236,12 @@ struct DistanceResult
b1(-1),
b2(-1)
{
+ request = NULL;
+ }
+
+ void setRequest(const DistanceRequest& request_)
+ {
+ request = &request_;
}
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 9965b95ff95a1663f007c9a7e117351248ec6c18..b4d0b52b593016d098885d6a222967462abbb0c3 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -53,6 +53,13 @@ enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP
class CollisionGeometry
{
public:
+ CollisionGeometry()
+ {
+ cost_density = 1;
+ threshold_occupied = 1;
+ threshold_free = 0;
+ }
+
virtual ~CollisionGeometry() {}
virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
@@ -71,6 +78,11 @@ public:
user_data = data;
}
+
+ inline bool isOccupied() const { return cost_density >= threshold_occupied; }
+ inline bool isFree() const { return cost_density <= threshold_free; }
+ inline bool isUncertain() const { return !isOccupied() && !isFree(); }
+
/// AABB center in local coordinate
Vec3f aabb_center;
@@ -85,6 +97,12 @@ public:
/// collision cost for unit volume
FCL_REAL cost_density;
+
+ /// threshold for occupied ( >= is occupied)
+ FCL_REAL threshold_occupied;
+
+ /// threshold for free (<= is free)
+ FCL_REAL threshold_free;
};
class CollisionObject
diff --git a/trunk/fcl/include/fcl/octomap_extension.h b/trunk/fcl/include/fcl/octomap_extension.h
deleted file mode 100644
index 4fc942a558cdb3c80005c55a2ac307e0d0487534..0000000000000000000000000000000000000000
--- a/trunk/fcl/include/fcl/octomap_extension.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#ifndef FCL_OCTOMAP_EXTENSION_H
-#define FCL_OCTOMAP_EXTENSION_H
-
-#include <vector>
-#include <set>
-#include "fcl/octree.h"
-#include "fcl/broad_phase_collision.h"
-
-namespace fcl
-{
-
-struct OcTreeNode_AABB_pair
-{
- OcTree::OcTreeNode* node;
- AABB aabb;
-
- OcTreeNode_AABB_pair(OcTree::OcTreeNode* node_, const AABB& aabb_) : node(node_), aabb(aabb_) {}
-
- bool operator < (const OcTreeNode_AABB_pair& other) const
- {
- return node < other.node;
- }
-};
-
-bool defaultCollisionCostOctomapFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
-
-bool defaultCollisionCostOctomapExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes);
-
-
-typedef bool (*CollisionCostOctomapCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
-
-typedef bool (*CollisionCostOctomapCallBackExt)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes);
-
-
-void collide(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCallBack callback);
-
-void distance(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, DistanceCallBack callback);
-
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback);
-
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes);
-
-
-}
-
-#endif
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index a88813b68a53ee955ed12a36f94f96dedca42c0b..338a540197d8cdf93810d6ca58667d2cffc1888a 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -87,6 +87,16 @@ public:
return tree->isNodeOccupied(node);
}
+ inline bool isNodeFree(const OcTreeNode* node) const
+ {
+ return false; // default no definitely free node
+ }
+
+ inline bool isNodeUncertain(const OcTreeNode* node) const
+ {
+ return (!isNodeOccupied(node)) && (!isNodeFree(node));
+ }
+
inline void updateNode(FCL_REAL x, FCL_REAL y, FCL_REAL z, bool occupied)
{
tree->updateNode(octomap::point3d(x, y, z), occupied);
@@ -114,6 +124,11 @@ public:
return boxes;
}
+ FCL_REAL getOccupancyThres() const
+ {
+ return tree->getOccupancyThres();
+ }
+
OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
};
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index d94045adcccf5d41f8f7ec9c21189da7ceaf9fdd..dc161d8c030124d48e43a5d8e32f4386e88d04d6 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -58,6 +58,7 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -80,6 +81,7 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -102,6 +104,7 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -124,6 +127,7 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -146,6 +150,7 @@ bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -169,6 +174,7 @@ bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -191,6 +197,7 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -213,6 +220,7 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -235,6 +243,7 @@ bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -258,6 +267,7 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -286,8 +296,10 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
node.model2 = &shape2;
node.tf2 = tf2;
node.nsolver = nsolver;
+
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.cost_density = shape1.cost_density * shape2.cost_density;
@@ -334,8 +346,11 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
+
node.request = request;
node.result = &result;
+ result.setRequest(request);
+
node.cost_density = model1.cost_density * model2.cost_density;
return true;
@@ -382,8 +397,11 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
+
node.request = request;
node.result = &result;
+ result.setRequest(request);
+
node.cost_density = model1.cost_density * model2.cost_density;
return true;
@@ -414,8 +432,11 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
+
node.request = request;
node.result = &result;
+ result.setRequest(request);
+
node.cost_density = model1.cost_density * model2.cost_density;
return true;
@@ -498,8 +519,11 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
+
node.request = request;
node.result = &result;
+ result.setRequest(request);
+
node.cost_density = model1.cost_density * model2.cost_density;
return true;
@@ -516,7 +540,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const CollisionRequest& request,
CollisionResult& result)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
@@ -528,7 +552,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const CollisionRequest& request,
CollisionResult& result)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
@@ -540,7 +564,7 @@ bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const CollisionRequest& request,
CollisionResult& result)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
@@ -552,7 +576,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
const CollisionRequest& request,
CollisionResult& result)
{
- return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
+ return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
@@ -617,6 +641,8 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
node.request = request;
node.result = &result;
+ result.setRequest(request);
+
node.cost_density = model1.cost_density * model2.cost_density;
return true;
@@ -712,6 +738,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node,
BVHExpand(model2, node.uc2.get(), expand_r);
node.request = request;
+
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
@@ -799,6 +826,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
BVHExpand(model1, node.uc1.get(), expand_r);
node.request = request;
+
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
@@ -896,6 +924,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -964,6 +993,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -1011,6 +1041,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -1043,6 +1074,7 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -1109,6 +1141,7 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhas
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index b812b6e6ae4fcf8a1de644c23140b82216d49013..186ecb41e65b942fd9c0b3f0cceed98dd1411388 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -168,43 +168,58 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- FCL_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
+ if(this->model1->isOccupied() && this->model2->isOccupied())
+ {
+ bool is_intersect = false;
- bool is_intersect = false;
+ if(!this->request.enable_contact)
+ {
+ if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
+ {
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
+ }
+ }
+ else
+ {
+ FCL_REAL penetration;
+ Vec3f normal;
+ Vec3f contactp;
+
+ if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
+ {
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
+ }
+ }
- if(!this->request.enable_contact) // only interested in collision or not
- {
- if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
+ if(is_intersect && this->request.enable_cost)
{
- is_intersect = true;
- this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
+ AABB overlap_part;
+ AABB shape_aabb;
+ computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
+ AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
+ this->result->addCostSource(CostSource(overlap_part, cost_density));
}
}
- else
+ if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
{
- if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
+ if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
- is_intersect = true;
- this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
+ AABB overlap_part;
+ AABB shape_aabb;
+ computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
+ AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
+ this->result->addCostSource(CostSource(overlap_part, cost_density));
}
}
-
- if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->numCostSources()))
- {
- AABB overlap_part;
- AABB shape_aabb;
- computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
- AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
- this->result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
- }
}
bool canStop() const
{
- return (this->result->numContacts() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->numContacts()) && (this->request.num_max_cost_sources <= this->result->numCostSources()) &&
- ( (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->numCostSources()) );
+ return (!this->request.exhaustive) && (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts());
}
Vec3f* vertices;
@@ -242,36 +257,49 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- FCL_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
-
- bool is_intersect = false;
-
- if(!request.enable_contact) // only interested in collision or not
+ if(model1->isOccupied() && model2.isOccupied())
{
- if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
+ bool is_intersect = false;
+
+ if(!request.enable_contact) // only interested in collision or not
{
- is_intersect = true;
- result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE));
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
+ {
+ is_intersect = true;
+ if(request.num_max_contacts > result.numContacts())
+ result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE));
+ }
}
- }
- else
- {
- if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
+ else
{
- is_intersect = true;
- result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
+ FCL_REAL penetration;
+ Vec3f normal;
+ Vec3f contactp;
+
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
+ {
+ is_intersect = true;
+ if(request.num_max_contacts > result.numContacts())
+ result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
+ }
}
- }
- if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.numCostSources()))
+ if(is_intersect && request.enable_cost)
+ {
+ AABB overlap_part;
+ AABB shape_aabb;
+ computeBV<AABB, S>(model2, tf2, shape_aabb);
+ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p2)).overlap(shape_aabb, overlap_part);
+ result.addCostSource(CostSource(overlap_part, cost_density));
+ }
+ }
+ else if((!model1->isFree() || model2.isFree()) && request.enable_cost)
{
AABB overlap_part;
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p2)).overlap(shape_aabb, overlap_part);
- result.addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+ result.addCostSource(CostSource(overlap_part, cost_density));
}
}
@@ -390,43 +418,58 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- FCL_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
-
- bool is_intersect = false;
-
- if(!this->request.enable_contact) // only interested in collision or not
+ if(this->model1->isOccupied() && this->model2->isOccupied())
{
- if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
+ bool is_intersect = false;
+
+ if(!this->request.enable_contact)
{
- is_intersect = true;
- this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
+ if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
+ {
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
+ }
}
- }
- else
- {
- if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
+ else
{
- is_intersect = true;
- this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
+ FCL_REAL penetration;
+ Vec3f normal;
+ Vec3f contactp;
+
+ if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
+ {
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
+ }
}
- }
- if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->numCostSources()))
+ if(is_intersect && this->request.enable_cost)
+ {
+ AABB overlap_part;
+ AABB shape_aabb;
+ computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
+ AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
+ this->result->addCostSource(CostSource(overlap_part, cost_density));
+ }
+ }
+ else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
{
- AABB overlap_part;
- AABB shape_aabb;
- computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
- AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
- this->result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+ if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
+ {
+ AABB overlap_part;
+ AABB shape_aabb;
+ computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
+ AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
+ this->result->addCostSource(CostSource(overlap_part, cost_density));
+ }
}
}
bool canStop() const
{
- return (this->result->numContacts() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->numContacts()) &&
- ( (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->numCostSources()) );
+ return (!this->request.exhaustive) && (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts());
}
Vec3f* vertices;
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index f0bd374cd16007d6bd3f43f3ee9294b554bda708..38c3439ebea4991d8aa5660c4f43e76c42fefaf9 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -174,58 +174,68 @@ public:
const Vec3f& q1 = vertices2[tri_id2[0]];
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
-
- FCL_REAL penetration;
- Vec3f normal;
- unsigned int n_contacts;
- Vec3f contacts[2];
- bool is_intersect = false;
-
- if(!this->request.enable_contact) // only interested in collision or not
+ if(this->model1->isOccupied() && this->model2->isOccupied())
{
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
+ bool is_intersect = false;
+
+ if(!this->request.enable_contact) // only interested in collision or not
{
- is_intersect = true;
- this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
+ {
+ is_intersect = true;
+ this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
+ }
}
- }
- else // need compute the contact information
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- contacts,
- &n_contacts,
- &penetration,
- &normal))
+ else // need compute the contact information
{
- is_intersect = true;
- if(!this->request.exhaustive)
+ FCL_REAL penetration;
+ Vec3f normal;
+ unsigned int n_contacts;
+ Vec3f contacts[2];
+
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
+ contacts,
+ &n_contacts,
+ &penetration,
+ &normal))
{
- if(this->request.num_max_contacts < n_contacts + this->result->numContacts())
- n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0;
- }
+ is_intersect = true;
+ if(!this->request.exhaustive)
+ {
+ if(this->request.num_max_contacts < n_contacts + this->result->numContacts())
+ n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0;
+ }
- for(unsigned int i = 0; i < n_contacts; ++i)
- {
- this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration));
+ for(unsigned int i = 0; i < n_contacts; ++i)
+ {
+ this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration));
+ }
}
}
- }
-
- if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->numCostSources()))
- {
- AABB overlap_part;
- AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
- this->result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+ if(is_intersect && this->request.enable_cost)
+ {
+ AABB overlap_part;
+ AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
+ this->result->addCostSource(CostSource(overlap_part, cost_density));
+ }
}
+ else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
+ {
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
+ {
+ AABB overlap_part;
+ AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
+ this->result->addCostSource(CostSource(overlap_part, cost_density));
+ }
+ }
}
/** \brief Whether the traversal process can stop early */
bool canStop() const
{
- return (this->result->numContacts() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->numContacts()) &&
- ( (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->numCostSources()) );
+ return (!this->request.exhaustive) && (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts());
}
Vec3f* vertices1;
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index 2b61055754df7f348048f2f8a7271541db60a503..9a81823fb4dbbbafc0f8a705743e3dafb6861a77 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))
+ if(tree1->isNodeOccupied(root1)) // occupied area
{
OBB obb1;
convertBV(bv1, tf1, obb1);
@@ -292,7 +292,6 @@ private:
constructBox(bv1, tf1, box, box_tf);
bool is_intersect = false;
-
if(!crequest->enable_contact)
{
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
@@ -315,18 +314,59 @@ private:
cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contact, normal, depth));
}
}
-
- return ((cresult->numContacts() >= crequest->num_max_contacts) && !crequest->exhaustive);
+
+ if(is_intersect && crequest->enable_cost)
+ {
+ AABB overlap_part;
+ AABB aabb1, aabb2;
+ computeBV<AABB, Box>(box, box_tf, aabb1);
+ computeBV<AABB, S>(s, tf2, aabb2);
+ aabb1.overlap(aabb2, overlap_part);
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density));
+ }
+
+ return (!crequest->exhaustive) && (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts());
}
else return false;
}
- else
+ else if(tree1->isNodeUncertain(root1) && crequest->enable_cost) // uncertain area
+ {
+ OBB obb1;
+ convertBV(bv1, tf1, obb1);
+ if(obb1.overlap(obb2))
+ {
+ Box box;
+ SimpleTransform box_tf;
+ constructBox(bv1, tf1, box, box_tf);
+
+ if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
+ {
+ AABB overlap_part;
+ AABB aabb1, aabb2;
+ computeBV<AABB, Box>(box, box_tf, aabb1);
+ computeBV<AABB, S>(s, tf2, aabb2);
+ aabb1.overlap(aabb2, overlap_part);
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density));
+ }
+ }
+
+ return false;
+ }
+ else // free area
return false;
}
- OBB obb1;
- convertBV(bv1, tf1, obb1);
- if(!tree1->isNodeOccupied(root1) || !obb1.overlap(obb2)) return false;
+ /// 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;
+ else
+ {
+ OBB obb1;
+ convertBV(bv1, tf1, obb1);
+ if(!obb1.overlap(obb2)) return false;
+ }
for(unsigned int i = 0; i < 8; ++i)
{
@@ -454,10 +494,15 @@ private:
const Vec3f& p2 = tree2->vertices[tri_id[1]];
const Vec3f& p3 = tree2->vertices[tri_id[2]];
+ bool is_intersect = false;
if(!crequest->enable_contact)
{
if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL))
- cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2));
+ {
+ is_intersect = true;
+ if(cresult->numContacts() < crequest->num_max_contacts)
+ cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2));
+ }
}
else
{
@@ -466,24 +511,75 @@ private:
Vec3f normal;
if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal))
- cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2, contact, normal, depth));
+ {
+ is_intersect = true;
+ if(cresult->numContacts() < crequest->num_max_contacts)
+ cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2, contact, normal, depth));
+ }
+ }
+
+ if(is_intersect && crequest->enable_cost)
+ {
+ AABB overlap_part;
+ AABB aabb1;
+ computeBV<AABB, Box>(box, box_tf, aabb1);
+ AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
+ aabb1.overlap(aabb2, overlap_part);
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density));
}
- return ((cresult->numContacts() >= crequest->num_max_contacts) && !crequest->exhaustive);
+ return (!crequest->exhaustive) && (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts());
}
else
return false;
}
- else
+ else if(tree1->isNodeUncertain(root1) && crequest->enable_cost) // uncertain area
+ {
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(tree2->getBV(root2).bv, tf2, obb2);
+ if(obb1.overlap(obb2))
+ {
+ Box box;
+ SimpleTransform box_tf;
+ constructBox(bv1, tf1, box, box_tf);
+
+ int primitive_id = tree2->getBV(root2).primitiveId();
+ const Triangle& tri_id = tree2->tri_indices[primitive_id];
+ const Vec3f& p1 = tree2->vertices[tri_id[0]];
+ const Vec3f& p2 = tree2->vertices[tri_id[1]];
+ const Vec3f& p3 = tree2->vertices[tri_id[2]];
+
+ if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL))
+ {
+ AABB overlap_part;
+ AABB aabb1;
+ computeBV<AABB, Box>(box, box_tf, aabb1);
+ AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
+ aabb1.overlap(aabb2, overlap_part);
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density));
+ }
+ }
+
+ return false;
+ }
+ else // free area
return false;
}
-
- OBB obb1, obb2;
- convertBV(bv1, tf1, obb1);
- convertBV(tree2->getBV(root2).bv, tf2, obb2);
- if(!tree1->isNodeOccupied(root1) || !obb1.overlap(obb2)) return false;
-
+ /// 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;
+ else
+ {
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(tree2->getBV(root2).bv, tf2, obb2);
+ if(!obb1.overlap(obb2)) return false;
+ }
+
if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size())))
{
for(unsigned int i = 0; i < 8; ++i)
@@ -598,8 +694,9 @@ private:
{
if(!root1->hasChildren() && !root2->hasChildren())
{
- if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
+ if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area
{
+ bool is_intersect = false;
if(!crequest->enable_contact)
{
OBB obb1, obb2;
@@ -607,7 +704,11 @@ private:
convertBV(bv2, tf2, obb2);
if(obb1.overlap(obb2))
- cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot()));
+ {
+ is_intersect = true;
+ if(cresult->numContacts() < crequest->num_max_contacts)
+ cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot()));
+ }
}
else
{
@@ -620,21 +721,69 @@ private:
FCL_REAL depth;
Vec3f normal;
if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal))
- cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contact, normal, depth));
+ {
+ is_intersect = true;
+ if(cresult->numContacts() < crequest->num_max_contacts)
+ cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contact, normal, depth));
+ }
}
- return ((cresult->numContacts() >= crequest->num_max_contacts) && !crequest->exhaustive);
+ if(is_intersect && crequest->enable_cost)
+ {
+ Box box1, box2;
+ SimpleTransform 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, root1->getOccupancy() * root2->getOccupancy()));
+ }
+
+ return (!crequest->exhaustive) && (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts());
}
- else
+ else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied)
+ {
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(bv2, tf2, obb2);
+
+ if(obb1.overlap(obb2))
+ {
+ Box box1, box2;
+ SimpleTransform 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, root1->getOccupancy() * root2->getOccupancy()));
+ }
+
+ return false;
+ }
+ else // free area (at least one node is free)
return false;
}
- if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false;
-
- OBB obb1, obb2;
- convertBV(bv1, tf1, obb1);
- convertBV(bv2, tf2, obb2);
- if(!obb1.overlap(obb2)) return false;
+ /// 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
+ if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false;
+ else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false;
+ else
+ {
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(bv2, tf2, obb2);
+ if(!obb1.overlap(obb2)) return false;
+ }
if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size())))
{
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 2c2da37921ef0e960620a27b267433e715178e30..2784ffdcc2a708e44cc0cc704f4091bf58dda165 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -65,34 +65,51 @@ public:
void leafTesting(int, int) const
{
- bool is_collision = false;
- if(request.enable_contact)
+ if(model1->isOccupied() && model2->isOccupied())
{
- Vec3f contact_point, normal;
- FCL_REAL penetration_depth;
- if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal))
+ bool is_collision = false;
+ if(request.enable_contact)
{
- is_collision = true;
- result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
+ Vec3f contact_point, normal;
+ FCL_REAL penetration_depth;
+ if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal))
+ {
+ is_collision = true;
+ if(request.num_max_contacts > result->numContacts())
+ result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
+ }
}
- }
- else
- {
- if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
+ else
{
- is_collision = true;
- result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE));
+ if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
+ {
+ is_collision = true;
+ if(request.num_max_contacts > result->numContacts())
+ result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE));
+ }
}
- }
- if(is_collision && request.enable_cost)
+ if(is_collision && request.enable_cost)
+ {
+ AABB aabb1, aabb2;
+ computeBV<AABB, S1>(*model1, tf1, aabb1);
+ computeBV<AABB, S2>(*model2, tf2, aabb2);
+ AABB overlap_part;
+ aabb1.overlap(aabb2, overlap_part);
+ result->addCostSource(CostSource(overlap_part, cost_density));
+ }
+ }
+ else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
{
- AABB aabb1, aabb2;
- computeBV<AABB, S1>(*model1, tf1, aabb1);
- computeBV<AABB, S2>(*model2, tf2, aabb2);
- AABB overlap_part;
- aabb1.overlap(aabb2, overlap_part);
- result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+ if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
+ {
+ AABB aabb1, aabb2;
+ computeBV<AABB, S1>(*model1, tf1, aabb1);
+ computeBV<AABB, S2>(*model2, tf2, aabb2);
+ AABB overlap_part;
+ aabb1.overlap(aabb2, overlap_part);
+ result->addCostSource(CostSource(overlap_part, cost_density));
+ }
}
}
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index cab7b423a375c881ae26a34600e2e0eec9a07960..3cf0b21aa3ba343cc90e481522d50885f6a8959a 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -85,6 +85,7 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cda
return cdata->done;
}
+
void NaiveCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
@@ -2337,7 +2338,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root1->isLeaf() && !root2->hasChildren())
{
- if(tree2->isNodeOccupied(root2))
+ if(!tree2->isNodeFree(root2))
{
OBB obb1, obb2;
convertBV(root1->bv, SimpleTransform(), obb1);
@@ -2348,6 +2349,10 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
Box* box = new Box();
SimpleTransform box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
+
+ 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);
}
@@ -2360,7 +2365,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
convertBV(root1->bv, SimpleTransform(), obb1);
convertBV(root2_bv, tf2, obb2);
- if(!tree2->isNodeOccupied(root2) || !obb1.overlap(obb2)) return false;
+ if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
{
@@ -2391,7 +2396,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root1->isLeaf() && !root2->hasChildren())
{
- if(tree2->isNodeOccupied(root2))
+ if(!tree2->isNodeFree(root2))
{
const AABB& root2_bv_t = translate(root2_bv, tf2);
if(root1->bv.overlap(root2_bv_t))
@@ -2399,6 +2404,10 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
Box* box = new Box();
SimpleTransform box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
+
+ 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);
}
@@ -2408,7 +2417,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
}
const AABB& root2_bv_t = translate(root2_bv, tf2);
- if(!tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv_t)) return false;
+ if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false;
if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
{
@@ -2941,7 +2950,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
{
- if(tree2->isNodeOccupied(root2))
+ if(!tree2->isNodeFree(root2))
{
OBB obb1, obb2;
convertBV(root1->bv, SimpleTransform(), obb1);
@@ -2952,6 +2961,10 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
Box* box = new Box();
SimpleTransform box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
+
+ 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);
}
@@ -2964,7 +2977,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
convertBV(root1->bv, SimpleTransform(), obb1);
convertBV(root2_bv, tf2, obb2);
- if(!tree2->isNodeOccupied(root2) || !obb1.overlap(obb2)) return false;
+ if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
{
@@ -2997,7 +3010,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
{
- if(tree2->isNodeOccupied(root2))
+ if(!tree2->isNodeFree(root2))
{
const AABB& root_bv_t = translate(root2_bv, tf2);
if(root1->bv.overlap(root_bv_t))
@@ -3005,6 +3018,10 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
Box* box = new Box();
SimpleTransform box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
+
+ 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);
}
@@ -3014,7 +3031,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
}
const AABB& root_bv_t = translate(root2_bv, tf2);
- if(!tree2->isNodeOccupied(root2) || !root1->bv.overlap(root_bv_t)) return false;
+ if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false;
if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
{
diff --git a/trunk/fcl/src/octomap_extension.cpp b/trunk/fcl/src/octomap_extension.cpp
deleted file mode 100644
index 115dad4f30139b3be1b01e793d74b0111ff3bd94..0000000000000000000000000000000000000000
--- a/trunk/fcl/src/octomap_extension.cpp
+++ /dev/null
@@ -1,356 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#include "fcl/octomap_extension.h"
-#include "fcl/geometric_shapes.h"
-
-namespace fcl
-{
-
-
-class ExtendedBox : public Box
-{
-public:
- ExtendedBox(FCL_REAL x, FCL_REAL y, FCL_REAL z) : Box(x, y, z)
- {
- prob = 0;
- node = NULL;
- }
-
- FCL_REAL prob;
- OcTree::OcTreeNode* node;
-};
-
-bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
- OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
- void* cdata, CollisionCallBack callback)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(tree2->isNodeOccupied(root2))
- {
- if(root1->bv.overlap(root2_bv))
- {
- Box* box = new Box(root2_bv.max_ - root2_bv.min_);
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
- }
- else return false;
- }
- else return false;
- }
-
- if(!tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv)) return false;
-
- if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
- {
- if(collisionRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback))
- return true;
- if(collisionRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback))
- return true;
- }
- else
- {
- for(unsigned int i = 0; i < 8; ++i)
- {
- if(root2->childExists(i))
- {
- OcTree::OcTreeNode* child = root2->getChild(i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- if(collisionRecurse(root1, tree2, child, child_bv, cdata, callback))
- return true;
- }
- }
- }
-
- return false;
-}
-
-
-void collide(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCallBack callback)
-{
- DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
- OcTree::OcTreeNode* root2 = octree->getRoot();
-
- collisionRecurse(root1, octree, root2, octree->getRootBV(),
- cdata, callback);
-}
-
-
-bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
- OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
- void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(tree2->isNodeOccupied(root2))
- {
- Box* box = new Box(root2_bv.max_ - root2_bv.min_);
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
- }
- else return false;
- }
-
- if(!tree2->isNodeOccupied(root2)) return false;
-
- if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
- {
- FCL_REAL d1 = root2_bv.distance(root1->childs[0]->bv);
- FCL_REAL d2 = root2_bv.distance(root1->childs[1]->bv);
-
- if(d2 < d1)
- {
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, min_dist))
- return true;
- }
-
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, min_dist))
- return true;
- }
- }
- else
- {
- if(d1 < min_dist)
- {
- if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, min_dist))
- return true;
- }
-
- if(d2 < min_dist)
- {
- if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, min_dist))
- return true;
- }
- }
- }
- else
- {
- for(unsigned int i = 0; i < 8; ++i)
- {
- if(root2->childExists(i))
- {
- OcTree::OcTreeNode* child = root2->getChild(i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- FCL_REAL d = root1->bv.distance(child_bv);
-
- if(d < min_dist)
- {
- if(distanceRecurse(root1, tree2, child, child_bv, cdata, callback, min_dist))
- return true;
- }
- }
- }
- }
-
- return false;
-}
-
-
-void distance(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, DistanceCallBack callback)
-{
- DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
- OcTree::OcTreeNode* root2 = octree->getRoot();
- FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
- distanceRecurse(root1, octree, root2, octree->getRootBV(),
- cdata, callback, min_dist);
-}
-
-
-bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
- OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
- void* cdata, CollisionCostOctomapCallBack callback, FCL_REAL& cost)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(!tree2->isNodeOccupied(root2))
- {
- if(root1->bv.overlap(root2_bv))
- {
- ExtendedBox* box = new ExtendedBox(root2_bv.max_[0] - root2_bv.min_[0],
- root2_bv.max_[1] - root2_bv.min_[1],
- root2_bv.max_[2] - root2_bv.min_[2]);
- box->prob = root2->getOccupancy();
- box->node = root2;
-
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, cost);
- }
- }
- }
-
- if(tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv)) return false;
-
- if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
- {
- if(collisionCostRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, cost))
- return true;
- if(collisionCostRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, cost))
- return true;
- }
- else
- {
- for(int i = 0; i < 8; ++i)
- {
- if(root2->childExists(i))
- {
- OcTree::OcTreeNode* child = root2->getChild(i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- if(collisionCostRecurse(root1, tree2, child, child_bv, cdata, callback, cost))
- return true;
- }
- }
- }
-
- return false;
-}
-
-
-bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
- OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
- void* cdata, CollisionCostOctomapCallBackExt callback, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes)
-{
- if(root1->isLeaf() && !root2->hasChildren())
- {
- if(!tree2->isNodeOccupied(root2))
- {
- if(root1->bv.overlap(root2_bv))
- {
- ExtendedBox* box = new ExtendedBox(root2_bv.max_[0] - root2_bv.min_[0],
- root2_bv.max_[1] - root2_bv.min_[1],
- root2_bv.max_[2] - root2_bv.min_[2]);
-
- box->prob = root2->getOccupancy();
- CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
- return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, cost, nodes);
- }
- }
- }
-
- if(tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv)) return false;
-
- if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
- {
- if(collisionCostExtRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, cost, nodes))
- return true;
- if(collisionCostExtRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, cost, nodes))
- return true;
- }
- else
- {
- for(int i = 0; i < 8; ++i)
- {
- if(root2->childExists(i))
- {
- OcTree::OcTreeNode* child = root2->getChild(i);
- AABB child_bv;
- computeChildBV(root2_bv, i, child_bv);
-
- if(collisionCostExtRecurse(root1, tree2, child, child_bv, cdata, callback, cost, nodes))
- return true;
- }
- }
- }
-
- return false;
-}
-
-
-
-bool defaultCollisionCostOctomapFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost)
-{
- const AABB& aabb1 = o1->getAABB();
- const AABB& aabb2 = o2->getAABB();
- Vec3f delta = min(aabb1.max_, aabb2.max_) - max(aabb1.min_, aabb2.min_);
- const ExtendedBox* box = static_cast<const ExtendedBox*>(o2->getCollisionGeometry());
- cost += delta[0] * delta[1] * delta[2] * box->prob;
- return false;
-}
-
-
-bool defaultCollisionCostOctomapExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes)
-{
- const AABB& aabb1 = o1->getAABB();
- const AABB& aabb2 = o2->getAABB();
- Vec3f delta = min(aabb1.max_, aabb2.max_) - max(aabb1.min_, aabb2.min_);
- const ExtendedBox* box = static_cast<const ExtendedBox*>(o2->getCollisionGeometry());
- cost += delta[0] * delta[1] * delta[2] * box->prob;
-
- const Vec3f& c = box->aabb_center;
- const Vec3f& side = box->side;
- AABB aabb(Vec3f(c[0] - side[0] / 2, c[1] - side[1] / 2, c[2] - side[2] / 2), Vec3f(c[0] + side[0] / 2, c[1] + side[1] / 2, c[2] + side[2] / 2));
- nodes.insert(OcTreeNode_AABB_pair(box->node, aabb));
- return false;
-}
-
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback)
-{
- DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
- OcTree::OcTreeNode* root2 = octree->getRoot();
-
- FCL_REAL cost = 0;
- collisionCostRecurse(root1, octree, root2, octree->getRootBV(), cdata, callback, cost);
- return cost;
-}
-
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes)
-{
- DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
- OcTree::OcTreeNode* root2 = octree->getRoot();
-
- FCL_REAL cost = 0;
- std::set<OcTreeNode_AABB_pair> pairs;
- collisionCostExtRecurse(root1, octree, root2, octree->getRootBV(), cdata, callback, cost, pairs);
- for(std::set<OcTreeNode_AABB_pair>::iterator it = pairs.begin(), end = pairs.end(); it != end; ++it)
- nodes.push_back(it->aabb);
- return cost;
-}
-
-
-
-
-
-}
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 53d472a685d0c6eb32ea4001363e6cd645265056..2014166549e9ec709b3e616e6ccb82e1b617ac23 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -66,6 +66,8 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.request = request;
node.result = &result;
+ result.setRequest(request);
+
node.cost_density = model1.cost_density * model2.cost_density;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
@@ -150,6 +152,7 @@ static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
BVHExpand(model2, node.uc2.get(), expand_r);
node.request = request;
+
node.collision_prob_threshold = collision_prob_threshold;
node.leaf_size_threshold = leaf_size_threshold;
@@ -263,6 +266,7 @@ static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
node.request = request;
node.result = &result;
+ result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 5c487612230b4b56f8c1565080e445a560fb6b88..e85457c7ad18118cdeaff1755cefc09d4556607a 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -73,49 +73,61 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
- FCL_REAL penetration;
- Vec3f normal;
- unsigned int n_contacts;
- Vec3f contacts[2];
-
- bool is_intersect = false;
-
- if(!request.enable_contact) // only interested in collision or not
+ if(model1->isOccupied() && model2->isOccupied())
{
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
+ bool is_intersect = false;
+
+ if(!request.enable_contact) // only interested in collision or not
{
- is_intersect = true;
- result.addContact(Contact(model1, model2, primitive_id1, primitive_id2));
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
+ {
+ is_intersect = true;
+ result.addContact(Contact(model1, model2, primitive_id1, primitive_id2));
+ }
}
- }
- else // need compute the contact information
- {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- R, T,
- contacts,
- &n_contacts,
- &penetration,
- &normal))
+ else // need compute the contact information
{
- is_intersect = true;
- if(!request.exhaustive)
+ FCL_REAL penetration;
+ Vec3f normal;
+ unsigned int n_contacts;
+ Vec3f contacts[2];
+
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
+ R, T,
+ contacts,
+ &n_contacts,
+ &penetration,
+ &normal))
{
- if(request.num_max_contacts < result.numContacts() + n_contacts)
- n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0;
+ is_intersect = true;
+ if(!request.exhaustive)
+ {
+ if(request.num_max_contacts < result.numContacts() + n_contacts)
+ n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0;
+ }
+
+ for(unsigned int i = 0; i < n_contacts; ++i)
+ {
+ result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(normal), penetration));
+ }
}
+ }
- for(unsigned int i = 0; i < n_contacts; ++i)
- {
- result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(normal), penetration));
- }
+ if(is_intersect && request.enable_cost)
+ {
+ AABB overlap_part;
+ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part);
+ result.addCostSource(CostSource(overlap_part, cost_density));
}
}
-
- if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.numCostSources()))
+ else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
{
- AABB overlap_part;
- AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part);
- result.addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
+ {
+ AABB overlap_part;
+ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part);
+ result.addCostSource(CostSource(overlap_part, cost_density));
+ }
}
}