From 0b9a887874e29f16cdc9938ad9c5534fe1e488ac Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 25 May 2012 20:38:03 +0000
Subject: [PATCH] remove local transform from geometric shapes
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@83 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/collision_func_matrix.h | 2 +-
trunk/fcl/include/fcl/collision_object.h | 2 +-
.../fcl/geometric_shape_to_BVH_model.h | 24 ++-----
trunk/fcl/include/fcl/geometric_shapes.h | 55 +--------------
trunk/fcl/include/fcl/transform.h | 14 ++++
trunk/fcl/src/BVH_model.cpp | 2 +-
trunk/fcl/src/collision_func_matrix.cpp | 67 ++++---------------
trunk/fcl/src/geometric_shapes_intersect.cpp | 36 +---------
trunk/fcl/src/geometric_shapes_utility.cpp | 50 +++++++-------
9 files changed, 64 insertions(+), 188 deletions(-)
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index 32582fb3..87cf1727 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -51,7 +51,7 @@ typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform&
struct CollisionFunctionMatrix
{
- CollisionFunc collision_matrix[14][14];
+ CollisionFunc collision_matrix[15][15];
CollisionFunctionMatrix();
};
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index d43893bc..fd05f5a9 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -47,7 +47,7 @@ namespace fcl
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
-enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KIOS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
+enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
class CollisionGeometry
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 563a1bf3..cd688802 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -79,9 +79,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const SimpleTransfo
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
- v = pose.transform(v);
- points[i] = v;
+ points[i] = pose.transform(points[i]);
}
model.beginModel();
@@ -146,9 +144,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const SimpleTran
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
- v = pose.transform(v);
- points[i] = v;
+ points[i] = pose.transform(points[i]);
}
model.beginModel();
@@ -221,9 +217,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const SimpleTra
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
- v = pose.transform(v);
- points[i] = v;
+ points[i] = pose.transform(points[i]);
}
model.beginModel();
@@ -298,9 +292,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const SimpleTr
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
- v = pose.transform(v);
- points[i] = v;
+ points[i] = pose.transform(points[i]);
}
model.beginModel();
@@ -378,9 +370,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const SimpleT
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
- v = pose.transform(v);
- points[i] = v;
+ points[i] = pose.transform(points[i]);
}
model.beginModel();
@@ -453,9 +443,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const SimpleTransf
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
- v = pose.transform(v);
- points[i] = v;
+ points[i] = pose.transform(points[i]);
}
model.beginModel();
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index 7e461a75..608fe846 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -51,63 +51,10 @@ class ShapeBase : public CollisionGeometry
{
public:
/** \brief Default Constructor */
- ShapeBase()
- {
- Rloc.setIdentity();
- }
-
- /** \brief Set the local frame of the shape */
- void setLocalTransform(const Matrix3f& R_, const Vec3f& T_)
- {
- Rloc = R_;
- Tloc = T_;
- }
-
- /** \brief Set the local orientation of the shape */
- void setLocalRotation(const Matrix3f& R)
- {
- Rloc = R;
- }
-
- /** \brief Set the local position of the shape */
- void setLocalTranslation(const Vec3f& T)
- {
- Tloc = T;
- }
-
- /** \brief Append additional transform to the local transform */
- void appendLocalTransform(const Matrix3f& R, const Vec3f& T)
- {
- Rloc = R * Rloc;
- Tloc = R * Tloc + T;
- }
-
- /** \brief Get local transform */
- void getLocalTransform(Matrix3f& R, Vec3f& T) const
- {
- T = Tloc;
- R = Rloc;
- }
-
- /** \brief Get local position */
- inline const Vec3f& getLocalTranslation() const
- {
- return Tloc;
- }
-
- /** \brief Get local orientation */
- inline const Matrix3f& getLocalRotation() const
- {
- return Rloc;
- }
+ ShapeBase() {}
/** \brief Get object type: a geometric shape */
OBJECT_TYPE getObjectType() const { return OT_GEOM; }
-
-protected:
-
- Matrix3f Rloc;
- Vec3f Tloc;
};
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index c6f45693..efc2ff75 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -151,6 +151,20 @@ public:
q.fromRotation(R_);
}
+ SimpleTransform(const Matrix3f& R_)
+ {
+ R = R_;
+ q.fromRotation(R_);
+ T.setValue(0.0);
+ }
+
+ SimpleTransform(const Vec3f& T_)
+ {
+ T = T_;
+ R.setIdentity();
+ q = SimpleQuaternion();
+ }
+
inline const Vec3f& getTranslation() const
{
return T;
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index da0e4c2a..81e3a9d3 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -894,7 +894,7 @@ NODE_TYPE BVHModel<RSS>::getNodeType() const
template<>
NODE_TYPE BVHModel<kIOS>::getNodeType() const
{
- return BV_KIOS;
+ return BV_kIOS;
}
template<>
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 1ad46a1c..d7d3f217 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -66,7 +66,7 @@ int ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, c
template<typename T_BVH, typename T_SH>
-int BVHShapeContactCollection(const std::vector<BVHShapeCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const T_SH* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+static inline int BVHShapeContactCollection(const std::vector<BVHShapeCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const T_SH* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
int num_contacts = pairs.size();
if(num_contacts > 0)
@@ -164,7 +164,7 @@ struct BVHShapeCollider<kIOS, T_SH>
*/
template<typename T_BVH>
-int BVHContactCollection(const std::vector<BVHCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+static inline int BVHContactCollection(const std::vector<BVHCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
int num_contacts = pairs.size();
if(num_contacts > 0)
@@ -188,7 +188,7 @@ int BVHContactCollection(const std::vector<BVHCollisionPair>& pairs, const BVHMo
template<typename T_BVH>
-int BVHContactCollection_OBB_and_RSS_and_kIOS(const std::vector<BVHCollisionPair>& pairs, const SimpleTransform& tf1, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+static inline int BVHContactCollection_OBB_and_RSS_and_kIOS(const std::vector<BVHCollisionPair>& pairs, const SimpleTransform& tf1, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
int num_contacts = pairs.size();
if(num_contacts > 0)
@@ -264,9 +264,9 @@ int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, co
CollisionFunctionMatrix::CollisionFunctionMatrix()
{
- for(int i = 0; i < 14; ++i)
+ for(int i = 0; i < 15; ++i)
{
- for(int j = 0; j < 14; ++j)
+ for(int j = 0; j < 15; ++j)
collision_matrix[i][j] = NULL;
}
@@ -374,55 +374,13 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex>::collide;
collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane>::collide;
- /*
- collision_matrix[GEOM_BOX][BV_AABB] = BoxAABBCollide;
- collision_matrix[GEOM_SPHERE][BV_AABB] = SphereAABBCollide;
- collision_matrix[GEOM_CAPSULE][BV_AABB] = CapAABBCollide;
- collision_matrix[GEOM_CONE][BV_AABB] = ConeAABBCollide;
- collision_matrix[GEOM_CYLINDER][BV_AABB] = CylinderAABBCollide;
- collision_matrix[GEOM_CONVEX][BV_AABB] = ConvexAABBCollide;
- collision_matrix[GEOM_PLANE][BV_AABB] = PlaneAABBCollide;
-
- collision_matrix[GEOM_BOX][BV_OBB] = BoxOBBCollide;
- collision_matrix[GEOM_SPHERE][BV_OBB] = SphereOBBCollide;
- collision_matrix[GEOM_CAPSULE][BV_OBB] = CapOBBCollide;
- collision_matrix[GEOM_CONE][BV_OBB] = ConeOBBCollide;
- collision_matrix[GEOM_CYLINDER][BV_OBB] = CylinderOBBCollide;
- collision_matrix[GEOM_CONVEX][BV_OBB] = ConvexOBBCollide;
- collision_matrix[GEOM_PLANE][BV_OBB] = PlaneOBBCollide;
-
- collision_matrix[GEOM_BOX][BV_RSS] = BoxRSSCollide;
- collision_matrix[GEOM_SPHERE][BV_RSS] = SphereRSSCollide;
- collision_matrix[GEOM_CAPSULE][BV_RSS] = CapRSSCollide;
- collision_matrix[GEOM_CONE][BV_RSS] = ConeRSSCollide;
- collision_matrix[GEOM_CYLINDER][BV_RSS] = CylinderRSSCollide;
- collision_matrix[GEOM_CONVEX][BV_RSS] = ConvexRSSCollide;
- collision_matrix[GEOM_PLANE][BV_RSS] = PlaneRSSCollide;
-
- collision_matrix[GEOM_BOX][BV_KDOP16] = BoxKDOP16Collide;
- collision_matrix[GEOM_SPHERE][BV_KDOP16] = SphereKDOP16Collide;
- collision_matrix[GEOM_CAPSULE][BV_KDOP16] = CapKDOP16Collide;
- collision_matrix[GEOM_CONE][BV_KDOP16] = ConeKDOP16Collide;
- collision_matrix[GEOM_CYLINDER][BV_KDOP16] = CylinderKDOP16Collide;
- collision_matrix[GEOM_CONVEX][BV_KDOP16] = ConvexKDOP16Collide;
- collision_matrix[GEOM_PLANE][BV_KDOP16] = PlaneKDOP16Collide;
-
- collision_matrix[GEOM_BOX][BV_KDOP18] = BoxKDOP18Collide;
- collision_matrix[GEOM_SPHERE][BV_KDOP18] = SphereKDOP18Collide;
- collision_matrix[GEOM_CAPSULE][BV_KDOP18] = CapKDOP18Collide;
- collision_matrix[GEOM_CONE][BV_KDOP18] = ConeKDOP18Collide;
- collision_matrix[GEOM_CYLINDER][BV_KDOP18] = CylinderKDOP18Collide;
- collision_matrix[GEOM_CONVEX][BV_KDOP18] = ConvexKDOP18Collide;
- collision_matrix[GEOM_PLANE][BV_KDOP18] = PlaneKDOP18Collide;
-
- collision_matrix[GEOM_BOX][BV_KDOP24] = BoxKDOP24Collide;
- collision_matrix[GEOM_SPHERE][BV_KDOP24] = SphereKDOP24Collide;
- collision_matrix[GEOM_CAPSULE][BV_KDOP24] = CapKDOP24Collide;
- collision_matrix[GEOM_CONE][BV_KDOP24] = ConeKDOP24Collide;
- collision_matrix[GEOM_CYLINDER][BV_KDOP24] = CylinderKDOP24Collide;
- collision_matrix[GEOM_CONVEX][BV_KDOP24] = ConvexKDOP24Collide;
- collision_matrix[GEOM_PLANE][BV_KDOP24] = PlaneKDOP24Collide;
- */
+ collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box>::collide;
+ collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere>::collide;
+ collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone>::collide;
+ collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex>::collide;
+ collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane>::collide;
collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB>;
collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB>;
@@ -430,6 +388,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16> >;
collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18> >;
collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24> >;
+ collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS>;
}
}
diff --git a/trunk/fcl/src/geometric_shapes_intersect.cpp b/trunk/fcl/src/geometric_shapes_intersect.cpp
index acc554e6..ea139967 100644
--- a/trunk/fcl/src/geometric_shapes_intersect.cpp
+++ b/trunk/fcl/src/geometric_shapes_intersect.cpp
@@ -82,46 +82,14 @@ struct ccd_triangle_t : public ccd_obj_t
ccd_vec3_t c;
};
-void transformGJKObject(void* obj, const Matrix3f& R, const Vec3f& T)
-{
- ccd_obj_t* o = (ccd_obj_t*)obj;
- SimpleQuaternion q_;
- q_.fromRotation(R);
-
- ccd_vec3_t t;
- ccd_quat_t q;
- ccdVec3Set(&t, T[0], T[1], T[2]);
- ccdQuatSet(&q, q_.getX(), q_.getY(), q_.getZ(), q_.getW());
-
- ccd_quat_t tmp;
- ccdQuatMul2(&tmp, &q, &o->rot); // make sure it is correct here!!
- ccdQuatCopy(&o->rot, &tmp);
- ccdQuatRotVec(&o->pos, &q);
- ccdVec3Add(&o->pos, &t);
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-}
-
/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, const SimpleTransform& tf, ccd_obj_t* o)
{
-
- SimpleQuaternion q;
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- q.fromRotation(R);
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ 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);
-/*
- SimpleQuaternion q;
- q.fromRotation(s.getLocalRotation());
- Vec3f T = s.getLocalTranslation();
- ccdVec3Set(&o->pos, T[0], T[1], T[2]);
- ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
- ccdQuatInvert2(&o->rot_inv, &o->rot);
-
- transformGJKObject(o, s.getRotation(), s.getTranslation());
-*/
}
static void boxToGJK(const Box& s, const SimpleTransform& tf, ccd_box_t* box)
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 06224945..d39a2d11 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -44,8 +44,8 @@ namespace fcl
template<>
void computeBV<AABB>(const Box& s, const SimpleTransform& tf, AABB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
BVH_REAL x_range = 0.5 * (fabs(R[0][0] * s.side[0]) + fabs(R[0][1] * s.side[1]) + fabs(R[0][2] * s.side[2]));
BVH_REAL y_range = 0.5 * (fabs(R[1][0] * s.side[0]) + fabs(R[1][1] * s.side[1]) + fabs(R[1][2] * s.side[2]));
@@ -58,7 +58,7 @@ void computeBV<AABB>(const Box& s, const SimpleTransform& tf, AABB& bv)
template<>
void computeBV<AABB>(const Sphere& s, const SimpleTransform& tf, AABB& bv)
{
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Vec3f& T = tf.getTranslation();
bv.max_ = T + Vec3f(s.radius, s.radius, s.radius);
bv.min_ = T + Vec3f(-s.radius, -s.radius, -s.radius);
@@ -67,8 +67,8 @@ void computeBV<AABB>(const Sphere& s, const SimpleTransform& tf, AABB& bv)
template<>
void computeBV<AABB>(const Capsule& s, const SimpleTransform& tf, AABB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
BVH_REAL x_range = 0.5 * fabs(R[0][2] * s.lz) + s.radius;
BVH_REAL y_range = 0.5 * fabs(R[1][2] * s.lz) + s.radius;
@@ -81,8 +81,8 @@ void computeBV<AABB>(const Capsule& s, const SimpleTransform& tf, AABB& bv)
template<>
void computeBV<AABB>(const Cone& s, const SimpleTransform& tf, AABB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
@@ -95,8 +95,8 @@ void computeBV<AABB>(const Cone& s, const SimpleTransform& tf, AABB& bv)
template<>
void computeBV<AABB>(const Cylinder& s, const SimpleTransform& tf, AABB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
@@ -109,8 +109,8 @@ void computeBV<AABB>(const Cylinder& s, const SimpleTransform& tf, AABB& bv)
template<>
void computeBV<AABB>(const Convex& s, const SimpleTransform& tf, AABB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
AABB bv_;
for(int i = 0; i < s.num_points; ++i)
@@ -125,7 +125,7 @@ void computeBV<AABB>(const Convex& s, const SimpleTransform& tf, AABB& bv)
template<>
void computeBV<AABB>(const Plane& s, const SimpleTransform& tf, AABB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
+ const Matrix3f& R = tf.getRotation();
Vec3f n = R * s.n;
@@ -156,8 +156,8 @@ void computeBV<AABB>(const Plane& s, const SimpleTransform& tf, AABB& bv)
template<>
void computeBV<OBB>(const Box& s, const SimpleTransform& tf, OBB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
bv.To = T;
bv.axis[0] = R.getColumn(0);
@@ -169,7 +169,7 @@ void computeBV<OBB>(const Box& s, const SimpleTransform& tf, OBB& bv)
template<>
void computeBV<OBB>(const Sphere& s, const SimpleTransform& tf, OBB& bv)
{
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Vec3f& T = tf.getTranslation();
bv.To = T;
bv.axis[0].setValue(1, 0, 0);
@@ -181,8 +181,8 @@ void computeBV<OBB>(const Sphere& s, const SimpleTransform& tf, OBB& bv)
template<>
void computeBV<OBB>(const Capsule& s, const SimpleTransform& tf, OBB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
bv.To = T;
bv.axis[0] = R.getColumn(0);
@@ -194,8 +194,8 @@ void computeBV<OBB>(const Capsule& s, const SimpleTransform& tf, OBB& bv)
template<>
void computeBV<OBB>(const Cone& s, const SimpleTransform& tf, OBB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
bv.To = T;
bv.axis[0] = R.getColumn(0);
@@ -207,8 +207,8 @@ void computeBV<OBB>(const Cone& s, const SimpleTransform& tf, OBB& bv)
template<>
void computeBV<OBB>(const Cylinder& s, const SimpleTransform& tf, OBB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
bv.To = T;
bv.axis[0] = R.getColumn(0);
@@ -220,8 +220,8 @@ void computeBV<OBB>(const Cylinder& s, const SimpleTransform& tf, OBB& bv)
template<>
void computeBV<OBB>(const Convex& s, const SimpleTransform& tf, OBB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
fit(s.points, s.num_points, bv);
@@ -236,8 +236,8 @@ void computeBV<OBB>(const Convex& s, const SimpleTransform& tf, OBB& bv)
template<>
void computeBV<OBB>(const Plane& s, const SimpleTransform& tf, OBB& bv)
{
- Matrix3f R = tf.getRotation() * s.getLocalRotation();
- Vec3f T = tf.getRotation() * s.getLocalTranslation() + tf.getTranslation();
+ const Matrix3f& R = tf.getRotation();
+ const Vec3f& T = tf.getTranslation();
// generate other two axes orthonormal to plane normal
generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
--
GitLab