Commit c90f59ef authored by jpan's avatar jpan
Browse files

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@140 253336fb-580f-4252-a368-f3cef5a2a82b
parent e292554c
......@@ -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;
}
};
......
......@@ -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
......
......@@ -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;
};
......
......@@ -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));