From e292554cc0ca4bf4a0541694d3d7b5bcffedb6fd Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Wed, 25 Jul 2012 06:38:05 +0000
Subject: [PATCH] add cost for exact collision
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@139 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/BV/AABB.h | 7 +
trunk/fcl/include/fcl/collision_data.h | 10 +-
trunk/fcl/include/fcl/collision_object.h | 30 ++-
trunk/fcl/include/fcl/intersect.h | 11 +-
trunk/fcl/include/fcl/motion.h | 4 +-
.../fcl/include/fcl/narrowphase/gjk_libccd.h | 2 +-
.../fcl/include/fcl/narrowphase/narrowphase.h | 56 +++--
trunk/fcl/include/fcl/simple_setup.h | 18 +-
trunk/fcl/include/fcl/transform.h | 60 +++--
trunk/fcl/include/fcl/traversal_node_base.h | 19 +-
.../include/fcl/traversal_node_bvh_shape.h | 206 +++++++++---------
trunk/fcl/include/fcl/traversal_node_bvhs.h | 33 ++-
trunk/fcl/include/fcl/traversal_node_octree.h | 77 ++++---
trunk/fcl/include/fcl/traversal_node_shapes.h | 15 ++
trunk/fcl/include/fcl/vec_3f.h | 5 +
trunk/fcl/src/intersect.cpp | 16 ++
trunk/fcl/src/narrowphase/gjk_libccd.cpp | 6 +-
trunk/fcl/src/narrowphase/narrowphase.cpp | 24 +-
trunk/fcl/src/simple_setup.cpp | 1 +
trunk/fcl/src/transform.cpp | 52 ++++-
trunk/fcl/src/traversal_node_bvhs.cpp | 67 ++++--
trunk/fcl/src/traversal_recurse.cpp | 2 +-
22 files changed, 456 insertions(+), 265 deletions(-)
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index 4370118f..68ccf5a3 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -75,6 +75,13 @@ public:
max_ = core.max_ + delta;
}
+ /** \brief Constructor creating an AABB with three points */
+ AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c)
+ {
+ min_ = min(min(a, b), c);
+ max_ = max(max(a, b), c);
+ }
+
/** \brief Check whether two AABB are overlap */
inline bool overlap(const AABB& other) const
{
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index cb25581c..bfa53f58 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -53,7 +53,15 @@ struct CostSource
{
Vec3f aabb_min;
Vec3f aabb_max;
- FCL_REAL cost; // density
+ FCL_REAL cost_density;
+
+ CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_),
+ aabb_max(aabb_max_),
+ cost_density(cost_density_)
+ {
+ }
+
+ CostSource() {}
};
struct CollisionRequest
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 9ce444e6..9965b95f 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -71,17 +71,20 @@ public:
user_data = data;
}
- /** AABB center in local coordinate */
+ /// AABB center in local coordinate
Vec3f aabb_center;
- /** AABB radius */
+ /// AABB radius
FCL_REAL aabb_radius;
- /** AABB in local coordinate, used for tight AABB when only translation transform */
+ /// AABB in local coordinate, used for tight AABB when only translation transform
AABB aabb_local;
- /** pointer to user defined data specific to this object */
+ /// pointer to user defined data specific to this object
void *user_data;
+
+ /// collision cost for unit volume
+ FCL_REAL cost_density;
};
class CollisionObject
@@ -219,17 +222,30 @@ public:
return cgeom.get();
}
+ FCL_REAL getCostDensity() const
+ {
+ if(cgeom)
+ return cgeom->cost_density;
+ else
+ return 0;
+ }
+
+ void setCostDensity(FCL_REAL c)
+ {
+ if(cgeom)
+ cgeom->cost_density = c;
+ }
+
protected:
- // const CollisionGeometry* cgeom;
boost::shared_ptr<CollisionGeometry> cgeom;
SimpleTransform t;
- /** AABB in global coordinate */
+ /// AABB in global coordinate
mutable AABB aabb;
- /** pointer to user defined data specific to this object */
+ /// pointer to user defined data specific to this object
void *user_data;
};
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index b2ae42eb..67c51778 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -37,7 +37,7 @@
#ifndef FCL_INTERSECT_H
#define FCL_INTERSECT_H
-#include "fcl/vec_3f.h"
+#include "fcl/transform.h"
#include "fcl/BVH_internal.h"
#include "fcl/primitive.h"
@@ -143,6 +143,15 @@ public:
FCL_REAL* penetration_depth = NULL,
Vec3f* normal = NULL);
+ static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
+ const SimpleTransform& tf,
+ Vec3f* contact_points = NULL,
+ unsigned int* num_contact_points = NULL,
+ FCL_REAL* penetration_depth = NULL,
+ Vec3f* normal = NULL);
+
+
#if USE_SVMLIGHT
static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index 357e5294..0566a0a6 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -409,7 +409,7 @@ public:
protected:
void computeScrewParameter()
{
- SimpleQuaternion deltaq = tf2.getQuatRotation() * tf1.getQuatRotation().inverse();
+ SimpleQuaternion deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
deltaq.toAxisAngle(axis, angular_vel);
if(angular_vel < 0)
{
@@ -601,7 +601,7 @@ protected:
void computeVelocity()
{
linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
- SimpleQuaternion deltaq = tf2.getQuatRotation() * tf1.getQuatRotation().inverse();
+ SimpleQuaternion deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
deltaq.toAxisAngle(angular_axis, angular_vel);
if(angular_vel < 0)
{
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
index dcd6dea6..793a16d3 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
@@ -148,7 +148,7 @@ GJKCenterFunction triGetCenterFunction();
void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3);
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T);
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf);
void triDeleteGJKObject(void* o);
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index 7582b0c6..e9c2f81c 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -88,12 +88,12 @@ struct GJKSolver_libccd
/** \brief intersection checking between one shape and a triangle with transformation */
template<typename S>
- bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2);
bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
@@ -150,12 +150,12 @@ struct GJKSolver_libccd
/** \brief distance computation between one shape and a triangle with transformation */
template<typename S>
- bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ bool shapeTriangleDistance(const S& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
FCL_REAL* dist) const
{
- void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2);
bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
o2, details::triGetSupportFunction(),
@@ -199,8 +199,8 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTrans
/** \brief Fast implementation for sphere-triangle collision */
template<>
-bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-sphere distance */
template<>
@@ -216,8 +216,8 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Simp
/** \brief Fast implementation for sphere-triangle distance */
template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
FCL_REAL* dist) const;
/** \brief Fast implementation for box-box collision */
@@ -283,7 +283,7 @@ struct GJKSolver_indep
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
shape.toshape1 = tf.getRotation();
- shape.toshape0 = tf.inverse();
+ shape.toshape0 = inverse(tf);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -316,18 +316,17 @@ struct GJKSolver_indep
}
template<typename S>
- bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Triangle2 tri(P1, P2, P3);
- SimpleTransform tf2(R, T);
Vec3f guess(1, 0, 0);
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf.getRotation());
- shape.toshape0 = tf.inverseTimes(tf2);
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -346,7 +345,7 @@ struct GJKSolver_indep
}
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = -epa.normal;
- if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
+ if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
else return false;
@@ -408,7 +407,7 @@ struct GJKSolver_indep
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
shape.toshape1 = tf.getRotation();
- shape.toshape0 = tf.inverse();
+ shape.toshape0 = inverse(tf);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -433,18 +432,17 @@ struct GJKSolver_indep
}
template<typename S>
- bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ bool shapeTriangleDistance(const S& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
FCL_REAL* distance) const
{
Triangle2 tri(P1, P2, P3);
- SimpleTransform tf2(R, T);
Vec3f guess(1, 0, 0);
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf.getRotation());
- shape.toshape0 = tf.inverseTimes(tf2);
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
@@ -500,8 +498,8 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransf
/** \brief Fast implementation for sphere-triangle collision */
template<>
-bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-sphere distance */
template<>
@@ -517,8 +515,8 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Simpl
/** \brief Fast implementation for sphere-triangle distance */
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
FCL_REAL* dist) const;
/** \brief Fast implementation for box-box collision */
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index acceed50..20c70115 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -76,7 +76,7 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
const DistanceRequest& request)
{
node.request = request;
-
+
node.model1 = &model1;
node.model2 = &model2;
@@ -266,6 +266,9 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
node.tf2 = tf2;
node.nsolver = nsolver;
node.request = request;
+
+ node.cost_density = shape1.cost_density * shape2.cost_density;
+
return true;
}
@@ -309,6 +312,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
node.request = request;
+ node.cost_density = model1.cost_density * model2.cost_density;
return true;
}
@@ -354,6 +358,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
node.request = request;
+ node.cost_density = model1.cost_density * model2.cost_density;
return true;
}
@@ -383,9 +388,7 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
node.request = request;
-
- node.R = tf1.getRotation();
- node.T = tf1.getTranslation();
+ node.cost_density = model1.cost_density * model2.cost_density;
return true;
}
@@ -463,9 +466,7 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
node.request = request;
-
- node.R = tf2.getRotation();
- node.T = tf2.getTranslation();
+ node.cost_density = model1.cost_density * model2.cost_density;
return true;
}
@@ -576,6 +577,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
node.tri_indices2 = model2.tri_indices;
node.request = request;
+ node.cost_density = model1.cost_density * model2.cost_density;
return true;
}
@@ -999,8 +1001,6 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
- node.R = tf1.getRotation();
- node.T = tf1.getTranslation();
return true;
}
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index cdaff8d4..b483b77c 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -59,10 +59,10 @@ public:
SimpleQuaternion(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
{
- data[0] = a; // w
- data[1] = b; // x
- data[2] = c; // y
- data[3] = d; // z
+ data[0] = a;
+ data[1] = b;
+ data[2] = c;
+ data[3] = d;
}
bool isIdentity() const
@@ -111,10 +111,10 @@ public:
const SimpleQuaternion& operator *= (FCL_REAL t);
/** \brief conjugate */
- SimpleQuaternion conj() const;
+ SimpleQuaternion& conj();
/** \brief inverse */
- SimpleQuaternion inverse() const;
+ SimpleQuaternion& inverse();
/** \brief rotate a vector */
Vec3f transform(const Vec3f& v) const;
@@ -134,6 +134,9 @@ private:
FCL_REAL data[4];
};
+SimpleQuaternion conj(const SimpleQuaternion& q);
+SimpleQuaternion inverse(const SimpleQuaternion& q);
+
/** \brief Simple transform class used locally by InterpMotion */
class SimpleTransform
{
@@ -160,18 +163,30 @@ public:
q.fromRotation(R_);
}
+ SimpleTransform(const SimpleQuaternion& q_, const Vec3f& T_)
+ {
+ q_.toRotation(R);
+ T = T_;
+
+ q = q_;
+ }
+
SimpleTransform(const Matrix3f& R_)
{
R = R_;
q.fromRotation(R_);
- T.setValue(0.0);
+ }
+
+ SimpleTransform(const SimpleQuaternion& q_)
+ {
+ q_.toRotation(R);
+ q = q_;
}
SimpleTransform(const Vec3f& T_)
{
T = T_;
R.setIdentity();
- q = SimpleQuaternion();
}
inline const Vec3f& getTranslation() const
@@ -226,16 +241,18 @@ public:
return q.transform(v) + T;
}
- SimpleTransform inverse() const
+ SimpleTransform& inverse()
{
- Matrix3f Rinv = transpose(R);
- return SimpleTransform(Rinv, Rinv * (-T));
+ q.conj();
+ R.transpose();
+ T = q.transform(-T);
+ return *this;
}
SimpleTransform inverseTimes(const SimpleTransform& other) const
{
- Vec3f v = other.T - T;
- return SimpleTransform(R.transposeTimes(other.R), R.transposeTimes(v));
+ const SimpleQuaternion& q_inv = fcl::conj(q);
+ return SimpleTransform(q_inv * other.q, q_inv.transform(other.T - T));
}
const SimpleTransform& operator *= (const SimpleTransform& other)
@@ -243,27 +260,18 @@ public:
T = q.transform(other.T) + T;
q *= other.q;
q.toRotation(R);
-
return *this;
}
SimpleTransform operator * (const SimpleTransform& other) const
{
SimpleQuaternion q_new = q * other.q;
- SimpleTransform t;
- t.q = q_new;
- q_new.toRotation(t.R);
- t.T = q.transform(other.T) + T;
- return t;
+ return SimpleTransform(q_new, q.transform(other.T) + T);
}
bool isIdentity() const
{
- return
- (R(0, 0) == 1) && (R(0, 1) == 0) && (R(0, 2) == 0) &&
- (R(1, 0) == 0) && (R(1, 1) == 1) && (R(1, 2) == 0) &&
- (R(2, 0) == 0) && (R(2, 1) == 0) && (R(2, 2) == 1) &&
- (T[0] == 0) && (T[1] == 0) && (T[2] == 0);
+ return q.isIdentity() && T.isZero();
}
void setIdentity()
@@ -275,6 +283,10 @@ public:
};
+SimpleTransform inverse(const SimpleTransform& tf);
+
+void relativeTransform(const SimpleTransform& tf1, const SimpleTransform& tf2,
+ SimpleTransform& tf);
}
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index 3881cc9f..e2749bf2 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -48,8 +48,6 @@ namespace fcl
class TraversalNodeBase
{
public:
- TraversalNodeBase() : enable_statistics(false) {}
-
virtual ~TraversalNodeBase();
virtual void preprocess() {}
@@ -77,9 +75,8 @@ public:
/** \brief Get the right child of the node b in the second tree */
virtual int getSecondRightChild(int b) const;
- void enableStatistics(bool enable) { enable_statistics = enable; }
-
- bool enable_statistics;
+ /** \brief Enable statistics (verbose mode) */
+ virtual void enableStatistics(bool enable) = 0;
SimpleTransform tf1;
@@ -89,7 +86,7 @@ public:
class CollisionTraversalNodeBase : public TraversalNodeBase
{
public:
- CollisionTraversalNodeBase() : TraversalNodeBase() {}
+ CollisionTraversalNodeBase() : enable_statistics(false) {}
virtual ~CollisionTraversalNodeBase();
@@ -102,13 +99,17 @@ public:
/** \brief Check whether the traversal can stop */
virtual bool canStop() const;
+ void enableStatistics(bool enable) { enable_statistics = enable; }
+
CollisionRequest request;
+
+ bool enable_statistics;
};
class DistanceTraversalNodeBase : public TraversalNodeBase
{
public:
- DistanceTraversalNodeBase() : TraversalNodeBase() {}
+ DistanceTraversalNodeBase() : enable_statistics(false) {}
virtual ~DistanceTraversalNodeBase();
@@ -118,7 +119,11 @@ public:
virtual bool canStop(FCL_REAL c) const;
+ void enableStatistics(bool enable) { enable_statistics = enable; }
+
DistanceRequest request;
+
+ 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 772b56d7..f4afbc8e 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -204,10 +204,13 @@ public:
Vec3f normal;
Vec3f contactp;
+ bool is_intersect = false;
+
if(!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));
}
}
@@ -215,14 +218,25 @@ 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));
}
}
+
+ if(is_intersect && request.enable_cost && (request.num_max_cost_sources > 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));
+ }
}
bool canStop() const
{
- return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= pairs.size());
+ 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()) );
}
Vec3f* vertices;
@@ -232,6 +246,10 @@ public:
mutable std::vector<BVHShapeCollisionPair> pairs;
+ mutable std::vector<CostSource> cost_sources;
+
+ FCL_REAL cost_density;
+
const NarrowPhaseSolver* nsolver;
};
@@ -242,13 +260,15 @@ template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices,
- const Matrix3f& R, const Vec3f& T,
- const SimpleTransform& tf2,
+ const SimpleTransform& tf1,
+ const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
+ FCL_REAL cost_density,
const CollisionRequest& request,
int& num_leaf_tests,
- std::vector<BVHShapeCollisionPair>& pairs)
+ std::vector<BVHShapeCollisionPair>& pairs,
+ std::vector<CostSource>& cost_sources)
{
if(enable_statistics) num_leaf_tests++;
@@ -266,20 +286,33 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
Vec3f normal;
Vec3f contactp;
+ bool is_intersect = false;
+
if(!request.enable_contact) // only interested in collision or not
{
- if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, R, T, NULL, NULL, NULL))
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
{
+ is_intersect = true;
pairs.push_back(BVHShapeCollisionPair(primitive_id));
}
}
else
{
- if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, R, T, &contactp, &penetration, &normal))
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal))
{
+ is_intersect = true;
pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
}
}
+
+ if(is_intersect && request.enable_cost && (request.num_max_cost_sources > 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));
+ }
}
}
@@ -290,24 +323,20 @@ class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNod
public:
MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
}
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
template<typename S, typename NarrowPhaseSolver>
@@ -316,24 +345,20 @@ class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNod
public:
MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
}
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
template<typename S, typename NarrowPhaseSolver>
@@ -342,24 +367,20 @@ class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNo
public:
MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
}
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
template<typename S, typename NarrowPhaseSolver>
@@ -368,24 +389,20 @@ class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversal
public:
MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
}
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
template<typename S, typename BV, typename NarrowPhaseSolver>
@@ -417,10 +434,13 @@ public:
Vec3f normal;
Vec3f contactp;
+ bool is_intersect = false;
+
if(!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));
}
}
@@ -428,14 +448,25 @@ 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));
}
}
+
+ if(is_intersect && request.enable_cost && (request.num_max_cost_sources > 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));
+ }
}
bool canStop() const
{
- return (pairs.size() > 0) && (!request.exhaustive) && (request.num_max_contacts <= pairs.size());
+ 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()) );
}
Vec3f* vertices;
@@ -445,6 +476,10 @@ public:
mutable std::vector<BVHShapeCollisionPair> pairs;
+ mutable std::vector<CostSource> cost_sources;
+
+ FCL_REAL cost_density;
+
const NarrowPhaseSolver* nsolver;
};
@@ -454,26 +489,21 @@ class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNod
public:
ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
// may need to change the order in pairs
}
-
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
@@ -483,26 +513,22 @@ class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNod
public:
ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
// may need to change the order in pairs
}
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
@@ -512,26 +538,22 @@ class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNo
public:
ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
// may need to change the order in pairs
}
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
@@ -541,26 +563,22 @@ class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversal
public:
ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
{
- R.setIdentity();
}
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return !overlap(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->request, this->num_leaf_tests, this->pairs);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->request, this->num_leaf_tests, this->pairs, this->cost_sources);
// may need to change the order in pairs
}
- // R, T is the transform of bvh model
- Matrix3f R;
- Vec3f T;
};
@@ -722,7 +740,7 @@ template<typename BV, typename S, typename NarrowPhaseSolver>
void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices,
- const Matrix3f&R, const Vec3f& T,
+ const SimpleTransform& tf1,
const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
@@ -742,7 +760,7 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
FCL_REAL dist;
- nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, R, T, &dist);
+ nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &dist);
if(dist < min_distance)
{
@@ -757,8 +775,7 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
template<typename S, typename NarrowPhaseSolver>
static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, int last_tri_id,
- const Matrix3f& R, const Vec3f& T,
- const S& s, const SimpleTransform& tf,
+ const S& s, const SimpleTransform& tf1, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
FCL_REAL& min_distance)
{
@@ -771,16 +788,18 @@ static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, i
Vec3f last_tri_P, last_tri_Q;
FCL_REAL dist;
- nsolver->shapeTriangleDistance(s, tf, p1, p2, p3, R, T, &dist);
+ nsolver->shapeTriangleDistance(s, tf2, p1, p2, p3, tf1, &dist);
min_distance = dist;
}
-static inline void distance_postprocess(const Matrix3f& R, const Vec3f& T, Vec3f& p2)
+static inline void distance_postprocess(const SimpleTransform& tf, Vec3f& p2)
{
- Vec3f u = p2 - T;
- p2 = R.transposeTimes(u);
+ const SimpleQuaternion& inv_q = conj(tf.getQuatRotation());
+ p2 = inv_q.transform(p2 - tf.getTranslation());
+ // Vec3f u = p2 - T;
+ // p2 = R.transposeTimes(u);
}
@@ -790,12 +809,11 @@ class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<
public:
MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>()
{
- R.setIdentity();
}
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model2), this->tf2, this->nsolver, this->min_distance);
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model2), this->tf1, this->tf2, this->nsolver, this->min_distance);
}
void postprocess()
@@ -806,17 +824,14 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
-
- Matrix3f R;
- Vec3f T;
};
@@ -826,12 +841,11 @@ class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode
public:
MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode<kIOS, S, NarrowPhaseSolver>()
{
- R.setIdentity();
}
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model2), this->tf2, this->nsolver, this->min_distance);
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model2), this->tf1, this->tf2, this->nsolver, this->min_distance);
}
void postprocess()
@@ -842,17 +856,15 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
-
- Matrix3f R;
- Vec3f T;
+
};
template<typename S, typename NarrowPhaseSolver>
@@ -861,12 +873,11 @@ class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNo
public:
MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
{
- R.setIdentity();
}
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model2), this->tf2, this->nsolver, this->min_distance);
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model2), this->tf1, this->tf2, this->nsolver, this->min_distance);
}
void postprocess()
@@ -877,17 +888,15 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
- Matrix3f R;
- Vec3f T;
};
@@ -960,12 +969,11 @@ class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<
public:
ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>()
{
- R.setIdentity();
}
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model1), this->tf1, this->nsolver, this->min_distance);
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model1), this->tf2, this->tf1, this->nsolver, this->min_distance);
}
void postprocess()
@@ -976,17 +984,15 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
-
- Matrix3f R;
- Vec3f T;
+
};
template<typename S, typename NarrowPhaseSolver>
@@ -995,12 +1001,11 @@ class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode
public:
ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>()
{
- R.setIdentity();
}
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model1), this->tf1, this->nsolver, this->min_distance);
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model1), this->tf2, this->tf1, this->nsolver, this->min_distance);
}
void postprocess()
@@ -1011,17 +1016,15 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
- Matrix3f R;
- Vec3f T;
};
template<typename S, typename NarrowPhaseSolver>
@@ -1030,12 +1033,11 @@ class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNo
public:
ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
{
- R.setIdentity();
}
void preprocess()
{
- distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model1), this->tf1, this->nsolver, this->min_distance);
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, *(this->model1), this->tf2, this->tf1, this->nsolver, this->min_distance);
}
void postprocess()
@@ -1046,17 +1048,15 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
- return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
void leafTesting(int b1, int b2) const
{
details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
- Matrix3f R;
- Vec3f T;
};
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 55fc88b2..3fe70851 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -214,41 +214,55 @@ public:
FCL_REAL penetration;
Vec3f normal;
- int n_contacts;
+ unsigned int n_contacts;
Vec3f contacts[2];
-
+
+ bool is_intersect = false;
if(!this->request.enable_contact) // only interested in collision or not
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
{
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
+ is_intersect = true;
+ pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
}
}
else // need compute the contact information
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
contacts,
- (unsigned int*)&n_contacts,
+ &n_contacts,
&penetration,
&normal))
{
+ is_intersect = true;
if(!this->request.exhaustive)
- n_contacts = std::min(n_contacts, (int)this->request.num_max_contacts - (int)pairs.size());
+ {
+ 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;
+ }
- for(int i = 0; i < n_contacts; ++i)
+ for(unsigned int i = 0; i < n_contacts; ++i)
{
- // if((!this->request.exhaustive) && (this->request.num_max_contacts <= pairs.size())) break;
pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
}
}
}
+
+
+ if(is_intersect && this->request.enable_cost && (this->request.num_max_cost_sources > 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));
+ }
}
/** \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());
+ 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()) );
}
Vec3f* vertices1;
@@ -258,6 +272,9 @@ public:
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 83391489..08cc6985 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -131,13 +131,14 @@ public:
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
{
request = request_;
OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
tree2, tree2->getRoot(), tree2->getRootBV(),
- tf1, tf2, pairs);
+ tf1, tf2, pairs, cost_sources);
}
@@ -157,13 +158,14 @@ public:
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
{
request = request_;
OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
tree2, 0,
- tf1, tf2, pairs);
+ tf1, tf2, pairs, cost_sources);
}
template<typename BV>
@@ -184,6 +186,7 @@ public:
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
{
@@ -191,7 +194,7 @@ public:
OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
tree1, 0,
- tf2, tf1, pairs);
+ tf2, tf1, pairs, cost_sources);
}
@@ -212,6 +215,7 @@ public:
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
{
request = request_;
@@ -222,7 +226,7 @@ public:
convertBV(bv2, tf2, obb2);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
s, obb2,
- tf1, tf2, pairs);
+ tf1, tf2, pairs, cost_sources);
}
@@ -230,6 +234,7 @@ public:
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
{
request = request_;
@@ -240,7 +245,7 @@ public:
convertBV(bv1, tf1, obb1);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
s, obb1,
- tf2, tf1, pairs);
+ tf2, tf1, pairs, cost_sources);
}
template<typename S>
@@ -326,7 +331,8 @@ private:
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) const
+ std::vector<OcTreeShapeCollisionPair>& pairs,
+ std::vector<CostSource>& cost_sources) const
{
if(!root1->hasChildren())
{
@@ -340,10 +346,16 @@ private:
SimpleTransform box_tf;
constructBox(bv1, tf1, box, box_tf);
+ bool is_intersect = false;
+
if(!request.enable_contact)
{
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
- pairs.push_back(OcTreeShapeCollisionPair(root1));
+ {
+ is_intersect = true;
+ if(pairs.size() < request.num_max_contacts)
+ pairs.push_back(OcTreeShapeCollisionPair(root1));
+ }
}
else
{
@@ -352,9 +364,13 @@ private:
Vec3f normal;
if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
- pairs.push_back(OcTreeShapeCollisionPair(root1, contact, normal, depth));
+ {
+ is_intersect = true;
+ if(pairs.size() < request.num_max_contacts)
+ pairs.push_back(OcTreeShapeCollisionPair(root1, contact, normal, depth));
+ }
}
-
+
return ((pairs.size() >= request.num_max_contacts) && !request.exhaustive);
}
else return false;
@@ -375,7 +391,7 @@ private:
AABB child_bv;
computeChildBV(bv1, i, child_bv);
- if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2, pairs))
+ if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2, pairs, cost_sources))
return true;
}
}
@@ -403,7 +419,7 @@ private:
const Vec3f& p3 = tree2->vertices[tri_id[2]];
FCL_REAL dist;
- solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), &dist);
+ solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist);
if(dist < min_dist) min_dist = dist;
return (min_dist <= 0);
}
@@ -471,7 +487,8 @@ private:
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) const
+ std::vector<OcTreeMeshCollisionPair>& pairs,
+ std::vector<CostSource>& cost_sources) const
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
{
@@ -494,7 +511,7 @@ private:
if(!request.enable_contact)
{
- if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), NULL, NULL, NULL))
+ if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL))
pairs.push_back(OcTreeMeshCollisionPair(root1, root2));
}
else
@@ -503,7 +520,7 @@ private:
FCL_REAL depth;
Vec3f normal;
- if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), &contact, &depth, &normal))
+ if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal))
pairs.push_back(OcTreeMeshCollisionPair(root1, root2, contact, normal, depth));
}
@@ -532,17 +549,17 @@ private:
AABB child_bv;
computeChildBV(bv1, i, child_bv);
- if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, pairs))
+ if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, pairs, cost_sources))
return true;
}
}
}
else
{
- if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2, pairs))
+ if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2, pairs, cost_sources))
return true;
- if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2, pairs))
+ if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2, pairs, cost_sources))
return true;
}
@@ -633,7 +650,8 @@ 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) const
+ std::vector<OcTreeCollisionPair>& pairs,
+ std::vector<CostSource>& cost_sources) const
{
if(!root1->hasChildren() && !root2->hasChildren())
{
@@ -687,7 +705,7 @@ private:
if(OcTreeIntersectRecurse(tree1, child, child_bv,
tree2, root2, bv2,
- tf1, tf2, pairs))
+ tf1, tf2, pairs, cost_sources))
return true;
}
}
@@ -704,7 +722,7 @@ private:
if(OcTreeIntersectRecurse(tree1, root1, bv1,
tree2, child, child_bv,
- tf1, tf2, pairs))
+ tf1, tf2, pairs, cost_sources))
return true;
}
}
@@ -737,7 +755,7 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeIntersect(model1, model2, tf1, tf2, pairs, request);
+ otsolver->OcTreeIntersect(model1, model2, tf1, tf2, pairs, cost_sources, request);
}
const OcTree* model1;
@@ -746,6 +764,7 @@ public:
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeCollisionPair> pairs;
+ mutable std::vector<CostSource> cost_sources;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -801,7 +820,7 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, pairs, request);
+ otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, pairs, cost_sources, request);
}
const S* model1;
@@ -810,7 +829,8 @@ public:
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeShapeCollisionPair> pairs;
-
+ mutable std::vector<CostSource> cost_sources;
+
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -833,7 +853,7 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, pairs, request);
+ otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, pairs, cost_sources, request);
}
const OcTree* model1;
@@ -842,7 +862,8 @@ public:
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeShapeCollisionPair> pairs;
-
+ mutable std::vector<CostSource> cost_sources;
+
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -926,7 +947,7 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, pairs, request);
+ otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, pairs, cost_sources, request);
}
const BVHModel<BV>* model1;
@@ -935,6 +956,7 @@ public:
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeMeshCollisionPair> pairs;
+ mutable std::vector<CostSource> cost_sources;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -958,7 +980,7 @@ public:
void leafTesting(int, int) const
{
- otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, pairs, request);
+ otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, pairs, cost_sources, request);
}
const OcTree* model1;
@@ -967,6 +989,7 @@ public:
SimpleTransform tf1, tf2;
mutable std::vector<OcTreeMeshCollisionPair> pairs;
+ mutable std::vector<CostSource> cost_sources;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index e545996e..76debef0 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -41,6 +41,7 @@
#include "fcl/collision_data.h"
#include "fcl/traversal_node_base.h"
#include "fcl/narrowphase/narrowphase.h"
+#include "fcl/geometric_shapes_utility.h"
namespace fcl
{
@@ -70,6 +71,16 @@ public:
is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal);
else
is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL);
+
+ if(is_collision && request.enable_cost)
+ {
+ AABB aabb1, aabb2;
+ computeBV<AABB, S1>(*model1, tf1, aabb1);
+ 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);
+ }
}
const S1* model1;
@@ -86,6 +97,10 @@ public:
mutable bool is_collision;
+ mutable CostSource cost_source;
+
+ FCL_REAL cost_density;
+
const NarrowPhaseSolver* nsolver;
};
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 83a5a02a..471e3f20 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -128,6 +128,11 @@ public:
return *this;
}
+ bool isZero() const
+ {
+ return (data[0] == 0) && (data[1] == 0) && (data[2] == 0);
+ }
+
};
template<typename T>
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 07c6802a..0e4ead66 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -818,6 +818,22 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
}
+bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
+ const SimpleTransform& tf,
+ Vec3f* contact_points,
+ unsigned int* num_contact_points,
+ FCL_REAL* penetration_depth,
+ Vec3f* normal)
+{
+ Vec3f Q1_ = tf.transform(Q1);
+ Vec3f Q2_ = tf.transform(Q2);
+ Vec3f Q3_ = tf.transform(Q3);
+
+ return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
+}
+
+
#if ODE_STYLE
bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
diff --git a/trunk/fcl/src/narrowphase/gjk_libccd.cpp b/trunk/fcl/src/narrowphase/gjk_libccd.cpp
index a3bfaa30..90f6674f 100644
--- a/trunk/fcl/src/narrowphase/gjk_libccd.cpp
+++ b/trunk/fcl/src/narrowphase/gjk_libccd.cpp
@@ -959,7 +959,7 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3)
return o;
}
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, Vec3f const& T)
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf)
{
ccd_triangle_t* o = new ccd_triangle_t;
Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
@@ -968,8 +968,8 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
ccdVec3Set(&o->c, center[0], center[1], center[2]);
- SimpleQuaternion q;
- q.fromRotation(R);
+ const SimpleQuaternion& q = tf.getQuatRotation();
+ const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
ccdQuatInvert2(&o->rot_inv, &o->rot);
diff --git a/trunk/fcl/src/narrowphase/narrowphase.cpp b/trunk/fcl/src/narrowphase/narrowphase.cpp
index 17bd02af..a5b0b4ab 100644
--- a/trunk/fcl/src/narrowphase/narrowphase.cpp
+++ b/trunk/fcl/src/narrowphase/narrowphase.cpp
@@ -1262,10 +1262,10 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTrans
}
template<>
-bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereTriangleIntersect(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, contact_points, penetration_depth, normal);
+ return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
}
template<>
@@ -1285,11 +1285,11 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Simp
}
template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
FCL_REAL* dist) const
{
- return details::sphereTriangleDistance(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, dist);
+ return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
}
@@ -1312,10 +1312,10 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransf
}
template<>
-bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereTriangleIntersect(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, contact_points, penetration_depth, normal);
+ return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
}
@@ -1337,11 +1337,11 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Simpl
}
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
FCL_REAL* dist) const
{
- return details::sphereTriangleDistance(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, dist);
+ return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
}
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index c14c6880..3c7fabd7 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -64,6 +64,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.tf2 = tf2;
node.request = request;
+ node.cost_density = model1.cost_density * model2.cost_density;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
diff --git a/trunk/fcl/src/transform.cpp b/trunk/fcl/src/transform.cpp
index 9fbbd5c3..fcb34bda 100644
--- a/trunk/fcl/src/transform.cpp
+++ b/trunk/fcl/src/transform.cpp
@@ -280,30 +280,66 @@ const SimpleQuaternion& SimpleQuaternion::operator *= (FCL_REAL t)
}
-SimpleQuaternion SimpleQuaternion::conj() const
+SimpleQuaternion& SimpleQuaternion::conj()
{
- return SimpleQuaternion(data[0], -data[1], -data[2], -data[3]);
+ data[1] = -data[1];
+ data[2] = -data[2];
+ data[3] = -data[3];
+ return *this;
}
-SimpleQuaternion SimpleQuaternion::inverse() const
+SimpleQuaternion& SimpleQuaternion::inverse()
{
- double sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
+ FCL_REAL sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
if(sqr_length > 0)
{
- double inv_length = 1.0 / sqrt(sqr_length);
- return SimpleQuaternion(data[0] * inv_length, -data[1] * inv_length, -data[2] * inv_length, -data[3] * inv_length);
+ FCL_REAL inv_length = 1 / std::sqrt(sqr_length);
+ data[0] *= inv_length;
+ data[1] *= (-inv_length);
+ data[2] *= (-inv_length);
+ data[3] *= (-inv_length);
}
else
{
- return SimpleQuaternion(data[0], -data[1], -data[2], -data[3]);
+ data[1] = -data[1];
+ data[2] = -data[2];
+ data[3] = -data[3];
}
+
+ return *this;
}
Vec3f SimpleQuaternion::transform(const Vec3f& v) const
{
- SimpleQuaternion r = (*this) * SimpleQuaternion(0, v[0], v[1], v[2]) * (this->conj());
+ SimpleQuaternion r = (*this) * SimpleQuaternion(0, v[0], v[1], v[2]) * (fcl::conj(*this));
return Vec3f(r.data[1], r.data[2], r.data[3]);
}
+SimpleQuaternion conj(const SimpleQuaternion& q)
+{
+ SimpleQuaternion r(q);
+ return r.conj();
+}
+
+SimpleQuaternion inverse(const SimpleQuaternion& q)
+{
+ SimpleQuaternion res(q);
+ return res.inverse();
+}
+
+SimpleTransform inverse(const SimpleTransform& tf)
+{
+ SimpleTransform res(tf);
+ return res.inverse();
+}
+
+void relativeTransform(const SimpleTransform& tf1, const SimpleTransform& tf2,
+ SimpleTransform& tf)
+{
+ const SimpleQuaternion& q1_inv = fcl::conj(tf1.getQuatRotation());
+ tf = SimpleTransform(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
+}
+
+
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 807890b2..f5206454 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -48,10 +48,13 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
Vec3f* vertices1, Vec3f* vertices2,
Triangle* tri_indices1, Triangle* tri_indices2,
const Matrix3f& R, const Vec3f& T,
+ 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<BVHCollisionPair>& pairs,
+ std::vector<CostSource>& cost_sources)
{
if(enable_statistics) num_leaf_tests++;
@@ -73,34 +76,48 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
FCL_REAL penetration;
Vec3f normal;
- int n_contacts;
+ unsigned int n_contacts;
Vec3f contacts[2];
-
+
+ bool is_intersect = false;
if(!request.enable_contact) // only interested in collision or not
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
- pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
+ {
+ is_intersect = true;
+ pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
+ }
}
else // need compute the contact information
{
if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
R, T,
contacts,
- (unsigned int*)&n_contacts,
+ &n_contacts,
&penetration,
&normal))
{
+ is_intersect = true;
if(!request.exhaustive)
- n_contacts = std::min(n_contacts, (int)request.num_max_contacts - (int)pairs.size());
+ {
+ if(request.num_max_contacts < pairs.size() + n_contacts)
+ n_contacts = (request.num_max_contacts > pairs.size()) ? (request.num_max_contacts - pairs.size()) : 0;
+ }
- for(int i = 0; i < n_contacts; ++i)
+ for(unsigned int i = 0; i < n_contacts; ++i)
{
- // if((!request.exhaustive) && (request.num_max_contacts <= pairs.size())) break;
pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
}
}
}
+
+ if(is_intersect && request.enable_cost && (request.num_max_cost_sources > 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));
+ }
}
@@ -163,7 +180,6 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode<OBB>()
{
R.setIdentity();
- // default T is 0
}
bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
@@ -177,9 +193,11 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, request,
+ tf1, tf2,
+ enable_statistics, cost_density, request,
num_leaf_tests,
- pairs);
+ pairs,
+ cost_sources);
}
@@ -194,9 +212,11 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f&
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, request,
+ tf1, tf2,
+ enable_statistics, cost_density, request,
num_leaf_tests,
- pairs);
+ pairs,
+ cost_sources);
}
@@ -204,7 +224,6 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f&
MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode<RSS>()
{
R.setIdentity();
- // default T is 0
}
bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
@@ -218,9 +237,11 @@ void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, request,
+ tf1, tf2,
+ enable_statistics, cost_density, request,
num_leaf_tests,
- pairs);
+ pairs,
+ cost_sources);
}
@@ -229,7 +250,6 @@ void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS() : MeshCollisionTraversalNode<kIOS>()
{
R.setIdentity();
- // default T is 0
}
bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
@@ -243,9 +263,11 @@ void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, request,
+ tf1, tf2,
+ enable_statistics, cost_density, request,
num_leaf_tests,
- pairs);
+ pairs,
+ cost_sources);
}
@@ -253,7 +275,6 @@ void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS() : MeshCollisionTraversalNode<OBBRSS>()
{
R.setIdentity();
- // default T is 0
}
bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
@@ -267,9 +288,11 @@ void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
tri_indices1, tri_indices2,
R, T,
- enable_statistics, request,
+ tf1, tf2,
+ enable_statistics, cost_density, request,
num_leaf_tests,
- pairs);
+ pairs,
+ cost_sources);
}
diff --git a/trunk/fcl/src/traversal_recurse.cpp b/trunk/fcl/src/traversal_recurse.cpp
index 2845baa9..5b4f4e65 100644
--- a/trunk/fcl/src/traversal_recurse.cpp
+++ b/trunk/fcl/src/traversal_recurse.cpp
@@ -302,7 +302,7 @@ struct BVTQ
bool full() const
{
- return ((int)pq.size() >= qsize - 1);
+ return (pq.size() + 1 >= qsize);
}
std::priority_queue<BVT, std::vector<BVT>, BVT_Comparer> pq;
--
GitLab