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

@@ -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})

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
 {

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_)

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

trunk/fcl/include/fcl/octomap_extension.h

-/*
- * 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

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

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;

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