From 8fdbc59eee6b26674dae67a27f9f74f596753af1 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sat, 1 Dec 2018 16:18:18 +0100
Subject: [PATCH] Simplify code: Remove computation of cost.
---
include/hpp/fcl/collision_object.h | 43 +---
.../fcl/traversal/traversal_node_bvh_shape.h | 187 +++++++++---------
.../hpp/fcl/traversal/traversal_node_bvhs.h | 64 +++---
.../hpp/fcl/traversal/traversal_node_octree.h | 4 +-
.../hpp/fcl/traversal/traversal_node_setup.h | 13 +-
.../hpp/fcl/traversal/traversal_node_shapes.h | 39 ++--
src/traversal/traversal_node_bvhs.cpp | 95 +++++----
src/traversal/traversal_node_setup.cpp | 2 -
8 files changed, 190 insertions(+), 257 deletions(-)
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index 91d32f1a..ee24a0ae 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -58,9 +58,7 @@ enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP
class CollisionGeometry
{
public:
- CollisionGeometry() : cost_density(1),
- threshold_occupied(1),
- threshold_free(0)
+ CollisionGeometry()
{
}
@@ -88,13 +86,13 @@ public:
}
/// @brief whether the object is completely occupied
- inline bool isOccupied() const { return cost_density >= threshold_occupied; }
+ inline bool isOccupied() const { return true; }
/// @brief whether the object is completely free
- inline bool isFree() const { return cost_density <= threshold_free; }
+ inline bool isFree() const { return false; }
/// @brief whether the object has some uncertainty
- inline bool isUncertain() const { return !isOccupied() && !isFree(); }
+ inline bool isUncertain() const { return false; }
/// @brief AABB center in local coordinate
Vec3f aabb_center;
@@ -108,9 +106,6 @@ public:
/// @brief pointer to user defined data specific to this object
void *user_data;
- /// @brief collision cost for unit volume
- FCL_REAL cost_density;
-
/// @brief threshold for occupied ( >= is occupied)
FCL_REAL threshold_occupied;
@@ -315,36 +310,6 @@ public:
return cgeom;
}
- /// @brief get object's cost density
- FCL_REAL getCostDensity() const
- {
- return cgeom->cost_density;
- }
-
- /// @brief set object's cost density
- void setCostDensity(FCL_REAL c)
- {
- cgeom->cost_density = c;
- }
-
- /// @brief whether the object is completely occupied
- inline bool isOccupied() const
- {
- return cgeom->isOccupied();
- }
-
- /// @brief whether the object is completely free
- inline bool isFree() const
- {
- return cgeom->isFree();
- }
-
- /// @brief whether the object is uncertain
- inline bool isUncertain() const
- {
- return cgeom->isUncertain();
- }
-
protected:
boost::shared_ptr<CollisionGeometry> cgeom;
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index 73e6a9d2..d72268b1 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
@@ -206,31 +206,33 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- if(this->model1->isOccupied() && this->model2->isOccupied())
- {
- bool is_intersect = false;
+ bool is_intersect = false;
- if(!this->request.enable_contact)
+ if(!this->request.enable_contact)
+ {
+ if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3,
+ NULL, NULL, NULL))
{
- if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
- {
- is_intersect = true;
- if(this->request.num_max_contacts > this->result->numContacts())
- this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE));
- }
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2,
+ primitive_id, Contact::NONE));
}
- else
+ }
+ else
+ {
+ FCL_REAL penetration;
+ Vec3f normal;
+ Vec3f contactp;
+
+ if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3,
+ &contactp, &penetration, &normal))
{
- FCL_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
-
- if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
- {
- is_intersect = true;
- if(this->request.num_max_contacts > this->result->numContacts())
- this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
- }
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2,
+ primitive_id, Contact::NONE,
+ contactp, -normal, penetration));
}
}
}
@@ -244,8 +246,6 @@ public:
Vec3f* vertices;
Triangle* tri_indices;
- FCL_REAL cost_density;
-
const NarrowPhaseSolver* nsolver;
};
@@ -257,7 +257,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting
(int b1, int b2, const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,
const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
- bool enable_statistics, FCL_REAL cost_density, int& num_leaf_tests,
+ bool enable_statistics, int& num_leaf_tests,
const CollisionRequest& request, CollisionResult& result,
FCL_REAL& sqrDistLowerBound)
{
@@ -272,49 +272,48 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- if(model1->isOccupied() && model2.isOccupied())
- {
- bool is_intersect = false;
+ bool is_intersect = false;
- if(!request.enable_contact) // only interested in collision or not
- {
- if (request.enable_distance_lower_bound) {
- FCL_REAL dist;
- if (nsolver->shapeTriangleDistance (model2, tf2, p1, p2, p3, tf1,
- &dist, 0x0, 0x0)) {
- sqrDistLowerBound = dist * dist;
- } else {
- // collision
- is_intersect = true;
- sqrDistLowerBound = 0;
- if(request.num_max_contacts > result.numContacts())
- result.addContact(Contact(model1, &model2, primitive_id,
- Contact::NONE));
- }
+ if(!request.enable_contact) // only interested in collision or not
+ {
+ if (request.enable_distance_lower_bound) {
+ FCL_REAL dist;
+ if (nsolver->shapeTriangleDistance (model2, tf2, p1, p2, p3, tf1,
+ &dist, 0x0, 0x0)) {
+ sqrDistLowerBound = dist * dist;
} else {
- if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1,
- NULL, NULL, NULL)) {
- is_intersect = true;
- if(request.num_max_contacts > result.numContacts())
- result.addContact(Contact(model1, &model2, primitive_id,
- Contact::NONE));
- }
+ // collision
+ is_intersect = true;
+ sqrDistLowerBound = 0;
+ if(request.num_max_contacts > result.numContacts())
+ result.addContact(Contact(model1, &model2, primitive_id,
+ Contact::NONE));
}
- }
- else
- {
- FCL_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
-
- if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
- {
+ } else {
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1,
+ NULL, NULL, NULL)) {
is_intersect = true;
if(request.num_max_contacts > result.numContacts())
- result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration));
+ result.addContact(Contact(model1, &model2, primitive_id,
+ Contact::NONE));
}
}
}
+ else
+ {
+ FCL_REAL penetration;
+ Vec3f normal;
+ Vec3f contactp;
+
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp,
+ &penetration, &normal))
+ {
+ is_intersect = true;
+ if(request.num_max_contacts > result.numContacts())
+ result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE,
+ contactp, -normal, penetration));
+ }
+ }
}
}
@@ -344,8 +343,7 @@ public:
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->num_leaf_tests, this->request,
- *(this->result), sqrDistLowerBound);
+ this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
}
};
@@ -371,8 +369,7 @@ public:
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->num_leaf_tests, this->request,
- *(this->result), sqrDistLowerBound);
+ this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
}
};
@@ -398,8 +395,7 @@ public:
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->num_leaf_tests, this->request,
- *(this->result), sqrDistLowerBound);
+ this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
}
};
@@ -434,8 +430,7 @@ public:
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->num_leaf_tests, this->request,
- *(this->result), sqrDistLowerBound);
+ this->num_leaf_tests, this->request, *(this->result), sqrDistLowerBound);
}
};
@@ -468,33 +463,34 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- if(this->model1->isOccupied() && this->model2->isOccupied())
- {
- bool is_intersect = false;
+ bool is_intersect = false;
- if(!this->request.enable_contact)
+ if(!this->request.enable_contact)
+ {
+ if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3,
+ NULL, NULL, NULL))
{
- if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
- {
- is_intersect = true;
- if(this->request.num_max_contacts > this->result->numContacts())
- this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id));
- }
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2,
+ Contact::NONE, primitive_id));
}
- else
+ }
+ else
+ {
+ FCL_REAL penetration;
+ Vec3f normal;
+ Vec3f contactp;
+
+ if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3,
+ &contactp, &penetration, &normal))
{
- FCL_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
-
- if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
- {
- is_intersect = true;
- if(this->request.num_max_contacts > this->result->numContacts())
- this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration));
- }
+ is_intersect = true;
+ if(this->request.num_max_contacts > this->result->numContacts())
+ this->result->addContact(Contact(this->model1, this->model2,
+ Contact::NONE, primitive_id,
+ contactp, normal, penetration));
}
-
}
}
@@ -507,8 +503,6 @@ public:
Vec3f* vertices;
Triangle* tri_indices;
- FCL_REAL cost_density;
-
const NarrowPhaseSolver* nsolver;
};
@@ -532,8 +526,7 @@ public:
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->num_leaf_tests, this->request,
- *(this->request), sqrDistLowerBound);
+ this->num_leaf_tests, this->request,*(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
@@ -559,8 +552,7 @@ public:
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->num_leaf_tests, this->request,
- *(this->request));
+ this->num_leaf_tests, this->request, *(this->request));
// may need to change the order in pairs
}
@@ -587,8 +579,7 @@ public:
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->num_leaf_tests, this->request,
- *(this->request), sqrDistLowerBound);
+ this->num_leaf_tests, this->request,*(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
@@ -623,7 +614,7 @@ public:
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->num_leaf_tests,
+ this->enable_statistics, this->num_leaf_tests,
this->request, *(this->request), sqrDistLowerBound);
// may need to change the order in pairs
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 2aff7688..f10424dc 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -187,44 +187,44 @@ public:
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
- if(this->model1->isOccupied() && this->model2->isOccupied())
- {
- bool is_intersect = false;
+ bool is_intersect = false;
- if(!this->request.enable_contact) // only interested in collision or not
+ if(!this->request.enable_contact) // only interested in collision or not
+ {
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
{
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
- {
- is_intersect = true;
- if(this->result->numContacts() < this->request.num_max_contacts)
- this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
- }
+ is_intersect = true;
+ if(this->result->numContacts() < this->request.num_max_contacts)
+ this->result->addContact(Contact(this->model1, this->model2,
+ primitive_id1, primitive_id2));
}
- else // need compute the contact information
+ }
+ else // need compute the contact information
+ {
+ FCL_REAL penetration;
+ Vec3f normal;
+ unsigned int n_contacts;
+ Vec3f contacts[2];
+
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, contacts,
+ &n_contacts, &penetration, &normal))
{
- FCL_REAL penetration;
- Vec3f normal;
- unsigned int n_contacts;
- Vec3f contacts[2];
-
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- contacts,
- &n_contacts,
- &penetration,
- &normal))
- {
- is_intersect = true;
-
- if(this->request.num_max_contacts < n_contacts + this->result->numContacts())
- n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0;
+ is_intersect = true;
+
+ if(this->request.num_max_contacts < n_contacts +
+ this->result->numContacts())
+ n_contacts =
+ (this->request.num_max_contacts >= this->result->numContacts()) ?
+ (this->request.num_max_contacts - this->result->numContacts()) : 0;
- for(unsigned int i = 0; i < n_contacts; ++i)
- {
- this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration));
- }
+ for(unsigned int i = 0; i < n_contacts; ++i)
+ {
+ this->result->addContact(Contact(this->model1, this->model2,
+ primitive_id1, primitive_id2,
+ contacts[i], normal, penetration));
}
}
- }
+ }
}
/// @brief Whether the traversal process can stop early
@@ -238,8 +238,6 @@ public:
Triangle* tri_indices1;
Triangle* tri_indices2;
-
- FCL_REAL cost_density;
};
diff --git a/include/hpp/fcl/traversal/traversal_node_octree.h b/include/hpp/fcl/traversal/traversal_node_octree.h
index d16d302a..ec9710e6 100644
--- a/include/hpp/fcl/traversal/traversal_node_octree.h
+++ b/include/hpp/fcl/traversal/traversal_node_octree.h
@@ -357,7 +357,7 @@ private:
/// stop when 1) bounding boxes of two objects not overlap; OR
/// 2) at least of one the nodes is free; OR
/// 2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required
- if(tree1->isNodeFree(root1) || s.isFree()) return false;
+ if(tree1->isNodeFree(root1)) return false;
else if((tree1->isNodeUncertain(root1) || s.isUncertain())) return false;
else
{
@@ -571,7 +571,7 @@ private:
/// stop when 1) bounding boxes of two objects not overlap; OR
/// 2) at least one of the nodes is free; OR
/// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required
- if(tree1->isNodeFree(root1) || tree2->isFree()) return false;
+ if(tree1->isNodeFree(root1)) return false;
else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
return false;
else
diff --git a/include/hpp/fcl/traversal/traversal_node_setup.h b/include/hpp/fcl/traversal/traversal_node_setup.h
index 964782d6..7bc9597b 100644
--- a/include/hpp/fcl/traversal/traversal_node_setup.h
+++ b/include/hpp/fcl/traversal/traversal_node_setup.h
@@ -284,6 +284,7 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const NarrowPhaseSolver* nsolver,
CollisionResult& result)
{
+ node.model1 = &shape1;
node.tf1 = tf1;
node.model2 = &shape2;
node.tf2 = tf2;
@@ -291,8 +292,6 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
node.result = &result;
- node.cost_density = shape1.cost_density * shape2.cost_density;
-
return true;
}
@@ -338,8 +337,6 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.result = &result;
- node.cost_density = model1.cost_density * model2.cost_density;
-
return true;
}
@@ -386,8 +383,6 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.result = &result;
- node.cost_density = model1.cost_density * model2.cost_density;
-
return true;
}
@@ -418,8 +413,6 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
node.result = &result;
- node.cost_density = model1.cost_density * model2.cost_density;
-
return true;
}
@@ -499,8 +492,6 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
node.result = &result;
- node.cost_density = model1.cost_density * model2.cost_density;
-
return true;
}
}
@@ -611,8 +602,6 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
node.result = &result;
- node.cost_density = model1.cost_density * model2.cost_density;
-
return true;
}
diff --git a/include/hpp/fcl/traversal/traversal_node_shapes.h b/include/hpp/fcl/traversal/traversal_node_shapes.h
index c684b556..278eff54 100644
--- a/include/hpp/fcl/traversal/traversal_node_shapes.h
+++ b/include/hpp/fcl/traversal/traversal_node_shapes.h
@@ -79,28 +79,29 @@ public:
/// @brief Intersection testing between leaves (two shapes)
void leafTesting(int, int, FCL_REAL&) const
{
- if(model1->isOccupied() && model2->isOccupied())
+ bool is_collision = false;
+ if(request.enable_contact)
{
- bool is_collision = false;
- if(request.enable_contact)
+ Vec3f contact_point, normal;
+ FCL_REAL penetration_depth;
+ if(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;
- if(request.num_max_contacts > result->numContacts())
- result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point, normal, penetration_depth));
- }
+ is_collision = true;
+ if(request.num_max_contacts > result->numContacts())
+ result->addContact(Contact(model1, model2, Contact::NONE,
+ Contact::NONE, contact_point,
+ normal, penetration_depth));
}
- else
+ }
+ else
+ {
+ if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
{
- if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL))
- {
- is_collision = true;
- if(request.num_max_contacts > result->numContacts())
- result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE));
- }
+ is_collision = true;
+ if(request.num_max_contacts > result->numContacts())
+ result->addContact(Contact(model1, model2, Contact::NONE,
+ Contact::NONE));
}
}
}
@@ -108,8 +109,6 @@ public:
const S1* model1;
const S2* model2;
- FCL_REAL cost_density;
-
const NarrowPhaseSolver* nsolver;
};
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index e17f5392..c66cd6a4 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -49,7 +49,7 @@ static inline void meshCollisionOrientedNodeLeafTesting
Vec3f* vertices1, Vec3f* vertices2, Triangle* tri_indices1,
Triangle* tri_indices2, const Matrix3f& R, const Vec3f& T,
const Transform3f& tf1, const Transform3f& tf2, bool enable_statistics,
- FCL_REAL cost_density, int& num_leaf_tests, const CollisionRequest& request,
+ int& num_leaf_tests, const CollisionRequest& request,
CollisionResult& result, FCL_REAL& sqrDistLowerBound)
{
if(enable_statistics) num_leaf_tests++;
@@ -70,61 +70,54 @@ static inline void meshCollisionOrientedNodeLeafTesting
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
- if(model1->isOccupied() && model2->isOccupied())
- {
- bool is_intersect = false;
+ bool is_intersect = false;
- if(!request.enable_contact) // only interested in collision or not
- {
- if (request.enable_distance_lower_bound) {
- Vec3f P, Q;
- sqrDistLowerBound = TriangleDistance::sqrTriDistance
- (p1, p2, p3, q1, q2, q3, R, T, P, Q);
- if (sqrDistLowerBound == 0) {
- is_intersect = true;
- if(result.numContacts() < request.num_max_contacts)
- result.addContact(Contact(model1, model2, primitive_id1,
- primitive_id2));
- }
- } else {
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) {
- is_intersect = true;
- if(result.numContacts() < request.num_max_contacts)
- result.addContact(Contact(model1, model2, primitive_id1,
- primitive_id2));
- }
+ if(!request.enable_contact) // only interested in collision or not
+ {
+ if (request.enable_distance_lower_bound) {
+ Vec3f P, Q;
+ sqrDistLowerBound = TriangleDistance::sqrTriDistance
+ (p1, p2, p3, q1, q2, q3, R, T, P, Q);
+ if (sqrDistLowerBound == 0) {
+ is_intersect = true;
+ if(result.numContacts() < request.num_max_contacts)
+ result.addContact(Contact(model1, model2, primitive_id1,
+ primitive_id2));
}
- }
- else // need compute the contact information
- {
- FCL_REAL penetration;
- Vec3f normal;
- unsigned int n_contacts;
- Vec3f contacts[2];
-
- if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
- R, T,
- contacts,
- &n_contacts,
- &penetration,
- &normal))
- {
+ } else {
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) {
is_intersect = true;
-
- if(request.num_max_contacts < result.numContacts() + n_contacts)
- n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0;
-
- for(unsigned int i = 0; i < n_contacts; ++i)
- {
- result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation()* normal, penetration));
- }
+ if(result.numContacts() < request.num_max_contacts)
+ result.addContact(Contact(model1, model2, primitive_id1,
+ primitive_id2));
}
}
}
-}
+ else // need compute the contact information
+ {
+ FCL_REAL penetration;
+ Vec3f normal;
+ unsigned int n_contacts;
+ Vec3f contacts[2];
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T, contacts,
+ &n_contacts, &penetration, &normal))
+ {
+ is_intersect = true;
+ if(request.num_max_contacts < result.numContacts() + n_contacts)
+ n_contacts = (request.num_max_contacts > result.numContacts()) ?
+ (request.num_max_contacts - result.numContacts()) : 0;
+ for(unsigned int i = 0; i < n_contacts; ++i)
+ {
+ result.addContact(Contact(model1, model2, primitive_id1, primitive_id2,
+ tf1.transform(contacts[i]),
+ tf1.getQuatRotation()* normal, penetration));
+ }
+ }
+ }
+}
template<typename BV>
static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
@@ -189,7 +182,7 @@ void MeshCollisionTraversalNodeOBB::leafTesting
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2, R, T, tf1, tf2,
- enable_statistics, cost_density, num_leaf_tests, request, *result,
+ enable_statistics, num_leaf_tests, request, *result,
sqrDistLowerBound);
}
@@ -212,7 +205,7 @@ void MeshCollisionTraversalNodeRSS::leafTesting
{
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests,
+ R, T, tf1, tf2, enable_statistics, num_leaf_tests,
request, *result, sqrDistLowerBound);
}
@@ -237,7 +230,7 @@ void MeshCollisionTraversalNodekIOS::leafTesting
{
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests,
+ R, T, tf1, tf2, enable_statistics, num_leaf_tests,
request, *result, sqrDistLowerBound);
}
@@ -269,7 +262,7 @@ void MeshCollisionTraversalNodeOBBRSS::leafTesting
{
details::meshCollisionOrientedNodeLeafTesting
(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests,
+ R, T, tf1, tf2, enable_statistics, num_leaf_tests,
request,*result, sqrDistLowerBound);
}
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 8e417309..ee7c5856 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -66,8 +66,6 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
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);
return true;
--
GitLab