From b7388c6f78a64e20d779939774ef58092026c2c1 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 27 Jul 2012 04:04:32 +0000
Subject: [PATCH] cost implementation done (not tested yet).
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@142 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 2 +-
trunk/fcl/include/fcl/broad_phase_collision.h | 3 +
trunk/fcl/include/fcl/collision_data.h | 48 ++-
trunk/fcl/include/fcl/collision_object.h | 18 +
trunk/fcl/include/fcl/octomap_extension.h | 83 ----
trunk/fcl/include/fcl/octree.h | 15 +
trunk/fcl/include/fcl/simple_setup.h | 41 +-
.../include/fcl/traversal_node_bvh_shape.h | 179 +++++----
trunk/fcl/include/fcl/traversal_node_bvhs.h | 82 ++--
trunk/fcl/include/fcl/traversal_node_octree.h | 207 ++++++++--
trunk/fcl/include/fcl/traversal_node_shapes.h | 59 +--
trunk/fcl/src/broad_phase_collision.cpp | 33 +-
trunk/fcl/src/octomap_extension.cpp | 356 ------------------
trunk/fcl/src/simple_setup.cpp | 4 +
trunk/fcl/src/traversal_node_bvhs.cpp | 78 ++--
15 files changed, 559 insertions(+), 649 deletions(-)
delete mode 100644 trunk/fcl/include/fcl/octomap_extension.h
delete mode 100644 trunk/fcl/src/octomap_extension.cpp
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index ef2e6599..b680d9f2 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 0993f1cc..ee6eb6e6 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 f72af25e..0457a500 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 9965b95f..b4d0b52b 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 4fc942a5..00000000
--- 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 a88813b6..338a5401 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 d94045ad..dc161d8c 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 b812b6e6..186ecb41 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 f0bd374c..38c3439e 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 2b610557..9a81823f 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 2c2da379..2784ffdc 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 cab7b423..3cf0b21a 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 115dad4f..00000000
--- 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 53d472a6..20141665 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 5c487612..e85457c7 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));
+ }
}
}
--
GitLab