diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index bfa53f58da7fa49b3ba5a319bfe8bce94bb0f6d2..f42f76dee79907efce70e41aeb2653d97077265b 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -19,6 +19,8 @@ struct Contact
   const CollisionGeometry* o2;
   int b1;
   int b2;
+  
+  static const int NONE = -1;
 
   Contact()
   {
@@ -47,6 +49,13 @@ struct Contact
     pos = pos_;
     penetration_depth = depth_;
   }
+
+  bool operator < (const Contact& other) const
+  {
+    if(b1 == other.b1)
+      return b2 < other.b2;
+    return b1 < other.b1;
+  }
 };
 
 struct CostSource
@@ -62,6 +71,11 @@ struct CostSource
   }
 
   CostSource() {}
+
+  bool operator < (const CostSource& other) const
+  {
+    return cost_density < other.cost_density;
+  }
 };
 
 struct CollisionRequest
@@ -92,17 +106,39 @@ struct CollisionResult
   std::vector<Contact> contacts;
   std::vector<CostSource> cost_sources;
 
-  bool is_collision;
+  CollisionResult()
+  {
+  }
+
+  inline void addContact(const Contact& c) 
+  {
+    contacts.push_back(c);
+  }
+
+  inline void addCostSource(const CostSource& c)
+  {
+    cost_sources.push_back(c);
+  }
+
+  bool isCollision() const
+  {
+    return contacts.size() > 0;
+  }
+
+  size_t numContacts() const
+  {
+    return contacts.size();
+  }
 
-  CollisionResult() : is_collision(false)
+  size_t numCostSources() const
   {
+    return cost_sources.size();
   }
 
   void clear()
   {
     contacts.clear();
     cost_sources.clear();
-    is_collision = false;
   }
 };
 
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 20c7011508c2f15e2892f4f80cc51664ee2fa097..626ac30208a407d2f4f012ba4bdfb78fe89c84e2 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -53,9 +53,11 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
                 const OcTree& model1, const SimpleTransform& tf1,
                 const OcTree& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -93,9 +95,11 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
                 const S& model1, const SimpleTransform& tf1,
                 const OcTree& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -113,9 +117,11 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
                 const OcTree& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -174,9 +180,11 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
                 const BVHModel<BV>& model1, const SimpleTransform& tf1,
                 const OcTree& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -194,9 +202,11 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
                 const OcTree& model1, const SimpleTransform& tf1,
                 const BVHModel<BV>& model2, const SimpleTransform& tf2,
                 const OcTreeSolver<NarrowPhaseSolver>* otsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
   node.request = request;
+  node.result = &result;
 
   node.model1 = &model1;
   node.model2 = &model2;
@@ -258,7 +268,8 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
                 const S1& shape1, const SimpleTransform& tf1,
                 const S2& shape2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
   node.model1 = &shape1;
   node.tf1 = tf1;
@@ -266,6 +277,7 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
   node.tf2 = tf2;
   node.nsolver = nsolver;
   node.request = request;
+  node.result = &result;
   
   node.cost_density = shape1.cost_density * shape2.cost_density;
 
@@ -279,6 +291,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
                 const CollisionRequest& request,
+                CollisionResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
@@ -312,6 +325,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
   node.vertices = model1.vertices;
   node.tri_indices = model1.tri_indices;
   node.request = request;
+  node.result = &result;
   node.cost_density = model1.cost_density * model2.cost_density;
 
   return true;
@@ -325,6 +339,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
                 BVHModel<BV>& model2, SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
                 const CollisionRequest& request,
+                CollisionResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -358,6 +373,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
   node.vertices = model2.vertices;
   node.tri_indices = model2.tri_indices;
   node.request = request;
+  node.result = &result;
   node.cost_density = model1.cost_density * model2.cost_density;
 
   return true;
@@ -372,7 +388,8 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
                                                        const BVHModel<BV>& model1, const SimpleTransform& tf1,
                                                        const S& model2, const SimpleTransform& tf2,
                                                        const NarrowPhaseSolver* nsolver,
-                                                       const CollisionRequest& request)
+                                                       const CollisionRequest& request,
+                                                       CollisionResult& result)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
@@ -388,6 +405,7 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
   node.vertices = model1.vertices;
   node.tri_indices = model1.tri_indices;
   node.request = request;
+  node.result = &result;
   node.cost_density = model1.cost_density * model2.cost_density;
 
   return true;
@@ -403,9 +421,10 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
                 const BVHModel<OBB>& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupMeshShapeCollisionOrientedNode(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 */
@@ -414,9 +433,10 @@ bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
                 const BVHModel<RSS>& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request, 
+                CollisionResult& result)
 {
-  return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupMeshShapeCollisionOrientedNode(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 */
@@ -425,9 +445,10 @@ bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
                 const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupMeshShapeCollisionOrientedNode(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 */
@@ -436,9 +457,10 @@ bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
                 const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
                 const S& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
 }
 
 
@@ -450,7 +472,8 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
                                                        const S& model1, const SimpleTransform& tf1,
                                                        const BVHModel<BV>& model2, const SimpleTransform& tf2,
                                                        const NarrowPhaseSolver* nsolver,
-                                                       const CollisionRequest& request)
+                                                       const CollisionRequest& request,
+                                                       CollisionResult& result)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
@@ -466,6 +489,7 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
   node.vertices = model2.vertices;
   node.tri_indices = model2.tri_indices;
   node.request = request;
+  node.result = &result;
   node.cost_density = model1.cost_density * model2.cost_density;
 
   return true;
@@ -479,9 +503,10 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
                 const S& model1, const SimpleTransform& tf1,
                 const BVHModel<OBB>& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return 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 */
@@ -490,9 +515,10 @@ bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
                 const S& model1, const SimpleTransform& tf1,
                 const BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return 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 */
@@ -501,9 +527,10 @@ bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
                 const S& model1, const SimpleTransform& tf1,
                 const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return 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 */
@@ -512,9 +539,10 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
                 const S& model1, const SimpleTransform& tf1,
                 const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
                 const NarrowPhaseSolver* nsolver,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request);
+  return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
 }
 
 
@@ -526,6 +554,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
                 BVHModel<BV>& model1, SimpleTransform& tf1,
                 BVHModel<BV>& model2, SimpleTransform& tf2,
                 const CollisionRequest& request,
+                CollisionResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -577,6 +606,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
   node.tri_indices2 = model2.tri_indices;
 
   node.request = request;
+  node.result = &result;
   node.cost_density = model1.cost_density * model2.cost_density;
 
   return true;
@@ -587,22 +617,22 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
 bool initialize(MeshCollisionTraversalNodeOBB& node,
                 const BVHModel<OBB>& model1, const SimpleTransform& tf1,
                 const BVHModel<OBB>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request);
+                const CollisionRequest& request, CollisionResult& result);
 
 bool initialize(MeshCollisionTraversalNodeRSS& node,
                 const BVHModel<RSS>& model1, const SimpleTransform& tf1,
                 const BVHModel<RSS>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request);
+                const CollisionRequest& request, CollisionResult& result);
 
 bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
                 const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
                 const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request);
+                const CollisionRequest& request, CollisionResult& result);
 
 bool initialize(MeshCollisionTraversalNodekIOS& node,
                 const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
                 const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request);
+                const CollisionRequest& request, CollisionResult& result);
 
 #if USE_SVMLIGHT
 
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index e2749bf28ff882186b3542981c57c0293c9cec5f..cc352d067ec719e3fb9e1b66d97dc6322c48ad4d 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -86,7 +86,7 @@ public:
 class CollisionTraversalNodeBase : public TraversalNodeBase
 {
 public:
-  CollisionTraversalNodeBase() : enable_statistics(false) {}
+  CollisionTraversalNodeBase() : result(NULL), enable_statistics(false) {}
 
   virtual ~CollisionTraversalNodeBase();
 
@@ -103,13 +103,15 @@ public:
 
   CollisionRequest request;
 
+  CollisionResult* result;
+
   bool enable_statistics;
 };
 
 class DistanceTraversalNodeBase : public TraversalNodeBase
 {
 public:
-  DistanceTraversalNodeBase() : enable_statistics(false) {}
+  DistanceTraversalNodeBase() : result(NULL), enable_statistics(false) {}
 
   virtual ~DistanceTraversalNodeBase();
 
@@ -123,6 +125,8 @@ public:
 
   DistanceRequest request;
 
+  DistanceResult* result;
+
   bool enable_statistics;
 };
 
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index f4afbc8ecd347675e30bdcc48d14ef5ff15220ab..d91c6baf712523b4b7025046be7f6475c2fbc87f 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -143,38 +143,6 @@ public:
 
 
 
-/** \brief The indices of in-collision primitives of objects */
-struct BVHShapeCollisionPair
-{
-  BVHShapeCollisionPair() {}
-
-  BVHShapeCollisionPair(int id_) : id(id_) {}
-
-  BVHShapeCollisionPair(int id_, const Vec3f& n, const Vec3f& contactp, FCL_REAL depth) : id(id_),
-      normal(n), contact_point(contactp), penetration_depth(depth) {}
-
-  /** \brief The index of BVH's in-collision primitive */
-  int id;
-
-  /** \brief Contact normal */
-  Vec3f normal;
-
-  /** \brief Contact points */
-  Vec3f contact_point;
-
-  /** \brief Penetration depth for two triangles */
-  FCL_REAL penetration_depth;
-};
-
-struct BVHShapeCollisionPairComp
-{
-  bool operator()(const BVHShapeCollisionPair& a, const BVHShapeCollisionPair& b)
-  {
-    return a.id < b.id;
-  }
-};
-
-
 template<typename BV, typename S, typename NarrowPhaseSolver>
 class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
 {
@@ -206,12 +174,12 @@ public:
 
     bool is_intersect = false;
 
-    if(!request.enable_contact) // only interested in collision or not
+    if(!this->request.enable_contact) // only interested in collision or not
     {
       if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
       {
         is_intersect = true;
-        pairs.push_back(BVHShapeCollisionPair(primitive_id));
+        this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
       }
     }
     else
@@ -219,34 +187,28 @@ public:
       if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
       {
         is_intersect = true;
-        pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
+        this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
       }
     }
 
-    if(is_intersect && request.enable_cost && (request.num_max_cost_sources > cost_sources.size()))
+    if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->cost_sources.size()))
     {
       AABB overlap_part;
       AABB shape_aabb;
       computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
       AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
-      cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+      this->result->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
     }
   }
 
   bool canStop() const
   {
-    return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= pairs.size()) && (request.num_max_cost_sources <= cost_sources.size()) &&
-      (  (!this->request.enable_cost) || (this->request.num_max_cost_sources <= cost_sources.size())  );
+    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())  );
   }
 
   Vec3f* vertices;
   Triangle* tri_indices;
-
-  CollisionRequest request;
-
-  mutable std::vector<BVHShapeCollisionPair> pairs;
-
-  mutable std::vector<CostSource> cost_sources;
   
   FCL_REAL cost_density;
 
@@ -265,11 +227,9 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
                                                              const NarrowPhaseSolver* nsolver,
                                                              bool enable_statistics, 
                                                              FCL_REAL cost_density,
-                                                             const CollisionRequest& request,
                                                              int& num_leaf_tests,
-                                                             std::vector<BVHShapeCollisionPair>& pairs,
-                                                             std::vector<CostSource>& cost_sources)
-                                                 
+                                                             const CollisionRequest& request,
+                                                             CollisionResult& result)
 {
   if(enable_statistics) num_leaf_tests++;
   const BVNode<BV>& node = model1->getBV(b1);
@@ -293,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;
-      pairs.push_back(BVHShapeCollisionPair(primitive_id));
+      result.contacts.push_back(Contact(model1, &model2, primitive_id, Contact::NONE));
     }
   }
   else
@@ -301,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;
-      pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
+      result.contacts.push_back(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
     }
   }
 
-  if(is_intersect && request.enable_cost && (request.num_max_cost_sources > cost_sources.size()))
+  if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.cost_sources.size()))
   {
     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);
-    cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+    result.cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
   }
 }
 
@@ -334,7 +294,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
-                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
   }
 
 };
@@ -356,7 +316,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
-                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
   }
 
 };
@@ -378,7 +338,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
-                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
   }
 
 };
@@ -400,7 +360,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
-                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
   }
 
 };
@@ -436,12 +396,12 @@ public:
 
     bool is_intersect = false;
 
-    if(!request.enable_contact) // only interested in collision or not
+    if(!this->request.enable_contact) // only interested in collision or not
     {
       if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
       {
         is_intersect = true;
-        pairs.push_back(BVHShapeCollisionPair(primitive_id));
+        this->result->contacts.push_back(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
       }
     }
     else
@@ -449,35 +409,29 @@ public:
       if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
       {
         is_intersect = true;
-        pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
+        this->result->contacts.push_back(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
       }
     }
 
-    if(is_intersect && request.enable_cost && (request.num_max_cost_sources > cost_sources.size()))
+    if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->cost_sources.size()))
     {
       AABB overlap_part;
       AABB shape_aabb;
       computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
       AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
-      cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+      this->result->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
     }
   }
 
   bool canStop() const
   {
-    return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= pairs.size()) &&
-      (  (!this->request.enable_cost) || (this->request.num_max_cost_sources <= cost_sources.size())  );
+    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())  );
   }
 
   Vec3f* vertices;
   Triangle* tri_indices;
 
-  CollisionRequest request;
-
-  mutable std::vector<BVHShapeCollisionPair> pairs;
-
-  mutable std::vector<CostSource> cost_sources;
-
   FCL_REAL cost_density;
 
   const NarrowPhaseSolver* nsolver;
@@ -500,7 +454,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
-                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
 
     // may need to change the order in pairs
   }
@@ -524,7 +478,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
-                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
 
     // may need to change the order in pairs
   }
@@ -549,7 +503,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
-                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
 
     // may need to change the order in pairs
   }
@@ -574,7 +528,7 @@ public:
   void leafTesting(int b1, int b2) const
   {
     details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, 
-                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
+                                                       this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
 
     // may need to change the order in pairs
   }
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 3fe70851f0f0c28a31aa5903a938d08a97df4ebe..cb29948de1adcd7b54af64fce0dc77885fba8a36 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -141,43 +141,6 @@ public:
 };
 
 
-/** \brief The collision/contact information between two primitives */
-struct BVHCollisionPair
-{
-  BVHCollisionPair() {}
-
-  BVHCollisionPair(int id1_, int id2_) : id1(id1_), id2(id2_) {}
-
-  BVHCollisionPair(int id1_, int id2_, const Vec3f& contactp, const Vec3f& n, FCL_REAL depth) : id1(id1_),
-      id2(id2_), contact_point(contactp), normal(n), penetration_depth(depth) {}
-
-  /** \brief The index of one in-collision primitive */
-  int id1;
-
-  /** \brief The index of the other in-collision primitive */
-  int id2;
-
-  /** \brief Contact points */
-  Vec3f contact_point;
-
-  /** \brief Contact normal */
-  Vec3f normal;
-
-  /** \brief Penetration depth for two triangles */
-  FCL_REAL penetration_depth;
-};
-
-/** \brief Sorting rule between two BVHCollisionPair, for testing */
-struct BVHCollisionPairComp
-{
-  bool operator()(const BVHCollisionPair& a, const BVHCollisionPair& b)
-  {
-    if(a.id1 == b.id1)
-      return a.id2 < b.id2;
-    return a.id1 < b.id1;
-  }
-};
-
 /** \brief Traversal node for collision between two meshes */
 template<typename BV>
 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
@@ -224,7 +187,7 @@ public:
       if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
       {
         is_intersect = true;
-        pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
+        this->result->contacts.push_back(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
       }
     }
     else // need compute the contact information
@@ -238,31 +201,31 @@ public:
         is_intersect = true;
         if(!this->request.exhaustive)
         {
-          if(this->request.num_max_contacts < n_contacts + pairs.size())
-            n_contacts = (this->request.num_max_contacts >= pairs.size()) ? (this->request.num_max_contacts - pairs.size()) : 0;
+          if(this->request.num_max_contacts < n_contacts + this->result->contacts.size())
+            n_contacts = (this->request.num_max_contacts >= this->result->contacts.size()) ? (this->request.num_max_contacts - this->result->contacts.size()) : 0;
         }
     
         for(unsigned int i = 0; i < n_contacts; ++i)
         {
-          pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
+          this->result->contacts.push_back(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 > cost_sources.size()))
+    if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > this->result->cost_sources.size()))
     {
       AABB overlap_part;
       AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
-      cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+      this->result->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
     }   
   }
 
   /** \brief Whether the traversal process can stop early */
   bool canStop() const
   {
-    return (pairs.size() > 0) && (!this->request.exhaustive) && (this->request.num_max_contacts <= pairs.size()) && 
-      (  (!this->request.enable_cost) || (this->request.num_max_cost_sources <= cost_sources.size())  );
+    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())  );
   }
 
   Vec3f* vertices1;
@@ -271,9 +234,6 @@ public:
   Triangle* tri_indices1;
   Triangle* tri_indices2;
 
-  mutable std::vector<BVHCollisionPair> pairs;
-  
-  mutable std::vector<CostSource> cost_sources;
   FCL_REAL cost_density;
 };
 
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index 08cc6985257e476d313997b69b2ea8cd8b748e05..fd2d082cfc9f4359ef19674c1bbc6da5c75eb919 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -47,75 +47,6 @@
 namespace fcl
 {
 
-struct OcTreeCollisionPair
-{
-  /** \brief The pointer to the octree nodes */
-  const OcTree::OcTreeNode* node1;
-  const OcTree::OcTreeNode* node2;
-
-  /** \brief contact_point */
-  Vec3f contact_point;
-  
-  /** \brief contact normal */
-  Vec3f normal;
-  
-  /** \brief penetration depth between two octree cells */
-  FCL_REAL penetration_depth;
-
-  OcTreeCollisionPair(const OcTree::OcTreeNode* node1_, const OcTree::OcTreeNode* node2_)
-    : node1(node1_), node2(node2_) {}
-  
-  OcTreeCollisionPair(const OcTree::OcTreeNode* node1_, const OcTree::OcTreeNode* node2_,
-                      const Vec3f& contact_point_, const Vec3f& normal_, FCL_REAL penetration_depth_)
-    : node1(node1_), node2(node2_), contact_point(contact_point_), normal(normal_), penetration_depth(penetration_depth_) {}
-};
-
-struct OcTreeMeshCollisionPair
-{
-  /** \brief The pointer to the octree node */
-  const OcTree::OcTreeNode* node;
-  
-  /** \brief The index of BVH primitive */ 
-  int id; 
-
-  /** \brief contact_point */
-  Vec3f contact_point;
-  
-  /** \brief contact normal */
-  Vec3f normal;
-  
-  /** \brief penetration depth between two octree cells */
-  FCL_REAL penetration_depth;
-
-  OcTreeMeshCollisionPair(const OcTree::OcTreeNode* node_, int id_)
-    : node(node_), id(id_) {}
-  
-  OcTreeMeshCollisionPair(const OcTree::OcTreeNode* node_, int id_,
-                          const Vec3f& contact_point_, const Vec3f& normal_, FCL_REAL penetration_depth_)
-    : node(node_), id(id_), contact_point(contact_point_), normal(normal_), penetration_depth(penetration_depth_) {}
-};
-
-
-struct OcTreeShapeCollisionPair
-{
-  /** \brief The pointer to the octree node */
-  const OcTree::OcTreeNode* node;
-
-  /** \brief contact_point */
-  Vec3f contact_point;
-  
-  /** \brief contact normal */
-  Vec3f normal;
-  
-  /** \brief penetration depth between two octree cells */
-  FCL_REAL penetration_depth;
-
-  OcTreeShapeCollisionPair(const OcTree::OcTreeNode* node_) : node(node_) {}
-  OcTreeShapeCollisionPair(const OcTree::OcTreeNode* node_,                          
-                           const Vec3f& contact_point_, const Vec3f& normal_, FCL_REAL penetration_depth_)
-    : node(node_), contact_point(contact_point_), normal(normal_), penetration_depth(penetration_depth_) {}
-};
-
 
 template<typename NarrowPhaseSolver>
 class OcTreeSolver
@@ -123,22 +54,32 @@ class OcTreeSolver
 private:
   const NarrowPhaseSolver* solver;
 
-  mutable CollisionRequest request;
+  mutable const CollisionRequest* crequest;
+  mutable const DistanceRequest* drequest;
+
+  mutable CollisionResult* cresult;
+  mutable DistanceResult* dresult;
 
 public:
-  OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_) {} 
+  OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_),
+                                                   crequest(NULL),
+                                                   drequest(NULL),
+                                                   cresult(NULL),
+                                                   dresult(NULL)
+  {
+  }
 
   void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
                        const SimpleTransform& tf1, const SimpleTransform& tf2,
-                       std::vector<OcTreeCollisionPair>& pairs,
-                       std::vector<CostSource>& cost_sources,
-                       const CollisionRequest& request_) const
+                       const CollisionRequest& request_,
+                       CollisionResult& result_) const
   {
-    request = request_;
+    crequest = &request_;
+    cresult = &result_;
     
     OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), 
                            tree2, tree2->getRoot(), tree2->getRootBV(), 
-                           tf1, tf2, pairs, cost_sources);
+                           tf1, tf2);
   }
 
 
@@ -157,15 +98,15 @@ public:
   template<typename BV>
   void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
                            const SimpleTransform& tf1, const SimpleTransform& tf2,
-                           std::vector<OcTreeMeshCollisionPair>& pairs,
-                           std::vector<CostSource>& cost_sources,
-                           const CollisionRequest& request_) const
+                           const CollisionRequest& request_,
+                           CollisionResult& result_) const
   {
-    request = request_;
+    crequest = &request_;
+    cresult = &result_;
 
     OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
                                tree2, 0,
-                               tf1, tf2, pairs, cost_sources);
+                               tf1, tf2);
   }
 
   template<typename BV>
@@ -185,16 +126,16 @@ public:
   template<typename BV>
   void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
                            const SimpleTransform& tf1, const SimpleTransform& tf2,
-                           std::vector<OcTreeMeshCollisionPair>& pairs,
-                           std::vector<CostSource>& cost_sources,
-                           const CollisionRequest& request_) const
+                           const CollisionRequest& request_,
+                           CollisionResult& result_) const
   
   {
-    request = request_;
+    crequest = &request_;
+    cresult = &result_;
 
     OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
                                tree1, 0,
-                               tf2, tf1, pairs, cost_sources);
+                               tf2, tf1);
   }
 
   
@@ -214,11 +155,11 @@ public:
   template<typename S>
   void OcTreeShapeIntersect(const OcTree* tree, const S& s,
                             const SimpleTransform& tf1, const SimpleTransform& tf2,
-                            std::vector<OcTreeShapeCollisionPair>& pairs,
-                            std::vector<CostSource>& cost_sources,
-                            const CollisionRequest& request_) const
+                            const CollisionRequest& request_,
+                            CollisionResult& result_) const
   {
-    request = request_;
+    crequest = &request_;
+    cresult = &result_;
 
     AABB bv2;
     computeBV<AABB>(s, SimpleTransform(), bv2);
@@ -226,18 +167,18 @@ public:
     convertBV(bv2, tf2, obb2);
     OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
                                 s, obb2,
-                                tf1, tf2, pairs, cost_sources);
+                                tf1, tf2);
     
   }
 
   template<typename S>
   void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
                             const SimpleTransform& tf1, const SimpleTransform& tf2,
-                            std::vector<OcTreeShapeCollisionPair>& pairs,
-                            std::vector<CostSource>& cost_sources,
-                            const CollisionRequest& request_) const
+                            const CollisionRequest& request_,
+                            CollisionResult& result_) const
   {
-    request = request_;
+    crequest = &request_;
+    cresult = &result_;
 
     AABB bv1;
     computeBV<AABB>(s, SimpleTransform(), bv1);
@@ -245,7 +186,7 @@ public:
     convertBV(bv1, tf1, obb1);
     OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
                                 s, obb1,
-                                tf2, tf1, pairs, cost_sources);
+                                tf2, tf1);
   }
 
   template<typename S>
@@ -330,9 +271,7 @@ private:
   template<typename S>
   bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
                                    const S& s, const OBB& obb2,
-                                   const SimpleTransform& tf1, const SimpleTransform& tf2,
-                                   std::vector<OcTreeShapeCollisionPair>& pairs,
-                                   std::vector<CostSource>& cost_sources) const
+                                   const SimpleTransform& tf1, const SimpleTransform& tf2) const
   {
     if(!root1->hasChildren())
     {
@@ -348,13 +287,13 @@ private:
 
           bool is_intersect = false;
           
-          if(!request.enable_contact)
+          if(!crequest->enable_contact)
           {
             if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
             {
               is_intersect = true;
-              if(pairs.size() < request.num_max_contacts)
-                pairs.push_back(OcTreeShapeCollisionPair(root1));
+              if(cresult->numContacts() < crequest->num_max_contacts)
+                cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE));
             }
           }
           else
@@ -366,12 +305,12 @@ private:
             if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
             {
               is_intersect = true;
-              if(pairs.size() < request.num_max_contacts)
-                pairs.push_back(OcTreeShapeCollisionPair(root1, contact, normal, depth));
+              if(cresult->numContacts() < crequest->num_max_contacts)
+                cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contact, normal, depth));
             }
           }
           
-          return ((pairs.size() >= request.num_max_contacts) && !request.exhaustive);        
+          return ((cresult->numContacts() >= crequest->num_max_contacts) && !crequest->exhaustive);        
         }
         else return false;
       }
@@ -391,7 +330,7 @@ private:
         AABB child_bv;
         computeChildBV(bv1, i, child_bv);
         
-        if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2, pairs, cost_sources))
+        if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2))
           return true;
       }
     }
@@ -486,9 +425,7 @@ private:
   template<typename BV>
   bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
                                   const BVHModel<BV>* tree2, int root2,
-                                  const SimpleTransform& tf1, const SimpleTransform& tf2,
-                                  std::vector<OcTreeMeshCollisionPair>& pairs,
-                                  std::vector<CostSource>& cost_sources) const
+                                  const SimpleTransform& tf1, const SimpleTransform& tf2) const
   {
     if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
     {
@@ -509,10 +446,10 @@ private:
           const Vec3f& p2 = tree2->vertices[tri_id[1]];
           const Vec3f& p3 = tree2->vertices[tri_id[2]];
         
-          if(!request.enable_contact)
+          if(!crequest->enable_contact)
           {
             if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL))
-              pairs.push_back(OcTreeMeshCollisionPair(root1, root2));
+              cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2));
           }
           else
           {
@@ -521,10 +458,10 @@ private:
             Vec3f normal;
 
             if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal))
-              pairs.push_back(OcTreeMeshCollisionPair(root1, root2, contact, normal, depth));
+              cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2, contact, normal, depth));
           }
 
-          return ((pairs.size() >= request.num_max_contacts) && !request.exhaustive);
+          return ((cresult->numContacts() >= crequest->num_max_contacts) && !crequest->exhaustive);
         }
         else
           return false;
@@ -549,17 +486,17 @@ private:
           AABB child_bv;
           computeChildBV(bv1, i, child_bv);
           
-          if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, pairs, cost_sources))
+          if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2))
             return true;
         }
       }
     }
     else
     {
-      if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2, pairs, cost_sources))
+      if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2))
         return true;
 
-      if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2, pairs, cost_sources))
+      if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2))
         return true;      
 
     }
@@ -649,22 +586,20 @@ private:
 
   bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
                               const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2,
-                              const SimpleTransform& tf1, const SimpleTransform& tf2,
-                              std::vector<OcTreeCollisionPair>& pairs,
-                              std::vector<CostSource>& cost_sources) const
+                              const SimpleTransform& tf1, const SimpleTransform& tf2) const
   {
     if(!root1->hasChildren() && !root2->hasChildren())
     {
       if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
       {
-        if(!request.enable_contact)
+        if(!crequest->enable_contact)
         {
           OBB obb1, obb2;
           convertBV(bv1, tf1, obb1);
           convertBV(bv2, tf2, obb2);
           
           if(obb1.overlap(obb2))
-            pairs.push_back(OcTreeCollisionPair(root1, root2));
+            cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot()));
         }
         else
         {
@@ -677,10 +612,10 @@ private:
           FCL_REAL depth;
           Vec3f normal;
           if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal))
-            pairs.push_back(OcTreeCollisionPair(root1, root2, contact, normal, depth));
+            cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contact, normal, depth));
         }
 
-        return ((pairs.size() >= request.num_max_contacts) && !request.exhaustive);       
+        return ((cresult->numContacts() >= crequest->num_max_contacts) && !crequest->exhaustive);       
       }
       else
         return false;
@@ -705,7 +640,7 @@ private:
         
           if(OcTreeIntersectRecurse(tree1, child, child_bv, 
                                     tree2, root2, bv2,
-                                    tf1, tf2, pairs, cost_sources))
+                                    tf1, tf2))
             return true;
         }
       }
@@ -722,7 +657,7 @@ private:
           
           if(OcTreeIntersectRecurse(tree1, root1, bv1,
                                     tree2, child, child_bv,
-                                    tf1, tf2, pairs, cost_sources))
+                                    tf1, tf2))
             return true;
         }
       }
@@ -740,7 +675,7 @@ template<typename NarrowPhaseSolver>
 class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
 {
 public:
-  OcTreeCollisionTraversalNode() : CollisionTraversalNodeBase()
+  OcTreeCollisionTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -755,7 +690,7 @@ public:
 
   void leafTesting(int, int) const
   {
-    otsolver->OcTreeIntersect(model1, model2, tf1, tf2, pairs, cost_sources, request);
+    otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
   }
 
   const OcTree* model1;
@@ -763,9 +698,6 @@ public:
 
   SimpleTransform tf1, tf2;
 
-  mutable std::vector<OcTreeCollisionPair> pairs;
-  mutable std::vector<CostSource> cost_sources;
-
   const OcTreeSolver<NarrowPhaseSolver>* otsolver;
 };
 
@@ -774,7 +706,7 @@ template<typename NarrowPhaseSolver>
 class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
 {
 public:
-  OcTreeDistanceTraversalNode() : DistanceTraversalNodeBase()
+  OcTreeDistanceTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -805,7 +737,7 @@ template<typename S, typename NarrowPhaseSolver>
 class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
 {
 public:
-  ShapeOcTreeCollisionTraversalNode() : CollisionTraversalNodeBase()
+  ShapeOcTreeCollisionTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -820,16 +752,13 @@ public:
 
   void leafTesting(int, int) const
   {
-    otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, pairs, cost_sources, request);
+    otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
   }
 
   const S* model1;
   const OcTree* model2;
 
   SimpleTransform tf1, tf2;
-  
-  mutable std::vector<OcTreeShapeCollisionPair> pairs;
-  mutable std::vector<CostSource> cost_sources;
 
   const OcTreeSolver<NarrowPhaseSolver>* otsolver;
 };
@@ -838,7 +767,7 @@ template<typename S, typename NarrowPhaseSolver>
 class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
 {
 public:
-  OcTreeShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
+  OcTreeShapeCollisionTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -853,17 +782,14 @@ public:
 
   void leafTesting(int, int) const
   {
-    otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, pairs, cost_sources, request);
+    otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
   }
 
   const OcTree* model1;
   const S* model2;
 
   SimpleTransform tf1, tf2;
-  
-  mutable std::vector<OcTreeShapeCollisionPair> pairs;
-  mutable std::vector<CostSource> cost_sources;
-
+ 
   const OcTreeSolver<NarrowPhaseSolver>* otsolver;  
 };
 
@@ -871,7 +797,7 @@ template<typename S, typename NarrowPhaseSolver>
 class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
 {
 public:
-  ShapeOcTreeDistanceTraversalNode() : DistanceTraversalNodeBase()
+  ShapeOcTreeDistanceTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -901,7 +827,7 @@ template<typename S, typename NarrowPhaseSolver>
 class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase
 {
 public:
-  OcTreeShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
+  OcTreeShapeDistanceTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -932,7 +858,7 @@ template<typename BV, typename NarrowPhaseSolver>
 class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
 {
 public:
-  MeshOcTreeCollisionTraversalNode() : CollisionTraversalNodeBase()
+  MeshOcTreeCollisionTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -947,17 +873,14 @@ public:
 
   void leafTesting(int, int) const
   {
-    otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, pairs, cost_sources, request);
+    otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
   }
 
   const BVHModel<BV>* model1;
   const OcTree* model2;
 
   SimpleTransform tf1, tf2;
-  
-  mutable std::vector<OcTreeMeshCollisionPair> pairs;
-  mutable std::vector<CostSource> cost_sources;
-  
+    
   const OcTreeSolver<NarrowPhaseSolver>* otsolver;
 };
 
@@ -965,7 +888,7 @@ template<typename BV, typename NarrowPhaseSolver>
 class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase
 {
 public:
-  OcTreeMeshCollisionTraversalNode() : CollisionTraversalNodeBase()
+  OcTreeMeshCollisionTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -980,17 +903,14 @@ public:
 
   void leafTesting(int, int) const
   {
-    otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, pairs, cost_sources, request);
+    otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
   }
 
   const OcTree* model1;
   const BVHModel<BV>* model2;
 
   SimpleTransform tf1, tf2;
-  
-  mutable std::vector<OcTreeMeshCollisionPair> pairs;
-  mutable std::vector<CostSource> cost_sources;
-  
+    
   const OcTreeSolver<NarrowPhaseSolver>* otsolver;
 };
 
@@ -999,7 +919,7 @@ template<typename BV, typename NarrowPhaseSolver>
 class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
 {
 public:
-  MeshOcTreeDistanceTraversalNode() : DistanceTraversalNodeBase()
+  MeshOcTreeDistanceTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
@@ -1030,7 +950,7 @@ template<typename BV, typename NarrowPhaseSolver>
 class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase
 {
 public:
-  OcTreeMeshDistanceTraversalNode() : DistanceTraversalNodeBase()
+  OcTreeMeshDistanceTraversalNode()
   {
     model1 = NULL;
     model2 = NULL;
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 76debef08d697460bd4f8a57a8961d5f0e81da71..9f8065f9e30f33e15c545768b7dbb27c24b14cbd 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -55,8 +55,6 @@ public:
     model1 = NULL;
     model2 = NULL;
 
-    is_collision = false;
-
     nsolver = NULL;
   }
 
@@ -67,10 +65,25 @@ public:
 
   void leafTesting(int, int) const
   {
+    bool is_collision = false;
     if(request.enable_contact)
-      is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal);
+    {
+      Vec3f contact_point, normal;
+      FCL_REAL penetration_depth;
+      if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal))
+      {
+        is_collision = true;
+        result->contacts.push_back(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
+      }
+    }
     else
-      is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL);
+    {
+      if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
+      {
+        is_collision = true;
+        result->contacts.push_back(Contact(model1, model2, Contact::NONE, Contact::NONE));
+      }
+    }
 
     if(is_collision && request.enable_cost)
     {
@@ -79,26 +92,13 @@ public:
       computeBV<AABB, S2>(*model2, tf2, aabb2);
       AABB overlap_part;
       aabb1.overlap(aabb2, overlap_part);
-      cost_source = CostSource(overlap_part.min_, overlap_part.max_, cost_density);
+      result->cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
     }
   }
 
   const S1* model1;
-
   const S2* model2;
 
-  mutable Vec3f normal;
-
-  mutable Vec3f contact_point;
-
-  mutable FCL_REAL penetration_depth;
-
-  CollisionRequest request;
-
-  mutable bool is_collision;
-
-  mutable CostSource cost_source;
-
   FCL_REAL cost_density;
 
   const NarrowPhaseSolver* nsolver;
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index fde4a63b612f06137b7578e38d983e504623dc35..3145339624175af33712f988fc8bbe40227594c3 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -60,8 +60,6 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
   CollisionResult local_result;
   int num_contacts = collide(o1, o2, request, local_result);
 
-  result.is_collision = (num_contacts > 0);
-
   std::vector<Contact>& out_contacts = result.contacts;
   const std::vector<Contact>& in_contacts = local_result.contacts;
   for(int i = 0; i < num_contacts; ++i)
@@ -71,7 +69,7 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
   }
 
   // set done flag
-  if( (!request.exhaustive) && (result.is_collision) && (result.contacts.size() >= request.num_max_contacts))
+  if( (!request.exhaustive) && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts))
     cdata->done = true;
 
   return cdata->done;
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 5443aacc4316cab055ad213781b0d3dec921af1c..74b9bd18df05fdbd141eb8811bfae8058db40ad6 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -46,7 +46,7 @@ namespace fcl
 {
 
 template<typename T_SH>
-static inline int OcTreeShapeContactCollection(const std::vector<OcTreeShapeCollisionPair>& pairs, const OcTree* obj1, const T_SH* obj2,
+static inline int OcTreeShapeContactCollection(const std::vector<Contact>& pairs, const OcTree* obj1, const T_SH* obj2,
                                                const CollisionRequest& request, CollisionResult& result)
 {
   int num_contacts = pairs.size();
@@ -55,16 +55,8 @@ static inline int OcTreeShapeContactCollection(const std::vector<OcTreeShapeColl
     if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
     std::vector<Contact>& contacts = result.contacts;
     contacts.resize(num_contacts);
-    if(!request.enable_contact)
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0);
-    }
-    else
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
-    }
+    for(int i = 0; i < num_contacts; ++i)
+      contacts[i] = pairs[i];
   }
   
   return num_contacts;
@@ -79,9 +71,11 @@ int ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
   const T_SH* obj1 = static_cast<const T_SH*>(o1);
   const OcTree* obj2 = static_cast<const OcTree*>(o2);
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
-  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
   collide(&node);
-  int num_contacts = OcTreeShapeContactCollection(node.pairs, obj2, obj1, request, result);
+  int num_contacts = OcTreeShapeContactCollection(local_result.contacts, obj2, obj1, request, result);
 
   return num_contacts;
 }
@@ -94,16 +88,17 @@ int OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
   OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
   const OcTree* obj1 = static_cast<const OcTree*>(o1);
   const T_SH* obj2 = static_cast<const T_SH*>(o2);
-  
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
-  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
   collide(&node);
-  int num_contacts = OcTreeShapeContactCollection(node.pairs, obj1, obj2, request, result);
+  int num_contacts = OcTreeShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
 
   return num_contacts;
 }
 
-static inline int OcTreeContactCollection(const std::vector<OcTreeCollisionPair>& pairs, const OcTree* obj1, const OcTree* obj2,
+static inline int OcTreeContactCollection(const std::vector<Contact>& pairs, const OcTree* obj1, const OcTree* obj2,
                                           const CollisionRequest& request, CollisionResult& result)
 {
   int num_contacts = pairs.size();
@@ -112,16 +107,8 @@ static inline int OcTreeContactCollection(const std::vector<OcTreeCollisionPair>
     if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
     std::vector<Contact>& contacts = result.contacts;
     contacts.resize(num_contacts);
-    if(!request.enable_contact)
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot());
-    }
-    else
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].node1 - obj1->getRoot(), pairs[i].node2 - obj2->getRoot(), pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
-    }
+    for(int i = 0; i < num_contacts; ++i)
+      contacts[i] = pairs[i];
   }
   
   return num_contacts;
@@ -135,17 +122,18 @@ int OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const
   OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
   const OcTree* obj1 = static_cast<const OcTree*>(o1);
   const OcTree* obj2 = static_cast<const OcTree*>(o2);
-
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
-  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
   collide(&node);
-  int num_contacts = OcTreeContactCollection(node.pairs, obj1, obj2, request, result);
+  int num_contacts = OcTreeContactCollection(local_result.contacts, obj1, obj2, request, result);
   return num_contacts;
 }
 
 
 template<typename T_BVH>
-static inline int OcTreeBVHContactCollection(const std::vector<OcTreeMeshCollisionPair>& pairs, const OcTree* obj1, const BVHModel<T_BVH>* obj2,
+static inline int OcTreeBVHContactCollection(const std::vector<Contact>& pairs, const OcTree* obj1, const BVHModel<T_BVH>* obj2,
                                              const CollisionRequest& request, CollisionResult& result)
 {
   int num_contacts = pairs.size();
@@ -154,16 +142,8 @@ static inline int OcTreeBVHContactCollection(const std::vector<OcTreeMeshCollisi
     if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
     std::vector<Contact>& contacts = result.contacts;
     contacts.resize(num_contacts);
-    if(!request.enable_contact)
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id);
-    }
-    else
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].node - obj1->getRoot(), pairs[i].id, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
-    }
+    for(int i = 0; i < num_contacts; ++i)
+      contacts[i] = pairs[i];
   }
   
   return num_contacts;
@@ -177,11 +157,12 @@ int OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, co
   OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
   const OcTree* obj1 = static_cast<const OcTree*>(o1);
   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
-
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
-  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
   collide(&node);
-  int num_contacts = OcTreeBVHContactCollection(node.pairs, obj1, obj2, request, result);
+  int num_contacts = OcTreeBVHContactCollection(local_result.contacts, obj1, obj2, request, result);
   return num_contacts;
 }
 
@@ -193,11 +174,12 @@ int BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, co
   MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
   const OcTree* obj2 = static_cast<const OcTree*>(o2);
-
   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
-  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request);
+
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, local_result);
   collide(&node);
-  int num_contacts = OcTreeBVHContactCollection(node.pairs, obj2, obj1, request, result);
+  int num_contacts = OcTreeBVHContactCollection(local_result.contacts, obj2, obj1, request, result);
   return num_contacts;
 }
 
@@ -210,19 +192,20 @@ int ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, c
   ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
   const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
   const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
-  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
   collide(&node);
-  if(!node.is_collision) return 0;
+  if(local_result.contacts.size() == 0) return 0;
   result.contacts.resize(1);                                                   
-  if(!request.enable_contact) result.contacts[0] = Contact(o1, o2, 0, 0);
-  else result.contacts[0] = Contact(o1, o2, 0, 0, node.contact_point, node.normal, node.penetration_depth);
+  result.contacts[0] = local_result.contacts[0];
   return 1;
 }
 
 
 
 template<typename T_BVH, typename T_SH>
-static inline int BVHShapeContactCollection(const std::vector<BVHShapeCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const T_SH* obj2,
+static inline int BVHShapeContactCollection(const std::vector<Contact>& pairs, const BVHModel<T_BVH>* obj1, const T_SH* obj2,
                                             const CollisionRequest& request, CollisionResult& result)
 {
   int num_contacts = pairs.size();
@@ -231,16 +214,8 @@ static inline int BVHShapeContactCollection(const std::vector<BVHShapeCollisionP
     if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
     std::vector<Contact>& contacts = result.contacts;
     contacts.resize(num_contacts);
-    if(!request.enable_contact)
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].id, 0);
-    }
-    else
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].id, 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
-    }
+    for(int i = 0; i < num_contacts; ++i)
+      contacts[i] = pairs[i];
   }
 
   return num_contacts;
@@ -260,10 +235,11 @@ struct BVHShapeCollider
     SimpleTransform tf1_tmp = tf1;
     const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
-    initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request);
+    CollisionResult local_result;
+    initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, local_result);
     fcl::collide(&node);
 
-    int num_contacts = BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
+    int num_contacts = BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
 
     delete obj1_tmp;
     return num_contacts;
@@ -282,10 +258,11 @@ struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
     const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
     const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
-    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+    CollisionResult local_result;
+    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
     fcl::collide(&node);
 
-    return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
+    return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
   } 
 };
 
@@ -301,10 +278,11 @@ struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
     const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
     const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
-    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+    CollisionResult local_result;
+    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
     fcl::collide(&node);
 
-    return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
+    return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
   } 
 };
 
@@ -320,10 +298,11 @@ struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
     const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
     const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
-    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+    CollisionResult local_result;
+    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
     fcl::collide(&node);
 
-    return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
+    return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
   } 
 };
 
@@ -339,41 +318,17 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
     const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
     const T_SH* obj2 = static_cast<const T_SH*>(o2);
 
-    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request);
+    CollisionResult local_result;
+    initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, local_result);
     fcl::collide(&node);
 
-    return BVHShapeContactCollection(node.pairs, obj1, obj2, request, result);
+    return BVHShapeContactCollection(local_result.contacts, obj1, obj2, request, result);
   } 
 };
 
 
 template<typename T_BVH>
-static inline int BVHContactCollection(const std::vector<BVHCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, const CollisionRequest& request, CollisionResult& result)
-{
-  int num_contacts = pairs.size();
-  if(num_contacts > 0)
-  {
-    if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
-    std::vector<Contact>& contacts = result.contacts;
-    contacts.resize(num_contacts);
-    if(!request.enable_contact)
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
-    }
-    else
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
-    }
-  }
-
-  return num_contacts;
-}
-
-// for OBB-alike contact 
-template<typename T_BVH>
-static inline int BVHContactCollection2(const std::vector<BVHCollisionPair>& pairs, const SimpleTransform& tf1, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, const CollisionRequest& request, CollisionResult& result)
+static inline int BVHContactCollection(const std::vector<Contact>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, const CollisionRequest& request, CollisionResult& result)
 {
   int num_contacts = pairs.size();
   if(num_contacts > 0)
@@ -381,27 +336,14 @@ static inline int BVHContactCollection2(const std::vector<BVHCollisionPair>& pai
     if((!request.exhaustive) && (num_contacts > request.num_max_contacts)) num_contacts = request.num_max_contacts;
     std::vector<Contact>& contacts = result.contacts;
     contacts.resize(num_contacts);
-    if(!request.enable_contact)
-    {
-      for(int i = 0; i < num_contacts; ++i)
-        contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
-    }
-    else
-    {
-      for(int i = 0; i < num_contacts; ++i)
-      {
-        Vec3f normal = tf1.getRotation() * pairs[i].normal;
-        Vec3f contact_point = tf1.transform(pairs[i].contact_point);
-        contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, contact_point, normal, pairs[i].penetration_depth);
-      }
-    }
+    for(int i = 0; i < num_contacts; ++i)
+      contacts[i] = pairs[i];
   }
 
   return num_contacts;
 }
 
 
-
 template<typename T_BVH>
 int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
 {
@@ -413,9 +355,10 @@ int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const Co
   BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
   SimpleTransform tf2_tmp = tf2;
   
-  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request);
+  CollisionResult local_result;
+  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, local_result);
   collide(&node);
-  int num_contacts = BVHContactCollection(node.pairs, obj1, obj2, request, result);
+  int num_contacts = BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
 
   delete obj1_tmp;
   delete obj2_tmp;
@@ -429,10 +372,11 @@ int BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, con
   const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
   const BVHModel<OBB>* obj2 = static_cast<const BVHModel<OBB>* >(o2);
 
-  initialize(node, *obj1, tf1, *obj2, tf2, request);
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
   collide(&node);
 
-  return BVHContactCollection2(node.pairs, tf1, obj1, obj2, request, result);
+  return BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
 }
 
 template<>
@@ -442,10 +386,11 @@ int BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1,
   const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
   const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
 
-  initialize(node, *obj1, tf1, *obj2, tf2, request);
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
   collide(&node);
 
-  return BVHContactCollection2(node.pairs, tf1, obj1, obj2, request, result);
+  return BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
 }
 
 
@@ -456,10 +401,11 @@ int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, co
   const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
   const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
 
-  initialize(node, *obj1, tf1, *obj2, tf2, request);
+  CollisionResult local_result;
+  initialize(node, *obj1, tf1, *obj2, tf2, request, local_result);
   collide(&node);
 
-  return BVHContactCollection2(node.pairs, tf1,  obj1, obj2, request, result);
+  return BVHContactCollection(local_result.contacts, obj1, obj2, request, result);
 }
 
 
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index fcac4dad3968c31e7a495268206702b4bbb35300..d4bd095196770ff09cf89abd3e2d591b9c597ba5 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -83,7 +83,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
 
   // whether the first start configuration is in collision
   MeshCollisionTraversalNodeRSS cnode;
-  if(!initialize(cnode, *model1, tf1, *model2, tf2, request))
+  if(!initialize(cnode, *model1, tf1, *model2, tf2, request, result))
     std::cout << "initialize error" << std::endl;
 
   relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), cnode.R, cnode.T);
@@ -93,7 +93,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
 
   collide(&cnode);
 
-  int b = cnode.pairs.size();
+  int b = result.contacts.size();
 
   if(b > 0)
   {
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 3c7fabd777773a14f17ef239e99b3e9125317b9b..a9d9c21188bc7de8af2b64f33c8fe9c93785b253 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -47,7 +47,8 @@ template<typename BV, typename OrientedNode>
 static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
                                                   const BVHModel<BV>& model1, const SimpleTransform& tf1,
                                                   const BVHModel<BV>& model2, const SimpleTransform& tf2,
-                                                  const CollisionRequest& request)
+                                                  const CollisionRequest& request,
+                                                  CollisionResult& result)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
     return false;
@@ -64,6 +65,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
   node.tf2 = tf2;
 
   node.request = request;
+  node.result = &result;
   node.cost_density = model1.cost_density * model2.cost_density;
 
   relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
@@ -77,35 +79,39 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
 bool initialize(MeshCollisionTraversalNodeOBB& node,
                 const BVHModel<OBB>& model1, const SimpleTransform& tf1,
                 const BVHModel<OBB>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
+  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
 }
 
 
 bool initialize(MeshCollisionTraversalNodeRSS& node,
                 const BVHModel<RSS>& model1, const SimpleTransform& tf1,
                 const BVHModel<RSS>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
+  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
 }
 
 
 bool initialize(MeshCollisionTraversalNodekIOS& node,
                 const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
                 const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
+  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
 }
 
 bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
                 const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
                 const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
-                const CollisionRequest& request)
+                const CollisionRequest& request,
+                CollisionResult& result)
 {
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request);
+  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result);
 }
 
 
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index f52064548e6f18698684b6c7c4e522db3664f485..e9a667fabedcf7d5b62d1b0a29986c7f039ebb16 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -51,10 +51,9 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
                                                         const SimpleTransform& tf1, const SimpleTransform& tf2,
                                                         bool enable_statistics,
                                                         FCL_REAL cost_density,
-                                                        const CollisionRequest& request,
                                                         int& num_leaf_tests,
-                                                        std::vector<BVHCollisionPair>& pairs,
-                                                        std::vector<CostSource>& cost_sources)
+                                                        const CollisionRequest& request,
+                                                        CollisionResult& result)
 {
   if(enable_statistics) num_leaf_tests++;
 
@@ -86,7 +85,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
     if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
     {
       is_intersect = true;
-      pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
+      result.contacts.push_back(Contact(model1, model2, primitive_id1, primitive_id2));
     }
   }
   else // need compute the contact information
@@ -101,22 +100,22 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
       is_intersect = true;
       if(!request.exhaustive)
       {
-        if(request.num_max_contacts < pairs.size() + n_contacts)
-          n_contacts = (request.num_max_contacts > pairs.size()) ? (request.num_max_contacts - pairs.size()) : 0;
+        if(request.num_max_contacts < result.contacts.size() + n_contacts)
+          n_contacts = (request.num_max_contacts > result.contacts.size()) ? (request.num_max_contacts - result.contacts.size()) : 0;
       }
 
       for(unsigned int i = 0; i < n_contacts; ++i)
       {
-        pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
+        result.contacts.push_back(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(normal), penetration));
       }
     }
   }
   
-  if(is_intersect && request.enable_cost && (request.num_max_cost_sources > cost_sources.size()))
+  if(is_intersect && request.enable_cost && (request.num_max_cost_sources > result.cost_sources.size()))
   {
     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);
-    cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
+    result.cost_sources.push_back(CostSource(overlap_part.min_, overlap_part.max_, cost_density));
   }
 }
 
@@ -194,10 +193,9 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
                                                 tri_indices1, tri_indices2, 
                                                 R, T, 
                                                 tf1, tf2,
-                                                enable_statistics, cost_density, request,
+                                                enable_statistics, cost_density,
                                                 num_leaf_tests,
-                                                pairs,
-                                                cost_sources);
+                                                request, *result);
 }
 
 
@@ -213,10 +211,9 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f&
                                                 tri_indices1, tri_indices2, 
                                                 R, T, 
                                                 tf1, tf2,
-                                                enable_statistics, cost_density, request,
+                                                enable_statistics, cost_density,
                                                 num_leaf_tests,
-                                                pairs,
-                                                cost_sources);
+                                                request, *result);
 }
 
 
@@ -238,10 +235,9 @@ void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
                                                 tri_indices1, tri_indices2, 
                                                 R, T, 
                                                 tf1, tf2,
-                                                enable_statistics, cost_density, request,
+                                                enable_statistics, cost_density,
                                                 num_leaf_tests,
-                                                pairs,
-                                                cost_sources);
+                                                request, *result);
 }
 
 
@@ -264,10 +260,9 @@ void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
                                                 tri_indices1, tri_indices2, 
                                                 R, T, 
                                                 tf1, tf2,
-                                                enable_statistics, cost_density, request,
+                                                enable_statistics, cost_density,
                                                 num_leaf_tests,
-                                                pairs, 
-                                                cost_sources);
+                                                request, *result);
 }
 
 
@@ -289,10 +284,9 @@ void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
                                                 tri_indices1, tri_indices2, 
                                                 R, T, 
                                                 tf1, tf2,
-                                                enable_statistics, cost_density, request,
+                                                enable_statistics, cost_density,
                                                 num_leaf_tests,
-                                                pairs,
-                                                cost_sources);
+                                                request,*result);
 }