diff --git a/trunk/fcl/include/fcl/BV.h b/trunk/fcl/include/fcl/BV.h
index 332600de3842176d9906ffa7d5eebc87a24963a4..6dd515b2df2d3ee54583e2e9879c1153d72a084e 100644
--- a/trunk/fcl/include/fcl/BV.h
+++ b/trunk/fcl/include/fcl/BV.h
@@ -55,7 +55,7 @@ template<typename BV1, typename BV2>
class Converter
{
private:
- static void convert(const BV1& bv1, const SimpleTransform& tf1, BV2& bv2)
+ static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
{
// should only use the specialized version, so it is private.
}
@@ -65,7 +65,7 @@ template<>
class Converter<AABB, AABB>
{
public:
- static void convert(const AABB& bv1, const SimpleTransform& tf1, AABB& bv2)
+ static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5;
@@ -80,7 +80,7 @@ template<>
class Converter<AABB, OBB>
{
public:
- static void convert(const AABB& bv1, const SimpleTransform& tf1, OBB& bv2)
+ static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
{
/*
bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
@@ -129,7 +129,7 @@ template<>
class Converter<OBB, OBB>
{
public:
- static void convert(const OBB& bv1, const SimpleTransform& tf1, OBB& bv2)
+ static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = bv1.extent;
bv2.To = tf1.transform(bv1.To);
@@ -143,7 +143,7 @@ template<>
class Converter<OBBRSS, OBB>
{
public:
- static void convert(const OBBRSS& bv1, const SimpleTransform& tf1, OBB& bv2)
+ static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2)
{
Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
}
@@ -153,7 +153,7 @@ template<>
class Converter<RSS, OBB>
{
public:
- static void convert(const RSS& bv1, const SimpleTransform& tf1, OBB& bv2)
+ static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.To = tf1.transform(bv1.Tr);
@@ -168,7 +168,7 @@ template<typename BV1>
class Converter<BV1, AABB>
{
public:
- static void convert(const BV1& bv1, const SimpleTransform& tf1, AABB& bv2)
+ static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
@@ -183,10 +183,10 @@ template<typename BV1>
class Converter<BV1, OBB>
{
public:
- static void convert(const BV1& bv1, const SimpleTransform& tf1, OBB& bv2)
+ static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
{
AABB bv;
- Converter<BV1, AABB>::convert(bv1, SimpleTransform(), bv);
+ Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
Converter<AABB, OBB>::convert(bv, tf1, bv2);
}
};
@@ -195,7 +195,7 @@ template<>
class Converter<OBB, RSS>
{
public:
- static void convert(const OBB& bv1, const SimpleTransform& tf1, RSS& bv2)
+ static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
@@ -212,7 +212,7 @@ template<>
class Converter<RSS, RSS>
{
public:
- static void convert(const RSS& bv1, const SimpleTransform& tf1, RSS& bv2)
+ static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
@@ -229,7 +229,7 @@ template<>
class Converter<OBBRSS, RSS>
{
public:
- static void convert(const OBBRSS& bv1, const SimpleTransform& tf1, RSS& bv2)
+ static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2)
{
Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
}
@@ -239,7 +239,7 @@ template<>
class Converter<AABB, RSS>
{
public:
- static void convert(const AABB& bv1, const SimpleTransform& tf1, RSS& bv2)
+ static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.center());
FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
@@ -282,7 +282,7 @@ public:
template<typename BV1, typename BV2>
-static inline void convertBV(const BV1& bv1, const SimpleTransform& tf1, BV2& bv2)
+static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
{
Converter<BV1, BV2>::convert(bv1, tf1, bv2);
}
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index ed5f51797841e4704f8b38fb80a05c5092a3cdf4..a7388d05bfcd51271c324877eb1ca91c1c7c1722 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -1454,9 +1454,9 @@ private:
void update_(CollisionObject* updated_obj);
/** \brief special manager-obj collision for octree */
- bool collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const;
+ bool collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
- bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+ bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
};
@@ -1641,9 +1641,9 @@ private:
void update_(CollisionObject* updated_obj);
/** \brief special manager-obj collision for octree */
- bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const;
+ bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
- bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+ bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
};
diff --git a/trunk/fcl/include/fcl/collision.h b/trunk/fcl/include/fcl/collision.h
index 0d49275f34b383f6219879139273e3f64fa31532..df3fa31de08ff5a019b6b6350c1ec8965e794244 100644
--- a/trunk/fcl/include/fcl/collision.h
+++ b/trunk/fcl/include/fcl/collision.h
@@ -59,8 +59,8 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
CollisionResult& result);
template<typename NarrowPhaseSolver>
-std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result);
@@ -69,8 +69,8 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
const CollisionRequest& request,
CollisionResult& result);
-std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result);
}
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index 5b4dad8368856f656c65d55761c089253a66e489..57c32b12cc1d625392b8f9a730e4bead06e23efa 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -48,7 +48,7 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix
{
- typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result);
+ typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result);
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index b73a42ee93c8130d6572f9d70fd8552dfbe9d406..6576194de5d4d1a081e91774e6b39e6e3ef6ac28 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -114,14 +114,14 @@ public:
computeAABB();
}
- CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const SimpleTransform& tf) : cgeom(cgeom_), t(tf)
+ CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Transform3f& tf) : cgeom(cgeom_), t(tf)
{
cgeom->computeLocalAABB();
computeAABB();
}
CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
- cgeom(cgeom_), t(SimpleTransform(R, T))
+ cgeom(cgeom_), t(Transform3f(R, T))
{
cgeom->computeLocalAABB();
computeAABB();
@@ -185,12 +185,12 @@ public:
return t.getRotation();
}
- inline const SimpleQuaternion& getQuatRotation() const
+ inline const Quaternion3f& getQuatRotation() const
{
return t.getQuatRotation();
}
- inline const SimpleTransform& getTransform() const
+ inline const Transform3f& getTransform() const
{
return t;
}
@@ -205,7 +205,7 @@ public:
t.setTranslation(T);
}
- void setQuatRotation(const SimpleQuaternion& q)
+ void setQuatRotation(const Quaternion3f& q)
{
t.setQuatRotation(q);
}
@@ -215,12 +215,12 @@ public:
t.setTransform(R, T);
}
- void setTransform(const SimpleQuaternion& q, const Vec3f& T)
+ void setTransform(const Quaternion3f& q, const Vec3f& T)
{
t.setTransform(q, T);
}
- void setTransform(const SimpleTransform& tf)
+ void setTransform(const Transform3f& tf)
{
t = tf;
}
@@ -269,7 +269,7 @@ protected:
boost::shared_ptr<CollisionGeometry> cgeom;
- SimpleTransform t;
+ Transform3f t;
/// AABB in global coordinate
mutable AABB aabb;
diff --git a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
deleted file mode 100644
index 6158e1fc1bf99d1ce1b2af5f930cdf9e424d7f3f..0000000000000000000000000000000000000000
--- a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
+++ /dev/null
@@ -1,959 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#include "fcl/geometric_shapes_intersect.h"
-#include <ccd/simplex.h>
-#include <ccd/vec3.h>
-
-namespace fcl
-{
-
-namespace details
-{
-
-struct ccd_obj_t
-{
- ccd_vec3_t pos;
- ccd_quat_t rot, rot_inv;
-};
-
-struct ccd_box_t : public ccd_obj_t
-{
- ccd_real_t dim[3];
-};
-
-struct ccd_cap_t : public ccd_obj_t
-{
- ccd_real_t radius, height;
-};
-
-struct ccd_cyl_t : public ccd_obj_t
-{
- ccd_real_t radius, height;
-};
-
-struct ccd_cone_t : public ccd_obj_t
-{
- ccd_real_t radius, height;
-};
-
-struct ccd_sphere_t : public ccd_obj_t
-{
- ccd_real_t radius;
-};
-
-struct ccd_convex_t : public ccd_obj_t
-{
- const Convex* convex;
-};
-
-struct ccd_triangle_t : public ccd_obj_t
-{
- ccd_vec3_t p[3];
- ccd_vec3_t c;
-};
-
-static void tripleCross(const ccd_vec3_t* a, const ccd_vec3_t* b,
- const ccd_vec3_t* c, ccd_vec3_t* d)
-{
- ccd_vec3_t e;
- ccdVec3Cross(&e, a, b);
- ccdVec3Cross(d, &e, c);
-}
-
-static int doSimplex2Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
-{
- const ccd_support_t *A, *B;
- ccd_vec3_t AB, AO, tmp;
- ccd_real_t dot;
-
- // get last added as A
- A = ccdSimplexLast(simplex);
- // get the other point
- B = ccdSimplexPoint(simplex, 0);
- // compute AB oriented segment
- ccdVec3Sub2(&AB, &B->v, &A->v);
- // compute AO vector
- ccdVec3Copy(&AO, &A->v);
- ccdVec3Scale(&AO, -CCD_ONE);
-
- // dot product AB . -AO
- dot = ccdVec3Dot(&AB, &AO);
-
- // check if origin doesn't lie on AB segment
- ccdVec3Cross(&tmp, &AB, &AO);
- if(ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO)
- {
- return 1;
- }
-
- *dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, &A->v, &B->v, NULL);
-
- // check if origin is in area where AB segment is
- if(ccdIsZero(dot) || dot < CCD_ZERO)
- {
- // origin is in outside are of A
- ccdSimplexSet(simplex, 0, A);
- ccdSimplexSetSize(simplex, 1);
- ccdVec3Copy(dir, &AO);
- }
- else
- {
- // origin is in area where AB segment is
-
- // keep simplex untouched and set direction to
- // AB x AO x AB
- tripleCross(&AB, &AO, &AB, dir);
- }
-
- return 0;
-}
-
-static int doSimplex3Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
-{
- const ccd_support_t *A, *B, *C;
- ccd_vec3_t AO, AB, AC, ABC, tmp;
- ccd_real_t dot;
-
- // get last added as A
- A = ccdSimplexLast(simplex);
- // get the other points
- B = ccdSimplexPoint(simplex, 1);
- C = ccdSimplexPoint(simplex, 0);
-
- // check touching contact
- *dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
- if(ccdIsZero(*dist))
- {
- return 1;
- }
-
- // check if triangle is really triangle (has area > 0)
- // if not simplex can't be expanded and thus no itersection is found
- if(ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v))
- {
- return -1;
- }
-
- // compute AO vector
- ccdVec3Copy(&AO, &A->v);
- ccdVec3Scale(&AO, -CCD_ONE);
-
- // compute AB and AC segments and ABC vector (perpendircular to triangle)
- ccdVec3Sub2(&AB, &B->v, &A->v);
- ccdVec3Sub2(&AC, &C->v, &A->v);
- ccdVec3Cross(&ABC, &AB, &AC);
-
- ccdVec3Cross(&tmp, &ABC, &AC);
- dot = ccdVec3Dot(&tmp, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
- dot = ccdVec3Dot(&AC, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
- // C is already in place
- ccdSimplexSet(simplex, 1, A);
- ccdSimplexSetSize(simplex, 2);
- tripleCross(&AC, &AO, &AC, dir);
- }
- else
- {
- ccd_do_simplex3_45:
- dot = ccdVec3Dot(&AB, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
- ccdSimplexSet(simplex, 0, B);
- ccdSimplexSet(simplex, 1, A);
- ccdSimplexSetSize(simplex, 2);
- tripleCross(&AB, &AO, &AB, dir);
- }
- else
- {
- ccdSimplexSet(simplex, 0, A);
- ccdSimplexSetSize(simplex, 1);
- ccdVec3Copy(dir, &AO);
- }
- }
- }
- else
- {
- ccdVec3Cross(&tmp, &AB, &ABC);
- dot = ccdVec3Dot(&tmp, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
- goto ccd_do_simplex3_45;
- }
- else
- {
- dot = ccdVec3Dot(&ABC, &AO);
- if (ccdIsZero(dot) || dot > CCD_ZERO)
- {
- ccdVec3Copy(dir, &ABC);
- }
- else
- {
- ccd_support_t Ctmp;
- ccdSupportCopy(&Ctmp, C);
- ccdSimplexSet(simplex, 0, B);
- ccdSimplexSet(simplex, 1, &Ctmp);
-
- ccdVec3Copy(dir, &ABC);
- ccdVec3Scale(dir, -CCD_ONE);
- }
- }
- }
-
-
- return 0;
-}
-
-static int doSimplex4Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
-{
- const ccd_support_t *A, *B, *C, *D;
- ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
- int B_on_ACD, C_on_ADB, D_on_ABC;
- int AB_O, AC_O, AD_O;
-
- // get last added as A
- A = ccdSimplexLast(simplex);
- // get the other points
- B = ccdSimplexPoint(simplex, 2);
- C = ccdSimplexPoint(simplex, 1);
- D = ccdSimplexPoint(simplex, 0);
-
- // check if tetrahedron is really tetrahedron (has volume > 0)
- // if it is not simplex can't be expanded and thus no intersection is
- // found
- ccd_real_t volume = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
- if(ccdIsZero(volume))
- {
- return -1;
- }
-
- // check if origin lies on some of tetrahedron's face - if so objects
- // intersect
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
- if(ccdIsZero(volume))
- return 1;
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
- if(ccdIsZero(volume))
- return 1;
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
- if(ccdIsZero(volume))
- return 1;
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
- if(ccdIsZero(volume))
- return 1;
-
- // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
- ccdVec3Copy(&AO, &A->v);
- ccdVec3Scale(&AO, -CCD_ONE);
- ccdVec3Sub2(&AB, &B->v, &A->v);
- ccdVec3Sub2(&AC, &C->v, &A->v);
- ccdVec3Sub2(&AD, &D->v, &A->v);
- ccdVec3Cross(&ABC, &AB, &AC);
- ccdVec3Cross(&ACD, &AC, &AD);
- ccdVec3Cross(&ADB, &AD, &AB);
-
- // side (positive or negative) of B, C, D relative to planes ACD, ADB
- // and ABC respectively
- B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
- C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
- D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
-
- // whether origin is on same side of ACD, ADB, ABC as B, C, D
- // respectively
- AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
- AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
- AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
-
- if(AB_O && AC_O && AD_O)
- {
- // origin is in tetrahedron
- return 1;
-
- // rearrange simplex to triangle and call doSimplex3Dist()
- }
- else if(!AB_O)
- {
- // B is farthest from the origin among all of the tetrahedron's
- // points, so remove it from the list and go on with the triangle
- // case
-
- // D and C are in place
- ccdSimplexSet(simplex, 2, A);
- ccdSimplexSetSize(simplex, 3);
- }
- else if(!AC_O)
- {
- // C is farthest
- ccdSimplexSet(simplex, 1, D);
- ccdSimplexSet(simplex, 0, B);
- ccdSimplexSet(simplex, 2, A);
- ccdSimplexSetSize(simplex, 3);
- }
- else
- {
- // (!AD_O)
- ccdSimplexSet(simplex, 0, C);
- ccdSimplexSet(simplex, 1, B);
- ccdSimplexSet(simplex, 2, A);
- ccdSimplexSetSize(simplex, 3);
- }
-
- return doSimplex3Dist(simplex, dir, dist);
-}
-
-static int doSimplexDist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
-{
- if(ccdSimplexSize(simplex) == 2)
- {
- // simplex contains segment only one segment
- return doSimplex2Dist(simplex, dir, dist);
- }
- else if(ccdSimplexSize(simplex) == 3)
- {
- // simplex contains triangle
- return doSimplex3Dist(simplex, dir, dist);
- }
- else
- {
- // ccdSimplexSize(simplex) == 4
- // tetrahedron - this is the only shape which can encapsule origin
- // so doSimplex4() also contains test on it
- return doSimplex4Dist(simplex, dir, dist);
- }
-}
-
-
-static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
- const ccd_t *ccd, ccd_simplex_t *simplex)
-{
- unsigned long iterations;
- ccd_vec3_t dir; // direction vector
- ccd_support_t last; // last support point
- int do_simplex_res;
- ccd_real_t min_dist = -1;
-
- // initialize simplex struct
- ccdSimplexInit(simplex);
-
- // get first direction
- ccd->first_dir(obj1, obj2, &dir);
- // get first support point
- __ccdSupport(obj1, obj2, &dir, ccd, &last);
- // and add this point to simplex as last one
- ccdSimplexAdd(simplex, &last);
-
- // set up direction vector to as (O - last) which is exactly -last
- ccdVec3Copy(&dir, &last.v);
- ccdVec3Scale(&dir, -CCD_ONE);
-
- // start iterations
- for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
- {
- // obtain support point
- __ccdSupport(obj1, obj2, &dir, ccd, &last);
-
- // check if farthest point in Minkowski difference in direction dir
- // isn't somewhere before origin (the test on negative dot product)
- // - because if it is, objects are not intersecting at all.
- if(ccdVec3Dot(&last.v, &dir) < CCD_ZERO)
- {
- ccd_real_t dist = ccdVec3Len2(&last.v);
- if(min_dist < 0) min_dist = dist;
- else
- min_dist = std::min(min_dist, dist);
- }
-
- // add last support vector to simplex
- ccdSimplexAdd(simplex, &last);
-
- // if doSimplex returns 1 if objects intersect, -1 if objects don't
- // intersect and 0 if algorithm should continue
- ccd_real_t dist;
- do_simplex_res = doSimplexDist(simplex, &dir, &dist);
-
- if(do_simplex_res == 1)
- {
- return -1; // intersection found
- }
- else if(do_simplex_res == -1)
- {
- return min_dist;
- }
-
- if(ccdIsZero(ccdVec3Len2(&dir)))
- {
- return min_dist; // intersection not found
- }
-
- if(min_dist > 0)
- {
- if(fabs(min_dist - dist) < 0.000001 && iterations > 0)
- break;
- else
- min_dist = std::min(min_dist, dist);
- }
- else min_dist = dist;
- }
-
- // intersection wasn't found
- return min_dist;
-}
-
-
-/** Basic shape to ccd shape */
-static void shapeToGJK(const ShapeBase& s, const SimpleTransform& tf, ccd_obj_t* o)
-{
- 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);
-}
-
-static void boxToGJK(const Box& s, const SimpleTransform& tf, ccd_box_t* box)
-{
- shapeToGJK(s, tf, box);
- box->dim[0] = s.side[0] / 2.0;
- box->dim[1] = s.side[1] / 2.0;
- box->dim[2] = s.side[2] / 2.0;
-}
-
-static void capToGJK(const Capsule& s, const SimpleTransform& tf, ccd_cap_t* cap)
-{
- shapeToGJK(s, tf, cap);
- cap->radius = s.radius;
- cap->height = s.lz / 2;
-}
-
-static void cylToGJK(const Cylinder& s, const SimpleTransform& tf, ccd_cyl_t* cyl)
-{
- shapeToGJK(s, tf, cyl);
- cyl->radius = s.radius;
- cyl->height = s.lz / 2;
-}
-
-static void coneToGJK(const Cone& s, const SimpleTransform& tf, ccd_cone_t* cone)
-{
- shapeToGJK(s, tf, cone);
- cone->radius = s.radius;
- cone->height = s.lz / 2;
-}
-
-static void sphereToGJK(const Sphere& s, const SimpleTransform& tf, ccd_sphere_t* sph)
-{
- shapeToGJK(s, tf, sph);
- sph->radius = s.radius;
-}
-
-static void convexToGJK(const Convex& s, const SimpleTransform& tf, ccd_convex_t* conv)
-{
- shapeToGJK(s, tf, conv);
- conv->convex = &s;
-}
-
-/** Support functions */
-static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_box_t* o = (const ccd_box_t*)obj;
- ccd_vec3_t dir;
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &o->rot_inv);
- ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
- ccdSign(ccdVec3Y(&dir)) * o->dim[1],
- ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
- ccdQuatRotVec(v, &o->rot);
- ccdVec3Add(v, &o->pos);
-}
-
-static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_cap_t* o = (const ccd_cap_t*)obj;
- ccd_vec3_t dir, pos1, pos2;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &o->rot_inv);
-
- ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
- ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
-
- ccdVec3Copy(v, &dir);
- ccdVec3Scale(v, o->radius);
- ccdVec3Add(&pos1, v);
- ccdVec3Add(&pos2, v);
-
- if(ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2))
- ccdVec3Copy(v, &pos1);
- else
- ccdVec3Copy(v, &pos2);
-
- // transform support vertex
- ccdQuatRotVec(v, &o->rot);
- ccdVec3Add(v, &o->pos);
-}
-
-static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_cyl_t* cyl = (const ccd_cyl_t*)obj;
- ccd_vec3_t dir;
- double zdist, rad;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &cyl->rot_inv);
-
- zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
- zdist = sqrt(zdist);
- if(ccdIsZero(zdist))
- ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
- else
- {
- rad = cyl->radius / zdist;
-
- ccdVec3Set(v, rad * ccdVec3X(&dir),
- rad * ccdVec3Y(&dir),
- ccdSign(ccdVec3Z(&dir)) * cyl->height);
- }
-
- // transform support vertex
- ccdQuatRotVec(v, &cyl->rot);
- ccdVec3Add(v, &cyl->pos);
-}
-
-static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_cone_t* cone = (const ccd_cone_t*)obj;
- ccd_vec3_t dir;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &cone->rot_inv);
-
- double zdist, len, rad;
- zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
- len = zdist + dir.v[2] * dir.v[2];
- zdist = sqrt(zdist);
- len = sqrt(len);
-
- double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
-
- if(dir.v[2] > len * sin_a)
- ccdVec3Set(v, 0., 0., cone->height);
- else if(zdist > 0)
- {
- rad = cone->radius / zdist;
- ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
- }
- else
- ccdVec3Set(v, 0, 0, -cone->height);
-
- // transform support vertex
- ccdQuatRotVec(v, &cone->rot);
- ccdVec3Add(v, &cone->pos);
-}
-
-static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_sphere_t* s = (const ccd_sphere_t*)obj;
- ccd_vec3_t dir;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &s->rot_inv);
-
- ccdVec3Copy(v, &dir);
- ccdVec3Scale(v, s->radius);
- ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
-
- // transform support vertex
- ccdQuatRotVec(v, &s->rot);
- ccdVec3Add(v, &s->pos);
-}
-
-static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_convex_t* c = (const ccd_convex_t*)obj;
- ccd_vec3_t dir, p;
- ccd_real_t maxdot, dot;
- int i;
- Vec3f* curp;
- const Vec3f& center = c->convex->center;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &c->rot_inv);
-
- maxdot = -CCD_REAL_MAX;
- curp = c->convex->points;
-
- for(i = 0; i < c->convex->num_points; ++i, curp += 1)
- {
- ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]);
- dot = ccdVec3Dot(&dir, &p);
- if(dot > maxdot)
- {
- ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]);
- maxdot = dot;
- }
- }
-
- // transform support vertex
- ccdQuatRotVec(v, &c->rot);
- ccdVec3Add(v, &c->pos);
-}
-
-static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
-{
- const ccd_triangle_t* tri = (const ccd_triangle_t*)obj;
- ccd_vec3_t dir, p;
- ccd_real_t maxdot, dot;
- int i;
-
- ccdVec3Copy(&dir, dir_);
- ccdQuatRotVec(&dir, &tri->rot_inv);
-
- maxdot = -CCD_REAL_MAX;
-
- for(i = 0; i < 3; ++i)
- {
- ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]);
- dot = ccdVec3Dot(&dir, &p);
- if(dot > maxdot)
- {
- ccdVec3Copy(v, &tri->p[i]);
- maxdot = dot;
- }
- }
-
- // transform support vertex
- ccdQuatRotVec(v, &tri->rot);
- ccdVec3Add(v, &tri->pos);
-}
-
-static void centerShape(const void* obj, ccd_vec3_t* c)
-{
- const ccd_obj_t *o = (const ccd_obj_t*)obj;
- ccdVec3Copy(c, &o->pos);
-}
-
-static void centerConvex(const void* obj, ccd_vec3_t* c)
-{
- const ccd_convex_t *o = (const ccd_convex_t*)obj;
- ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]);
- ccdQuatRotVec(c, &o->rot);
- ccdVec3Add(c, &o->pos);
-}
-
-static void centerTriangle(const void* obj, ccd_vec3_t* c)
-{
- const ccd_triangle_t *o = (const ccd_triangle_t*)obj;
- ccdVec3Copy(c, &o->c);
- ccdQuatRotVec(c, &o->rot);
- ccdVec3Add(c, &o->pos);
-}
-
-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, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- ccd_t ccd;
- int res;
- ccd_real_t depth;
- ccd_vec3_t dir, pos;
-
-
- CCD_INIT(&ccd);
- ccd.support1 = supp1;
- ccd.support2 = supp2;
- ccd.center1 = cen1;
- ccd.center2 = cen2;
- ccd.max_iterations = 500;
- ccd.mpr_tolerance = 1e-6;
-
- if(!contact_points)
- {
- return ccdMPRIntersect(obj1, obj2, &ccd);
- }
-
-
- res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
- if(res == 0)
- {
- contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
- *penetration_depth = depth;
- normal->setValue(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
-
- return true;
- }
-
- return false;
-}
-
-bool GJKDistance(void* obj1, ccd_support_fn supp1,
- void* obj2, ccd_support_fn supp2,
- FCL_REAL* res)
-{
- ccd_t ccd;
- ccd_real_t dist;
- ccd_vec3_t dir, pos;
- CCD_INIT(&ccd);
- ccd.support1 = supp1;
- ccd.support2 = supp2;
-
- ccd.max_iterations = 500;
-
- ccd_simplex_t simplex;
- dist = __ccdGJKDist(obj1, obj2, &ccd, &simplex);
- *res = dist;
- if(dist < 0) return false;
- else return true;
-}
-
-
-GJKSupportFunction GJKInitializer<Cylinder>::getSupportFunction()
-{
- return &supportCyl;
-}
-
-
-GJKCenterFunction GJKInitializer<Cylinder>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s, const SimpleTransform& tf)
-{
- ccd_cyl_t* o = new ccd_cyl_t;
- cylToGJK(s, tf, o);
- return o;
-}
-
-
-void GJKInitializer<Cylinder>::deleteGJKObject(void* o_)
-{
- ccd_cyl_t* o = (ccd_cyl_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Sphere>::getSupportFunction()
-{
- return &supportSphere;
-}
-
-
-GJKCenterFunction GJKInitializer<Sphere>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s, const SimpleTransform& tf)
-{
- ccd_sphere_t* o = new ccd_sphere_t;
- sphereToGJK(s, tf, o);
- return o;
-}
-
-void GJKInitializer<Sphere>::deleteGJKObject(void* o_)
-{
- ccd_sphere_t* o = (ccd_sphere_t*)o_;
- delete o;
-}
-
-GJKSupportFunction GJKInitializer<Box>::getSupportFunction()
-{
- return &supportBox;
-}
-
-
-GJKCenterFunction GJKInitializer<Box>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Box>::createGJKObject(const Box& s, const SimpleTransform& tf)
-{
- ccd_box_t* o = new ccd_box_t;
- boxToGJK(s, tf, o);
- return o;
-}
-
-
-void GJKInitializer<Box>::deleteGJKObject(void* o_)
-{
- ccd_box_t* o = (ccd_box_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Capsule>::getSupportFunction()
-{
- return &supportCap;
-}
-
-
-GJKCenterFunction GJKInitializer<Capsule>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s, const SimpleTransform& tf)
-{
- ccd_cap_t* o = new ccd_cap_t;
- capToGJK(s, tf, o);
- return o;
-}
-
-
-void GJKInitializer<Capsule>::deleteGJKObject(void* o_)
-{
- ccd_cap_t* o = (ccd_cap_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Cone>::getSupportFunction()
-{
- return &supportCone;
-}
-
-
-GJKCenterFunction GJKInitializer<Cone>::getCenterFunction()
-{
- return ¢erShape;
-}
-
-
-void* GJKInitializer<Cone>::createGJKObject(const Cone& s, const SimpleTransform& tf)
-{
- ccd_cone_t* o = new ccd_cone_t;
- coneToGJK(s, tf, o);
- return o;
-}
-
-
-void GJKInitializer<Cone>::deleteGJKObject(void* o_)
-{
- ccd_cone_t* o = (ccd_cone_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction GJKInitializer<Convex>::getSupportFunction()
-{
- return &supportConvex;
-}
-
-
-GJKCenterFunction GJKInitializer<Convex>::getCenterFunction()
-{
- return ¢erConvex;
-}
-
-
-void* GJKInitializer<Convex>::createGJKObject(const Convex& s, const SimpleTransform& tf)
-{
- ccd_convex_t* o = new ccd_convex_t;
- convexToGJK(s, tf, o);
- return o;
-}
-
-
-void GJKInitializer<Convex>::deleteGJKObject(void* o_)
-{
- ccd_convex_t* o = (ccd_convex_t*)o_;
- delete o;
-}
-
-
-GJKSupportFunction triGetSupportFunction()
-{
- return &supportTriangle;
-}
-
-
-GJKCenterFunction triGetCenterFunction()
-{
- return ¢erTriangle;
-}
-
-
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3)
-{
- 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);
-
- ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
- 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]);
- ccdVec3Set(&o->pos, 0., 0., 0.);
- ccdQuatSet(&o->rot, 0., 0., 0., 1.);
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-
- return o;
-}
-
-void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, Vec3f const& T)
-{
- 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);
-
- ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
- 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);
- 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);
-
- return o;
-}
-
-void triDeleteGJKObject(void* o_)
-{
- ccd_triangle_t* o = (ccd_triangle_t*)o_;
- delete o;
-}
-
-} // details
-
-} // fcl
diff --git a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
deleted file mode 100644
index 78e5a51361702737ae9c84ae3122245c6d3d7d8e..0000000000000000000000000000000000000000
--- a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#ifndef FCL_GEOMETRIC_SHAPES_INTERSECT_H
-#define FCL_GEOMETRIC_SHAPES_INTERSECT_H
-
-#include "fcl/geometric_shapes.h"
-#include "fcl/transform.h"
-
-#include <ccd/ccd.h>
-#include <ccd/quat.h>
-
-namespace fcl
-{
-
-namespace details
-{
-
-/** \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);
-
-/** \brief initialize GJK stuffs */
-template<typename T>
-class GJKInitializer
-{
-public:
- /** \brief Get GJK support function */
- static GJKSupportFunction getSupportFunction() { return NULL; }
-
- /** \brief Get GJK center function */
- static GJKCenterFunction getCenterFunction() { return NULL; }
-
- /** \brief Get GJK object from a shape
- * Notice that only local transformation is applied.
- * Gloal transformation are considered later
- */
- static void* createGJKObject(const T& s, const SimpleTransform& tf) { return NULL; }
-
- /** \brief Delete GJK object */
- static void deleteGJKObject(void* o) {}
-};
-
-/** \brief initialize GJK Cylinder */
-template<>
-class GJKInitializer<Cylinder>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Cylinder& s, const SimpleTransform& tf);
- static void deleteGJKObject(void* o);
-};
-
-/** \brief initialize GJK Sphere */
-template<>
-class GJKInitializer<Sphere>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Sphere& s, const SimpleTransform& tf);
- static void deleteGJKObject(void* o);
-};
-
-/** \brief initialize GJK Box */
-template<>
-class GJKInitializer<Box>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Box& s, const SimpleTransform& tf);
- static void deleteGJKObject(void* o);
-};
-
-/** \brief initialize GJK Capsule */
-template<>
-class GJKInitializer<Capsule>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Capsule& s, const SimpleTransform& tf);
- static void deleteGJKObject(void* o);
-};
-
-/** \brief initialize GJK Cone */
-template<>
-class GJKInitializer<Cone>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Cone& s, const SimpleTransform& tf);
- static void deleteGJKObject(void* o);
-};
-
-/** \brief initialize GJK Convex */
-template<>
-class GJKInitializer<Convex>
-{
-public:
- static GJKSupportFunction getSupportFunction();
- static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Convex& s, const SimpleTransform& tf);
- static void deleteGJKObject(void* o);
-};
-
-/** \brief initialize GJK Triangle */
-GJKSupportFunction triGetSupportFunction();
-
-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 triDeleteGJKObject(void* o);
-
-/** \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, FCL_REAL* penetration_depth, Vec3f* normal);
-
-bool GJKDistance(void* obj1, ccd_support_fn supp1,
- void* obj2, ccd_support_fn supp2,
- FCL_REAL* dist);
-
-
-} // details
-
-
-/** intersection checking between two shapes */
-template<typename S1, typename S2>
-bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
- void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
-
- bool res = details::GJKCollide(o1, details::GJKInitializer<S1>::getSupportFunction(), details::GJKInitializer<S1>::getCenterFunction(),
- o2, details::GJKInitializer<S2>::getSupportFunction(), details::GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
-
- details::GJKInitializer<S1>::deleteGJKObject(o1);
- details::GJKInitializer<S2>::deleteGJKObject(o2);
-
- return res;
-}
-
-/** \brief intersection checking between one shape and a triangle */
-template<typename S>
-bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = details::triCreateGJKObject(P1, P2, P3);
-
- bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
- o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
- contact_points, penetration_depth, normal);
-
- details::GJKInitializer<S>::deleteGJKObject(o1);
- details::triDeleteGJKObject(o2);
-
- return res;
-}
-
-/** \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,
- Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
-
- bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
- o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
- contact_points, penetration_depth, normal);
-
- details::GJKInitializer<S>::deleteGJKObject(o1);
- details::triDeleteGJKObject(o2);
-
- return res;
-}
-
-
-/** \brief distance computation between two shapes */
-template<typename S1, typename S2>
-bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- FCL_REAL* dist)
-{
- void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
- void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
-
- bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
- o2, details::GJKInitializer<S2>::getSupportFunction(),
- dist);
-
- if(*dist > 0) *dist = std::sqrt(*dist);
-
- details::GJKInitializer<S1>::deleteGJKObject(o1);
- details::GJKInitializer<S2>::deleteGJKObject(o2);
-
- return res;
-}
-
-}
-
-#endif
diff --git a/trunk/fcl/include/fcl/deprecated/gjk.h b/trunk/fcl/include/fcl/deprecated/gjk.h
deleted file mode 100644
index b5946cb48ec91f1210cfe91c47e17400143bd973..0000000000000000000000000000000000000000
--- a/trunk/fcl/include/fcl/deprecated/gjk.h
+++ /dev/null
@@ -1,412 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-#ifndef FCL_GJK_H
-#define FCL_GJK_H
-
-#include "fcl/geometric_shapes.h"
-#include "fcl/matrix_3f.h"
-#include "fcl/transform.h"
-
-namespace fcl
-{
-
-namespace details
-{
-
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
-
-struct MinkowskiDiff
-{
- const ShapeBase* shapes[2];
- Matrix3f toshape1;
- SimpleTransform toshape0;
-
- MinkowskiDiff() { }
-
- inline Vec3f support0(const Vec3f& d) const
- {
- return getSupport(shapes[0], d);
- }
-
- inline Vec3f support1(const Vec3f& d) const
- {
- return toshape0.transform(getSupport(shapes[1], toshape1 * d));
- }
-
- inline Vec3f support(const Vec3f& d) const
- {
- return support0(d) - support1(-d);
- }
-
- inline Vec3f support(const Vec3f& d, size_t index) const
- {
- if(index)
- return support1(d);
- else
- return support0(d);
- }
-
-};
-
-
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m);
-
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m);
-
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m);
-
-
-static const FCL_REAL GJK_EPS = 0.000001;
-static const size_t GJK_MAX_ITERATIONS = 128;
-
-struct GJK
-{
- struct SimplexV
- {
- Vec3f d; // support direction
- Vec3f w; // support vector
- };
-
- struct Simplex
- {
- SimplexV* c[4]; // simplex vertex
- FCL_REAL p[4]; // weight
- size_t rank; // size of simplex (number of vertices)
- };
-
- enum Status {Valid, Inside, Failed};
-
- MinkowskiDiff shape;
- Vec3f ray;
- FCL_REAL distance;
- Simplex simplices[2];
-
-
- GJK() { initialize(); }
-
- void initialize();
-
- Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);
-
- void getSupport(const Vec3f& d, SimplexV& sv) const;
-
- void removeVertex(Simplex& simplex);
-
- void appendVertex(Simplex& simplex, const Vec3f& v);
-
- bool encloseOrigin();
-
- inline Simplex* getSimplex() const
- {
- return simplex;
- }
-
-private:
- SimplexV store_v[4];
- SimplexV* free_v[4];
- size_t nfree;
- size_t current;
- Simplex* simplex;
- Status status;
-};
-
-
-static const size_t EPA_MAX_FACES = 128;
-static const size_t EPA_MAX_VERTICES = 64;
-static const FCL_REAL EPA_EPS = 0.000001;
-static const size_t EPA_MAX_ITERATIONS = 255;
-
-struct EPA
-{
-private:
- typedef GJK::SimplexV SimplexV;
- struct SimplexF
- {
- Vec3f n;
- FCL_REAL d;
- SimplexV* c[3]; // a face has three vertices
- SimplexF* f[3]; // a face has three adjacent faces
- SimplexF* l[2]; // the pre and post faces in the list
- size_t e[3];
- size_t pass;
- };
-
- struct SimplexList
- {
- SimplexF* root;
- size_t count;
- SimplexList() : root(NULL), count(0) {}
- void append(SimplexF* face)
- {
- face->l[0] = NULL;
- face->l[1] = root;
- if(root) root->l[0] = face;
- root = face;
- ++count;
- }
-
- void remove(SimplexF* face)
- {
- if(face->l[1]) face->l[1]->l[0] = face->l[0];
- if(face->l[0]) face->l[0]->l[1] = face->l[1];
- if(face == root) root = face->l[1];
- --count;
- }
- };
-
- static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
- {
- fa->e[ea] = eb; fa->f[ea] = fb;
- fb->e[eb] = ea; fb->f[eb] = fa;
- }
-
- struct SimplexHorizon
- {
- SimplexF* cf; // current face in the horizon
- SimplexF* ff; // first face in the horizon
- size_t nf; // number of faces in the horizon
- SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
- };
-
-public:
-
- enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
-
- Status status;
- GJK::Simplex result;
- Vec3f normal;
- FCL_REAL depth;
- SimplexV sv_store[EPA_MAX_VERTICES];
- SimplexF fc_store[EPA_MAX_FACES];
- size_t nextsv;
- SimplexList hull, stock;
-
- EPA()
- {
- initialize();
- }
-
- void initialize();
-
- bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
-
- SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
-
- /** \brief Find the best polytope face to split */
- SimplexF* findBest();
-
- Status evaluate(GJK& gjk, const Vec3f& guess);
-
- /** \brief the goal is to add a face connecting vertex w and face edge f[e] */
- bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
-};
-
-
-} // details
-
-
-
-
-
-template<typename S1, typename S2>
-bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- FCL_REAL* distance)
-{
- Vec3f guess(1, 0, 0);
- details::MinkowskiDiff shape;
- shape.shapes[0] = &s1;
- shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
- shape.toshape0 = tf1.inverseTimes(tf2);
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- if(gjk_status == details::GJK::Valid)
- {
- Vec3f w0, w1;
- for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
- {
- FCL_REAL p = gjk.getSimplex()->p[i];
- w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
- }
-
- *distance = (w0 - w1).length();
- return true;
- }
- else
- {
- *distance = -1;
- return false;
- }
-}
-
-template<typename S1, typename S2>
-bool shapeIntersect2(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- Vec3f guess(1, 0, 0);
- details::MinkowskiDiff shape;
- shape.shapes[0] = &s1;
- shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
- shape.toshape0 = tf1.inverseTimes(tf2);
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- switch(gjk_status)
- {
- case details::GJK::Inside:
- {
- details::EPA epa;
- details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
- if(epa_status != details::EPA::Failed)
- {
- Vec3f w0;
- for(size_t i = 0; i < epa.result.rank; ++i)
- {
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
- }
- if(penetration_depth) *penetration_depth = -epa.depth;
- if(normal) *normal = -epa.normal;
- if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
- return true;
- }
- else return false;
- }
- break;
- default:
- ;
- }
-
- return false;
-}
-
-
-template<typename S>
-bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- Triangle2 tri(P1, P2, P3);
- Vec3f guess(1, 0, 0);
- details::MinkowskiDiff shape;
- shape.shapes[0] = &s;
- shape.shapes[1] = &tri;
- shape.toshape1 = tf.getRotation();
- shape.toshape0 = tf.inverse();
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- switch(gjk_status)
- {
- case details::GJK::Inside:
- {
- details::EPA epa;
- details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
- if(epa_status != details::EPA::Failed)
- {
- Vec3f w0;
- for(size_t i = 0; i < epa.result.rank; ++i)
- {
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
- }
- 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));
- return true;
- }
- else return false;
- }
- break;
- default:
- ;
- }
-
- return false;
-}
-
-template<typename S>
-bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- 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);
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- switch(gjk_status)
- {
- case details::GJK::Inside:
- {
- details::EPA epa;
- details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
- if(epa_status != details::EPA::Failed)
- {
- Vec3f w0;
- for(size_t i = 0; i < epa.result.rank; ++i)
- {
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
- }
- 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));
- return true;
- }
- else return false;
- }
- break;
- default:
- ;
- }
-
- return false;
-}
-
-}
-
-
-#endif
diff --git a/trunk/fcl/include/fcl/deprecated/vec_3f.h b/trunk/fcl/include/fcl/deprecated/vec_3f.h
deleted file mode 100644
index ead30c2d1ae30fc770725c5958ae95adaf327df1..0000000000000000000000000000000000000000
--- a/trunk/fcl/include/fcl/deprecated/vec_3f.h
+++ /dev/null
@@ -1,301 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/** \author Jia Pan */
-
-
-#ifndef FCL_DEPRECATED_VEC_3F_H
-#define FCL_DEPRECATED_VEC_3F_H
-
-namespace fcl
-{
-
-namespace deprecated
-{
-
-
-/** \brief A class describing a three-dimensional vector */
-class Vec3f
-{
-public:
- /** \brief vector data */
- FCL_REAL v_[3];
-
- Vec3f() { v_[0] = 0; v_[1] = 0; v_[2] = 0; }
-
- Vec3f(const Vec3f& other)
- {
- memcpy(v_, other.v_, sizeof(FCL_REAL) * 3);
- }
-
- Vec3f(const FCL_REAL* v)
- {
- memcpy(v_, v, sizeof(FCL_REAL) * 3);
- }
-
- Vec3f(FCL_REAL x, FCL_REAL y, FCL_REAL z)
- {
- v_[0] = x;
- v_[1] = y;
- v_[2] = z;
- }
-
- ~Vec3f() {}
-
- /** \brief Get the ith element */
- inline FCL_REAL operator [] (size_t i) const
- {
- return v_[i];
- }
-
- inline FCL_REAL& operator[] (size_t i)
- {
- return v_[i];
- }
-
- /** \brief Add the other vector */
- inline Vec3f& operator += (const Vec3f& other)
- {
- v_[0] += other.v_[0];
- v_[1] += other.v_[1];
- v_[2] += other.v_[2];
- return *this;
- }
-
-
- /** \brief Minus the other vector */
- inline Vec3f& operator -= (const Vec3f& other)
- {
- v_[0] -= other.v_[0];
- v_[1] -= other.v_[1];
- v_[2] -= other.v_[2];
- return *this;
- }
-
- inline Vec3f& operator *= (FCL_REAL t)
- {
- v_[0] *= t;
- v_[1] *= t;
- v_[2] *= t;
- return *this;
- }
-
- /** \brief Negate the vector */
- inline Vec3f& negate()
- {
- v_[0] = - v_[0];
- v_[1] = - v_[1];
- v_[2] = - v_[2];
- return *this;
- }
-
- /** \brief Return a negated vector */
- inline Vec3f operator - () const
- {
- return Vec3f(-v_[0], -v_[1], -v_[2]);
- }
-
- /** \brief Return a summation vector */
- inline Vec3f operator + (const Vec3f& other) const
- {
- return Vec3f(v_[0] + other.v_[0], v_[1] + other.v_[1], v_[2] + other.v_[2]);
- }
-
- /** \brief Return a substraction vector */
- inline Vec3f operator - (const Vec3f& other) const
- {
- return Vec3f(v_[0] - other.v_[0], v_[1] - other.v_[1], v_[2] - other.v_[2]);
- }
-
- /** \brief Scale the vector by t */
- inline Vec3f operator * (FCL_REAL t) const
- {
- return Vec3f(v_[0] * t, v_[1] * t, v_[2] * t);
- }
-
- /** \brief Return the cross product with another vector */
- inline Vec3f cross(const Vec3f& other) const
- {
- return Vec3f(v_[1] * other.v_[2] - v_[2] * other.v_[1],
- v_[2] * other.v_[0] - v_[0] * other.v_[2],
- v_[0] * other.v_[1] - v_[1] * other.v_[0]);
- }
-
- /** \brief Return the dot product with another vector */
- inline FCL_REAL dot(const Vec3f& other) const
- {
- return v_[0] * other.v_[0] + v_[1] * other.v_[1] + v_[2] * other.v_[2];
- }
-
- /** \brief Normalization */
- inline bool normalize()
- {
- const FCL_REAL EPSILON = 1e-11;
- FCL_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
- if(sqr_length > EPSILON * EPSILON)
- {
- FCL_REAL inv_length = (FCL_REAL)1.0 / (FCL_REAL)sqrt(sqr_length);
- v_[0] *= inv_length;
- v_[1] *= inv_length;
- v_[2] *= inv_length;
- return true;
- }
- return false;
- }
-
- inline Vec3f normalized() const
- {
- const FCL_REAL EPSILON = 1e-11;
- FCL_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
- if(sqr_length > EPSILON * EPSILON)
- {
- FCL_REAL inv_length = (FCL_REAL)1.0 / (FCL_REAL)sqrt(sqr_length);
- return *this * inv_length;
- }
- else
- {
- return *this;
- }
- }
-
-
- /** \brief Return vector length */
- inline FCL_REAL length() const
- {
- return sqrt(v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2]);
- }
-
- /** \brief Return vector square length */
- inline FCL_REAL sqrLength() const
- {
- return v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
- }
-
- /** \brief Set the vector using new values */
- inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z)
- {
- v_[0] = x; v_[1] = y; v_[2] = z;
- }
-
- /** \brief Set the vector using new values */
- inline void setValue(FCL_REAL x)
- {
- v_[0] = x; v_[1] = x; v_[2] = x;
- }
-
- /** \brief Check whether two vectors are the same in value */
- inline bool equal(const Vec3f& other) const
- {
- const FCL_REAL EPSILON = 1e-11;
- return ((v_[0] - other.v_[0] < EPSILON) &&
- (v_[0] - other.v_[0] > -EPSILON) &&
- (v_[1] - other.v_[1] < EPSILON) &&
- (v_[1] - other.v_[1] > -EPSILON) &&
- (v_[2] - other.v_[2] < EPSILON) &&
- (v_[2] - other.v_[2] > -EPSILON));
- }
-
- inline FCL_REAL triple(const Vec3f& v1, const Vec3f& v2) const
- {
- return v_[0] * (v1.v_[1] * v2.v_[2] - v1.v_[2] * v2.v_[1]) +
- v_[1] * (v1.v_[2] * v2.v_[0] - v1.v_[0] * v2.v_[2]) +
- v_[2] * (v1.v_[0] * v2.v_[1] - v1.v_[1] * v2.v_[0]);
- }
-};
-
-
-
-
-/** \brief The minimum of two vectors */
-inline Vec3f min(const Vec3f& a, const Vec3f& b)
-{
- Vec3f ret(std::min(a[0], b[0]), std::min(a[1], b[1]), std::min(a[2], b[2]));
- return ret;
-}
-
-/** \brief the maximum of two vectors */
-inline Vec3f max(const Vec3f& a, const Vec3f& b)
-{
- Vec3f ret(std::max(a[0], b[0]), std::max(a[1], b[1]), std::max(a[2], b[2]));
- return ret;
-}
-
-inline Vec3f abs(const Vec3f& v)
-{
- FCL_REAL x = v[0] < 0 ? -v[0] : v[0];
- FCL_REAL y = v[1] < 0 ? -v[1] : v[1];
- FCL_REAL z = v[2] < 0 ? -v[2] : v[2];
-
- return Vec3f(x, y, z);
-}
-
-
-inline FCL_REAL triple(const Vec3f& a, const Vec3f& b, const Vec3f& c)
-{
- return a.triple(b, c);
-}
-
-/** \brief generate a coordinate given a vector (i.e., generating three orthonormal vectors given a vector)
- * w should be normalized
- */
-static void generateCoordinateSystem(const Vec3f& w, Vec3f& u, Vec3f& v)
-{
- FCL_REAL inv_length;
- if(fabs(w[0]) >= fabs(w[1]))
- {
- inv_length = (FCL_REAL)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = (FCL_REAL)0;
- u[2] = w[0] * inv_length;
- v[0] = w[1] * u[2];
- v[1] = w[2] * u[0] - w[0] * u[2];
- v[2] = -w[1] * u[0];
- }
- else
- {
- inv_length = (FCL_REAL)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = (FCL_REAL)0;
- u[1] = w[2] * inv_length;
- u[2] = -w[1] * inv_length;
- v[0] = w[1] * u[2] - w[2] * u[1];
- v[1] = -w[0] * u[2];
- v[2] = w[0] * u[1];
- }
-}
-
-} // deprecated
-
-} // fcl
-
-#endif
diff --git a/trunk/fcl/include/fcl/distance.h b/trunk/fcl/include/fcl/distance.h
index 2497fcbbc8a064caf9229832d7708c3f8413b69f..201e6a5057a0a61435524792b968f4df9ecf87c1 100644
--- a/trunk/fcl/include/fcl/distance.h
+++ b/trunk/fcl/include/fcl/distance.h
@@ -49,16 +49,16 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Na
const DistanceRequest& request, DistanceResult& result);
template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
const DistanceRequest& request, DistanceResult& result);
-FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result);
}
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
index 3f8c1830c35b65a16c7a1c4a0beaa8221bfacce9..e469d265cf7018c0156fa98ecd5634a7016dcd79 100644
--- a/trunk/fcl/include/fcl/distance_func_matrix.h
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -46,7 +46,7 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix
{
- typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
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 cd6888025d02fec5b0125f0a0343a4801cc31e23..efd171d5d468e9d0e1bc3341c737960ab10a5374 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -48,7 +48,7 @@ namespace fcl
/** \brief Generate BVH model from box */
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box& shape, const SimpleTransform& pose = SimpleTransform())
+void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose = Transform3f())
{
double a = shape.side[0];
double b = shape.side[1];
@@ -90,7 +90,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const SimpleTransfo
/** Generate BVH model from sphere */
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int seg = 16, unsigned int ring = 16)
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose = Transform3f(), unsigned int seg = 16, unsigned int ring = 16)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
@@ -158,7 +158,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const SimpleTran
* then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
*/
template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int n_faces_for_unit_sphere = 100)
+void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose = Transform3f(), unsigned int n_faces_for_unit_sphere = 100)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
@@ -229,7 +229,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const SimpleTra
/** \brief Generate BVH model from cylinder */
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose = Transform3f(), unsigned int tot = 16)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
@@ -306,7 +306,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const SimpleTr
* larger radius, the number of circle split number is r * tot.
*/
template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot_for_unit_cylinder = 100)
+void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose = Transform3f(), unsigned int tot_for_unit_cylinder = 100)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
@@ -382,7 +382,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const SimpleT
/** \brief Generate BVH model from cone */
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose = Transform3f(), unsigned int tot = 16)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/geometric_shapes_utility.h
index 4251becdd47de9b44596b4b9caadafc8aae83ea7..ffecf921715ec25df4e2064a06cf272bc9c799bc 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_utility.h
@@ -47,122 +47,122 @@ namespace fcl
namespace details
{
-std::vector<Vec3f> getBoundVertices(const Box& box, const SimpleTransform& tf);
-std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const SimpleTransform& tf);
-std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const SimpleTransform& tf);
-std::vector<Vec3f> getBoundVertices(const Cone& cone, const SimpleTransform& tf);
-std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const SimpleTransform& tf);
-std::vector<Vec3f> getBoundVertices(const Convex& convex, const SimpleTransform& tf);
-std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const SimpleTransform& tf);
+std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f& tf);
} // end detail
template<typename BV, typename S>
-void computeBV(const S& s, const SimpleTransform& tf, BV& bv)
+void computeBV(const S& s, const Transform3f& tf, BV& bv)
{
std::vector<Vec3f> convex_bound_vertices = details::getBoundVertices(s, tf);
fit(&convex_bound_vertices[0], (int)convex_bound_vertices.size(), bv);
}
template<>
-void computeBV<AABB, Box>(const Box& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<AABB, Sphere>(const Sphere& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<AABB, Capsule>(const Capsule& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<AABB, Cone>(const Cone& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<AABB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<AABB, Convex>(const Convex& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<AABB, Triangle2>(const Triangle2& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv);
/** \brief the bounding volume for half space back of plane for OBB, it is the plane itself */
template<>
-void computeBV<AABB, Plane>(const Plane& s, const SimpleTransform& tf, AABB& bv);
+void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<OBB, Box>(const Box& s, const SimpleTransform& tf, OBB& bv);
+void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<OBB, Sphere>(const Sphere& s, const SimpleTransform& tf, OBB& bv);
+void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<OBB, Capsule>(const Capsule& s, const SimpleTransform& tf, OBB& bv);
+void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<OBB, Cone>(const Cone& s, const SimpleTransform& tf, OBB& bv);
+void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<OBB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, OBB& bv);
+void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<OBB, Convex>(const Convex& s, const SimpleTransform& tf, OBB& bv);
+void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<OBB, Plane>(const Plane& s, const SimpleTransform& tf, OBB& bv);
+void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
template<>
-void computeBV<RSS, Plane>(const Plane& s, const SimpleTransform& tf, RSS& bv);
+void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv);
template<>
-void computeBV<OBBRSS, Plane>(const Plane& s, const SimpleTransform& tf, OBBRSS& bv);
+void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv);
template<>
-void computeBV<kIOS, Plane>(const Plane& s, const SimpleTransform& tf, kIOS& bv);
+void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv);
template<>
-void computeBV<KDOP<16>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<16>& bv);
+void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv);
template<>
-void computeBV<KDOP<18>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<18>& bv);
+void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv);
template<>
-void computeBV<KDOP<24>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<24>& bv);
+void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv);
-void constructBox(const AABB& bv, Box& box, SimpleTransform& tf);
+void constructBox(const AABB& bv, Box& box, Transform3f& tf);
-void constructBox(const OBB& bv, Box& box, SimpleTransform& tf);
+void constructBox(const OBB& bv, Box& box, Transform3f& tf);
-void constructBox(const OBBRSS& bv, Box& box, SimpleTransform& tf);
+void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf);
-void constructBox(const kIOS& bv, Box& box, SimpleTransform& tf);
+void constructBox(const kIOS& bv, Box& box, Transform3f& tf);
-void constructBox(const RSS& bv, Box& box, SimpleTransform& tf);
+void constructBox(const RSS& bv, Box& box, Transform3f& tf);
-void constructBox(const KDOP<16>& bv, Box& box, SimpleTransform& tf);
+void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf);
-void constructBox(const KDOP<18>& bv, Box& box, SimpleTransform& tf);
+void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf);
-void constructBox(const KDOP<24>& bv, Box& box, SimpleTransform& tf);
+void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf);
-void constructBox(const AABB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
-void constructBox(const OBB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
-void constructBox(const OBBRSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
-void constructBox(const kIOS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
-void constructBox(const RSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
-void constructBox(const KDOP<16>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
-void constructBox(const KDOP<18>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
-void constructBox(const KDOP<24>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
}
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index 67c517784f48bf9573ceb4dedabac05c4d102bf5..fdd00cc3477f3f649bde6d2f5ad3b803e9a6950f 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -145,7 +145,7 @@ public:
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,
+ const Transform3f& tf,
Vec3f* contact_points = NULL,
unsigned int* num_contact_points = NULL,
FCL_REAL* penetration_depth = NULL,
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index 0566a0a649742bc9466b1aa26402a6572f137b7b..59e37e744befc7823514901749ab25a365eebb2a 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -103,7 +103,7 @@ public:
FCL_REAL cur_angle = cur_w.length();
cur_w.normalize();
- SimpleQuaternion cur_q;
+ Quaternion3f cur_q;
cur_q.fromAxisAngle(cur_w, cur_angle);
tf.setTransform(cur_q, cur_T);
@@ -155,7 +155,7 @@ public:
T = tf.getTranslation();
}
- void getCurrentTransform(SimpleTransform& tf_) const
+ void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
@@ -310,7 +310,7 @@ protected:
FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
/** \brief The transformation at current time t */
- SimpleTransform tf;
+ Transform3f tf;
/** \brief The time related with tf */
FCL_REAL tf_t;
@@ -335,14 +335,19 @@ public:
/** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2)
+ const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
+ tf2(R2, T2),
+ tf(tf1)
{
- tf1 = SimpleTransform(R1, T1);
- tf2 = SimpleTransform(R2, T2);
-
- /** Current time is zero, so the transformation is t1 */
- tf = tf1;
+ computeScrewParameter();
+ }
+ /** \brief Construct motion from the initial transform and goal transform */
+ ScrewMotion(const Transform3f& tf1_,
+ const Transform3f& tf2_) : tf1(tf1_),
+ tf2(tf2_),
+ tf(tf1)
+ {
computeScrewParameter();
}
@@ -355,7 +360,7 @@ public:
tf.setQuatRotation(absoluteRotation(dt));
- SimpleQuaternion delta_rot = deltaRotation(dt);
+ Quaternion3f delta_rot = deltaRotation(dt);
tf.setTranslation(p + axis * (dt * linear_vel) + delta_rot.transform(tf1.getTranslation() - p));
return true;
@@ -401,7 +406,7 @@ public:
T = tf.getTranslation();
}
- void getCurrentTransform(SimpleTransform& tf_) const
+ void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
@@ -409,7 +414,7 @@ public:
protected:
void computeScrewParameter()
{
- SimpleQuaternion deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
+ Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
deltaq.toAxisAngle(axis, angular_vel);
if(angular_vel < 0)
{
@@ -432,27 +437,27 @@ protected:
}
}
- SimpleQuaternion deltaRotation(FCL_REAL dt) const
+ Quaternion3f deltaRotation(FCL_REAL dt) const
{
- SimpleQuaternion res;
+ Quaternion3f res;
res.fromAxisAngle(axis, (FCL_REAL)(dt * angular_vel));
return res;
}
- SimpleQuaternion absoluteRotation(FCL_REAL dt) const
+ Quaternion3f absoluteRotation(FCL_REAL dt) const
{
- SimpleQuaternion delta_t = deltaRotation(dt);
+ Quaternion3f delta_t = deltaRotation(dt);
return delta_t * tf1.getQuatRotation();
}
/** \brief The transformation at time 0 */
- SimpleTransform tf1;
+ Transform3f tf1;
/** \brief The transformation at time 1 */
- SimpleTransform tf2;
+ Transform3f tf2;
/** \brief The transformation at current time t */
- SimpleTransform tf;
+ Transform3f tf;
/** \brief screw axis */
Vec3f axis;
@@ -502,16 +507,19 @@ public:
/** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2)
+ const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
+ tf2(R2, T2),
+ tf(tf1)
{
- tf1 = SimpleTransform(R1, T1);
- tf2 = SimpleTransform(R2, T2);
+ /** Compute the velocities for the motion */
+ computeVelocity();
+ }
- /** Current time is zero, so the transformation is t1 */
- tf = tf1;
-
- /** Default reference point is local zero point */
+ InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) : tf1(tf1_),
+ tf2(tf2_),
+ tf(tf1)
+ {
/** Compute the velocities for the motion */
computeVelocity();
}
@@ -520,18 +528,22 @@ public:
*/
InterpMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2,
- const Vec3f& O)
+ const Vec3f& O) : tf1(R1, T1),
+ tf2(T2, T2),
+ tf(tf1),
+ reference_p(O)
{
- tf1 = SimpleTransform(R1, T1);
- tf2 = SimpleTransform(R2, T2);
- tf = tf1;
-
- reference_p = O;
-
/** Compute the velocities for the motion */
computeVelocity();
}
+ InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) : tf1(tf1_),
+ tf2(tf2_),
+ tf(tf1),
+ reference_p(O)
+ {
+ }
+
/** \brief Integrate the motion from 0 to dt
* We compute the current transformation from zero point instead of from last integrate time, for precision.
@@ -591,7 +603,7 @@ public:
T = tf.getTranslation();
}
- void getCurrentTransform(SimpleTransform& tf_) const
+ void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
}
@@ -601,7 +613,7 @@ protected:
void computeVelocity()
{
linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
- SimpleQuaternion deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
+ Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
deltaq.toAxisAngle(angular_axis, angular_vel);
if(angular_vel < 0)
{
@@ -611,27 +623,27 @@ protected:
}
- SimpleQuaternion deltaRotation(FCL_REAL dt) const
+ Quaternion3f deltaRotation(FCL_REAL dt) const
{
- SimpleQuaternion res;
+ Quaternion3f res;
res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel));
return res;
}
- SimpleQuaternion absoluteRotation(FCL_REAL dt) const
+ Quaternion3f absoluteRotation(FCL_REAL dt) const
{
- SimpleQuaternion delta_t = deltaRotation(dt);
+ Quaternion3f delta_t = deltaRotation(dt);
return delta_t * tf1.getQuatRotation();
}
/** \brief The transformation at time 0 */
- SimpleTransform tf1;
+ Transform3f tf1;
/** \brief The transformation at time 1 */
- SimpleTransform tf2;
+ Transform3f tf2;
/** \brief The transformation at current time t */
- SimpleTransform tf;
+ Transform3f tf;
/** \brief Linear velocity */
Vec3f linear_vel;
diff --git a/trunk/fcl/include/fcl/motion_base.h b/trunk/fcl/include/fcl/motion_base.h
index 6a310d44eee98bcd79b4ef0748974e7bcb9ca7ae..ba48ce3282fb664bab084164552bb774aaf05a34 100644
--- a/trunk/fcl/include/fcl/motion_base.h
+++ b/trunk/fcl/include/fcl/motion_base.h
@@ -67,7 +67,7 @@ public:
virtual void getCurrentTranslation(Vec3f& T) const = 0;
- virtual void getCurrentTransform(SimpleTransform& tf) const = 0;
+ virtual void getCurrentTransform(Transform3f& tf) const = 0;
};
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk.h b/trunk/fcl/include/fcl/narrowphase/gjk.h
index 1cacbdf93090a940b7fb1c9dd2f6a48932f728bf..f29a93513c6ac87d8b9737b9ef65b7a28546a73e 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk.h
@@ -53,7 +53,7 @@ struct MinkowskiDiff
{
const ShapeBase* shapes[2];
Matrix3f toshape1;
- SimpleTransform toshape0;
+ Transform3f toshape0;
MinkowskiDiff() { }
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
index 793a16d3ae9f562405df782fe978f0fc0c8a1832..4c110661d30fe031ad894723bb1a82e862e0de45 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
@@ -69,7 +69,7 @@ public:
* Notice that only local transformation is applied.
* Gloal transformation are considered later
*/
- static void* createGJKObject(const T& s, const SimpleTransform& tf) { return NULL; }
+ static void* createGJKObject(const T& s, const Transform3f& tf) { return NULL; }
/** \brief Delete GJK object */
static void deleteGJKObject(void* o) {}
@@ -82,7 +82,7 @@ class GJKInitializer<Cylinder>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Cylinder& s, const SimpleTransform& tf);
+ static void* createGJKObject(const Cylinder& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
@@ -93,7 +93,7 @@ class GJKInitializer<Sphere>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Sphere& s, const SimpleTransform& tf);
+ static void* createGJKObject(const Sphere& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
@@ -104,7 +104,7 @@ class GJKInitializer<Box>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Box& s, const SimpleTransform& tf);
+ static void* createGJKObject(const Box& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
@@ -115,7 +115,7 @@ class GJKInitializer<Capsule>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Capsule& s, const SimpleTransform& tf);
+ static void* createGJKObject(const Capsule& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
@@ -126,7 +126,7 @@ class GJKInitializer<Cone>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Cone& s, const SimpleTransform& tf);
+ static void* createGJKObject(const Cone& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
@@ -137,7 +137,7 @@ class GJKInitializer<Convex>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
- static void* createGJKObject(const Convex& s, const SimpleTransform& tf);
+ static void* createGJKObject(const Convex& s, const Transform3f& tf);
static void deleteGJKObject(void* o);
};
@@ -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 SimpleTransform& tf);
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf);
void triDeleteGJKObject(void* o);
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index e9c2f81c00334221d18150e2d2d66646837cab72..adcc3d1b8dd91a2487e5aadde11f7e413ed7b598 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -49,8 +49,8 @@ struct GJKSolver_libccd
{
/** intersection checking between two shapes */
template<typename S1, typename S2>
- bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
+ bool shapeIntersect(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
@@ -69,7 +69,7 @@ struct GJKSolver_libccd
/** \brief intersection checking between one shape and a triangle */
template<typename S>
- bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+ bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
@@ -88,8 +88,8 @@ struct GJKSolver_libccd
/** \brief intersection checking between one shape and a triangle with transformation */
template<typename S>
- bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+ bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
@@ -109,8 +109,8 @@ struct GJKSolver_libccd
/** \brief distance computation between two shapes */
template<typename S1, typename S2>
- bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
+ bool shapeDistance(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
FCL_REAL* dist) const
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
@@ -130,7 +130,7 @@ struct GJKSolver_libccd
/** \brief distance computation between one shape and a triangle */
template<typename S>
- bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist) const
{
@@ -150,8 +150,8 @@ struct GJKSolver_libccd
/** \brief distance computation between one shape and a triangle with transformation */
template<typename S>
- bool shapeTriangleDistance(const S& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
@@ -188,50 +188,50 @@ struct GJKSolver_libccd
/** \brief Fast implementation for sphere-sphere collision */
template<>
-bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
-bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
-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;
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-sphere distance */
template<>
-bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const;
/** \brief Fast implementation for box-box collision */
template<>
-bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
- const Box& s2, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
struct GJKSolver_indep
{
template<typename S1, typename S2>
- bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
+ bool shapeIntersect(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Vec3f guess(1, 0, 0);
@@ -273,7 +273,7 @@ struct GJKSolver_indep
template<typename S>
- bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+ bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
@@ -316,8 +316,8 @@ struct GJKSolver_indep
}
template<typename S>
- bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+ bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Triangle2 tri(P1, P2, P3);
@@ -360,8 +360,8 @@ struct GJKSolver_indep
template<typename S1, typename S2>
- bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
+ bool shapeDistance(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
FCL_REAL* distance) const
{
Vec3f guess(1, 0, 0);
@@ -397,7 +397,7 @@ struct GJKSolver_indep
template<typename S>
- bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* distance) const
{
@@ -432,8 +432,8 @@ struct GJKSolver_indep
}
template<typename S>
- bool shapeTriangleDistance(const S& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* distance) const
{
Triangle2 tri(P1, P2, P3);
@@ -487,42 +487,42 @@ struct GJKSolver_indep
/** \brief Fast implementation for sphere-sphere collision */
template<>
-bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
-bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
-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;
+bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-sphere distance */
template<>
-bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const;
/** \brief Fast implementation for box-box collision */
template<>
-bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
- const Box& s2, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
}
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index dc161d8c030124d48e43a5d8e32f4386e88d04d6..1d1a6518c72ab7c8175d4c622282c668bd334803 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -50,8 +50,8 @@ namespace fcl
template<typename NarrowPhaseSolver>
bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
- const OcTree& model1, const SimpleTransform& tf1,
- const OcTree& model2, const SimpleTransform& tf2,
+ const OcTree& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -73,8 +73,8 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
template<typename NarrowPhaseSolver>
bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
- const OcTree& model1, const SimpleTransform& tf1,
- const OcTree& model2, const SimpleTransform& tf2,
+ const OcTree& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -96,8 +96,8 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const OcTree& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -119,8 +119,8 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
- const OcTree& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const OcTree& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -142,8 +142,8 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const OcTree& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -166,8 +166,8 @@ bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
- const OcTree& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const OcTree& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -189,8 +189,8 @@ bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
template<typename BV, typename NarrowPhaseSolver>
bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const OcTree& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -212,8 +212,8 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
template<typename BV, typename NarrowPhaseSolver>
bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
- const OcTree& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const OcTree& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -235,8 +235,8 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
template<typename BV, typename NarrowPhaseSolver>
bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const OcTree& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -259,8 +259,8 @@ bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
template<typename BV, typename NarrowPhaseSolver>
bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
- const OcTree& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const OcTree& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const OcTreeSolver<NarrowPhaseSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -285,8 +285,8 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
/** \brief Initialize traversal node for collision between two geometric shapes */
template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
- const S1& shape1, const SimpleTransform& tf1,
- const S2& shape2, const SimpleTransform& tf2,
+ const S1& shape1, const Transform3f& tf1,
+ const S2& shape2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -309,8 +309,8 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
template<typename BV, typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
- BVHModel<BV>& model1, SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ BVHModel<BV>& model1, Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result,
@@ -360,8 +360,8 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
template<typename S, typename BV, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- BVHModel<BV>& model2, SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ BVHModel<BV>& model2, Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result,
@@ -413,8 +413,8 @@ namespace details
template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -449,8 +449,8 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
- const BVHModel<OBB>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<OBB>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -461,8 +461,8 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -473,8 +473,8 @@ bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
- const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<kIOS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -485,8 +485,8 @@ bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -500,8 +500,8 @@ namespace details
{
template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -534,8 +534,8 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<OBB>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -546,8 +546,8 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -558,8 +558,8 @@ bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<kIOS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -570,8 +570,8 @@ bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
@@ -585,8 +585,8 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
/** \brief Initialize traversal node for collision between two meshes, given the current transforms */
template<typename BV>
bool initialize(MeshCollisionTraversalNode<BV>& node,
- BVHModel<BV>& model1, SimpleTransform& tf1,
- BVHModel<BV>& model2, SimpleTransform& tf2,
+ BVHModel<BV>& model1, Transform3f& tf1,
+ BVHModel<BV>& model2, Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result,
bool use_refit = false, bool refit_bottomup = false)
@@ -651,23 +651,23 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
/** \brief Initialize traversal node for collision between two meshes, specialized for OBB type */
bool initialize(MeshCollisionTraversalNodeOBB& node,
- const BVHModel<OBB>& model1, const SimpleTransform& tf1,
- const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ const BVHModel<OBB>& model1, const Transform3f& tf1,
+ const BVHModel<OBB>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
bool initialize(MeshCollisionTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
bool initialize(MeshCollisionTraversalNodekIOS& node,
- const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const BVHModel<kIOS>& model1, const Transform3f& tf1,
+ const BVHModel<kIOS>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
#if USE_SVMLIGHT
@@ -675,8 +675,8 @@ bool initialize(MeshCollisionTraversalNodekIOS& node,
/** \brief Initialize traversal node for collision between two point clouds, given current transforms */
template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudCollisionTraversalNode<BV>& node,
- BVHModel<BV>& model1, SimpleTransform& tf1,
- BVHModel<BV>& model2, SimpleTransform& tf2,
+ BVHModel<BV>& model1, Transform3f& tf1,
+ BVHModel<BV>& model2, Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
@@ -747,8 +747,8 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node,
/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for OBB type */
bool initialize(PointCloudCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const SimpleTransform& tf1,
- BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ BVHModel<OBB>& model1, const Transform3f& tf1,
+ BVHModel<OBB>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
@@ -756,8 +756,8 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node,
/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
bool initialize(PointCloudCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const SimpleTransform& tf1,
- BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ BVHModel<RSS>& model1, const Transform3f& tf1,
+ BVHModel<RSS>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
@@ -766,8 +766,8 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node,
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
- BVHModel<BV>& model1, SimpleTransform& tf1,
- BVHModel<BV>& model2, SimpleTransform& tf2,
+ BVHModel<BV>& model1, Transform3f& tf1,
+ BVHModel<BV>& model2, Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
@@ -836,8 +836,8 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for OBB type */
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const SimpleTransform& tf1,
- const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ BVHModel<OBB>& model1, const Transform3f& tf1,
+ const BVHModel<OBB>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
@@ -845,8 +845,8 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
@@ -858,8 +858,8 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
/** \brief Initialize traversal node for distance between two geometric shapes */
template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
- const S1& shape1, const SimpleTransform& tf1,
- const S2& shape2, const SimpleTransform& tf2,
+ const S1& shape1, const Transform3f& tf1,
+ const S2& shape2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -879,8 +879,8 @@ bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV>& node,
- BVHModel<BV>& model1, SimpleTransform& tf1,
- BVHModel<BV>& model2, SimpleTransform& tf2,
+ BVHModel<BV>& model1, Transform3f& tf1,
+ BVHModel<BV>& model2, Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result,
bool use_refit = false, bool refit_bottomup = false)
@@ -943,29 +943,29 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
bool initialize(MeshDistanceTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
bool initialize(MeshDistanceTraversalNodekIOS& node,
- const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const BVHModel<kIOS>& model1, const Transform3f& tf1,
+ const BVHModel<kIOS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
template<typename BV, typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
- BVHModel<BV>& model1, SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ BVHModel<BV>& model1, Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result,
@@ -1012,8 +1012,8 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
template<typename S, typename BV, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- BVHModel<BV>& model2, SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ BVHModel<BV>& model2, Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result,
@@ -1063,8 +1063,8 @@ namespace details
template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1094,8 +1094,8 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1105,8 +1105,8 @@ bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
- const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<kIOS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1116,8 +1116,8 @@ bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1130,8 +1130,8 @@ namespace details
{
template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1164,8 +1164,8 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhas
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1175,8 +1175,8 @@ bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<kIOS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1186,8 +1186,8 @@ bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
@@ -1201,8 +1201,8 @@ bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
/** \brief Initialize traversal node for continuous collision detection between two meshes */
template<typename BV>
bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const CollisionRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -1230,8 +1230,8 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
/** \brief Initialize traversal node for continuous collision detection between one mesh and one point cloud */
template<typename BV>
bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const CollisionRequest& request)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
@@ -1257,8 +1257,8 @@ bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node,
/** \brief Initialize traversal node for continuous collision detection between one point cloud and one mesh */
template<typename BV>
bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const CollisionRequest& request)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index 8efcbb87fa82902c3dee181c2da48320ef0be558..979885e3be468c0c7c67d81d5cbf9b3e566b70cd 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -45,11 +45,11 @@ namespace fcl
{
/** \brief Quaternion used locally by InterpMotion */
-class SimpleQuaternion
+class Quaternion3f
{
public:
/** \brief Default quaternion is identity rotation */
- SimpleQuaternion()
+ Quaternion3f()
{
data[0] = 1;
data[1] = 0;
@@ -57,7 +57,7 @@ public:
data[3] = 0;
}
- SimpleQuaternion(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
+ Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
{
data[0] = a;
data[1] = b;
@@ -89,32 +89,32 @@ public:
void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const;
/** \brief Dot product between quaternions */
- FCL_REAL dot(const SimpleQuaternion& other) const;
+ FCL_REAL dot(const Quaternion3f& other) const;
/** \brief addition */
- SimpleQuaternion operator + (const SimpleQuaternion& other) const;
- const SimpleQuaternion& operator += (const SimpleQuaternion& other);
+ Quaternion3f operator + (const Quaternion3f& other) const;
+ const Quaternion3f& operator += (const Quaternion3f& other);
/** \brief minus */
- SimpleQuaternion operator - (const SimpleQuaternion& other) const;
- const SimpleQuaternion& operator -= (const SimpleQuaternion& other);
+ Quaternion3f operator - (const Quaternion3f& other) const;
+ const Quaternion3f& operator -= (const Quaternion3f& other);
/** \brief multiplication */
- SimpleQuaternion operator * (const SimpleQuaternion& other) const;
- const SimpleQuaternion& operator *= (const SimpleQuaternion& other);
+ Quaternion3f operator * (const Quaternion3f& other) const;
+ const Quaternion3f& operator *= (const Quaternion3f& other);
/** \brief division */
- SimpleQuaternion operator - () const;
+ Quaternion3f operator - () const;
/** \brief scalar multiplication */
- SimpleQuaternion operator * (FCL_REAL t) const;
- const SimpleQuaternion& operator *= (FCL_REAL t);
+ Quaternion3f operator * (FCL_REAL t) const;
+ const Quaternion3f& operator *= (FCL_REAL t);
/** \brief conjugate */
- SimpleQuaternion& conj();
+ Quaternion3f& conj();
/** \brief inverse */
- SimpleQuaternion& inverse();
+ Quaternion3f& inverse();
/** \brief rotate a vector */
Vec3f transform(const Vec3f& v) const;
@@ -134,11 +134,11 @@ private:
FCL_REAL data[4];
};
-SimpleQuaternion conj(const SimpleQuaternion& q);
-SimpleQuaternion inverse(const SimpleQuaternion& q);
+Quaternion3f conj(const Quaternion3f& q);
+Quaternion3f inverse(const Quaternion3f& q);
/** \brief Simple transform class used locally by InterpMotion */
-class SimpleTransform
+class Transform3f
{
/** \brief Whether matrix cache is set */
mutable bool matrix_set;
@@ -150,41 +150,41 @@ class SimpleTransform
Vec3f T;
/** \brief Rotation*/
- SimpleQuaternion q;
+ Quaternion3f q;
public:
/** \brief Default transform is no movement */
- SimpleTransform()
+ Transform3f()
{
setIdentity(); // set matrix_set true
}
- SimpleTransform(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
+ Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
R(R_),
T(T_)
{
q.fromRotation(R_);
}
- SimpleTransform(const SimpleQuaternion& q_, const Vec3f& T_) : matrix_set(false),
+ Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false),
T(T_),
q(q_)
{
}
- SimpleTransform(const Matrix3f& R_) : matrix_set(true),
+ Transform3f(const Matrix3f& R_) : matrix_set(true),
R(R_)
{
q.fromRotation(R_);
}
- SimpleTransform(const SimpleQuaternion& q_) : matrix_set(false),
+ Transform3f(const Quaternion3f& q_) : matrix_set(false),
q(q_)
{
}
- SimpleTransform(const Vec3f& T_) : matrix_set(true),
+ Transform3f(const Vec3f& T_) : matrix_set(true),
T(T_)
{
R.setIdentity();
@@ -206,7 +206,7 @@ public:
return R;
}
- inline const SimpleQuaternion& getQuatRotation() const
+ inline const Quaternion3f& getQuatRotation() const
{
return q;
}
@@ -220,7 +220,7 @@ public:
q.fromRotation(R_);
}
- inline void setTransform(const SimpleQuaternion& q_, const Vec3f& T_)
+ inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
{
matrix_set = false;
q = q_;
@@ -239,7 +239,7 @@ public:
T = T_;
}
- inline void setQuatRotation(const SimpleQuaternion& q_)
+ inline void setQuatRotation(const Quaternion3f& q_)
{
matrix_set = false;
q = q_;
@@ -250,7 +250,7 @@ public:
return q.transform(v) + T;
}
- SimpleTransform& inverse()
+ Transform3f& inverse()
{
matrix_set = false;
q.conj();
@@ -258,13 +258,13 @@ public:
return *this;
}
- SimpleTransform inverseTimes(const SimpleTransform& other) const
+ Transform3f inverseTimes(const Transform3f& other) const
{
- const SimpleQuaternion& q_inv = fcl::conj(q);
- return SimpleTransform(q_inv * other.q, q_inv.transform(other.T - T));
+ const Quaternion3f& q_inv = fcl::conj(q);
+ return Transform3f(q_inv * other.q, q_inv.transform(other.T - T));
}
- const SimpleTransform& operator *= (const SimpleTransform& other)
+ const Transform3f& operator *= (const Transform3f& other)
{
matrix_set = false;
T = q.transform(other.T) + T;
@@ -272,10 +272,10 @@ public:
return *this;
}
- SimpleTransform operator * (const SimpleTransform& other) const
+ Transform3f operator * (const Transform3f& other) const
{
- SimpleQuaternion q_new = q * other.q;
- return SimpleTransform(q_new, q.transform(other.T) + T);
+ Quaternion3f q_new = q * other.q;
+ return Transform3f(q_new, q.transform(other.T) + T);
}
bool isIdentity() const
@@ -287,16 +287,16 @@ public:
{
R.setIdentity();
T.setValue(0);
- q = SimpleQuaternion();
+ q = Quaternion3f();
matrix_set = true;
}
};
-SimpleTransform inverse(const SimpleTransform& tf);
+Transform3f inverse(const Transform3f& tf);
-void relativeTransform(const SimpleTransform& tf1, const SimpleTransform& tf2,
- SimpleTransform& tf);
+void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
+ Transform3f& tf);
}
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index cc352d067ec719e3fb9e1b66d97dc6322c48ad4d..c85631fb19754bcba6fa6dcc7915c8bc81280afa 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -78,9 +78,9 @@ public:
/** \brief Enable statistics (verbose mode) */
virtual void enableStatistics(bool enable) = 0;
- SimpleTransform tf1;
+ Transform3f tf1;
- SimpleTransform tf2;
+ Transform3f tf2;
};
class CollisionTraversalNodeBase : public TraversalNodeBase
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index abc252799b947f87299a64c560b6f6a291f8dbd3..b03905e5262526d014190da7616c49646212c60a 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -237,8 +237,8 @@ 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 SimpleTransform& tf1,
- const SimpleTransform& tf2,
+ const Transform3f& tf1,
+ const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
FCL_REAL cost_density,
@@ -728,8 +728,8 @@ 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 SimpleTransform& tf1,
- const SimpleTransform& tf2,
+ const Transform3f& tf1,
+ const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_statistics,
int & num_leaf_tests,
@@ -755,7 +755,7 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
- const S& model2, const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const S& model2, const Transform3f& tf1, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index 3f03387a6ad5d341ac9c9cdaa63c0c200130a40b..0125cf85da4637287bb5594b1d91942b9fe95406 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -70,7 +70,7 @@ public:
}
void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request_,
CollisionResult& result_) const
{
@@ -84,7 +84,7 @@ public:
void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const DistanceRequest& request_,
DistanceResult& result_) const
{
@@ -98,7 +98,7 @@ public:
template<typename BV>
void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request_,
CollisionResult& result_) const
{
@@ -112,7 +112,7 @@ public:
template<typename BV>
void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const DistanceRequest& request_,
DistanceResult& result_) const
{
@@ -127,7 +127,7 @@ public:
template<typename BV>
void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request_,
CollisionResult& result_) const
@@ -143,7 +143,7 @@ public:
template<typename BV>
void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const DistanceRequest& request_,
DistanceResult& result_) const
{
@@ -157,7 +157,7 @@ public:
template<typename S>
void OcTreeShapeIntersect(const OcTree* tree, const S& s,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request_,
CollisionResult& result_) const
{
@@ -165,7 +165,7 @@ public:
cresult = &result_;
AABB bv2;
- computeBV<AABB>(s, SimpleTransform(), bv2);
+ computeBV<AABB>(s, Transform3f(), bv2);
OBB obb2;
convertBV(bv2, tf2, obb2);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
@@ -176,7 +176,7 @@ public:
template<typename S>
void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request_,
CollisionResult& result_) const
{
@@ -184,7 +184,7 @@ public:
cresult = &result_;
AABB bv1;
- computeBV<AABB>(s, SimpleTransform(), bv1);
+ computeBV<AABB>(s, Transform3f(), bv1);
OBB obb1;
convertBV(bv1, tf1, obb1);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
@@ -194,7 +194,7 @@ public:
template<typename S>
void OcTreeShapeDistance(const OcTree* tree, const S& s,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const DistanceRequest& request_,
DistanceResult& result_) const
{
@@ -210,7 +210,7 @@ public:
template<typename S>
void ShapeOcTreeDistance(const S& s, const OcTree* tree,
- const SimpleTransform& tf1, const SimpleTransform& tf2,
+ const Transform3f& tf1, const Transform3f& tf2,
const DistanceRequest& request_,
DistanceResult& result_) const
{
@@ -229,14 +229,14 @@ private:
template<typename S>
bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const S& s, const AABB& aabb2,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren())
{
if(tree1->isNodeOccupied(root1))
{
Box box;
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
FCL_REAL dist;
@@ -277,7 +277,7 @@ private:
template<typename S>
bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const S& s, const OBB& obb2,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren())
{
@@ -288,7 +288,7 @@ private:
if(obb1.overlap(obb2))
{
Box box;
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
bool is_intersect = false;
@@ -336,7 +336,7 @@ private:
if(obb1.overlap(obb2))
{
Box box;
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
@@ -387,14 +387,14 @@ private:
template<typename BV>
bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const BVHModel<BV>* tree2, int root2,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
{
if(tree1->isNodeOccupied(root1))
{
Box box;
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
int primitive_id = tree2->getBV(root2).primitiveId();
@@ -473,7 +473,7 @@ private:
template<typename BV>
bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
const BVHModel<BV>* tree2, int root2,
- const SimpleTransform& tf1, const SimpleTransform& tf2) const
+ const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren() && tree2->getBV(root2).isLeaf())
{
@@ -485,7 +485,7 @@ private:
if(obb1.overlap(obb2))
{
Box box;
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
int primitive_id = tree2->getBV(root2).primitiveId();
@@ -541,7 +541,7 @@ private:
if(obb1.overlap(obb2))
{
Box box;
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
int primitive_id = tree2->getBV(root2).primitiveId();
@@ -610,14 +610,14 @@ private:
bool OcTreeDistanceRecurse(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) const
+ const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren() && !root2->hasChildren())
{
if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
{
Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
+ Transform3f box1_tf, box2_tf;
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
@@ -690,7 +690,7 @@ 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) const
+ const Transform3f& tf1, const Transform3f& tf2) const
{
if(!root1->hasChildren() && !root2->hasChildren())
{
@@ -713,7 +713,7 @@ private:
else
{
Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
+ Transform3f box1_tf, box2_tf;
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
@@ -731,7 +731,7 @@ private:
if(is_intersect && crequest->enable_cost)
{
Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
+ Transform3f box1_tf, box2_tf;
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
@@ -754,7 +754,7 @@ private:
if(obb1.overlap(obb2))
{
Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
+ Transform3f box1_tf, box2_tf;
constructBox(bv1, tf1, box1, box1_tf);
constructBox(bv2, tf2, box2, box2_tf);
@@ -853,7 +853,7 @@ public:
const OcTree* model1;
const OcTree* model2;
- SimpleTransform tf1, tf2;
+ Transform3f tf1, tf2;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -913,7 +913,7 @@ public:
const S* model1;
const OcTree* model2;
- SimpleTransform tf1, tf2;
+ Transform3f tf1, tf2;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -943,7 +943,7 @@ public:
const OcTree* model1;
const S* model2;
- SimpleTransform tf1, tf2;
+ Transform3f tf1, tf2;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -1030,7 +1030,7 @@ public:
const BVHModel<BV>* model1;
const OcTree* model2;
- SimpleTransform tf1, tf2;
+ Transform3f tf1, tf2;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
@@ -1060,7 +1060,7 @@ public:
const OcTree* model1;
const BVHModel<BV>* model2;
- SimpleTransform tf1, tf2;
+ Transform3f tf1, tf2;
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index 40b4f56a6d64816623b7a64ea4348c1c708f17b7..dbe3c07406adba4c2e31868c26db55fbf868eae1 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -301,13 +301,13 @@ OBB OBB::merge_smalldist(const OBB& b1, const OBB& b2)
{
OBB b;
b.To = (b1.To + b2.To) * 0.5;
- SimpleQuaternion q0, q1;
+ Quaternion3f q0, q1;
q0.fromAxes(b1.axis);
q1.fromAxes(b2.axis);
if(q0.dot(q1) < 0)
q1 = -q1;
- SimpleQuaternion q = q0 + q1;
+ Quaternion3f q = q0 + q1;
FCL_REAL inv_length = 1.0 / sqrt(q.dot(q));
q = q * inv_length;
q.toAxes(b.axis);
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index e637eec3e09886d7dbd4f5f1cc7c39e79ef23d13..eb68ff739959c8f8601f26530fe8b42f21d8375d 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -2331,7 +2331,7 @@ bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root,
}
-bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback)
+bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
{
if(root1->isLeaf() && !root2->hasChildren())
{
@@ -2340,13 +2340,13 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
OBB obb1, obb2;
- convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root1->bv, Transform3f(), obb1);
convertBV(root2_bv, tf2, obb2);
if(obb1.overlap(obb2))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
box->cost_density = root2->getOccupancy();
@@ -2361,7 +2361,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
}
OBB obb1, obb2;
- convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root1->bv, Transform3f(), obb1);
convertBV(root2_bv, tf2, obb2);
if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
@@ -2403,7 +2403,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
if(root1->bv.overlap(root2_bv_t))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
box->cost_density = root2->getOccupancy();
@@ -2447,7 +2447,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
-bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const
{
if(tf2.getQuatRotation().isIdentity())
return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
@@ -2463,7 +2463,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c
if(tree2->isNodeOccupied(root2))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
@@ -2534,14 +2534,14 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c
}
-bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
{
if(root1->isLeaf() && !root2->hasChildren())
{
if(tree2->isNodeOccupied(root2))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
@@ -2614,7 +2614,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c
return false;
}
-bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(tf2.getQuatRotation().isIdentity())
return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
@@ -2945,7 +2945,7 @@ bool DynamicAABBTreeCollisionManager2::selfDistanceRecurse(DynamicAABBNode* node
return false;
}
-bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback)
+bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback)
{
DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
@@ -2954,13 +2954,13 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
if(!tree2->isNodeFree(root2) && !obj1->isFree())
{
OBB obb1, obb2;
- convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root1->bv, Transform3f(), obb1);
convertBV(root2_bv, tf2, obb2);
if(obb1.overlap(obb2))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
box->cost_density = root2->getOccupancy();
@@ -2975,7 +2975,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
}
OBB obb1, obb2;
- convertBV(root1->bv, SimpleTransform(), obb1);
+ convertBV(root1->bv, Transform3f(), obb1);
convertBV(root2_bv, tf2, obb2);
if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
@@ -3018,7 +3018,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
if(root1->bv.overlap(root_bv_t))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
box->cost_density = root2->getOccupancy();
@@ -3063,7 +3063,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1
-bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const
{
if(tf2.getQuatRotation().isIdentity())
return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
@@ -3072,7 +3072,7 @@ bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1,
}
-bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
{
DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
@@ -3080,7 +3080,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1,
if(tree2->isNodeOccupied(root2))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
@@ -3162,7 +3162,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1,
if(tree2->isNodeOccupied(root2))
{
Box* box = new Box();
- SimpleTransform box_tf;
+ Transform3f box_tf;
constructBox(root2_bv, tf2, *box, box_tf);
CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
@@ -3233,7 +3233,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1,
return false;
}
-bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(tf2.getQuatRotation().isIdentity())
return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index 0310abe826427eda3e8d079096113c967431009c..5ec8c1fc9551b83daee0ac8b77a3451072fef361 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -62,8 +62,8 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
}
template<typename NarrowPhaseSolver>
-std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver_,
const CollisionRequest& request,
CollisionResult& result)
@@ -117,8 +117,8 @@ std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
template std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
template std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
-template std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
-template std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
+template std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
+template std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
@@ -128,8 +128,8 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
return collide<GJKSolver_libccd>(o1, o2, &solver, request, result);
}
-std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result)
{
GJKSolver_libccd solver;
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index f94893a3cd368560c9de5e25f17f6a97f53507f4..b3d343bc9b15604ce61901cae89d83f0ccd1bfa0 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -46,7 +46,7 @@ namespace fcl
{
template<typename T_SH, typename NarrowPhaseSolver>
-std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -64,7 +64,7 @@ std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransfor
}
template<typename T_SH, typename NarrowPhaseSolver>
-std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -82,7 +82,7 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransfor
}
template<typename NarrowPhaseSolver>
-std::size_t OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -100,7 +100,7 @@ std::size_t OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf
}
template<typename T_BVH, typename NarrowPhaseSolver>
-std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -118,7 +118,7 @@ std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform&
}
template<typename T_BVH, typename NarrowPhaseSolver>
-std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -137,7 +137,7 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform&
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
-std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -156,7 +156,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform
template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeCollider
{
- static std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -165,7 +165,7 @@ struct BVHShapeCollider
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
- SimpleTransform tf1_tmp = tf1;
+ Transform3f tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
@@ -180,7 +180,7 @@ struct BVHShapeCollider
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
{
- static std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -201,7 +201,7 @@ struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
{
- static std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -222,7 +222,7 @@ struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
{
- static std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -243,7 +243,7 @@ struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
{
- static std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
@@ -262,7 +262,7 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
template<typename T_BVH>
-std::size_t BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
+std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
@@ -270,9 +270,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
- SimpleTransform tf1_tmp = tf1;
+ Transform3f tf1_tmp = tf1;
BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
- SimpleTransform tf2_tmp = tf2;
+ Transform3f tf2_tmp = tf2;
initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
collide(&node);
@@ -284,7 +284,7 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1,
}
template<>
-std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
+std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
@@ -299,7 +299,7 @@ std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform&
}
template<>
-std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
+std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
@@ -315,7 +315,7 @@ std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const SimpleTransfor
template<>
-std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const CollisionRequest& request, CollisionResult& result)
+std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
@@ -331,7 +331,7 @@ std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform&
template<typename T_BVH, typename NarrowPhaseSolver>
-std::size_t BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index 44a0d9317d15d2dba9323ce792b28de5b89302f1..b9b1ec1b47bcf9d6e1a0a66e96b6a37e5537de40 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -77,7 +77,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
const BVHModel<RSS>* model1 = (const BVHModel<RSS>*)o1;
const BVHModel<RSS>* model2 = (const BVHModel<RSS>*)o2;
- SimpleTransform tf1, tf2;
+ Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
diff --git a/trunk/fcl/src/distance.cpp b/trunk/fcl/src/distance.cpp
index 4ff8070954f820f26f72df1e7fe4f1c972decde1..fde2650f032aeee6398f960ef7cd052be4a25141 100644
--- a/trunk/fcl/src/distance.cpp
+++ b/trunk/fcl/src/distance.cpp
@@ -59,8 +59,8 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Na
}
template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver_,
const DistanceRequest& request, DistanceResult& result)
{
@@ -108,8 +108,8 @@ FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
-template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
-template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
+template FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
+template FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result)
{
@@ -117,8 +117,8 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Di
return distance<GJKSolver_libccd>(o1, o2, &solver, request, result);
}
-FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
GJKSolver_libccd solver;
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index e020917232dff19d43f52af7850095bac6989677..a2af52e6d01d4b52b7f5007d12b89fcf04277404 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -44,7 +44,7 @@ namespace fcl
{
template<typename T_SH, typename NarrowPhaseSolver>
-FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
ShapeOcTreeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
@@ -59,7 +59,7 @@ FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform&
}
template<typename T_SH, typename NarrowPhaseSolver>
-FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
OcTreeShapeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
@@ -74,7 +74,7 @@ FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const SimpleTransform&
}
template<typename NarrowPhaseSolver>
-FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
@@ -89,7 +89,7 @@ FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1,
}
template<typename T_BVH, typename NarrowPhaseSolver>
-FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
MeshOcTreeDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
@@ -104,7 +104,7 @@ FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const SimpleTransform& t
}
template<typename T_BVH, typename NarrowPhaseSolver>
-FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
OcTreeMeshDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
@@ -121,7 +121,7 @@ FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const SimpleTransform& t
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
-FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
ShapeDistanceTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
@@ -137,13 +137,13 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform&
template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
- SimpleTransform tf1_tmp = tf1;
+ Transform3f tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
@@ -157,7 +157,7 @@ struct BVHShapeDistancer
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
@@ -175,7 +175,7 @@ struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
@@ -192,7 +192,7 @@ struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
@@ -208,16 +208,16 @@ struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
template<typename T_BVH>
-FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNode<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
- SimpleTransform tf1_tmp = tf1;
+ Transform3f tf1_tmp = tf1;
BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
- SimpleTransform tf2_tmp = tf2;
+ Transform3f tf2_tmp = tf2;
initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
distance(&node);
@@ -226,7 +226,7 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, co
}
template<>
-FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNodeRSS node;
@@ -240,7 +240,7 @@ FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf
}
template<>
-FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNodekIOS node;
@@ -255,7 +255,7 @@ FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& t
template<>
-FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
MeshDistanceTraversalNodeOBBRSS node;
@@ -270,7 +270,7 @@ FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform&
template<typename T_BVH, typename NarrowPhaseSolver>
-FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
diff --git a/trunk/fcl/src/geometric_shapes.cpp b/trunk/fcl/src/geometric_shapes.cpp
index ddc5db2d7212eacc94a288d7c8904fc76e384278..dec91fb82095e1f61eae932b33d2b8cdfae6c12b 100644
--- a/trunk/fcl/src/geometric_shapes.cpp
+++ b/trunk/fcl/src/geometric_shapes.cpp
@@ -116,56 +116,56 @@ void Plane::unitNormalTest()
void Box::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Sphere::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = radius;
}
void Capsule::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Cone::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Cylinder::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Convex::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Plane::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Triangle2::computeLocalAABB()
{
- computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index a7a035cb911b2f13ee14e4c525806caa67df53dc..8c9c660234a68543a415d46600d1218aa531b3e2 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -44,7 +44,7 @@ namespace fcl
namespace details
{
-std::vector<Vec3f> getBoundVertices(const Box& box, const SimpleTransform& tf)
+std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf)
{
std::vector<Vec3f> result(8);
FCL_REAL a = box.side[0] / 2;
@@ -63,7 +63,7 @@ std::vector<Vec3f> getBoundVertices(const Box& box, const SimpleTransform& tf)
}
// we use icosahedron to bound the sphere
-std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const SimpleTransform& tf)
+std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf)
{
std::vector<Vec3f> result(12);
const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
@@ -87,7 +87,7 @@ std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const SimpleTransform&
return result;
}
-std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const SimpleTransform& tf)
+std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf)
{
std::vector<Vec3f> result(36);
const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
@@ -145,7 +145,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const SimpleTransfor
}
-std::vector<Vec3f> getBoundVertices(const Cone& cone, const SimpleTransform& tf)
+std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
{
std::vector<Vec3f> result(7);
@@ -166,7 +166,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const SimpleTransform& tf)
return result;
}
-std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const SimpleTransform& tf)
+std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf)
{
std::vector<Vec3f> result(12);
@@ -192,7 +192,7 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const SimpleTransf
return result;
}
-std::vector<Vec3f> getBoundVertices(const Convex& convex, const SimpleTransform& tf)
+std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf)
{
std::vector<Vec3f> result(convex.num_points);
for(int i = 0; i < convex.num_points; ++i)
@@ -203,7 +203,7 @@ std::vector<Vec3f> getBoundVertices(const Convex& convex, const SimpleTransform&
return result;
}
-std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const SimpleTransform& tf)
+std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f& tf)
{
std::vector<Vec3f> result(3);
result[0] = tf.transform(triangle.a);
@@ -217,7 +217,7 @@ std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const SimpleTrans
template<>
-void computeBV<AABB, Box>(const Box& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -232,7 +232,7 @@ void computeBV<AABB, Box>(const Box& s, const SimpleTransform& tf, AABB& bv)
}
template<>
-void computeBV<AABB, Sphere>(const Sphere& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv)
{
const Vec3f& T = tf.getTranslation();
@@ -242,7 +242,7 @@ void computeBV<AABB, Sphere>(const Sphere& s, const SimpleTransform& tf, AABB& b
}
template<>
-void computeBV<AABB, Capsule>(const Capsule& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -257,7 +257,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const SimpleTransform& tf, AABB&
}
template<>
-void computeBV<AABB, Cone>(const Cone& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -272,7 +272,7 @@ void computeBV<AABB, Cone>(const Cone& s, const SimpleTransform& tf, AABB& bv)
}
template<>
-void computeBV<AABB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -287,7 +287,7 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, AAB
}
template<>
-void computeBV<AABB, Convex>(const Convex& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -303,13 +303,13 @@ void computeBV<AABB, Convex>(const Convex& s, const SimpleTransform& tf, AABB& b
}
template<>
-void computeBV<AABB, Triangle2>(const Triangle2& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv)
{
bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
}
template<>
-void computeBV<AABB, Plane>(const Plane& s, const SimpleTransform& tf, AABB& bv)
+void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
{
const Matrix3f& R = tf.getRotation();
@@ -340,7 +340,7 @@ void computeBV<AABB, Plane>(const Plane& s, const SimpleTransform& tf, AABB& bv)
template<>
-void computeBV<OBB, Box>(const Box& s, const SimpleTransform& tf, OBB& bv)
+void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -353,7 +353,7 @@ void computeBV<OBB, Box>(const Box& s, const SimpleTransform& tf, OBB& bv)
}
template<>
-void computeBV<OBB, Sphere>(const Sphere& s, const SimpleTransform& tf, OBB& bv)
+void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
{
const Vec3f& T = tf.getTranslation();
@@ -365,7 +365,7 @@ void computeBV<OBB, Sphere>(const Sphere& s, const SimpleTransform& tf, OBB& bv)
}
template<>
-void computeBV<OBB, Capsule>(const Capsule& s, const SimpleTransform& tf, OBB& bv)
+void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -378,7 +378,7 @@ void computeBV<OBB, Capsule>(const Capsule& s, const SimpleTransform& tf, OBB& b
}
template<>
-void computeBV<OBB, Cone>(const Cone& s, const SimpleTransform& tf, OBB& bv)
+void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -391,7 +391,7 @@ void computeBV<OBB, Cone>(const Cone& s, const SimpleTransform& tf, OBB& bv)
}
template<>
-void computeBV<OBB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, OBB& bv)
+void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -404,7 +404,7 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, OBB&
}
template<>
-void computeBV<OBB, Convex>(const Convex& s, const SimpleTransform& tf, OBB& bv)
+void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -419,7 +419,7 @@ void computeBV<OBB, Convex>(const Convex& s, const SimpleTransform& tf, OBB& bv)
}
template<>
-void computeBV<OBB, Plane>(const Plane& s, const SimpleTransform& tf, OBB& bv)
+void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -435,7 +435,7 @@ void computeBV<OBB, Plane>(const Plane& s, const SimpleTransform& tf, OBB& bv)
}
template<>
-void computeBV<RSS, Plane>(const Plane& s, const SimpleTransform& tf, RSS& bv)
+void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
{
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
@@ -450,143 +450,143 @@ void computeBV<RSS, Plane>(const Plane& s, const SimpleTransform& tf, RSS& bv)
}
template<>
-void computeBV<OBBRSS, Plane>(const Plane& s, const SimpleTransform& tf, OBBRSS& bv)
+void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv)
{
}
template<>
-void computeBV<kIOS, Plane>(const Plane& s, const SimpleTransform& tf, kIOS& bv)
+void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv)
{
}
template<>
-void computeBV<KDOP<16>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<16>& bv)
+void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv)
{
}
template<>
-void computeBV<KDOP<18>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<18>& bv)
+void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv)
{
}
template<>
-void computeBV<KDOP<24>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<24>& bv)
+void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv)
{
}
-void constructBox(const AABB& bv, Box& box, SimpleTransform& tf)
+void constructBox(const AABB& bv, Box& box, Transform3f& tf)
{
box = Box(bv.max_ - bv.min_);
- tf = SimpleTransform(bv.center());
+ tf = Transform3f(bv.center());
}
-void constructBox(const OBB& bv, Box& box, SimpleTransform& tf)
+void constructBox(const OBB& bv, Box& box, Transform3f& tf)
{
box = Box(bv.extent * 2);
- tf = SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
}
-void constructBox(const OBBRSS& bv, Box& box, SimpleTransform& tf)
+void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = SimpleTransform(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
}
-void constructBox(const kIOS& bv, Box& box, SimpleTransform& tf)
+void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb_bv.extent * 2);
- tf = SimpleTransform(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
+ tf = Transform3f(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
}
-void constructBox(const RSS& bv, Box& box, SimpleTransform& tf)
+void constructBox(const RSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
}
-void constructBox(const KDOP<16>& bv, Box& box, SimpleTransform& tf)
+void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = SimpleTransform(bv.center());
+ tf = Transform3f(bv.center());
}
-void constructBox(const KDOP<18>& bv, Box& box, SimpleTransform& tf)
+void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = SimpleTransform(bv.center());
+ tf = Transform3f(bv.center());
}
-void constructBox(const KDOP<24>& bv, Box& box, SimpleTransform& tf)
+void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = SimpleTransform(bv.center());
+ tf = Transform3f(bv.center());
}
-void constructBox(const AABB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.max_ - bv.min_);
- tf = tf_bv * SimpleTransform(bv.center());
+ tf = tf_bv * Transform3f(bv.center());
}
-void constructBox(const OBB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.extent * 2);
- tf = tf_bv *SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ tf = tf_bv *Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
}
-void constructBox(const OBBRSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = tf_bv * SimpleTransform(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
}
-void constructBox(const kIOS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb_bv.extent * 2);
- tf = tf_bv * SimpleTransform(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
+ tf = tf_bv * Transform3f(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
}
-void constructBox(const RSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = tf_bv * SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ tf = tf_bv * Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
}
-void constructBox(const KDOP<16>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = tf_bv * SimpleTransform(bv.center());
+ tf = tf_bv * Transform3f(bv.center());
}
-void constructBox(const KDOP<18>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = tf_bv * SimpleTransform(bv.center());
+ tf = tf_bv * Transform3f(bv.center());
}
-void constructBox(const KDOP<24>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = tf_bv * SimpleTransform(bv.center());
+ tf = tf_bv * Transform3f(bv.center());
}
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 0e4ead665ea8cf1906ef527f90cf348e7742233c..6a2ab62e0c6ad4197a41dc3e87a4c09295ef9e09 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -820,7 +820,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
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,
+ const Transform3f& tf,
Vec3f* contact_points,
unsigned int* num_contact_points,
FCL_REAL* penetration_depth,
diff --git a/trunk/fcl/src/narrowphase/gjk_libccd.cpp b/trunk/fcl/src/narrowphase/gjk_libccd.cpp
index 90f6674ff3f0abf98c7e91bde82ea0b2e12bf730..bae4aaa5f87874d82f5e4122e7480ee78e1dcc05 100644
--- a/trunk/fcl/src/narrowphase/gjk_libccd.cpp
+++ b/trunk/fcl/src/narrowphase/gjk_libccd.cpp
@@ -438,16 +438,16 @@ static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
/** Basic shape to ccd shape */
-static void shapeToGJK(const ShapeBase& s, const SimpleTransform& tf, ccd_obj_t* o)
+static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
{
- const SimpleQuaternion& q = tf.getQuatRotation();
+ const Quaternion3f& 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);
}
-static void boxToGJK(const Box& s, const SimpleTransform& tf, ccd_box_t* box)
+static void boxToGJK(const Box& s, const Transform3f& tf, ccd_box_t* box)
{
shapeToGJK(s, tf, box);
box->dim[0] = s.side[0] / 2.0;
@@ -455,34 +455,34 @@ static void boxToGJK(const Box& s, const SimpleTransform& tf, ccd_box_t* box)
box->dim[2] = s.side[2] / 2.0;
}
-static void capToGJK(const Capsule& s, const SimpleTransform& tf, ccd_cap_t* cap)
+static void capToGJK(const Capsule& s, const Transform3f& tf, ccd_cap_t* cap)
{
shapeToGJK(s, tf, cap);
cap->radius = s.radius;
cap->height = s.lz / 2;
}
-static void cylToGJK(const Cylinder& s, const SimpleTransform& tf, ccd_cyl_t* cyl)
+static void cylToGJK(const Cylinder& s, const Transform3f& tf, ccd_cyl_t* cyl)
{
shapeToGJK(s, tf, cyl);
cyl->radius = s.radius;
cyl->height = s.lz / 2;
}
-static void coneToGJK(const Cone& s, const SimpleTransform& tf, ccd_cone_t* cone)
+static void coneToGJK(const Cone& s, const Transform3f& tf, ccd_cone_t* cone)
{
shapeToGJK(s, tf, cone);
cone->radius = s.radius;
cone->height = s.lz / 2;
}
-static void sphereToGJK(const Sphere& s, const SimpleTransform& tf, ccd_sphere_t* sph)
+static void sphereToGJK(const Sphere& s, const Transform3f& tf, ccd_sphere_t* sph)
{
shapeToGJK(s, tf, sph);
sph->radius = s.radius;
}
-static void convexToGJK(const Convex& s, const SimpleTransform& tf, ccd_convex_t* conv)
+static void convexToGJK(const Convex& s, const Transform3f& tf, ccd_convex_t* conv)
{
shapeToGJK(s, tf, conv);
conv->convex = &s;
@@ -783,7 +783,7 @@ GJKCenterFunction GJKInitializer<Cylinder>::getCenterFunction()
}
-void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s, const SimpleTransform& tf)
+void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s, const Transform3f& tf)
{
ccd_cyl_t* o = new ccd_cyl_t;
cylToGJK(s, tf, o);
@@ -810,7 +810,7 @@ GJKCenterFunction GJKInitializer<Sphere>::getCenterFunction()
}
-void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s, const SimpleTransform& tf)
+void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s, const Transform3f& tf)
{
ccd_sphere_t* o = new ccd_sphere_t;
sphereToGJK(s, tf, o);
@@ -835,7 +835,7 @@ GJKCenterFunction GJKInitializer<Box>::getCenterFunction()
}
-void* GJKInitializer<Box>::createGJKObject(const Box& s, const SimpleTransform& tf)
+void* GJKInitializer<Box>::createGJKObject(const Box& s, const Transform3f& tf)
{
ccd_box_t* o = new ccd_box_t;
boxToGJK(s, tf, o);
@@ -862,7 +862,7 @@ GJKCenterFunction GJKInitializer<Capsule>::getCenterFunction()
}
-void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s, const SimpleTransform& tf)
+void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s, const Transform3f& tf)
{
ccd_cap_t* o = new ccd_cap_t;
capToGJK(s, tf, o);
@@ -889,7 +889,7 @@ GJKCenterFunction GJKInitializer<Cone>::getCenterFunction()
}
-void* GJKInitializer<Cone>::createGJKObject(const Cone& s, const SimpleTransform& tf)
+void* GJKInitializer<Cone>::createGJKObject(const Cone& s, const Transform3f& tf)
{
ccd_cone_t* o = new ccd_cone_t;
coneToGJK(s, tf, o);
@@ -916,7 +916,7 @@ GJKCenterFunction GJKInitializer<Convex>::getCenterFunction()
}
-void* GJKInitializer<Convex>::createGJKObject(const Convex& s, const SimpleTransform& tf)
+void* GJKInitializer<Convex>::createGJKObject(const Convex& s, const Transform3f& tf)
{
ccd_convex_t* o = new ccd_convex_t;
convexToGJK(s, tf, o);
@@ -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 SimpleTransform& tf)
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& 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,7 +968,7 @@ 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]);
- const SimpleQuaternion& q = tf.getQuatRotation();
+ const Quaternion3f& 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());
diff --git a/trunk/fcl/src/narrowphase/narrowphase.cpp b/trunk/fcl/src/narrowphase/narrowphase.cpp
index a5b0b4ab76bd265b297665ca6d2f3c998ec43fcd..d44cf48fb34fbc1366ccc108bd7442ca4abd2bb0 100644
--- a/trunk/fcl/src/narrowphase/narrowphase.cpp
+++ b/trunk/fcl/src/narrowphase/narrowphase.cpp
@@ -44,8 +44,8 @@ namespace fcl
namespace details
{
-bool sphereSphereIntersect(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
@@ -70,8 +70,8 @@ bool sphereSphereIntersect(const Sphere& s1, const SimpleTransform& tf1,
}
-bool sphereSphereDistance(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist)
{
Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
@@ -140,7 +140,7 @@ bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const
}
-bool sphereTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
+bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
{
Vec3f normal = (P2 - P1).cross(P3 - P1);
@@ -225,7 +225,7 @@ bool sphereTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
return false;
}
-bool sphereTriangleDistance(const Sphere& sp, const SimpleTransform& tf,
+bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist)
{
@@ -1208,8 +1208,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
-bool boxBoxIntersect(const Box& s1, const SimpleTransform& tf1,
- const Box& s2, const SimpleTransform& tf2,
+bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth_, Vec3f* normal_)
{
std::vector<ContactPoint> contacts;
@@ -1247,37 +1247,37 @@ bool boxBoxIntersect(const Box& s1, const SimpleTransform& tf1,
template<>
-bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
template<>
-bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
}
template<>
-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
+bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
}
template<>
-bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist) const
{
return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
}
template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist) const
{
@@ -1285,8 +1285,8 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Simp
}
template<>
-bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const
{
return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
@@ -1297,31 +1297,31 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Simp
template<>
-bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
template<>
-bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
}
template<>
-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
+bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal);
}
template<>
-bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
- const Sphere& s2, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist) const
{
return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
@@ -1329,7 +1329,7 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Simp
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist) const
{
@@ -1337,8 +1337,8 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Simpl
}
template<>
-bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const
{
return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
@@ -1346,16 +1346,16 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Simpl
template<>
-bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
- const Box& s2, const SimpleTransform& tf2,
+bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
template<>
-bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
- const Box& s2, const SimpleTransform& tf2,
+bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 2014166549e9ec709b3e616e6ccb82e1b617ac23..815d5dd2a7a42f444384bc919e2628cc29992508 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -45,8 +45,8 @@ namespace details
{
template<typename BV, typename OrientedNode>
static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result)
{
@@ -79,8 +79,8 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
bool initialize(MeshCollisionTraversalNodeOBB& node,
- const BVHModel<OBB>& model1, const SimpleTransform& tf1,
- const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ const BVHModel<OBB>& model1, const Transform3f& tf1,
+ const BVHModel<OBB>& model2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result)
{
@@ -89,8 +89,8 @@ bool initialize(MeshCollisionTraversalNodeOBB& node,
bool initialize(MeshCollisionTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result)
{
@@ -99,8 +99,8 @@ bool initialize(MeshCollisionTraversalNodeRSS& node,
bool initialize(MeshCollisionTraversalNodekIOS& node,
- const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const BVHModel<kIOS>& model1, const Transform3f& tf1,
+ const BVHModel<kIOS>& model2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result)
{
@@ -108,8 +108,8 @@ bool initialize(MeshCollisionTraversalNodekIOS& node,
}
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result)
{
@@ -123,8 +123,8 @@ namespace details
{
template<typename BV, typename OrientedNode>
static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
- BVHModel<BV>& model1, const SimpleTransform& tf1,
- BVHModel<BV>& model2, const SimpleTransform& tf2,
+ BVHModel<BV>& model1, const Transform3f& tf1,
+ BVHModel<BV>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
@@ -164,8 +164,8 @@ static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
}
bool initialize(PointCloudCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const SimpleTransform& tf1,
- BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ BVHModel<OBB>& model1, const Transform3f& tf1,
+ BVHModel<OBB>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
@@ -176,8 +176,8 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node,
bool initialize(PointCloudCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const SimpleTransform& tf1,
- BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ BVHModel<RSS>& model1, const Transform3f& tf1,
+ BVHModel<RSS>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
@@ -191,8 +191,8 @@ namespace details
template<typename BV, typename OrientedNode>
static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
- BVHModel<BV>& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
@@ -227,8 +227,8 @@ static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
}
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const SimpleTransform& tf1,
- const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ BVHModel<OBB>& model1, const Transform3f& tf1,
+ const BVHModel<OBB>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
@@ -239,8 +239,8 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const CollisionRequest& request,
FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
@@ -256,8 +256,8 @@ namespace details
{
template<typename BV, typename OrientedNode>
static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
- const BVHModel<BV>& model1, const SimpleTransform& tf1,
- const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -288,8 +288,8 @@ static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
}
bool initialize(MeshDistanceTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -298,8 +298,8 @@ bool initialize(MeshDistanceTraversalNodeRSS& node,
bool initialize(MeshDistanceTraversalNodekIOS& node,
- const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const BVHModel<kIOS>& model1, const Transform3f& tf1,
+ const BVHModel<kIOS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -307,8 +307,8 @@ bool initialize(MeshDistanceTraversalNodekIOS& node,
}
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result)
{
diff --git a/trunk/fcl/src/transform.cpp b/trunk/fcl/src/transform.cpp
index fcb34bda6e223fcc027973f061e7e46531d21160..4a597ec2ae3ca95d29f777758167a6b07f542aa5 100644
--- a/trunk/fcl/src/transform.cpp
+++ b/trunk/fcl/src/transform.cpp
@@ -40,7 +40,7 @@
namespace fcl
{
-void SimpleQuaternion::fromRotation(const Matrix3f& R)
+void Quaternion3f::fromRotation(const Matrix3f& R)
{
const int next[3] = {1, 2, 0};
@@ -82,7 +82,7 @@ void SimpleQuaternion::fromRotation(const Matrix3f& R)
}
}
-void SimpleQuaternion::toRotation(Matrix3f& R) const
+void Quaternion3f::toRotation(Matrix3f& R) const
{
FCL_REAL twoX = 2.0*data[1];
FCL_REAL twoY = 2.0*data[2];
@@ -103,7 +103,7 @@ void SimpleQuaternion::toRotation(Matrix3f& R) const
}
-void SimpleQuaternion::fromAxes(const Vec3f axis[3])
+void Quaternion3f::fromAxes(const Vec3f axis[3])
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
@@ -148,7 +148,7 @@ void SimpleQuaternion::fromAxes(const Vec3f axis[3])
}
}
-void SimpleQuaternion::toAxes(Vec3f axis[3]) const
+void Quaternion3f::toAxes(Vec3f axis[3]) const
{
FCL_REAL twoX = 2.0*data[1];
FCL_REAL twoY = 2.0*data[2];
@@ -169,7 +169,7 @@ void SimpleQuaternion::toAxes(Vec3f axis[3]) const
}
-void SimpleQuaternion::fromAxisAngle(const Vec3f& axis, FCL_REAL angle)
+void Quaternion3f::fromAxisAngle(const Vec3f& axis, FCL_REAL angle)
{
FCL_REAL half_angle = 0.5 * angle;
FCL_REAL sn = sin((double)half_angle);
@@ -179,7 +179,7 @@ void SimpleQuaternion::fromAxisAngle(const Vec3f& axis, FCL_REAL angle)
data[3] = sn * axis[2];
}
-void SimpleQuaternion::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
+void Quaternion3f::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
{
double sqr_length = data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
if(sqr_length > 0)
@@ -199,18 +199,18 @@ void SimpleQuaternion::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
}
}
-FCL_REAL SimpleQuaternion::dot(const SimpleQuaternion& other) const
+FCL_REAL Quaternion3f::dot(const Quaternion3f& other) const
{
return data[0] * other.data[0] + data[1] * other.data[1] + data[2] * other.data[2] + data[3] * other.data[3];
}
-SimpleQuaternion SimpleQuaternion::operator + (const SimpleQuaternion& other) const
+Quaternion3f Quaternion3f::operator + (const Quaternion3f& other) const
{
- return SimpleQuaternion(data[0] + other.data[0], data[1] + other.data[1],
+ return Quaternion3f(data[0] + other.data[0], data[1] + other.data[1],
data[2] + other.data[2], data[3] + other.data[3]);
}
-const SimpleQuaternion& SimpleQuaternion::operator += (const SimpleQuaternion& other)
+const Quaternion3f& Quaternion3f::operator += (const Quaternion3f& other)
{
data[0] += other.data[0];
data[1] += other.data[1];
@@ -220,13 +220,13 @@ const SimpleQuaternion& SimpleQuaternion::operator += (const SimpleQuaternion& o
return *this;
}
-SimpleQuaternion SimpleQuaternion::operator - (const SimpleQuaternion& other) const
+Quaternion3f Quaternion3f::operator - (const Quaternion3f& other) const
{
- return SimpleQuaternion(data[0] - other.data[0], data[1] - other.data[1],
+ return Quaternion3f(data[0] - other.data[0], data[1] - other.data[1],
data[2] - other.data[2], data[3] - other.data[3]);
}
-const SimpleQuaternion& SimpleQuaternion::operator -= (const SimpleQuaternion& other)
+const Quaternion3f& Quaternion3f::operator -= (const Quaternion3f& other)
{
data[0] -= other.data[0];
data[1] -= other.data[1];
@@ -236,16 +236,16 @@ const SimpleQuaternion& SimpleQuaternion::operator -= (const SimpleQuaternion& o
return *this;
}
-SimpleQuaternion SimpleQuaternion::operator * (const SimpleQuaternion& other) const
+Quaternion3f Quaternion3f::operator * (const Quaternion3f& other) const
{
- return SimpleQuaternion(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
+ return Quaternion3f(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2],
data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1],
data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
}
-const SimpleQuaternion& SimpleQuaternion::operator *= (const SimpleQuaternion& other)
+const Quaternion3f& Quaternion3f::operator *= (const Quaternion3f& other)
{
FCL_REAL a = data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3];
FCL_REAL b = data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2];
@@ -259,17 +259,17 @@ const SimpleQuaternion& SimpleQuaternion::operator *= (const SimpleQuaternion& o
return *this;
}
-SimpleQuaternion SimpleQuaternion::operator - () const
+Quaternion3f Quaternion3f::operator - () const
{
- return SimpleQuaternion(-data[0], -data[1], -data[2], -data[3]);
+ return Quaternion3f(-data[0], -data[1], -data[2], -data[3]);
}
-SimpleQuaternion SimpleQuaternion::operator * (FCL_REAL t) const
+Quaternion3f Quaternion3f::operator * (FCL_REAL t) const
{
- return SimpleQuaternion(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
+ return Quaternion3f(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
}
-const SimpleQuaternion& SimpleQuaternion::operator *= (FCL_REAL t)
+const Quaternion3f& Quaternion3f::operator *= (FCL_REAL t)
{
data[0] *= t;
data[1] *= t;
@@ -280,7 +280,7 @@ const SimpleQuaternion& SimpleQuaternion::operator *= (FCL_REAL t)
}
-SimpleQuaternion& SimpleQuaternion::conj()
+Quaternion3f& Quaternion3f::conj()
{
data[1] = -data[1];
data[2] = -data[2];
@@ -288,7 +288,7 @@ SimpleQuaternion& SimpleQuaternion::conj()
return *this;
}
-SimpleQuaternion& SimpleQuaternion::inverse()
+Quaternion3f& Quaternion3f::inverse()
{
FCL_REAL sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
if(sqr_length > 0)
@@ -309,35 +309,35 @@ SimpleQuaternion& SimpleQuaternion::inverse()
return *this;
}
-Vec3f SimpleQuaternion::transform(const Vec3f& v) const
+Vec3f Quaternion3f::transform(const Vec3f& v) const
{
- SimpleQuaternion r = (*this) * SimpleQuaternion(0, v[0], v[1], v[2]) * (fcl::conj(*this));
+ Quaternion3f r = (*this) * Quaternion3f(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)
+Quaternion3f conj(const Quaternion3f& q)
{
- SimpleQuaternion r(q);
+ Quaternion3f r(q);
return r.conj();
}
-SimpleQuaternion inverse(const SimpleQuaternion& q)
+Quaternion3f inverse(const Quaternion3f& q)
{
- SimpleQuaternion res(q);
+ Quaternion3f res(q);
return res.inverse();
}
-SimpleTransform inverse(const SimpleTransform& tf)
+Transform3f inverse(const Transform3f& tf)
{
- SimpleTransform res(tf);
+ Transform3f res(tf);
return res.inverse();
}
-void relativeTransform(const SimpleTransform& tf1, const SimpleTransform& tf2,
- SimpleTransform& tf)
+void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
+ Transform3f& tf)
{
- const SimpleQuaternion& q1_inv = fcl::conj(tf1.getQuatRotation());
- tf = SimpleTransform(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
+ const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation());
+ tf = Transform3f(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 e3057954dd00b103c32a013b72e5c582093ce35d..73a6d6eb5bf04b65a70705deee4b0e1b9b6d2685 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -48,7 +48,7 @@ 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,
+ const Transform3f& tf1, const Transform3f& tf2,
bool enable_statistics,
FCL_REAL cost_density,
int& num_leaf_tests,
@@ -513,7 +513,7 @@ static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1, co
template<typename BV>
static inline void distancePostprocessOrientedNode(const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- const SimpleTransform& tf1, const DistanceRequest& request, DistanceResult& result)
+ const Transform3f& tf1, const DistanceRequest& request, DistanceResult& result)
{
/// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space.
if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2))