More change on request/result interface.

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@141 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/include/fcl/collision.h

@@ -49,7 +49,7 @@ namespace fcl
 /** \brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning 
  * returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
  * performs the collision between them. 
- * Return value is the number of contacts returned 
+ * Return value is the number of contacts generated between the two objects.
  */
 
 template<typename NarrowPhaseSolver>

trunk/fcl/include/fcl/collision_data.h

@@ -103,9 +103,10 @@ struct CollisionRequest
 
 struct CollisionResult
 {
+private:
   std::vector<Contact> contacts;
   std::vector<CostSource> cost_sources;
-
+public:
   CollisionResult()
   {
   }
@@ -135,6 +136,34 @@ struct CollisionResult
     return cost_sources.size();
   }
 
+  const Contact& getContact(size_t i) const
+  {
+    if(i < contacts.size()) 
+      return contacts[i];
+    else
+      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());
+    std::copy(contacts.begin(), contacts.end(), contacts_.begin());
+  }
+
+  void getCostSources(std::vector<CostSource>& cost_sources_)
+  {
+    cost_sources_.resize(cost_sources.size());
+    std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin());
+  }
+
   void clear()
   {
     contacts.clear();
@@ -159,24 +188,81 @@ struct CollisionData
 struct DistanceRequest
 {
   bool enable_nearest_points;
-  DistanceRequest() : enable_nearest_points(false)
+
+  DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
   {
   }
 };
 
 struct DistanceResult
 {
+
   FCL_REAL min_distance;
 
   Vec3f nearest_points[2];
+
+  const CollisionGeometry* o1;
+  const CollisionGeometry* o2;
+  int b1;
+  int b2;
+
+  static const int NONE = -1;
   
-  DistanceResult() : min_distance(std::numeric_limits<FCL_REAL>::max())
+  DistanceResult(FCL_REAL min_distance_ = std::numeric_limits<FCL_REAL>::max()) : min_distance(min_distance_), 
+                                                                                  o1(NULL),
+                                                                                  o2(NULL),
+                                                                                  b1(-1),
+                                                                                  b2(-1)
   {
   }
 
+  void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
+  {
+    if(min_distance > distance)
+    {
+      min_distance = distance;
+      o1 = o1_;
+      o2 = o2_;
+      b1 = b1_;
+      b2 = b2_;
+    }
+  }
+
+  void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2)
+  {
+    if(min_distance > distance)
+    {
+      min_distance = distance;
+      o1 = o1_;
+      o2 = o2_;
+      b1 = b1_;
+      b2 = b2_;
+      nearest_points[0] = p1;
+      nearest_points[1] = p2;
+    }
+  }
+
+  void update(const DistanceResult& other_result)
+  {
+    if(min_distance > other_result.min_distance)
+    {
+      min_distance = other_result.min_distance;
+      o1 = other_result.o1;
+      o2 = other_result.o2;
+      b1 = other_result.b1;
+      b2 = other_result.b2;
+      nearest_points[0] = other_result.nearest_points[0];
+      nearest_points[1] = other_result.nearest_points[1];
+    }
+  }
+
   void clear()
   {
     min_distance = std::numeric_limits<FCL_REAL>::max();
+    o1 = NULL;
+    o2 = NULL;
+    b1 = -1;
+    b2 = -1;
   }
 };
 

trunk/fcl/include/fcl/simple_setup.h

@@ -75,9 +75,11 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
                 const OcTree& model1, const SimpleTransform& tf1,
                 const OcTree& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
   node.request = request;
+  node.result = &result;
  
   node.model1 = &model1;
   node.model2 = &model2;
@@ -139,9 +141,11 @@ bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
                 const S& model1, const SimpleTransform& tf1,
                 const OcTree& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -160,9 +164,11 @@ bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
                 const OcTree& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -224,9 +230,11 @@ bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
                 const BVHModel<BV>& model1, const SimpleTransform& tf1,
                 const OcTree& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -245,9 +253,11 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
                 const OcTree& model1, const SimpleTransform& tf1,
                 const BVHModel<BV>& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -823,9 +833,11 @@ bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
                 const S1& shape1, const SimpleTransform& tf1,
                 const S2& shape2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &shape1;
   node.tf1 = tf1;
@@ -842,6 +854,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
                 BVHModel<BV>& model1, SimpleTransform& tf1,
                 BVHModel<BV>& model2, SimpleTransform& tf2,
                 const DistanceRequest& request,
+                DistanceResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -882,6 +895,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
   }
 
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.tf1 = tf1;
@@ -902,18 +916,21 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
 bool initialize(MeshDistanceTraversalNodeRSS& node,
                 const BVHModel<RSS>& model1, const SimpleTransform& tf1,
                 const BVHModel<RSS>& model2, const SimpleTransform& tf2,
-                const DistanceRequest& request);
+                const DistanceRequest& request,
+                DistanceResult& result);
 
 
 bool initialize(MeshDistanceTraversalNodekIOS& node,
                 const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
                 const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
-                const DistanceRequest& request);
+                const DistanceRequest& request,
+                DistanceResult& result);
 
 bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
                 const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
                 const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
-                const DistanceRequest& request);
+                const DistanceRequest& request,
+                DistanceResult& result);
 
 
 template<typename BV, typename S, typename NarrowPhaseSolver>
@@ -922,6 +939,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
                 const DistanceRequest& request,
+                DistanceResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
@@ -945,6 +963,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
   }
 
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.tf1 = tf1;
@@ -967,6 +986,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
                 BVHModel<BV>& model2, SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
                 const DistanceRequest& request,
+                DistanceResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -990,6 +1010,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
   }
 
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.tf1 = tf1;
@@ -1014,12 +1035,14 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
                                                       const BVHModel<BV>& model1, const SimpleTransform& tf1,
                                                       const S& model2, const SimpleTransform& tf2,
                                                       const NarrowPhaseSolver* nsolver,
-                                                      const DistanceRequest& request)
+                                                      const DistanceRequest& request,
+                                                      DistanceResult& result)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.tf1 = tf1;
@@ -1042,9 +1065,10 @@ bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
                 const BVHModel<RSS>& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request, 
+                DistanceResult& result)
 {
-  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
 }
 
 template<typename S, typename NarrowPhaseSolver>
@@ -1052,9 +1076,10 @@ bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
                 const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
-  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);  
+  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);  
 }
 
 template<typename S, typename NarrowPhaseSolver>
@@ -1062,9 +1087,10 @@ bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
                 const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
-  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
 }
 
 
@@ -1075,12 +1101,14 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhas
                                                       const S& model1, const SimpleTransform& tf1,
                                                       const BVHModel<BV>& model2, const SimpleTransform& tf2,
                                                       const NarrowPhaseSolver* nsolver,
-                                                      const DistanceRequest& request)
+                                                      const DistanceRequest& request,
+                                                      DistanceResult& result)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
 
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.tf1 = tf1;
@@ -1106,9 +1134,10 @@ bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
                 const S& model1, const SimpleTransform& tf1,
                 const BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
-  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
 }
 
 template<typename S, typename NarrowPhaseSolver>
@@ -1116,9 +1145,10 @@ bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
                 const S& model1, const SimpleTransform& tf1,
                 const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
-  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
 }
 
 template<typename S, typename NarrowPhaseSolver>
@@ -1126,9 +1156,10 @@ bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
                 const S& model1, const SimpleTransform& tf1,
                 const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const DistanceRequest& request)
+                const DistanceRequest& request,
+                DistanceResult& result)
 {
-  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
 }
 
 

trunk/fcl/include/fcl/traversal_node_bvh_shape.h

@@ -179,7 +179,7 @@ public:
       if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
       {
         is_intersect = true;
-        this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
+        this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
       }
     }
     else
@@ -187,24 +187,24 @@ public:
       if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
       {
         is_intersect = true;
-        this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
+        this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
       }
     }
 
-    if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->cost_sources.size()))
+    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->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+      this->result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
     }
   }
 
   bool canStop() const
   {
-    return (this->result->contacts.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->contacts.size()) && (this->request.num_max_cost_sources <= this->result->cost_sources.size()) &&
-      (  (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->cost_sources.size())  );
+    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())  );
   }
 
   Vec3f* vertices;
@@ -253,7 +253,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
     if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
     {
       is_intersect = true;
-      result.contacts.push_back(Contact(model1, &model2, primitive_id, Contact::NONE));
+      result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE));
     }
   }
   else
@@ -261,17 +261,17 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
     if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
     {
       is_intersect = true;
-      result.contacts.push_back(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
+      result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
     }
   }
 
-  if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.cost_sources.size()))
+  if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.numCostSources()))
   {
     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.cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+    result.addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
   }
 }
 
@@ -401,7 +401,7 @@ public:
       if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
       {
         is_intersect = true;
-        this->result->contacts.push_back(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
+        this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
       }
     }
     else
@@ -409,24 +409,24 @@ public:
       if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
       {
         is_intersect = true;
-        this->result->contacts.push_back(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
+        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->cost_sources.size()))
+    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->model1), this->tf1, shape_aabb);
       AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
-      this->result->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+      this->result->addCostSource(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
     }
   }
 
   bool canStop() const
   {
-    return (this->result->contacts.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= this->result->contacts.size()) &&
-      (  (!this->request.enable_cost) || (this->request.num_max_cost_sources <= this->result->cost_sources.size())  );
+    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())  );
   }
 
   Vec3f* vertices;
@@ -633,13 +633,9 @@ public:
     vertices = NULL;
     tri_indices = NULL;
 
-    last_tri_id = 0;
-
     rel_err = 0;
     abs_err = 0;
 
-    min_distance = std::numeric_limits<FCL_REAL>::max();
-
     nsolver = NULL;
   }
 
@@ -660,17 +656,12 @@ public:
     FCL_REAL d;
     nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d);
 
-    if(d < min_distance)
-    {
-      min_distance = d;
-      
-      last_tri_id = primitive_id;
-    }
+    this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE);
   }
 
   bool canStop(FCL_REAL c) const
   {
-    if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
+    if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
       return true;
     return false;
   }
@@ -680,10 +671,7 @@ public:
 
   FCL_REAL rel_err;
   FCL_REAL abs_err;
-  
-  mutable FCL_REAL min_distance;
-  mutable int last_tri_id;
-  
+    
   const NarrowPhaseSolver* nsolver;
 };
 
@@ -699,8 +687,8 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
                                               const NarrowPhaseSolver* nsolver,
                                               bool enable_statistics,
                                               int & num_leaf_tests,
-                                              int& last_tri_id,
-                                              FCL_REAL& min_distance)
+                                              const DistanceRequest& request,
+                                              DistanceResult& result)
 {
   if(enable_statistics) num_leaf_tests++;
     
@@ -712,51 +700,38 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
   const Vec3f& p2 = vertices[tri_id[1]];
   const Vec3f& p3 = vertices[tri_id[2]];
     
-  FCL_REAL dist;
-
-  nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &dist);
-    
-  if(dist < min_distance)
-  {
-    min_distance = dist;
-
-    last_tri_id = primitive_id;
-  }
-}
+  FCL_REAL distance;
+  nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance);
 
+  result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE);
 }
 
-
-template<typename S, typename NarrowPhaseSolver>
-static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, int last_tri_id,
-                                       const S& s, const SimpleTransform& tf1, const SimpleTransform& tf2,
-                                       const NarrowPhaseSolver* nsolver,
-                                       FCL_REAL& min_distance)
+template<typename BV, typename S, typename NarrowPhaseSolver>
+static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
+                                                  Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
+                                                  const S& model2, const SimpleTransform& tf1, const SimpleTransform& tf2,
+                                                  const NarrowPhaseSolver* nsolver,
+                                                  const DistanceRequest& request,
+                                                  DistanceResult& result)
 {
-  const Triangle& last_tri = tri_indices[last_tri_id];