From 035bab78186b270556d9b5eb01e4b750fc0cf464 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 12 Sep 2011 06:16:08 +0000
Subject: [PATCH] fix some bug in shape collision and more refactoring.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@30 253336fb-580f-4252-a368-f3cef5a2a82b
---
.../src/environmentFCL.cpp | 4 +-
trunk/fcl/include/fcl/BVH_model.h | 2 +-
trunk/fcl/include/fcl/broad_phase_collision.h | 12 +-
trunk/fcl/include/fcl/collision_object.h | 16 +-
.../fcl/geometric_shape_to_BVH_model.h | 14 +-
trunk/fcl/include/fcl/geometric_shapes.h | 22 +-
.../include/fcl/geometric_shapes_intersect.h | 98 +-------
trunk/fcl/include/fcl/motion.h | 4 +-
trunk/fcl/include/fcl/primitive.h | 2 +-
trunk/fcl/include/fcl/simple_setup.h | 29 +--
trunk/fcl/include/fcl/traversal_node_shapes.h | 15 +-
trunk/fcl/include/fcl/vec_3f.h | 12 +-
trunk/fcl/src/BVH_model.cpp | 8 +-
trunk/fcl/src/BV_fitter.cpp | 12 +-
trunk/fcl/src/OBB.cpp | 2 +-
trunk/fcl/src/RSS.cpp | 4 +-
trunk/fcl/src/broad_phase_collision.cpp | 82 +++----
trunk/fcl/src/collision_func_matrix.cpp | 4 +-
trunk/fcl/src/collision_node.cpp | 4 +-
trunk/fcl/src/geometric_shapes_intersect.cpp | 53 ++--
trunk/fcl/src/geometric_shapes_utility.cpp | 110 ++++-----
trunk/fcl/src/intersect.cpp | 24 +-
trunk/fcl/src/traversal_node_bvhs.cpp | 4 +-
trunk/fcl/src/vec_3f.cpp | 12 +-
trunk/fcl/test/test_core_collision.cpp | 2 +-
trunk/fcl/test/test_core_distance.cpp | 4 +-
trunk/fcl/test/test_core_geometric_shapes.cpp | 231 +++++++++++++++++-
trunk/fcl/test/test_core_utility.h | 25 ++
28 files changed, 494 insertions(+), 317 deletions(-)
diff --git a/trunk/collision_space_fcl/src/environmentFCL.cpp b/trunk/collision_space_fcl/src/environmentFCL.cpp
index 03aa8362..60a2aa1c 100644
--- a/trunk/collision_space_fcl/src/environmentFCL.cpp
+++ b/trunk/collision_space_fcl/src/environmentFCL.cpp
@@ -258,7 +258,7 @@ fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape
g->beginModel();
g->addSubModel(points, tri_indices);
g->endModel();
- g->computeAABB();
+ g->computeLocalAABB();
}
}
break;
@@ -276,7 +276,7 @@ void EnvironmentModelFCL::updateGeom(fcl::CollisionObject* geom, const btTransf
const btVector3& o = pose.getOrigin();
geom->setQuatRotation(fcl::SimpleQuaternion(q.getW(), q.getX(), q.getY(), q.getZ()));
geom->setTranslation(fcl::Vec3f(o.getX(), o.getY(), o.getZ()));
- geom->computeCachedAABB(); // update AABB
+ geom->computeAABB(); // update AABB
}
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index c376869d..2eef52b6 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -151,7 +151,7 @@ public:
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
/** \brief Compute the AABB for the BVH, used for broad-phase collision */
- void computeAABB();
+ void computeLocalAABB();
/** \brief Geometry point data */
Vec3f* vertices;
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index 13127a34..087e90a3 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -334,7 +334,7 @@ protected:
{
bool operator()(const CollisionObject* a, const CollisionObject* b) const
{
- if(a->getCachedAABB().min_[0] < b->getCachedAABB().min_[0])
+ if(a->getAABB().min_[0] < b->getAABB().min_[0])
return true;
return false;
}
@@ -345,7 +345,7 @@ protected:
{
bool operator()(const CollisionObject* a, const CollisionObject* b) const
{
- if(a->getCachedAABB().min_[1] < b->getCachedAABB().min_[1])
+ if(a->getAABB().min_[1] < b->getAABB().min_[1])
return true;
return false;
}
@@ -356,7 +356,7 @@ protected:
{
bool operator()(const CollisionObject* a, const CollisionObject* b) const
{
- if(a->getCachedAABB().min_[2] < b->getCachedAABB().min_[2])
+ if(a->getAABB().min_[2] < b->getAABB().min_[2])
return true;
return false;
}
@@ -366,12 +366,12 @@ protected:
class DummyCollisionObject : public CollisionObject
{
public:
- DummyCollisionObject(const AABB& aabb)
+ DummyCollisionObject(const AABB& aabb_)
{
- aabb_cache = aabb;
+ aabb = aabb_;
}
- void computeAABB() {}
+ void computeLocalAABB() {}
};
/** \brief check collision between one object and a list of objects */
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 64acea69..338b3791 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -59,19 +59,19 @@ public:
virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
- virtual void computeAABB() = 0;
+ virtual void computeLocalAABB() = 0;
- inline const AABB& getCachedAABB() const
+ inline const AABB& getAABB() const
{
- return aabb_cache;
+ return aabb;
}
- inline void computeCachedAABB()
+ inline void computeAABB()
{
Vec3f center = t.transform(aabb_center);
Vec3f delta(aabb_radius, aabb_radius, aabb_radius);
- aabb_cache.min_ = center - delta;
- aabb_cache.max_ = center + delta;
+ aabb.min_ = center - delta;
+ aabb.max_ = center + delta;
}
inline const Vec3f& getTranslation() const
@@ -117,10 +117,10 @@ public:
protected:
/** AABB in global coordinate */
- mutable AABB aabb_cache;
+ mutable AABB aabb;
/** AABB in local coordinate */
- AABB aabb;
+ AABB aabb_local;
/** AABB center in local coordinate */
Vec3f aabb_center;
diff --git a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
index e5990f25..64cca63e 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -79,15 +79,15 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
- v = MxV(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
+ v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
points[i] = v;
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
- model.computeAABB();
+ model.computeLocalAABB();
}
/** Generate BVH model from sphere */
@@ -146,15 +146,15 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
- v = MxV(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
+ v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
points[i] = v;
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
- model.computeAABB();
+ model.computeLocalAABB();
}
@@ -224,7 +224,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
- model.computeAABB();
+ model.computeLocalAABB();
}
}
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index 93417027..e6bca8cb 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -87,12 +87,12 @@ public:
Vec3f R0[3];
for(int i = 0; i < 3; ++i)
R0[i] = Rloc[i];
- MxM(R, R0, Rloc);
- Tloc = MxV(R, Tloc) + T;
+ matMulMat(R, R0, Rloc);
+ Tloc = matMulVec(R, Tloc) + T;
}
/** \brief Get local transform */
- void getLocalTransfrom(Vec3f R[3], Vec3f& T) const
+ void getLocalTransform(Vec3f R[3], Vec3f& T) const
{
T = Tloc;
R[0] = Rloc[0];
@@ -101,7 +101,7 @@ public:
}
/** \brief Get local position */
- inline const Vec3f& getLocalPosition() const
+ inline const Vec3f& getLocalTranslation() const
{
return Tloc;
}
@@ -132,7 +132,7 @@ public:
Vec3f side;
/** \brief Compute AABB */
- void computeAABB();
+ void computeLocalAABB();
/** \brief Get node type: a box */
NODE_TYPE getNodeType() const { return GEOM_BOX; }
@@ -148,7 +148,7 @@ public:
BVH_REAL radius;
/** \brief Compute AABB */
- void computeAABB();
+ void computeLocalAABB();
/** \brief Get node type: a sphere */
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
@@ -167,7 +167,7 @@ public:
BVH_REAL lz;
/** \brief Compute AABB */
- void computeAABB();
+ void computeLocalAABB();
/** \brief Get node type: a capsule */
NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
@@ -186,7 +186,7 @@ public:
BVH_REAL lz;
/** \brief Compute AABB */
- void computeAABB();
+ void computeLocalAABB();
/** \brief Get node type: a cone */
NODE_TYPE getNodeType() const { return GEOM_CONE; }
@@ -205,7 +205,7 @@ public:
BVH_REAL lz;
/** \brief Compute AABB */
- void computeAABB();
+ void computeLocalAABB();
/** \brief Get node type: a cylinder */
NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
@@ -259,7 +259,7 @@ public:
}
/** Compute AABB */
- void computeAABB();
+ void computeLocalAABB();
/** Get node type: a conex polytope */
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
@@ -309,7 +309,7 @@ public:
}
/** \brief Compute AABB */
- void computeAABB();
+ void computeLocalAABB();
/** \brief Get node type: a plane */
NODE_TYPE getNodeType() const { return GEOM_PLANE; }
diff --git a/trunk/fcl/include/fcl/geometric_shapes_intersect.h b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
index 5edc8d90..d2f8bd13 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_intersect.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
@@ -47,11 +47,11 @@
namespace fcl
{
-/** recall function used by GJK algorithm */
+/** \brief recall function used by GJK algorithm */
typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v);
typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c);
-/** initialize GJK stuffs */
+/** \brief initialize GJK stuffs */
template<typename T>
class GJKInitializer
{
@@ -72,6 +72,7 @@ public:
static void deleteGJKObject(void* o) {}
};
+/** \brief initialize GJK Cylinder */
template<>
class GJKInitializer<Cylinder>
{
@@ -82,7 +83,7 @@ public:
static void deleteGJKObject(void* o);
};
-
+/** \brief initialize GJK Sphere */
template<>
class GJKInitializer<Sphere>
{
@@ -93,7 +94,7 @@ public:
static void deleteGJKObject(void* o);
};
-
+/** \brief initialize GJK Box */
template<>
class GJKInitializer<Box>
{
@@ -104,7 +105,7 @@ public:
static void deleteGJKObject(void* o);
};
-
+/** \brief initialize GJK Capsule */
template<>
class GJKInitializer<Capsule>
{
@@ -115,7 +116,7 @@ public:
static void deleteGJKObject(void* o);
};
-
+/** \brief initialize GJK Cone */
template<>
class GJKInitializer<Cone>
{
@@ -126,6 +127,7 @@ public:
static void deleteGJKObject(void* o);
};
+/** \brief initialize GJK Convex */
template<>
class GJKInitializer<Convex>
{
@@ -136,7 +138,7 @@ public:
static void deleteGJKObject(void* o);
};
-
+/** \brief initialize GJK Triangle */
GJKSupportFunction triGetSupportFunction();
GJKCenterFunction triGetCenterFunction();
@@ -147,15 +149,13 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
void triDeleteGJKObject(void* o);
-/** GJK collision algorithm */
+/** \brief GJK collision algorithm */
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
-void transformGJKObject(void* obj, const Vec3f R[3], const Vec3f& T);
-
-/** collision algorithm between two shapes */
+/** intersection checking between two shapes */
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
@@ -170,42 +170,7 @@ bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BV
GJKInitializer<S2>::deleteGJKObject(o2);
}
-template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const S2& s2, const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S1>::createGJKObject(s1);
- void* o2 = GJKInitializer<S2>::createGJKObject(s2);
-
- transformGJKObject(o1, R1, T1);
- transformGJKObject(o2, R2, T2);
-
- return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
- o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S1>::deleteGJKObject(o1);
- GJKInitializer<S2>::deleteGJKObject(o2);
-}
-
-template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const S2& s2, const Vec3f R[3], const Vec3f& T,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S1>::createGJKObject(s1);
- void* o2 = GJKInitializer<S2>::createGJKObject(s2);
-
- transformGJKObject(o2, R, T);
-
- return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
- o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S1>::deleteGJKObject(o1);
- GJKInitializer<S2>::deleteGJKObject(o2);
-}
-
-
+/** \brief intersection checking between one shape and a triangle */
template<typename S>
bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
@@ -220,25 +185,7 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
triDeleteGJKObject(o2);
}
-
-template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = GJKInitializer<S>::createGJKObject(s);
- void* o2 = triCreateGJKObject(P1, P2, P3, R2, T2);
-
- transformGJKObject(o1, R1, T1);
-
- return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- o2, triGetSupportFunction(), triGetCenterFunction(),
- contact_points, penetration_depth, normal);
-
- GJKInitializer<S>::deleteGJKObject(o1);
- triDeleteGJKObject(o2);
-}
-
-
+/** \brief intersection checking between one shape and a triangle with transformation */
template<typename S>
bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], const Vec3f& T,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
@@ -254,25 +201,6 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
triDeleteGJKObject(o2);
}
-template<typename S>
-bool shapeTriangleIntersect(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const S& s, const Vec3f R[3], const Vec3f& T,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = triCreateGJKObject(P1, P2, P3);
- void* o2 = GJKInitializer<S>::createGJKObject(s);
- transformGJKObject(o2, R, T);
-
-
- return GJKCollide(o1, triGetSupportFunction(), triGetCenterFunction(),
- o2, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- triDeleteGJKObject(o1);
- GJKInitializer<S>::deleteGJKObject(o2);
-}
-
-
-
}
#endif
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index 857f6cdf..fb0e2790 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -83,8 +83,8 @@ public:
const Vec3f R2[3], const Vec3f& T2,
const Vec3f& O)
{
- t1 = SimpleTransform(R1, T1 - MxV(R1, O));
- t2 = SimpleTransform(R2, T2 - MxV(R2, O));
+ t1 = SimpleTransform(R1, T1 - matMulVec(R1, O));
+ t2 = SimpleTransform(R2, T2 - matMulVec(R2, O));
t = t1;
/** Compute the velocities for the motion */
diff --git a/trunk/fcl/include/fcl/primitive.h b/trunk/fcl/include/fcl/primitive.h
index 85ab7f72..2a948c09 100644
--- a/trunk/fcl/include/fcl/primitive.h
+++ b/trunk/fcl/include/fcl/primitive.h
@@ -59,7 +59,7 @@ struct Uncertainty
/** preprocess performs the eigen decomposition on the Sigma matrix */
void preprocess()
{
- Meigen(Sigma, sigma, axis);
+ matEigen(Sigma, sigma, axis);
}
/** sqrt performs the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 4339a589..754834b2 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -61,13 +61,6 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, con
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
bool enable_contact = false)
{
- for(int i = 0; i < 3; ++i)
- {
- node.R1[i] = R1[i];
- node.R2[i] = R2[i];
- }
- node.T1 = T1;
- node.T2 = T2;
node.enable_contact = enable_contact;
return true;
@@ -109,7 +102,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
+ Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
@@ -209,7 +202,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
+ Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
@@ -218,7 +211,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
+ Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
@@ -315,7 +308,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
+ Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
@@ -324,7 +317,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
+ Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
@@ -433,7 +426,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
+ Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
@@ -442,7 +435,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
+ Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
@@ -606,7 +599,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
+ Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
@@ -615,7 +608,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
+ Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
@@ -657,7 +650,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = MxV(R1, p) + T1;
+ Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
@@ -666,7 +659,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = MxV(R2, p) + T2;
+ Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 3b43f24f..d1399100 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -53,13 +53,6 @@ public:
model1 = NULL;
model2 = NULL;
- R1[0] = Vec3f(1, 0, 0);
- R1[1] = Vec3f(0, 1, 0);
- R1[2] = Vec3f(0, 0, 1);
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
-
enable_contact = false;
}
@@ -71,9 +64,9 @@ public:
void leafTesting(int, int) const
{
if(enable_contact)
- is_collision = shapeIntersect(*model1, *model2, R1, T1, R2, T2, &contact_point, &penetration_depth, &normal);
+ is_collision = shapeIntersect(*model1, *model2, &contact_point, &penetration_depth, &normal);
else
- is_collision = shapeIntersect(*model1, *model2, R1, T1, R2, T2);
+ is_collision = shapeIntersect(*model1, *model2);
}
const S1* model1;
@@ -88,10 +81,6 @@ public:
bool enable_contact;
mutable bool is_collision;
-
- Vec3f R1[3];
- Vec3f R2[3];
- Vec3f T1, T2;
};
}
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 6ba98c5b..3f50753e 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -453,25 +453,25 @@ namespace fcl
#endif
/** \brief M * v */
- Vec3f MxV(const Vec3f M[3], const Vec3f& v);
+ Vec3f matMulVec(const Vec3f M[3], const Vec3f& v);
/** \brief M' * v */
- Vec3f MTxV(const Vec3f M[3], const Vec3f& v);
+ Vec3f matTransMulVec(const Vec3f M[3], const Vec3f& v);
/** \brief v' * M * v */
- BVH_REAL vTMv(const Vec3f M[3], const Vec3f& v);
+ BVH_REAL quadraticForm(const Vec3f M[3], const Vec3f& v);
/** \brief S * M * S' */
- void SMST(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3]);
+ void tensorTransform(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3]);
/** \brief A * B */
- void MxM(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3]);
+ void matMulMat(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3]);
/** \brief The relative transform from (R1, T1) to (R2, T2) */
void relativeTransform(const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, Vec3f R[3], Vec3f& T);
/** \brief compute eigen values and vectors */
- void Meigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3]);
+ void matEigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3]);
}
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index 5e77b275..3e2343f5 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -853,7 +853,7 @@ int BVHModel<BV>::refitTree_topdown()
}
template<typename BV>
-void BVHModel<BV>::computeAABB()
+void BVHModel<BV>::computeLocalAABB()
{
AABB aabb_;
for(int i = 0; i < num_vertices; ++i)
@@ -861,11 +861,11 @@ void BVHModel<BV>::computeAABB()
aabb_ += vertices[i];
}
- aabb = aabb_;
+ aabb_local = aabb_;
- aabb_cache = aabb;
+ aabb = aabb_local;
- aabb_center = aabb.center();
+ aabb_center = aabb_local.center();
aabb_radius = 0;
for(int i = 0; i < num_vertices; ++i)
diff --git a/trunk/fcl/src/BV_fitter.cpp b/trunk/fcl/src/BV_fitter.cpp
index e72bba55..08edcc3c 100644
--- a/trunk/fcl/src/BV_fitter.cpp
+++ b/trunk/fcl/src/BV_fitter.cpp
@@ -144,7 +144,7 @@ void fitn(Vec3f* ps, int n, OBB& bv)
BVH_REAL s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, n, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
int min, mid, max;
if(s[0] > s[1]) { max = 0; min = 1; }
@@ -279,7 +279,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
BVH_REAL s[3] = {0, 0, 0};
getCovariance(ps, NULL, NULL, n, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
int min, mid, max;
if(s[0] > s[1]) { max = 0; min = 1; }
@@ -360,12 +360,12 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
if(type == BVH_MODEL_TRIANGLES)
{
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
}
else if(type == BVH_MODEL_POINTCLOUD)
{
getCovariance(vertices, prev_vertices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
}
int min, mid, max;
@@ -417,12 +417,12 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
if(type == BVH_MODEL_TRIANGLES)
{
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
}
else if(type == BVH_MODEL_POINTCLOUD)
{
getCovariance(vertices, prev_vertices, primitive_indices, num_primitives, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
}
int min, mid, max;
diff --git a/trunk/fcl/src/OBB.cpp b/trunk/fcl/src/OBB.cpp
index 27d8b81c..77c6610d 100644
--- a/trunk/fcl/src/OBB.cpp
+++ b/trunk/fcl/src/OBB.cpp
@@ -288,7 +288,7 @@ OBB OBB::merge_largedist(const OBB& b1, const OBB& b2)
vertex_proj[i] = vertex[i] - R[0] * vertex[i].dot(R[0]);
getCovariance(vertex_proj, NULL, NULL, 16, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
int min, mid, max;
if (s[0] > s[1]) { max = 0; min = 1; }
diff --git a/trunk/fcl/src/RSS.cpp b/trunk/fcl/src/RSS.cpp
index cdc4da99..59da2040 100644
--- a/trunk/fcl/src/RSS.cpp
+++ b/trunk/fcl/src/RSS.cpp
@@ -334,7 +334,7 @@ RSS RSS::operator + (const RSS& other) const
BVH_REAL s[3] = {0, 0, 0};
getCovariance(v, NULL, NULL, 16, M);
- Meigen(M, s, E);
+ matEigen(M, s, E);
int min, mid, max;
if(s[0] > s[1]) { max = 0; min = 1; }
@@ -423,7 +423,7 @@ BVH_REAL RSS::rectDistance(const Vec3f Rab[3], Vec3f const& Tab, const BVH_REAL
bA0_dot_B1 = b[1] * A0_dot_B1;
bA1_dot_B1 = b[1] * A1_dot_B1;
- Vec3f Tba = MTxV(Rab, Tab);
+ Vec3f Tba = matTransMulVec(Rab, Tab);
Vec3f S;
BVH_REAL t, u;
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index ca3b2f8f..2bedad2e 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -139,7 +139,7 @@ void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
{
setup();
- DummyCollisionObject dummyHigh(AABB(obj->getCachedAABB().max_));
+ DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
std::vector<CollisionObject*>::iterator pos_start1 = objs_x.begin();
std::vector<CollisionObject*>::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
@@ -227,7 +227,7 @@ void SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterat
{
while(pos_start < pos_end)
{
- if((*pos_start)->getCachedAABB().overlap(obj->getCachedAABB()))
+ if((*pos_start)->getAABB().overlap(obj->getAABB()))
{
if(callback(*pos_start, obj, cdata))
return;
@@ -241,7 +241,7 @@ void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionC
{
static const unsigned int CUTOFF = 100;
- DummyCollisionObject dummyHigh(AABB(obj->getCachedAABB().max_));
+ DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
std::vector<CollisionObject*>::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
@@ -286,9 +286,9 @@ void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionC
void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
{
// simple sweep and prune method
- double delta_x = (objs_x[objs_x.size() - 1])->getCachedAABB().min_[0] - (objs_x[0])->getCachedAABB().min_[0];
- double delta_y = (objs_x[objs_y.size() - 1])->getCachedAABB().min_[1] - (objs_y[0])->getCachedAABB().min_[1];
- double delta_z = (objs_z[objs_z.size() - 1])->getCachedAABB().min_[2] - (objs_z[0])->getCachedAABB().min_[2];
+ double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
+ double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
+ double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2];
int axis = 0;
if(delta_y > delta_x && delta_y > delta_z)
@@ -324,7 +324,7 @@ void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) cons
while(1)
{
- if((*run_pos)->getCachedAABB().min_[axis] < obj->getCachedAABB().min_[axis])
+ if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis])
{
run_pos++;
if(run_pos == pos_end) break;
@@ -341,14 +341,14 @@ void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) cons
{
std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
- while((*run_pos2)->getCachedAABB().min_[axis] <= obj->getCachedAABB().max_[axis])
+ while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis])
{
CollisionObject* obj2 = *run_pos2;
run_pos2++;
- if((obj->getCachedAABB().max_[axis2] >= obj2->getCachedAABB().min_[axis2]) && (obj2->getCachedAABB().max_[axis2] >= obj->getCachedAABB().min_[axis2]))
+ if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2]))
{
- if((obj->getCachedAABB().max_[axis3] >= obj2->getCachedAABB().min_[axis3]) && (obj2->getCachedAABB().max_[axis3] >= obj->getCachedAABB().min_[axis3]))
+ if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3]))
{
if(callback(obj, obj2, cdata))
return;
@@ -414,7 +414,7 @@ void SaPCollisionManager::unregisterObject(CollisionObject* obj)
void SaPCollisionManager::registerObject(CollisionObject* obj)
{
SaPAABB* curr = new SaPAABB;
- curr->cached = obj->getCachedAABB();
+ curr->cached = obj->getAABB();
curr->obj = obj;
curr->lo = new EndPoint;
curr->lo->minmax = 0;
@@ -502,11 +502,11 @@ void SaPCollisionManager::update()
{
SaPAABB* current = *it;
- Vec3f new_min = current->obj->getCachedAABB().min_;
- Vec3f new_max = current->obj->getCachedAABB().max_;
+ Vec3f new_min = current->obj->getAABB().min_;
+ Vec3f new_max = current->obj->getAABB().max_;
SaPAABB dummy;
- dummy.cached = current->obj->getCachedAABB();
+ dummy.cached = current->obj->getAABB();
EndPoint lo, hi;
dummy.lo = &lo;
@@ -683,7 +683,7 @@ void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCa
{
for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
{
- if((*it)->obj->getCachedAABB().overlap(obj->getCachedAABB()))
+ if((*it)->obj->getAABB().overlap(obj->getAABB()))
{
if(callback(obj, (*it)->obj, cdata))
return;
@@ -716,9 +716,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
setup();
EndPoint p;
- p.value = obj->getCachedAABB().min_[0];
+ p.value = obj->getAABB().min_[0];
std::vector<EndPoint>::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p, SortByValue());
- p.value = obj->getCachedAABB().max_[0];
+ p.value = obj->getAABB().max_[0];
std::vector<EndPoint>::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p, SortByValue());
if(start1 < end1)
@@ -742,9 +742,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
endpoints[0].resize(endpoints[0].size() - 2);
}
- p.value = obj->getCachedAABB().min_[1];
+ p.value = obj->getAABB().min_[1];
std::vector<EndPoint>::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p, SortByValue());
- p.value = obj->getCachedAABB().max_[1];
+ p.value = obj->getAABB().max_[1];
std::vector<EndPoint>::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p, SortByValue());
if(start2 < end2)
@@ -769,9 +769,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj)
}
- p.value = obj->getCachedAABB().min_[2];
+ p.value = obj->getAABB().min_[2];
std::vector<EndPoint>::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p, SortByValue());
- p.value = obj->getCachedAABB().max_[2];
+ p.value = obj->getAABB().max_[2];
std::vector<EndPoint>::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p, SortByValue());
if(start3 < end3)
@@ -804,18 +804,18 @@ void IntervalTreeCollisionManager::registerObject(CollisionObject* obj)
q.obj = obj;
p.minmax = 0;
q.minmax = 1;
- p.value = obj->getCachedAABB().min_[0];
- q.value = obj->getCachedAABB().max_[0];
+ p.value = obj->getAABB().min_[0];
+ q.value = obj->getAABB().max_[0];
endpoints[0].push_back(p);
endpoints[0].push_back(q);
- p.value = obj->getCachedAABB().min_[1];
- q.value = obj->getCachedAABB().max_[1];
+ p.value = obj->getAABB().min_[1];
+ q.value = obj->getAABB().max_[1];
endpoints[1].push_back(p);
endpoints[1].push_back(q);
- p.value = obj->getCachedAABB().min_[2];
- q.value = obj->getCachedAABB().max_[2];
+ p.value = obj->getAABB().min_[2];
+ q.value = obj->getAABB().max_[2];
endpoints[2].push_back(p);
endpoints[2].push_back(q);
setup_ = false;
@@ -841,9 +841,9 @@ void IntervalTreeCollisionManager::setup()
CollisionObject* obj = p.obj;
if(p.minmax == 0)
{
- SAPInterval* ivl1 = new SAPInterval(obj->getCachedAABB().min_[0], obj->getCachedAABB().max_[0], obj);
- SAPInterval* ivl2 = new SAPInterval(obj->getCachedAABB().min_[1], obj->getCachedAABB().max_[1], obj);
- SAPInterval* ivl3 = new SAPInterval(obj->getCachedAABB().min_[2], obj->getCachedAABB().max_[2], obj);
+ SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj);
+ SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj);
+ SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj);
interval_trees[0]->insert(ivl1);
interval_trees[1]->insert(ivl2);
interval_trees[2]->insert(ivl3);
@@ -861,25 +861,25 @@ void IntervalTreeCollisionManager::update()
for(unsigned int i = 0; i < endpoints[0].size(); ++i)
{
if(endpoints[0][i].minmax == 0)
- endpoints[0][i].value = endpoints[0][i].obj->getCachedAABB().min_[0];
+ endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0];
else
- endpoints[0][i].value = endpoints[0][i].obj->getCachedAABB().max_[0];
+ endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0];
}
for(unsigned int i = 0; i < endpoints[1].size(); ++i)
{
if(endpoints[1][i].minmax == 0)
- endpoints[1][i].value = endpoints[1][i].obj->getCachedAABB().min_[1];
+ endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1];
else
- endpoints[1][i].value = endpoints[1][i].obj->getCachedAABB().max_[1];
+ endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1];
}
for(unsigned int i = 0; i < endpoints[2].size(); ++i)
{
if(endpoints[2][i].minmax == 0)
- endpoints[2][i].value = endpoints[2][i].obj->getCachedAABB().min_[2];
+ endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2];
else
- endpoints[2][i].value = endpoints[2][i].obj->getCachedAABB().max_[2];
+ endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2];
}
setup();
@@ -919,13 +919,13 @@ void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, Co
std::deque<Interval*> results0, results1, results2;
- results0 = interval_trees[0]->query(obj->getCachedAABB().min_[0], obj->getCachedAABB().max_[0]);
+ results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
if(results0.size() > CUTOFF)
{
- results1 = interval_trees[1]->query(obj->getCachedAABB().min_[1], obj->getCachedAABB().max_[1]);
+ results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]);
if(results1.size() > CUTOFF)
{
- results2 = interval_trees[2]->query(obj->getCachedAABB().min_[2], obj->getCachedAABB().max_[2]);
+ results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]);
if(results2.size() > CUTOFF)
{
int d1 = results0.size();
@@ -1021,8 +1021,8 @@ void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callba
for(; iter != end; ++iter)
{
CollisionObject* active_index = *iter;
- const AABB& b0 = active_index->getCachedAABB();
- const AABB& b1 = index->getCachedAABB();
+ const AABB& b0 = active_index->getAABB();
+ const AABB& b1 = index->getAABB();
int axis2 = (axis + 1) % 3;
int axis3 = (axis + 2) % 3;
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 5c64b2e2..b7c210d7 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -157,8 +157,8 @@ namespace fcl
{ \
for(int i = 0; i < num_contacts; ++i) \
{ \
- Vec3f normal = MxV(obj1->getRotation(), node.pairs[i].normal); \
- Vec3f contact_point = MxV(obj1->getRotation(), node.pairs[i].contact_point) + obj1->getTranslation(); \
+ Vec3f normal = matMulVec(obj1->getRotation(), node.pairs[i].normal); \
+ Vec3f contact_point = matMulVec(obj1->getRotation(), node.pairs[i].contact_point) + obj1->getTranslation(); \
contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2, contact_point, normal, node.pairs[i].penetration_depth); \
} \
} \
diff --git a/trunk/fcl/src/collision_node.cpp b/trunk/fcl/src/collision_node.cpp
index c3dbe090..2e48e73e 100644
--- a/trunk/fcl/src/collision_node.cpp
+++ b/trunk/fcl/src/collision_node.cpp
@@ -97,7 +97,7 @@ void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int
last_tri2_points[0], last_tri2_points[1], last_tri2_points[2],
node->R, node->T, last_tri_P, last_tri_Q);
node->p1 = last_tri_P;
- node->p2 = MTxV(node->R, last_tri_Q - node->T);
+ node->p2 = matTransMulVec(node->R, last_tri_Q - node->T);
if(qsize <= 2)
@@ -106,7 +106,7 @@ void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int
distanceQueueRecurse(node, 0, 0, front_list, qsize);
Vec3f u = node->p2 - node->T;
- node->p2 = MTxV(node->R, u);
+ node->p2 = matTransMulVec(node->R, u);
}
diff --git a/trunk/fcl/src/geometric_shapes_intersect.cpp b/trunk/fcl/src/geometric_shapes_intersect.cpp
index af96816a..0f6fafd3 100644
--- a/trunk/fcl/src/geometric_shapes_intersect.cpp
+++ b/trunk/fcl/src/geometric_shapes_intersect.cpp
@@ -82,15 +82,47 @@ struct ccd_triangle_t : public ccd_obj_t
ccd_vec3_t c;
};
+void transformGJKObject(void* obj, const Vec3f R[3], const Vec3f& T)
+{
+ ccd_obj_t* o = (ccd_obj_t*)obj;
+ SimpleQuaternion q_;
+ q_.fromRotation(R);
+
+ ccd_vec3_t t;
+ ccd_quat_t q;
+ ccdVec3Set(&t, T[0], T[1], T[2]);
+ ccdQuatSet(&q, q_.getX(), q_.getY(), q_.getZ(), q_.getW());
+
+ ccd_quat_t tmp;
+ ccdQuatMul2(&tmp, &q, &o->rot); // make sure it is correct here!!
+ ccdQuatCopy(&o->rot, &tmp);
+ ccdQuatRotVec(&o->pos, &q);
+ ccdVec3Add(&o->pos, &t);
+ ccdQuatInvert2(&o->rot_inv, &o->rot);
+}
/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, ccd_obj_t* o)
{
+
+ SimpleQuaternion q;
+ Vec3f R[3];
+ matMulMat(s.getRotation(), s.getLocalRotation(), R);
+ q.fromRotation(R);
+ Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.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);
+/*
SimpleQuaternion q;
q.fromRotation(s.getLocalRotation());
- ccdVec3Set(&o->pos, s.getLocalPosition()[0], s.getLocalPosition()[1], s.getLocalPosition()[2]);
+ Vec3f T = s.getLocalTranslation();
+ 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);
+
+ transformGJKObject(o, s.getRotation(), s.getTranslation());
+*/
}
static void boxToGJK(const Box& s, ccd_box_t* box)
@@ -580,23 +612,4 @@ void triDeleteGJKObject(void* o_)
delete o;
}
-void transformGJKObject(void* obj, const Vec3f R[3], const Vec3f& T)
-{
- ccd_obj_t* o = (ccd_obj_t*)obj;
- SimpleQuaternion q_;
- q_.fromRotation(R);
-
- ccd_vec3_t t;
- ccd_quat_t q;
- ccdVec3Set(&t, T[0], T[1], T[2]);
- ccdQuatSet(&q, q_.getX(), q_.getY(), q_.getZ(), q_.getW());
-
- ccd_quat_t tmp;
- ccdQuatMul2(&tmp, &q, &o->rot); // make sure it is correct here!!
- ccdQuatCopy(&o->rot, &tmp);
- ccdQuatRotVec(&o->pos, &q);
- ccdVec3Add(&o->pos, &t);
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-}
-
}
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index dd78440b..e8c4a0a5 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -48,15 +48,15 @@ void computeBV<AABB>(const Box& s, AABB& bv)
BVH_REAL y_range = 0.5 * (fabs(s.getLocalRotation()[1][0] * s.side[0]) + fabs(s.getLocalRotation()[1][1] * s.side[1]) + fabs(s.getLocalRotation()[1][2] * s.side[2]));
BVH_REAL z_range = 0.5 * (fabs(s.getLocalRotation()[2][0] * s.side[0]) + fabs(s.getLocalRotation()[2][1] * s.side[1]) + fabs(s.getLocalRotation()[2][2] * s.side[2]));
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
+ bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
+ bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
}
template<>
void computeBV<AABB>(const Sphere& s, AABB& bv)
{
- bv.max_ = s.getLocalPosition() + Vec3f(s.radius, s.radius, s.radius);
- bv.min_ = s.getLocalPosition() + Vec3f(-s.radius, -s.radius, -s.radius);
+ bv.max_ = s.getLocalTranslation() + Vec3f(s.radius, s.radius, s.radius);
+ bv.min_ = s.getLocalTranslation() + Vec3f(-s.radius, -s.radius, -s.radius);
}
template<>
@@ -66,8 +66,8 @@ void computeBV<AABB>(const Capsule& s, AABB& bv)
BVH_REAL y_range = 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz) + s.radius;
BVH_REAL z_range = 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz) + s.radius;
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
+ bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
+ bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
}
template<>
@@ -77,8 +77,8 @@ void computeBV<AABB>(const Cone& s, AABB& bv)
BVH_REAL y_range = fabs(s.getLocalRotation()[1][0] * s.radius) + fabs(s.getLocalRotation()[1][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz);
BVH_REAL z_range = fabs(s.getLocalRotation()[2][0] * s.radius) + fabs(s.getLocalRotation()[2][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz);
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
+ bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
+ bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
}
template<>
@@ -88,8 +88,8 @@ void computeBV<AABB>(const Cylinder& s, AABB& bv)
BVH_REAL y_range = fabs(s.getLocalRotation()[1][0] * s.radius) + fabs(s.getLocalRotation()[1][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[1][2] * s.lz);
BVH_REAL z_range = fabs(s.getLocalRotation()[2][0] * s.radius) + fabs(s.getLocalRotation()[2][1] * s.radius) + 0.5 * fabs(s.getLocalRotation()[2][2] * s.lz);
- bv.max_ = s.getLocalPosition() + Vec3f(x_range, y_range, z_range);
- bv.min_ = s.getLocalPosition() + Vec3f(-x_range, -y_range, -z_range);
+ bv.max_ = s.getLocalTranslation() + Vec3f(x_range, y_range, z_range);
+ bv.min_ = s.getLocalTranslation() + Vec3f(-x_range, -y_range, -z_range);
}
template<>
@@ -98,7 +98,7 @@ void computeBV<AABB>(const Convex& s, AABB& bv)
AABB bv_;
for(int i = 0; i < s.num_points; ++i)
{
- Vec3f new_p = MxV(s.getLocalRotation(), s.points[i]) + s.getLocalPosition();
+ Vec3f new_p = matMulVec(s.getLocalRotation(), s.points[i]) + s.getLocalTranslation();
bv_ += new_p;
}
@@ -135,7 +135,7 @@ void computeBV<AABB>(const Plane& s, AABB& bv)
template<>
void computeBV<OBB>(const Box& s, OBB& bv)
{
- bv.To = s.getLocalPosition();
+ bv.To = s.getLocalTranslation();
bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
@@ -145,7 +145,7 @@ void computeBV<OBB>(const Box& s, OBB& bv)
template<>
void computeBV<OBB>(const Sphere& s, OBB& bv)
{
- bv.To = s.getLocalPosition();
+ bv.To = s.getLocalTranslation();
bv.axis[0] = Vec3f(1, 0, 0);
bv.axis[1] = Vec3f(0, 1, 0);
bv.axis[2] = Vec3f(0, 0, 1);
@@ -155,7 +155,7 @@ void computeBV<OBB>(const Sphere& s, OBB& bv)
template<>
void computeBV<OBB>(const Capsule& s, OBB& bv)
{
- bv.To = s.getLocalPosition();
+ bv.To = s.getLocalTranslation();
bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
@@ -165,7 +165,7 @@ void computeBV<OBB>(const Capsule& s, OBB& bv)
template<>
void computeBV<OBB>(const Cone& s, OBB& bv)
{
- bv.To = s.getLocalPosition();
+ bv.To = s.getLocalTranslation();
bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
@@ -175,7 +175,7 @@ void computeBV<OBB>(const Cone& s, OBB& bv)
template<>
void computeBV<OBB>(const Cylinder& s, OBB& bv)
{
- bv.To = s.getLocalPosition();
+ bv.To = s.getLocalTranslation();
bv.axis[0] = Vec3f(s.getLocalRotation()[0][0], s.getLocalRotation()[1][0], s.getLocalRotation()[2][0]);
bv.axis[1] = Vec3f(s.getLocalRotation()[0][1], s.getLocalRotation()[1][1], s.getLocalRotation()[2][1]);
bv.axis[2] = Vec3f(s.getLocalRotation()[0][2], s.getLocalRotation()[1][2], s.getLocalRotation()[2][2]);
@@ -188,15 +188,15 @@ void computeBV<OBB>(const Convex& s, OBB& bv)
fit(s.points, s.num_points, bv);
Vec3f axis[3];
- axis[0] = MxV(s.getLocalRotation(), bv.axis[0]);
- axis[1] = MxV(s.getLocalRotation(), bv.axis[1]);
- axis[2] = MxV(s.getLocalRotation(), bv.axis[2]);
+ axis[0] = matMulVec(s.getLocalRotation(), bv.axis[0]);
+ axis[1] = matMulVec(s.getLocalRotation(), bv.axis[1]);
+ axis[2] = matMulVec(s.getLocalRotation(), bv.axis[2]);
bv.axis[0] = axis[0];
bv.axis[1] = axis[1];
bv.axis[2] = axis[2];
- bv.To = MxV(s.getLocalRotation(), bv.To) + s.getLocalPosition();
+ bv.To = matMulVec(s.getLocalRotation(), bv.To) + s.getLocalTranslation();
}
@@ -235,63 +235,63 @@ void computeBV<OBB>(const Plane& s, OBB& bv)
bv.extent = Vec3f(0, std::numeric_limits<BVH_REAL>::max(), std::numeric_limits<BVH_REAL>::max());
Vec3f p = s.n * s.d;
- bv.To = MxV(s.getLocalRotation(), p) + s.getLocalPosition();
+ bv.To = matMulVec(s.getLocalRotation(), p) + s.getLocalTranslation();
}
-void Box::computeAABB()
+void Box::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb_local);
+ aabb = aabb_local;
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
-void Sphere::computeAABB()
+void Sphere::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
+ computeBV<AABB>(*this, aabb_local);
+ aabb = aabb_local;
+ aabb_center = aabb_local.center();
aabb_radius = radius;
}
-void Capsule::computeAABB()
+void Capsule::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb_local);
+ aabb = aabb_local;
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
-void Cone::computeAABB()
+void Cone::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb_local);
+ aabb = aabb_local;
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
-void Cylinder::computeAABB()
+void Cylinder::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb_local);
+ aabb = aabb_local;
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
-void Convex::computeAABB()
+void Convex::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb_local);
+ aabb = aabb_local;
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
-void Plane::computeAABB()
+void Plane::computeLocalAABB()
{
- computeBV<AABB>(*this, aabb);
- aabb_cache = aabb;
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, aabb_local);
+ aabb = aabb_local;
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 2dcde43d..f35fd554 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -1463,14 +1463,14 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
- double sigma = MxV(uc1[i].Sigma, fgrad).dot(fgrad);
+ double sigma = matMulVec(uc1[i].Sigma, fgrad).dot(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
else
{
- double sigma = MxV(uc2[i - nPositiveExamples].Sigma, fgrad).dot(fgrad);
+ double sigma = matMulVec(uc2[i - nPositiveExamples].Sigma, fgrad).dot(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1576,7 +1576,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
coord0[0] = cloud2[i][0];
coord0[1] = cloud2[i][1];
coord0[2] = cloud2[i][2];
- coord1 = MxV(R, coord0) + T; // rotate the coordinate
+ coord1 = matMulVec(R, coord0) + T; // rotate the coordinate
words[0].wnum = 1;
words[0].weight = coord1[0];
@@ -1651,7 +1651,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
- double sigma = MxV(uc1[i].Sigma, fgrad).dot(fgrad);
+ double sigma = matMulVec(uc1[i].Sigma, fgrad).dot(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1660,7 +1660,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
{
Vec3f rotatedSigma[3];
SMST(uc2[i - nPositiveExamples].Sigma, R, rotatedSigma);
- double sigma = MxV(rotatedSigma, fgrad).dot(fgrad);
+ double sigma = matMulVec(rotatedSigma, fgrad).dot(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1738,7 +1738,7 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
BVH_REAL max_prob = 0;
for(int i = 0; i < size_cloud1; ++i)
{
- Vec3f projected_p = MxV(P, cloud1[i]) + delta;
+ Vec3f projected_p = matMulVec(P, cloud1[i]) + delta;
// compute the projected uncertainty by P * S * P'
const Vec3f* S = uc1[i].Sigma;
@@ -2177,9 +2177,9 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
Vec3f& P, Vec3f& Q)
{
Vec3f T_transformed[3];
- T_transformed[0] = MxV(R, T[0]) + Tl;
- T_transformed[1] = MxV(R, T[1]) + Tl;
- T_transformed[2] = MxV(R, T[2]) + Tl;
+ T_transformed[0] = matMulVec(R, T[0]) + Tl;
+ T_transformed[1] = matMulVec(R, T[1]) + Tl;
+ T_transformed[2] = matMulVec(R, T[2]) + Tl;
return triDistance(S, T_transformed, P, Q);
}
@@ -2189,9 +2189,9 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const V
const Vec3f R[3], const Vec3f& Tl,
Vec3f& P, Vec3f& Q)
{
- Vec3f T1_transformed = MxV(R, T1) + Tl;
- Vec3f T2_transformed = MxV(R, T2) + Tl;
- Vec3f T3_transformed = MxV(R, T3) + Tl;
+ Vec3f T1_transformed = matMulVec(R, T1) + Tl;
+ Vec3f T2_transformed = matMulVec(R, T2) + Tl;
+ Vec3f T3_transformed = matMulVec(R, T3) + Tl;
return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index bdfb6caa..8fb13f5f 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -565,7 +565,7 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
/** turn n into the global frame */
Vec3f R[3];
motion1->getCurrentRotation(R);
- Vec3f n_transformed = MxV(R, n);
+ Vec3f n_transformed = matMulVec(R, n);
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, n_transformed);
@@ -608,7 +608,7 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[2] + model1->getBV(c1).bv.axis[2] * n[2];
Vec3f R[3];
motion1->getCurrentRotation(R);
- n_transformed = MxV(R, n_transformed);
+ n_transformed = matMulVec(R, n_transformed);
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
diff --git a/trunk/fcl/src/vec_3f.cpp b/trunk/fcl/src/vec_3f.cpp
index c17fd4fd..f4bc98b0 100644
--- a/trunk/fcl/src/vec_3f.cpp
+++ b/trunk/fcl/src/vec_3f.cpp
@@ -45,23 +45,23 @@ const float Vec3f::EPSILON = 1e-11;
const BVH_REAL Vec3f::EPSILON = 1e-11;
#endif
-Vec3f MxV(const Vec3f M[3], const Vec3f& v)
+Vec3f matMulVec(const Vec3f M[3], const Vec3f& v)
{
return Vec3f(M[0].dot(v), M[1].dot(v), M[2].dot(v));
}
-Vec3f MTxV(const Vec3f M[3], const Vec3f& v)
+Vec3f matTransMulVec(const Vec3f M[3], const Vec3f& v)
{
return M[0] * v[0] + M[1] * v[1] + M[2] * v[2];
}
-BVH_REAL vTMv(const Vec3f M[3], const Vec3f& v)
+BVH_REAL quadraticForm(const Vec3f M[3], const Vec3f& v)
{
return v.dot(Vec3f(M[0].dot(v), M[1].dot(v), M[2].dot(v)));
}
-void SMST(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3])
+void tensorTransform(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3])
{
Vec3f SMT_col[3] = {Vec3f(M[0].dot(S[0]), M[1].dot(S[0]), M[2].dot(S[0])),
Vec3f(M[0].dot(S[1]), M[1].dot(S[1]), M[2].dot(S[1])),
@@ -91,7 +91,7 @@ void relativeTransform(const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], co
R1[0][2] * temp[0] + R1[1][2] * temp[1] + R1[2][2] * temp[2]);
}
-void Meigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3])
+void matEigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3])
{
int n = 3;
int j, iq, ip, i;
@@ -179,7 +179,7 @@ void Meigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3])
}
-void MxM(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3])
+void matMulMat(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3])
{
for(int i = 0; i < 3; ++i)
{
diff --git a/trunk/fcl/test/test_core_collision.cpp b/trunk/fcl/test/test_core_collision.cpp
index d3d258cd..a60a2117 100644
--- a/trunk/fcl/test/test_core_collision.cpp
+++ b/trunk/fcl/test/test_core_collision.cpp
@@ -552,7 +552,7 @@ bool collide_Test2(const Transform& tf,
std::vector<Vec3f> vertices1_new(vertices1.size());
for(unsigned int i = 0; i < vertices1_new.size(); ++i)
{
- vertices1_new[i] = MxV(tf.R, vertices1[i]) + tf.T;
+ vertices1_new[i] = matMulVec(tf.R, vertices1[i]) + tf.T;
}
diff --git a/trunk/fcl/test/test_core_distance.cpp b/trunk/fcl/test/test_core_distance.cpp
index e97b6be6..5dd26a38 100644
--- a/trunk/fcl/test/test_core_distance.cpp
+++ b/trunk/fcl/test/test_core_distance.cpp
@@ -214,8 +214,8 @@ void distance_Test(const Transform& tf,
distance(&node, NULL, qsize);
// points are in local coordinate, to global coordinate
- Vec3f p1 = MxV(tf.R, node.p1) + tf.T;
- Vec3f p2 = MxV(R2, node.p2) + T2;
+ Vec3f p1 = matMulVec(tf.R, node.p1) + tf.T;
+ Vec3f p2 = matMulVec(R2, node.p2) + T2;
distance_result.distance = node.min_distance;
diff --git a/trunk/fcl/test/test_core_geometric_shapes.cpp b/trunk/fcl/test/test_core_geometric_shapes.cpp
index 9af5e8d8..a96b05d9 100644
--- a/trunk/fcl/test/test_core_geometric_shapes.cpp
+++ b/trunk/fcl/test/test_core_geometric_shapes.cpp
@@ -36,40 +36,90 @@
#include "fcl/geometric_shapes_intersect.h"
+#include "test_core_utility.h"
#include <gtest/gtest.h>
using namespace fcl;
+BVH_REAL extents [6] = {0, 0, 0, 10, 10, 10};
+
TEST(shapeIntersection, spheresphere)
{
Sphere s1(20);
Sphere s2(10);
+ Transform transform;
+ generateRandomTransform(extents, transform);
+ Transform identity;
+
bool res;
s2.setLocalTranslation(Vec3f(40, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(30, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(29.9, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(0, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(-29.9, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(-30, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
}
TEST(shapeIntersection, boxbox)
@@ -77,15 +127,34 @@ TEST(shapeIntersection, boxbox)
Box s1(20, 40, 50);
Box s2(10, 10, 10);
+ Transform transform;
+ generateRandomTransform(extents, transform);
+ Transform identity;
+
bool res;
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(15, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
+
SimpleQuaternion q;
q.fromAxisAngle(Vec3f(0, 0, 1), (BVH_REAL)3.140 / 6);
Vec3f R[3];
@@ -93,6 +162,14 @@ TEST(shapeIntersection, boxbox)
s2.setLocalRotation(R);
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
}
TEST(shapeIntersection, spherebox)
@@ -100,18 +177,46 @@ TEST(shapeIntersection, spherebox)
Sphere s1(20);
Box s2(5, 5, 5);
+ Transform transform;
+ generateRandomTransform(extents, transform);
+ Transform identity;
+
+
bool res;
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
+
s2.setLocalTranslation(Vec3f(22.5, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(22.4, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
}
TEST(shapeIntersection, cylindercylinder)
@@ -119,18 +224,44 @@ TEST(shapeIntersection, cylindercylinder)
Cylinder s1(5, 10);
Cylinder s2(5, 10);
+ Transform transform;
+ generateRandomTransform(extents, transform);
+ Transform identity;
+
+
bool res;
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(9.9, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(10, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
}
TEST(shapeIntersection, conecone)
@@ -138,26 +269,65 @@ TEST(shapeIntersection, conecone)
Cone s1(5, 10);
Cone s2(5, 10);
+ Transform transform;
+ generateRandomTransform(extents, transform);
+ Transform identity;
+
bool res;
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(9.9, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
- s2.setLocalTranslation(Vec3f(10, 0, 0));
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
+ s2.setLocalTranslation(Vec3f(10.001, 0, 0));
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
s2.setLocalTranslation(Vec3f(0, 0, 9.9));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(0, 0, 10));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
}
TEST(shapeIntersection, conecylinder)
@@ -165,26 +335,66 @@ TEST(shapeIntersection, conecylinder)
Cylinder s1(5, 10);
Cone s2(5, 10);
+ Transform transform;
+ generateRandomTransform(extents, transform);
+ Transform identity;
+
+
bool res;
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(9.9, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(10, 0, 0));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(0, 0, 9.9));
res = shapeIntersect(s1, s2);
ASSERT_TRUE(res);
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_TRUE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
+
s2.setLocalTranslation(Vec3f(0, 0, 10));
res = shapeIntersect(s1, s2);
ASSERT_FALSE(res);
+
+ s1.setTransform(transform.R, transform.T);
+ s2.setTransform(transform.R, transform.T);
+ res = shapeIntersect(s1, s2);
+ ASSERT_FALSE(res);
+ s1.setTransform(identity.R, identity.T);
+ s2.setTransform(identity.R, identity.T);
}
TEST(shapeIntersection, spheretriangle)
@@ -195,23 +405,42 @@ TEST(shapeIntersection, spheretriangle)
t[1] = Vec3f(-20, 0, 0);
t[2] = Vec3f(0, 20, 0);
+ Transform transform;
+ generateRandomTransform(extents, transform);
+ Transform identity;
+
bool res;
res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
ASSERT_TRUE(res);
+ s.setTransform(transform.R, transform.T);
+ res = shapeTriangleIntersect(s, t[0], t[1], t[2], transform.R, transform.T);
+ ASSERT_TRUE(res);
+ s.setTransform(identity.R, identity.T);
+
t[0] = Vec3f(30, 0, 0);
t[1] = Vec3f(10, -20, 0);
t[2] = Vec3f(10, 20, 0);
res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
ASSERT_FALSE(res);
+ s.setTransform(transform.R, transform.T);
+ res = shapeTriangleIntersect(s, t[0], t[1], t[2], transform.R, transform.T);
+ ASSERT_FALSE(res);
+ s.setTransform(identity.R, identity.T);
+
t[0] = Vec3f(30, 0, 0);
t[1] = Vec3f(9.9, -20, 0);
t[2] = Vec3f(9.9, 20, 0);
res = shapeTriangleIntersect(s, t[0], t[1], t[2]);
ASSERT_TRUE(res);
+ s.setTransform(transform.R, transform.T);
+ res = shapeTriangleIntersect(s, t[0], t[1], t[2], transform.R, transform.T);
+ ASSERT_TRUE(res);
+ s.setTransform(identity.R, identity.T);
+
}
diff --git a/trunk/fcl/test/test_core_utility.h b/trunk/fcl/test/test_core_utility.h
index d2b410a4..b6afa4e3 100644
--- a/trunk/fcl/test/test_core_utility.h
+++ b/trunk/fcl/test/test_core_utility.h
@@ -39,7 +39,10 @@
#define FCL_TEST_CORE_UTILITY_H
#include "fcl/vec_3f.h"
+#include "fcl/primitive.h"
#include <cstdio>
+#include <vector>
+#include <iostream>
using namespace fcl;
#if USE_PQP
@@ -50,6 +53,13 @@ struct Transform
{
Vec3f R[3];
Vec3f T;
+
+ Transform()
+ {
+ R[0][0] = 1;
+ R[1][1] = 1;
+ R[2][2] = 1;
+ }
};
@@ -557,6 +567,21 @@ inline void eulerToMatrix(BVH_REAL a, BVH_REAL b, BVH_REAL c, Vec3f R[3])
R[2] = Vec3f(s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3);
}
+inline void generateRandomTransform(BVH_REAL extents[6], Transform& transform)
+{
+ BVH_REAL x = rand_interval(extents[0], extents[3]);
+ BVH_REAL y = rand_interval(extents[1], extents[4]);
+ BVH_REAL z = rand_interval(extents[2], extents[5]);
+
+ const BVH_REAL pi = 3.1415926;
+ BVH_REAL a = rand_interval(0, 2 * pi);
+ BVH_REAL b = rand_interval(0, 2 * pi);
+ BVH_REAL c = rand_interval(0, 2 * pi);
+
+ eulerToMatrix(a, b, c, transform.R);
+ transform.T = Vec3f(x, y, z);
+}
+
inline void generateRandomTransform(BVH_REAL extents[6], std::vector<Transform>& transforms, std::vector<Transform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n)
{
transforms.resize(n);
--
GitLab