diff --git a/trunk/fcl/include/fcl/BVH_utility.h b/trunk/fcl/include/fcl/BVH_utility.h
index 8acb9c5528c55c43b7b5b1d38b69064f0a7314b8..436dcd32cf45229448c6c017daf620f9bda2ae1e 100644
--- a/trunk/fcl/include/fcl/BVH_utility.h
+++ b/trunk/fcl/include/fcl/BVH_utility.h
@@ -83,10 +83,10 @@ void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r);
void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs);
/** \brief Compute the covariance matrix for a set or subset of points. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f M[3]);
+void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Matrix3f& M);
/** \brief Compute the covariance matrix for a set or subset of triangles. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f M[3]);
+void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
diff --git a/trunk/fcl/include/fcl/OBB.h b/trunk/fcl/include/fcl/OBB.h
index 34565d22cd600de5397b3e239b3be00d99cd4e0e..7d6500911b742493065d9d3ee3c83feebcd2810e 100644
--- a/trunk/fcl/include/fcl/OBB.h
+++ b/trunk/fcl/include/fcl/OBB.h
@@ -39,6 +39,7 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
/** \brief Main namespace */
namespace fcl
@@ -141,12 +142,12 @@ private:
public:
/** Kernel check whether two OBB are disjoint */
- static bool obbDisjoint(const Vec3f B[3], const Vec3f& T, const Vec3f& a, const Vec3f& b);
+ static bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
};
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const OBB& b1, const OBB& b2);
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
}
diff --git a/trunk/fcl/include/fcl/RSS.h b/trunk/fcl/include/fcl/RSS.h
index c839ac72442390c4d9c8298eb4b302ec1bc42f46..08c069b27092baee871132038a51a20bd749f8d0 100644
--- a/trunk/fcl/include/fcl/RSS.h
+++ b/trunk/fcl/include/fcl/RSS.h
@@ -39,6 +39,7 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
namespace fcl
{
@@ -159,7 +160,7 @@ public:
/** \brief distance between two oriented rectangles
* P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle
*/
- static BVH_REAL rectDistance(const Vec3f Rab[3], Vec3f const& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
+ static BVH_REAL rectDistance(const Matrix3f& Rab, const Vec3f& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
};
@@ -167,9 +168,9 @@ public:
* P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
* Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
*/
-BVH_REAL distance(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2);
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
}
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 845b233af5f2aac42aa61b05456f0f43c284de45..48bcee6719f234b4e6e749d7a602d243a70080a0 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -89,7 +89,7 @@ public:
return t.getTranslation();
}
- inline const Vec3f* getRotation() const
+ inline const Matrix3f& getRotation() const
{
return t.getRotation();
}
@@ -99,7 +99,7 @@ public:
return t.getQuatRotation();
}
- void setRotation(const Vec3f R[3])
+ void setRotation(const Matrix3f& R)
{
t.setRotation(R);
}
@@ -114,7 +114,7 @@ public:
t.setQuatRotation(q);
}
- void setTransform(const Vec3f R[3], const Vec3f& T)
+ void setTransform(const Matrix3f& R, const Vec3f& T)
{
t.setTransform(R, T);
}
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 18926dd51831c324308e9fa1a22f41cd7d73c7af..c721baacfa3a679be1e47022bc1587673bd705c8 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -79,8 +79,8 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
- v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+ v = shape.getRotation() * v + shape.getTranslation();
points[i] = v;
}
@@ -146,8 +146,8 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
- v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+ v = shape.getRotation() * v + shape.getTranslation();
points[i] = v;
}
@@ -221,8 +221,8 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
- v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+ v = shape.getRotation() * v + shape.getTranslation();
points[i] = v;
}
@@ -298,8 +298,8 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
- v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+ v = shape.getRotation() * v + shape.getTranslation();
points[i] = v;
}
@@ -378,8 +378,8 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
- v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+ v = shape.getRotation() * v + shape.getTranslation();
points[i] = v;
}
@@ -453,8 +453,8 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot =
for(unsigned int i = 0; i < points.size(); ++i)
{
- Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
- v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
+ Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
+ v = shape.getRotation() * v + shape.getTranslation();
points[i] = v;
}
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index e6bca8cb363663f508bd6d4a9e440fd31b6c6c62..ac3c771a92dd53505ea04a788b1fd6d19652da4d 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -53,26 +53,20 @@ public:
/** \brief Default Constructor */
ShapeBase()
{
- Rloc[0][0] = 1;
- Rloc[1][1] = 1;
- Rloc[2][2] = 1;
+ Rloc.setIdentity();
}
/** \brief Set the local frame of the shape */
- void setLocalTransform(const Vec3f R_[3], const Vec3f& T_)
+ void setLocalTransform(const Matrix3f& R_, const Vec3f& T_)
{
- Rloc[0] = R_[0];
- Rloc[1] = R_[1];
- Rloc[2] = R_[2];
+ Rloc = R_;
Tloc = T_;
}
/** \brief Set the local orientation of the shape */
- void setLocalRotation(const Vec3f R[3])
+ void setLocalRotation(const Matrix3f& R)
{
- Rloc[0] = R[0];
- Rloc[1] = R[1];
- Rloc[2] = R[2];
+ Rloc = R;
}
/** \brief Set the local position of the shape */
@@ -82,22 +76,17 @@ public:
}
/** \brief Append additional transform to the local transform */
- void appendLocalTransform(const Vec3f R[3], const Vec3f& T)
+ void appendLocalTransform(const Matrix3f& R, const Vec3f& T)
{
- Vec3f R0[3];
- for(int i = 0; i < 3; ++i)
- R0[i] = Rloc[i];
- matMulMat(R, R0, Rloc);
- Tloc = matMulVec(R, Tloc) + T;
+ Rloc = R * Rloc;
+ Tloc = R * Tloc + T;
}
/** \brief Get local transform */
- void getLocalTransform(Vec3f R[3], Vec3f& T) const
+ void getLocalTransform(Matrix3f& R, Vec3f& T) const
{
T = Tloc;
- R[0] = Rloc[0];
- R[1] = Rloc[1];
- R[2] = Rloc[2];
+ R = Rloc;
}
/** \brief Get local position */
@@ -107,7 +96,7 @@ public:
}
/** \brief Get local orientation */
- inline const Vec3f* getLocalRotation() const
+ inline const Matrix3f& getLocalRotation() const
{
return Rloc;
}
@@ -117,7 +106,7 @@ public:
protected:
- Vec3f Rloc[3];
+ Matrix3f Rloc;
Vec3f Tloc;
};
diff --git a/trunk/fcl/include/fcl/geometric_shapes_intersect.h b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
index d2f8bd13dc0114cc2cf7e1b9408546991f5b2b65..e53a3a2ac015595a6851ce2cee5d032175483b5d 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_intersect.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
@@ -145,7 +145,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 Vec3f R[3], const Vec3f& T);
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T);
void triDeleteGJKObject(void* o);
@@ -187,7 +187,7 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
/** \brief intersection checking between one shape and a triangle with transformation */
template<typename S>
-bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], const Vec3f& T,
+bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = GJKInitializer<S>::createGJKObject(s);
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index 5b1944d400d323490691f4d2a23b19260d8632aa..80ec05efe47fe8d2198dc2c6374cf24da9660273 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -137,7 +137,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 Vec3f R[3], const Vec3f& T,
+ const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points = NULL,
unsigned int* num_contact_points = NULL,
BVH_REAL* penetration_depth = NULL,
@@ -151,14 +151,14 @@ public:
static BVH_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
- const Vec3f R[3], const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
+ const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3);
static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Vec3f R[3], const Vec3f& T);
+ const Matrix3f& R, const Vec3f& T);
#endif
private:
@@ -320,12 +320,12 @@ public:
* The returned P and Q are both in the coordinate of the first triangle's coordinate
*/
static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
- const Vec3f R[3], const Vec3f& Tl,
+ const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- const Vec3f R[3], const Vec3f& Tl,
+ const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
};
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
index fc38585a0471179d723104d7cb7b64a3c58e93c6..8569223f7f2a241dc44662b38d684fb1132d327a 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -54,7 +54,7 @@ namespace fcl
BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
{
setValue(xx, xy, xz,
- yz, yy, yz,
+ yx, yy, yz,
zx, zy, zz);
}
@@ -95,6 +95,8 @@ namespace fcl
Matrix3f& operator *= (const Matrix3f& other);
+ Matrix3f& operator += (BVH_REAL c);
+
void setIdentity()
{
setValue((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
@@ -102,6 +104,11 @@ namespace fcl
(BVH_REAL)0.0, (BVH_REAL)0.0, (BVH_REAL)1.0);
}
+ void setZero()
+ {
+ setValue((BVH_REAL)0.0);
+ }
+
static const Matrix3f& getIdentity()
{
static const Matrix3f I((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
@@ -113,6 +120,7 @@ namespace fcl
BVH_REAL determinant() const;
Matrix3f transpose() const;
Matrix3f inverse() const;
+ Matrix3f abs() const;
Matrix3f transposeTimes(const Matrix3f& m) const;
Matrix3f timesTranspose(const Matrix3f& m) const;
@@ -146,15 +154,20 @@ namespace fcl
return v_[0][2] * v[0] + v_[1][2] * v[1] + v_[2][2] * v[2];
}
- inline Matrix3f& setValue(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
- BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
- BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
+ inline void setValue(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
+ BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
+ BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
{
v_[0].setValue(xx, xy, xz);
v_[1].setValue(yx, yy, yz);
v_[2].setValue(zx, zy, zz);
+ }
- return *this;
+ inline void setValue(BVH_REAL x)
+ {
+ v_[0].setValue(x);
+ v_[1].setValue(x);
+ v_[2].setValue(x);
}
};
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index f197bd1d0f373a64f14661ff25b8803f5b6eb712..6c10a22138901c22d0895257ea9652cbe384cc42 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -39,6 +39,7 @@
#define FCL_MOTION_H
#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
#include "fcl/RSS.h"
#include "fcl/transform.h"
#include "fcl/motion_base.h"
@@ -138,20 +139,15 @@ public:
}
/** \brief Get the rotation and translation in current step */
- void getCurrentTransform(Vec3f R[3], Vec3f& T) const
+ void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
- for(int i = 0; i < 3; ++i)
- {
- R[i] = tf.getRotation()[i];
- }
-
+ R = tf.getRotation();
T = tf.getTranslation();
}
- void getCurrentRotation(Vec3f R[3]) const
+ void getCurrentRotation(Matrix3f& R) const
{
- for(int i = 0; i < 3; ++i)
- R[i] = tf.getRotation()[i];
+ R = tf.getRotation();
}
void getCurrentTranslation(Vec3f& T) const
@@ -333,8 +329,8 @@ public:
}
/** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
- ScrewMotion(const Vec3f R1[3], const Vec3f& T1,
- const Vec3f R2[3], const Vec3f& T2)
+ ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2)
{
tf1 = SimpleTransform(R1, T1);
tf2 = SimpleTransform(R2, T2);
@@ -384,20 +380,15 @@ public:
}
/** \brief Get the rotation and translation in current step */
- void getCurrentTransform(Vec3f R[3], Vec3f& T) const
+ void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
- for(int i = 0; i < 3; ++i)
- {
- R[i] = tf.getRotation()[i];
- }
-
+ R = tf.getRotation();
T = tf.getTranslation();
}
- void getCurrentRotation(Vec3f R[3]) const
+ void getCurrentRotation(Matrix3f& R) const
{
- for(int i = 0; i < 3; ++i)
- R[i] = tf.getRotation()[i];
+ R = tf.getRotation();
}
void getCurrentTranslation(Vec3f& T) const
@@ -491,7 +482,7 @@ public:
InterpMotion()
{
/** Default angular velocity is zero */
- angular_axis = Vec3f(1, 0, 0);
+ angular_axis.setValue(1, 0, 0);
angular_vel = 0;
/** Default reference point is local zero point */
@@ -500,8 +491,8 @@ public:
}
/** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
- InterpMotion(const Vec3f R1[3], const Vec3f& T1,
- const Vec3f R2[3], const Vec3f& T2)
+ InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2)
{
tf1 = SimpleTransform(R1, T1);
tf2 = SimpleTransform(R2, T2);
@@ -517,8 +508,8 @@ public:
/** \brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center
*/
- InterpMotion(const Vec3f R1[3], const Vec3f& T1,
- const Vec3f R2[3], const Vec3f& T2,
+ InterpMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2,
const Vec3f& O)
{
tf1 = SimpleTransform(R1, T1);
@@ -574,20 +565,15 @@ public:
}
/** \brief Get the rotation and translation in current step */
- void getCurrentTransform(Vec3f R[3], Vec3f& T) const
+ void getCurrentTransform(Matrix3f& R, Vec3f& T) const
{
- for(int i = 0; i < 3; ++i)
- {
- R[i] = tf.getRotation()[i];
- }
-
+ R = tf.getRotation();
T = tf.getTranslation();
}
- void getCurrentRotation(Vec3f R[3]) const
+ void getCurrentRotation(Matrix3f& R) const
{
- for(int i = 0; i < 3; ++i)
- R[i] = tf.getRotation()[i];
+ R = tf.getRotation();
}
void getCurrentTranslation(Vec3f& T) const
diff --git a/trunk/fcl/include/fcl/motion_base.h b/trunk/fcl/include/fcl/motion_base.h
index 005a3d740b8925a44cebd5c511677a98ae32fb59..3f17afff8fef580cd0b37f57111ddfbfba1e98d0 100644
--- a/trunk/fcl/include/fcl/motion_base.h
+++ b/trunk/fcl/include/fcl/motion_base.h
@@ -39,6 +39,7 @@
#define FCL_MOTION_BASE_H
#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
#include "fcl/RSS.h"
namespace fcl
{
@@ -59,9 +60,9 @@ public:
virtual BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
/** \brief Get the rotation and translation in current step */
- virtual void getCurrentTransform(Vec3f R[3], Vec3f& T) const = 0;
+ virtual void getCurrentTransform(Matrix3f& R, Vec3f& T) const = 0;
- virtual void getCurrentRotation(Vec3f R[3]) const = 0;
+ virtual void getCurrentRotation(Matrix3f& R) const = 0;
virtual void getCurrentTranslation(Vec3f& T) const = 0;
};
diff --git a/trunk/fcl/include/fcl/primitive.h b/trunk/fcl/include/fcl/primitive.h
index 2a948c09202e543144a31ae0d39b8124806c3bcc..0352506db559c025bee038fd29cdc3737e1fb690 100644
--- a/trunk/fcl/include/fcl/primitive.h
+++ b/trunk/fcl/include/fcl/primitive.h
@@ -39,6 +39,7 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
/** \brief Main namespace */
namespace fcl
@@ -49,10 +50,8 @@ struct Uncertainty
{
Uncertainty() {}
- Uncertainty(Vec3f Sigma_[3])
+ Uncertainty(Matrix3f& Sigma_) : Sigma(Sigma_)
{
- for(int i = 0; i < 3; ++i)
- Sigma[i] = Sigma_[i];
preprocess();
}
@@ -74,14 +73,7 @@ struct Uncertainty
}
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- Sigma[i][j] = 0;
- }
- }
-
+ Sigma.setZero();
for(int i = 0; i < 3; ++i)
{
for(int j = 0; j < 3; ++j)
@@ -94,7 +86,7 @@ struct Uncertainty
}
/** \brief Variation matrix for uncertainty */
- Vec3f Sigma[3];
+ Matrix3f Sigma;
/** \brief Variations along the eigen axes */
BVH_REAL sigma[3];
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 6ac27527511f4c1bff54b4af907df1236a3659a3..4740c146bace361f7d47f37559f5aac73030f0f7 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -75,7 +75,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = matMulVec(model1.getRotation(), p) + model1.getTranslation();
+ Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
vertices_transformed[i] = new_v;
}
@@ -114,7 +114,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node, const S& model1, B
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = matMulVec(model2.getRotation(), p) + model2.getTranslation();
+ Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
vertices_transformed[i] = new_v;
}
@@ -152,8 +152,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
- for(int i = 0; i < 3; ++i)
- node.R[i] = model1.getRotation()[i];
+ node.R = model1.getRotation();
node.T = model1.getTranslation();
return true;
@@ -176,8 +175,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node, const S& model1, co
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
- for(int i = 0; i < 3; ++i)
- node.R[i] = model2.getRotation()[i];
+ node.R = model2.getRotation();
node.T = model2.getTranslation();
return true;
@@ -198,7 +196,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = matMulVec(model1.getRotation(), p) + model1.getTranslation();
+ Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
vertices_transformed1[i] = new_v;
}
@@ -215,7 +213,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = matMulVec(model2.getRotation(), p) + model2.getTranslation();
+ Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
vertices_transformed2[i] = new_v;
}
@@ -273,7 +271,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = matMulVec(model1.getRotation(), p) + model1.getTranslation();
+ Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
vertices_transformed1[i] = new_v;
}
@@ -290,7 +288,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = matMulVec(model2.getRotation(), p) + model2.getTranslation();
+ Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
vertices_transformed2[i] = new_v;
}
@@ -362,7 +360,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = matMulVec(model1.getRotation(), p) + model1.getTranslation();
+ Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
vertices_transformed1[i] = new_v;
}
@@ -379,7 +377,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = matMulVec(model2.getRotation(), p) + model2.getTranslation();
+ Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
vertices_transformed2[i] = new_v;
}
@@ -448,7 +446,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = matMulVec(model1.getRotation(), p) + model1.getTranslation();
+ Vec3f new_v = model1.getRotation() * p + model1.getTranslation();
vertices_transformed1[i] = new_v;
}
@@ -465,7 +463,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = matMulVec(model2.getRotation(), p) + model2.getTranslation();
+ Vec3f new_v = model2.getRotation() * p + model2.getTranslation();
vertices_transformed2[i] = new_v;
}
@@ -574,7 +572,7 @@ bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const
/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms */
template<typename BV>
bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, BVH_REAL w = 1,
+ const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, BVH_REAL w = 1,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -584,7 +582,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = matMulVec(R1, p) + T1;
+ Vec3f new_v = R1 * p + T1;
vertices_transformed1[i] = new_v;
}
@@ -593,7 +591,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = matMulVec(R2, p) + T2;
+ Vec3f new_v = R2 * p + T2;
vertices_transformed2[i] = new_v;
}
@@ -622,7 +620,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS */
inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, BVH_REAL w = 1)
+ const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, BVH_REAL w = 1)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index 2db81c9f76b0e7cd0f2a7cf502f655465c0e7419..845c27ddbb15fb0afb0d5f9c2849ba92b705ee87 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -39,6 +39,7 @@
#define FCL_TRANSFORM_H
#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
namespace fcl
{
@@ -65,10 +66,10 @@ public:
}
/** \brief Matrix to quaternion */
- void fromRotation(const Vec3f R[3]);
+ void fromRotation(const Matrix3f& R);
/** \brief Quaternion to matrix */
- void toRotation(Vec3f R[3]) const;
+ void toRotation(Matrix3f& R) const;
/** \brief Axes to quaternion */
void fromAxes(const Vec3f axis[3]);
@@ -128,7 +129,7 @@ private:
class SimpleTransform
{
/** \brief Rotation matrix and translation vector */
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
/** \brief Quaternion representation for R */
@@ -139,13 +140,12 @@ public:
/** \brief Default transform is no movement */
SimpleTransform()
{
- R[0][0] = 1; R[1][1] = 1; R[2][2] = 1;
+ R.setIdentity();
}
- SimpleTransform(const Vec3f R_[3], const Vec3f& T_)
+ SimpleTransform(const Matrix3f& R_, const Vec3f& T_)
{
- for(int i = 0; i < 3; ++i)
- R[i] = R_[i];
+ R = R_;
T = T_;
q.fromRotation(R_);
@@ -156,7 +156,7 @@ public:
return T;
}
- inline const Vec3f* getRotation() const
+ inline const Matrix3f& getRotation() const
{
return R;
}
@@ -166,10 +166,9 @@ public:
return q;
}
- inline void setTransform(const Vec3f R_[3], const Vec3f& T_)
+ inline void setTransform(const Matrix3f& R_, const Vec3f& T_)
{
- for(int i = 0; i < 3; ++i)
- R[i] = R_[i];
+ R = R_;
T = T_;
q.fromRotation(R_);
@@ -182,10 +181,9 @@ public:
q.toRotation(R);
}
- inline void setRotation(const Vec3f R_[3])
+ inline void setRotation(const Matrix3f& R_)
{
- for(int i = 0; i < 3; ++i)
- R[i] = R_[i];
+ R = R_;
q.fromRotation(R_);
}
@@ -213,10 +211,8 @@ public:
void setIdentity()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
- T = Vec3f();
+ R.setIdentity();
+ T.setValue(0.0);
q = SimpleQuaternion();
}
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index 4e07274f767066fcaecd8120287bfa31e35149de..e8c0c1d3d07eb2360fad2c92904fbb70498445a1 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -37,12 +37,7 @@
#ifndef FCL_TRAVERSAL_NODE_BASE_H
#define FCL_TRAVERSAL_NODE_BASE_H
-#include "fcl/vec_3f.h"
#include "fcl/primitive.h"
-#include "fcl/BVH_front.h"
-#include "fcl/BVH_model.h"
-#include <vector>
-
/** \brief Main namespace */
namespace fcl
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index ca595bda3936166d77b7dcf80b2597992a870bf2..73ee6423fbbd6567528e9cd1be4b86f3645b917e 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -244,9 +244,7 @@ class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNod
public:
MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
+ R.setIdentity();
}
bool BVTesting(int b1, int b2) const
@@ -291,7 +289,7 @@ public:
}
// R, T is the transformation of bvh model
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -364,9 +362,7 @@ class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNod
public:
ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
+ R.setIdentity();
}
bool BVTesting(int b1, int b2) const
@@ -411,7 +407,7 @@ public:
}
// R, T is the transformation of bvh model
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 082941a7457059becd550f0e7874f8a5aa4a8e5f..726b5f073f0de9901b19deaf994e7f2cc6d95a03 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -275,7 +275,7 @@ public:
void leafTesting(int b1, int b2) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -289,7 +289,7 @@ public:
void leafTesting(int b1, int b2) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -412,7 +412,7 @@ public:
void leafTesting(int b1, int b2) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -426,7 +426,7 @@ public:
void leafTesting(int b1, int b2) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -515,7 +515,7 @@ public:
void leafTesting(int b1, int b2) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -528,7 +528,7 @@ public:
void leafTesting(int b1, int b2) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -1035,7 +1035,7 @@ public:
void leafTesting(int b1, int b2) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
@@ -1231,7 +1231,7 @@ public:
bool canStop(BVH_REAL c) const;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
};
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index ad1ed408472d20942b78a9033dca93413dd0f5ca..6534e9635493322852dc8521ce2c3090ed8f089d 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -230,10 +230,15 @@ namespace fcl
}
/** \brief Set the vector using new values */
- inline Vec3f& setValue(float x, float y, float z)
+ inline void setValue(float x, float y, float z)
{
v_[0] = x; v_[1] = y; v_[2] = z; v_[3] = 0.0f;
- return *this;
+ }
+
+ /** \brief Set the vector using new values */
+ inline void setValue(BVH_REAL x)
+ {
+ v_[0] = x; v_[1] = x; v_[2] = x; v_[3] = 0.0f;
}
/** \brief Check whether two vectors are the same in abstracted value */
@@ -420,10 +425,15 @@ namespace fcl
}
/** \brief Set the vector using new values */
- inline Vec3f& setValue(BVH_REAL x, BVH_REAL y, BVH_REAL z)
+ inline void setValue(BVH_REAL x, BVH_REAL y, BVH_REAL z)
{
v_[0] = x; v_[1] = y; v_[2] = z;
- return *this;
+ }
+
+ /** \brief Set the vector using new values */
+ inline void setValue(BVH_REAL x)
+ {
+ v_[0] = x; v_[1] = x; v_[2] = x;
}
/** \brief Check whether two vectors are the same in value */
@@ -480,26 +490,10 @@ namespace fcl
return a.triple(b, c);
}
- /** \brief M * v */
- Vec3f matMulVec(const Vec3f M[3], const Vec3f& v);
-
- /** \brief M' * v */
- Vec3f matTransMulVec(const Vec3f M[3], const Vec3f& v);
-
- /** \brief v' * M * v */
- BVH_REAL quadraticForm(const Vec3f M[3], const Vec3f& v);
-
- /** \brief S * M * S' */
- void tensorTransform(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3]);
-
- /** \brief A * B */
- void matMulMat(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3]);
-
- /** \brief The relative transform from (R1, T1) to (R2, T2) */
- void relativeTransform(const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, Vec3f R[3], Vec3f& T);
-
- /** \brief compute eigen values and vectors */
- void matEigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3]);
+ /** \brief generate a coordinate given a vector (i.e., generating three orthonormal vectors given a vector)
+ * w should be normalized
+ */
+ void generateCoordinateSystem(const Vec3f& w, Vec3f& u, Vec3f& v);
}
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index 25fb62f99d0f46872d34f972f5ba1476db2c6349..ffe147783a38098137e6213e963703d8397f9004 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -710,7 +710,7 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_pri
BVH_REAL x = (p1[0] + p2[0] + p3[0]) / 3;
BVH_REAL y = (p1[1] + p2[1] + p3[1]) / 3;
BVH_REAL z = (p1[2] + p2[2] + p3[2]) / 3;
- p = Vec3f(x, y, z);
+ p.setValue(x, y, z);
}
else
{
diff --git a/trunk/fcl/src/BVH_utility.cpp b/trunk/fcl/src/BVH_utility.cpp
index 0452b5fcbcb217c27758b01679abdeca5c470d4c..81ae732af551824e7061f201cde1d540158557a9 100644
--- a/trunk/fcl/src/BVH_utility.cpp
+++ b/trunk/fcl/src/BVH_utility.cpp
@@ -191,7 +191,7 @@ void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty*
}
/** \brief Compute the covariance matrix for a set or subset of points. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f M[3])
+void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Matrix3f& M)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
@@ -237,7 +237,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f M[
}
/** \brief Compute the covariance matrix for a set or subset of triangles. */
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f M[3])
+void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
diff --git a/trunk/fcl/src/BV_fitter.cpp b/trunk/fcl/src/BV_fitter.cpp
index 08edcc3c3ff8bf058f71d6f6966c7eca9ad8568d..f65a3679e9288c44132e25824979329c694c17ce 100644
--- a/trunk/fcl/src/BV_fitter.cpp
+++ b/trunk/fcl/src/BV_fitter.cpp
@@ -48,10 +48,10 @@ namespace OBB_fit_functions
void fit1(Vec3f* ps, OBB& bv)
{
bv.To = ps[0];
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
- bv.extent = Vec3f(0, 0, 0);
+ bv.axis[0].setValue(1, 0, 0);
+ bv.axis[1].setValue(0, 1, 0);
+ bv.axis[2].setValue(0, 0, 1);
+ bv.extent.setValue(0);
}
void fit2(Vec3f* ps, OBB& bv)
@@ -60,41 +60,15 @@ void fit2(Vec3f* ps, OBB& bv)
Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
Vec3f p1p2 = p1 - p2;
float len_p1p2 = p1p2.length();
- Vec3f w = p1p2;
- w.normalize();
-
- // then generate other two axes orthonormal to w
- Vec3f u, v;
- float inv_length;
- if(fabs(w[0]) >= fabs(w[1]))
- {
- inv_length = 1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = 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 = 1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = 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];
- }
+ p1p2.normalize();
- bv.axis[0] = w;
- bv.axis[1] = u;
- bv.axis[2] = v;
+ bv.axis[0] = p1p2;
+ generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]);
- bv.extent = Vec3f(len_p1p2 * 0.5, 0, 0);
- bv.To = Vec3f(0.5 * (p1[0] + p2[0]),
- 0.5 * (p1[1] + p2[1]),
- 0.5 * (p1[2] + p2[2]));
+ bv.extent.setValue(len_p1p2 * 0.5, 0, 0);
+ bv.To.setValue(0.5 * (p1[0] + p2[0]),
+ 0.5 * (p1[1] + p2[1]),
+ 0.5 * (p1[2] + p2[2]));
}
@@ -116,14 +90,15 @@ void fit3(Vec3f* ps, OBB& bv)
if(len[1] > len[0]) imax = 1;
if(len[2] > len[imax]) imax = 2;
- Vec3f w = e[0].cross(e[1]);
+ Vec3f& u = bv.axis[0];
+ Vec3f& v = bv.axis[1];
+ Vec3f& w = bv.axis[2];
+
+ w = e[0].cross(e[1]);
w.normalize();
- Vec3f u = e[imax];
+ u = e[imax];
u.normalize();
- Vec3f v = w.cross(u);
- bv.axis[0] = u;
- bv.axis[1] = v;
- bv.axis[2] = w;
+ v = w.cross(u);
getExtentAndCenter(ps, NULL, NULL, 3, bv.axis, bv.To, bv.extent);
}
@@ -139,8 +114,8 @@ void fit6(Vec3f* ps, OBB& bv)
void fitn(Vec3f* ps, int n, OBB& bv)
{
- Vec3f M[3]; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
+ Matrix3f M;
+ Vec3f E[3];
BVH_REAL s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, n, M);
@@ -153,22 +128,15 @@ void fitn(Vec3f* ps, int n, OBB& bv)
else if(s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
- Vec3f R[3]; // column first matrix, as the axis in OBB
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
+ bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
// set obb centers and extensions
Vec3f center, extent;
- getExtentAndCenter(ps, NULL, NULL, n, R, center, extent);
+ getExtentAndCenter(ps, NULL, NULL, n, bv.axis, center, extent);
bv.To = center;
bv.extent = extent;
@@ -182,9 +150,9 @@ namespace RSS_fit_functions
void fit1(Vec3f* ps, RSS& bv)
{
bv.Tr = ps[0];
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
+ bv.axis[0].setValue(1, 0, 0);
+ bv.axis[1].setValue(0, 1, 0);
+ bv.axis[2].setValue(0, 0, 1);
bv.l[0] = 0;
bv.l[1] = 0;
bv.r = 0;
@@ -196,37 +164,10 @@ void fit2(Vec3f* ps, RSS& bv)
Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
Vec3f p1p2 = p1 - p2;
float len_p1p2 = p1p2.length();
- Vec3f w = p1p2;
- w.normalize();
-
- // then generate other two axes orthonormal to w
- Vec3f u, v;
- float inv_length;
- if(fabs(w[0]) >= fabs(w[1]))
- {
- inv_length = 1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = 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 = 1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = 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];
- }
-
- bv.axis[0] = w;
- bv.axis[1] = u;
- bv.axis[2] = v;
+ p1p2.normalize();
+ bv.axis[0] = p1p2;
+ generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]);
bv.l[0] = len_p1p2;
bv.l[1] = 0;
@@ -252,14 +193,15 @@ void fit3(Vec3f* ps, RSS& bv)
if(len[1] > len[0]) imax = 1;
if(len[2] > len[imax]) imax = 2;
- Vec3f w = e[0].cross(e[1]);
+ Vec3f& u = bv.axis[0];
+ Vec3f& v = bv.axis[1];
+ Vec3f& w = bv.axis[2];
+
+ w = e[0].cross(e[1]);
w.normalize();
- Vec3f u = e[imax];
+ u = e[imax];
u.normalize();
- Vec3f v = w.cross(u);
- bv.axis[0] = u;
- bv.axis[1] = v;
- bv.axis[2] = w;
+ v = w.cross(u);
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, 3, bv.axis, bv.Tr, bv.l, bv.r);
}
@@ -274,7 +216,7 @@ void fit6(Vec3f* ps, RSS& bv)
void fitn(Vec3f* ps, int n, RSS& bv)
{
- Vec3f M[3]; // row first matrix
+ Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
BVH_REAL s[3] = {0, 0, 0};
@@ -288,20 +230,14 @@ void fitn(Vec3f* ps, int n, RSS& bv)
else if(s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
- Vec3f R[3]; // column first matrix, as the axis in RSS
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
+ bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, n, R, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, n, bv.axis, bv.Tr, bv.l, bv.r);
}
}
@@ -353,7 +289,7 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
{
OBB bv;
- Vec3f M[3]; // row first matrix
+ Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
BVH_REAL s[3]; // three eigen values
@@ -375,28 +311,21 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
else if(s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
- Vec3f R[3]; // column first matrix, as the axis in OBB
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
+ bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
// set obb centers and extensions
Vec3f center, extent;
if(type == BVH_MODEL_TRIANGLES)
{
- getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, R, center, extent);
+ getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, center, extent);
}
else if(type == BVH_MODEL_POINTCLOUD)
{
- getExtentAndCenter(vertices, prev_vertices, primitive_indices, num_primitives, R, center, extent);
+ getExtentAndCenter(vertices, prev_vertices, primitive_indices, num_primitives, bv.axis, center, extent);
}
bv.To = center;
@@ -410,7 +339,7 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
{
RSS bv;
- Vec3f M[3]; // row first matrix
+ Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
BVH_REAL s[3]; // three eigen values
@@ -432,17 +361,11 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
else if(s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
- Vec3f R[3]; // column first matrix, as the axis in OBB
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
- // set rss axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
+ bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
// set rss origin, rectangle size and radius
@@ -452,11 +375,11 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
if(type == BVH_MODEL_TRIANGLES)
{
- getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, R, origin, l, r);
+ getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, origin, l, r);
}
else if(type == BVH_MODEL_POINTCLOUD)
{
- getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, primitive_indices, num_primitives, R, origin, l, r);
+ getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, primitive_indices, num_primitives, bv.axis, origin, l, r);
}
bv.Tr = origin;
diff --git a/trunk/fcl/src/OBB.cpp b/trunk/fcl/src/OBB.cpp
index 77c6610d9075ea09ea7bb4c430053631eb4145fd..0dad4ab7b1be01be4d9b13bf9297539b36d8919d 100644
--- a/trunk/fcl/src/OBB.cpp
+++ b/trunk/fcl/src/OBB.cpp
@@ -51,10 +51,9 @@ bool OBB::overlap(const OBB& other) const
// First compute the rotation part, then translation part
Vec3f t = other.To - To; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Vec3f R[3];
- R[0] = Vec3f(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]));
- R[1] = Vec3f(axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]));
- R[2] = Vec3f(axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
// R is row first
return (obbDisjoint(R, T, extent, other.extent) == 0);
@@ -87,7 +86,7 @@ OBB& OBB::operator += (const Vec3f& p)
bvp.axis[0] = axis[0];
bvp.axis[1] = axis[1];
bvp.axis[2] = axis[2];
- bvp.extent = Vec3f(0, 0, 0);
+ bvp.extent.setValue(0);
*this += bvp;
return *this;
@@ -109,30 +108,13 @@ OBB OBB::operator + (const OBB& other) const
}
-bool OBB::obbDisjoint(const Vec3f B[3], const Vec3f& T, const Vec3f& a, const Vec3f& b)
+bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b)
{
register BVH_REAL t, s;
- Vec3f Bf[3];
const BVH_REAL reps = 1e-6;
- Bf[0] = abs(B[0]);
- Bf[1] = abs(B[1]);
- Bf[2] = abs(B[2]);
-
- Vec3f reps_vec(reps, reps, reps);
-
- Bf[0] += reps_vec;
- Bf[1] += reps_vec;
- Bf[2] += reps_vec;
-
- Vec3f Bf_col[3] = {Vec3f(Bf[0][0], Bf[1][0], Bf[2][0]),
- Vec3f(Bf[0][1], Bf[1][1], Bf[2][1]),
- Vec3f(Bf[0][2], Bf[1][2], Bf[2][2])};
-
- Vec3f B_col[3] = {Vec3f(B[0][0], B[1][0], B[2][0]),
- Vec3f(B[0][1], B[1][1], B[2][1]),
- Vec3f(B[0][2], B[1][2], B[2][2])};
-
+ Matrix3f Bf = B.abs();
+ Bf += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
@@ -143,10 +125,10 @@ bool OBB::obbDisjoint(const Vec3f B[3], const Vec3f& T, const Vec3f& a, const Ve
return true;
// B1 x B2 = B0
- s = T.dot(B_col[0]);
+ s = B.transposeDotX(T);
t = ((s < 0) ? -s : s);
- if(t > (b[0] + a.dot(Bf_col[0])))
+ if(t > (b[0] + Bf.transposeDotX(a)))
return true;
// A2 x A0 = A1
@@ -162,17 +144,17 @@ bool OBB::obbDisjoint(const Vec3f B[3], const Vec3f& T, const Vec3f& a, const Ve
return true;
// B2 x B0 = B1
- s = T.dot(B_col[1]);
+ s = B.transposeDotY(T);
t = ((s < 0) ? -s : s);
- if(t > (b[1] + a.dot(Bf_col[1])))
+ if(t > (b[1] + Bf.transposeDotY(a)))
return true;
// B0 x B1 = B2
- s = T.dot(B_col[2]);
+ s = B.transposeDotZ(T);
t = ((s < 0) ? -s : s);
- if(t > (b[2] + a.dot(Bf_col[2])))
+ if(t > (b[2] + Bf.transposeDotZ(a)))
return true;
// A0 x B0
@@ -275,17 +257,21 @@ OBB OBB::merge_largedist(const OBB& b1, const OBB& b2)
Vec3f vertex[16];
b1.computeVertices(vertex);
b2.computeVertices(vertex + 8);
- Vec3f M[3];
+ Matrix3f M;
Vec3f E[3];
BVH_REAL s[3] = {0, 0, 0};
- Vec3f R[3];
- R[0] = b1.To - b2.To;
- R[0].normalize();
+ // obb axes
+ Vec3f& R0 = b.axis[0];
+ Vec3f& R1 = b.axis[1];
+ Vec3f& R2 = b.axis[2];
+
+ R0 = b1.To - b2.To;
+ R0.normalize();
Vec3f vertex_proj[16];
for(int i = 0; i < 16; ++i)
- vertex_proj[i] = vertex[i] - R[0] * vertex[i].dot(R[0]);
+ vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0);
getCovariance(vertex_proj, NULL, NULL, 16, M);
matEigen(M, s, E);
@@ -298,17 +284,12 @@ OBB OBB::merge_largedist(const OBB& b1, const OBB& b2)
else { mid = 2; }
- R[1] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[2] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
-
- // set obb axes
- b.axis[0] = R[0];
- b.axis[1] = R[1];
- b.axis[2] = R[2];
+ R1.setValue(E[0][max], E[1][max], E[2][max]);
+ R2.setValue(E[0][mid], E[1][mid], E[2][mid]);
// set obb centers and extensions
Vec3f center, extent;
- getExtentAndCenter(vertex, NULL, NULL, 16, R, center, extent);
+ getExtentAndCenter(vertex, NULL, NULL, 16, b.axis, center, extent);
b.To = center;
b.extent = extent;
@@ -382,23 +363,18 @@ BVH_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
}
// R is row first
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const OBB& b1, const OBB& b2)
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
{
- // R0 R2
- Vec3f Rtemp_col[3];
- Rtemp_col[0] = Vec3f(R0[0].dot(b2.axis[0]), R0[1].dot(b2.axis[0]), R0[2].dot(b2.axis[0]));
- Rtemp_col[1] = Vec3f(R0[0].dot(b2.axis[1]), R0[1].dot(b2.axis[1]), R0[2].dot(b2.axis[1]));
- Rtemp_col[2] = Vec3f(R0[0].dot(b2.axis[2]), R0[1].dot(b2.axis[2]), R0[2].dot(b2.axis[2]));
-
- // R1'Rtemp
- Vec3f R[3];
- R[0] = Vec3f(b1.axis[0].dot(Rtemp_col[0]), b1.axis[0].dot(Rtemp_col[1]), b1.axis[0].dot(Rtemp_col[2]));
- R[1] = Vec3f(b1.axis[1].dot(Rtemp_col[0]), b1.axis[1].dot(Rtemp_col[1]), b1.axis[1].dot(Rtemp_col[2]));
- R[2] = Vec3f(b1.axis[2].dot(Rtemp_col[0]), b1.axis[2].dot(Rtemp_col[1]), b1.axis[2].dot(Rtemp_col[2]));
+ Matrix3f R0b2(R0[0].dot(b2.axis[0]), R0[0].dot(b2.axis[1]), R0[0].dot(b2.axis[2]),
+ R0[1].dot(b2.axis[0]), R0[1].dot(b2.axis[1]), R0[1].dot(b2.axis[2]),
+ R0[2].dot(b2.axis[0]), R0[2].dot(b2.axis[1]), R0[2].dot(b2.axis[2]));
- Vec3f Ttemp = Vec3f(R0[0].dot(b2.To), R0[1].dot(b2.To), R0[2].dot(b2.To)) + T0 - b1.To;
+ Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
+ R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
+ R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
- Vec3f T = Vec3f(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
+ Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
+ Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
return (OBB::obbDisjoint(R, T, b1.extent, b2.extent) == 0);
}
diff --git a/trunk/fcl/src/RSS.cpp b/trunk/fcl/src/RSS.cpp
index 59da20402d0a6ed25cc8ac9e387526c90fbdae60..5c30c0c2df6f1dacfab64241f07df7363981c1f2 100644
--- a/trunk/fcl/src/RSS.cpp
+++ b/trunk/fcl/src/RSS.cpp
@@ -47,32 +47,26 @@ bool RSS::overlap(const RSS& other) const
// First compute the rotation part, then translation part
Vec3f t = other.Tr - Tr; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Vec3f R[3];
- R[0] = Vec3f(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]));
- R[1] = Vec3f(axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]));
- R[2] = Vec3f(axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
BVH_REAL dist = rectDistance(R, T, l, other.l);
if(dist <= (r + other.r)) return true;
return false;
}
-bool overlap(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2)
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
{
- // R0 R2
- Vec3f Rtemp_col[3];
- Rtemp_col[0] = Vec3f(R0[0].dot(b2.axis[0]), R0[1].dot(b2.axis[0]), R0[2].dot(b2.axis[0]));
- Rtemp_col[1] = Vec3f(R0[0].dot(b2.axis[1]), R0[1].dot(b2.axis[1]), R0[2].dot(b2.axis[1]));
- Rtemp_col[2] = Vec3f(R0[0].dot(b2.axis[2]), R0[1].dot(b2.axis[2]), R0[2].dot(b2.axis[2]));
+ Matrix3f R0b2(R0[0].dot(b2.axis[0]), R0[0].dot(b2.axis[1]), R0[0].dot(b2.axis[2]),
+ R0[1].dot(b2.axis[0]), R0[1].dot(b2.axis[1]), R0[1].dot(b2.axis[2]),
+ R0[2].dot(b2.axis[0]), R0[2].dot(b2.axis[1]), R0[2].dot(b2.axis[2]));
- // R1'Rtemp
- Vec3f R[3];
- R[0] = Vec3f(b1.axis[0].dot(Rtemp_col[0]), b1.axis[0].dot(Rtemp_col[1]), b1.axis[0].dot(Rtemp_col[2]));
- R[1] = Vec3f(b1.axis[1].dot(Rtemp_col[0]), b1.axis[1].dot(Rtemp_col[1]), b1.axis[1].dot(Rtemp_col[2]));
- R[2] = Vec3f(b1.axis[2].dot(Rtemp_col[0]), b1.axis[2].dot(Rtemp_col[1]), b1.axis[2].dot(Rtemp_col[2]));
-
- Vec3f Ttemp = Vec3f(R0[0].dot(b2.Tr), R0[1].dot(b2.Tr), R0[2].dot(b2.Tr)) + T0 - b1.Tr;
+ Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
+ R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
+ R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+ Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
Vec3f T = Vec3f(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
BVH_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l);
@@ -261,35 +255,6 @@ RSS& RSS::operator += (const Vec3f& p)
return *this;
}
-/*
-RSS RSS::operator + (const RSS& other) const
-{
- RSS res = *this;
-
- Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r);
- Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r);
- Vec3f d0_neg = other.axis[0] * (-other.r);
- Vec3f d1_neg = other.axis[1] * (-other.r);
- Vec3f d2_pos = other.axis[2] * other.r;
- Vec3f d2_neg = other.axis[2] * (-other.r);
-
- Vec3f v[8];
- v[0] = other.Tr + d0_pos + d1_pos + d2_pos;
- v[1] = other.Tr + d0_pos + d1_pos + d2_neg;
- v[2] = other.Tr + d0_pos + d1_neg + d2_pos;
- v[3] = other.Tr + d0_pos + d1_neg + d2_neg;
- v[4] = other.Tr + d0_neg + d1_pos + d2_pos;
- v[5] = other.Tr + d0_neg + d1_pos + d2_neg;
- v[6] = other.Tr + d0_neg + d1_neg + d2_pos;
- v[7] = other.Tr + d0_neg + d1_neg + d2_neg;
-
- for(int i = 0; i < 8; ++i)
- {
- res += v[i];
- }
- return res;
-}
-*/
RSS RSS::operator + (const RSS& other) const
{
@@ -329,7 +294,7 @@ RSS RSS::operator + (const RSS& other) const
v[15] = Tr + d0_neg + d1_neg + d2_neg;
- Vec3f M[3]; // row first matrix
+ Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
BVH_REAL s[3] = {0, 0, 0};
@@ -343,20 +308,15 @@ RSS RSS::operator + (const RSS& other) const
else if(s[2] > s[max]) { mid = max; max = 2; }
else { mid = 2; }
- Vec3f R[3]; // column first matrix, as the axis in RSS
- R[0] = Vec3f(E[0][max], E[1][max], E[2][max]);
- R[1] = Vec3f(E[0][mid], E[1][mid], E[2][mid]);
- R[2] = Vec3f(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
- // set obb axes
- bv.axis[0] = R[0];
- bv.axis[1] = R[1];
- bv.axis[2] = R[2];
+ // column first matrix, as the axis in RSS
+ bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(v, NULL, NULL, 16, R, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(v, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r);
return bv;
}
@@ -368,31 +328,26 @@ BVH_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
// First compute the rotation part, then translation part
Vec3f t = other.Tr - Tr; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Vec3f R[3];
- R[0] = Vec3f(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]));
- R[1] = Vec3f(axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]));
- R[2] = Vec3f(axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
BVH_REAL dist = rectDistance(R, T, l, other.l, P, Q);
dist -= (r + other.r);
return (dist < (BVH_REAL)0.0) ? (BVH_REAL)0.0 : dist;
}
-BVH_REAL distance(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
{
- // R0 R2
- Vec3f Rtemp_col[3];
- Rtemp_col[0] = Vec3f(R0[0].dot(b2.axis[0]), R0[1].dot(b2.axis[0]), R0[2].dot(b2.axis[0]));
- Rtemp_col[1] = Vec3f(R0[0].dot(b2.axis[1]), R0[1].dot(b2.axis[1]), R0[2].dot(b2.axis[1]));
- Rtemp_col[2] = Vec3f(R0[0].dot(b2.axis[2]), R0[1].dot(b2.axis[2]), R0[2].dot(b2.axis[2]));
+ Matrix3f R0b2(R0[0].dot(b2.axis[0]), R0[0].dot(b2.axis[1]), R0[0].dot(b2.axis[2]),
+ R0[1].dot(b2.axis[0]), R0[1].dot(b2.axis[1]), R0[1].dot(b2.axis[2]),
+ R0[2].dot(b2.axis[0]), R0[2].dot(b2.axis[1]), R0[2].dot(b2.axis[2]));
- // R1'Rtemp
- Vec3f R[3];
- R[0] = Vec3f(b1.axis[0].dot(Rtemp_col[0]), b1.axis[0].dot(Rtemp_col[1]), b1.axis[0].dot(Rtemp_col[2]));
- R[1] = Vec3f(b1.axis[1].dot(Rtemp_col[0]), b1.axis[1].dot(Rtemp_col[1]), b1.axis[1].dot(Rtemp_col[2]));
- R[2] = Vec3f(b1.axis[2].dot(Rtemp_col[0]), b1.axis[2].dot(Rtemp_col[1]), b1.axis[2].dot(Rtemp_col[2]));
+ Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
+ R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
+ R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
- Vec3f Ttemp = Vec3f(R0[0].dot(b2.Tr), R0[1].dot(b2.Tr), R0[2].dot(b2.Tr)) + T0 - b1.Tr;
+ Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
Vec3f T = Vec3f(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
@@ -402,7 +357,7 @@ BVH_REAL distance(const Vec3f R0[3], const Vec3f& T0, const RSS& b1, const RSS&
}
-BVH_REAL RSS::rectDistance(const Vec3f Rab[3], Vec3f const& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P, Vec3f* Q)
+BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P, Vec3f* Q)
{
BVH_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
@@ -423,7 +378,7 @@ BVH_REAL RSS::rectDistance(const Vec3f Rab[3], Vec3f const& Tab, const BVH_REAL
bA0_dot_B1 = b[1] * A0_dot_B1;
bA1_dot_B1 = b[1] * A1_dot_B1;
- Vec3f Tba = matTransMulVec(Rab, Tab);
+ Vec3f Tba = Rab.transposeTimes(Tab);
Vec3f S;
BVH_REAL t, u;
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index d202c9041313f19c725fc4a20f72d81c7d5762ef..ad5098a416f5145390a75f669949e21643dff6bf 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -176,8 +176,8 @@ namespace fcl
{ \
for(int i = 0; i < num_contacts; ++i) \
{ \
- Vec3f normal = matMulVec(obj1->getRotation(), node.pairs[i].normal); \
- Vec3f contact_point = matMulVec(obj1->getRotation(), node.pairs[i].contact_point) + obj1->getTranslation(); \
+ Vec3f normal = obj1->getRotation() * node.pairs[i].normal; \
+ Vec3f contact_point = obj1->getRotation() * node.pairs[i].contact_point + obj1->getTranslation(); \
contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2, contact_point, normal, node.pairs[i].penetration_depth); \
} \
} \
diff --git a/trunk/fcl/src/collision_node.cpp b/trunk/fcl/src/collision_node.cpp
index 2e48e73e27b135c286128a19ae94f0b782f880da..3bd1b07125f8bbb42539eb3a8a34bdfe5012fe59 100644
--- a/trunk/fcl/src/collision_node.cpp
+++ b/trunk/fcl/src/collision_node.cpp
@@ -97,8 +97,7 @@ void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int
last_tri2_points[0], last_tri2_points[1], last_tri2_points[2],
node->R, node->T, last_tri_P, last_tri_Q);
node->p1 = last_tri_P;
- node->p2 = matTransMulVec(node->R, last_tri_Q - node->T);
-
+ node->p2 = node->R.transposeTimes(last_tri_Q - node->T);
if(qsize <= 2)
distanceRecurse(node, 0, 0, front_list);
@@ -106,7 +105,7 @@ void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int
distanceQueueRecurse(node, 0, 0, front_list, qsize);
Vec3f u = node->p2 - node->T;
- node->p2 = matTransMulVec(node->R, u);
+ node->p2 = node->R.transposeTimes(u);
}
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index 0d30d94e7eac898efda5620f636f4e5bbfff92ae..62ce70d6f75756385ce0e57704fd4f30e3d81ae1 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -82,11 +82,8 @@ int conservativeAdvancement(const CollisionObject* o1,
if(!initialize(cnode, *model1, *model2))
std::cout << "initialize error" << std::endl;
-
- Vec3f R1_0[3];
- Vec3f R2_0[3];
- Vec3f T1_0;
- Vec3f T2_0;
+ Matrix3f R1_0, R2_0;
+ Vec3f T1_0, T2_0;
motion1->getCurrentTransform(R1_0, T1_0);
motion2->getCurrentTransform(R2_0, T2_0);
@@ -120,10 +117,8 @@ int conservativeAdvancement(const CollisionObject* o1,
do
{
- Vec3f R1_t[3];
- Vec3f R2_t[3];
- Vec3f T1_t;
- Vec3f T2_t;
+ Matrix3f R1_t, R2_t;
+ Vec3f T1_t, T2_t;
node.motion1->getCurrentTransform(R1_t, T1_t);
node.motion2->getCurrentTransform(R2_t, T2_t);
diff --git a/trunk/fcl/src/geometric_shapes.cpp b/trunk/fcl/src/geometric_shapes.cpp
index eaae35d734beb694b47edbf795c8d1e067325161..5309517571555a379173d5021b7f97868e5e54e8 100644
--- a/trunk/fcl/src/geometric_shapes.cpp
+++ b/trunk/fcl/src/geometric_shapes.cpp
@@ -103,14 +103,12 @@ void Plane::unitNormalTest()
if(l > 0)
{
BVH_REAL inv_l = 1.0 / l;
- n[0] *= inv_l;
- n[1] *= inv_l;
- n[2] *= inv_l;
+ n *= inv_l;
d *= inv_l;
}
else
{
- n = Vec3f(1, 0, 0);
+ n.setValue(1, 0, 0);
d = 0;
}
}
diff --git a/trunk/fcl/src/geometric_shapes_intersect.cpp b/trunk/fcl/src/geometric_shapes_intersect.cpp
index 0f6fafd36c3013b012580c804760878df6419b33..5e808cb9be0a88066450a67480b717f079f3dd85 100644
--- a/trunk/fcl/src/geometric_shapes_intersect.cpp
+++ b/trunk/fcl/src/geometric_shapes_intersect.cpp
@@ -82,7 +82,7 @@ struct ccd_triangle_t : public ccd_obj_t
ccd_vec3_t c;
};
-void transformGJKObject(void* obj, const Vec3f R[3], const Vec3f& T)
+void transformGJKObject(void* obj, const Matrix3f& R, const Vec3f& T)
{
ccd_obj_t* o = (ccd_obj_t*)obj;
SimpleQuaternion q_;
@@ -106,10 +106,9 @@ static void shapeToGJK(const ShapeBase& s, ccd_obj_t* o)
{
SimpleQuaternion q;
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
q.fromRotation(R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
ccdQuatInvert2(&o->rot_inv, &o->rot);
@@ -588,7 +587,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 Vec3f R[3], Vec3f const& T)
+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);
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index d0261cb0ad53a3d8e671fcf99c058fe9b0ef55a0..71551bd121db06b660e63c8597e539d69a90daca 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -44,9 +44,8 @@ namespace fcl
template<>
void computeBV<AABB>(const Box& s, AABB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.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]));
@@ -59,7 +58,7 @@ void computeBV<AABB>(const Box& s, AABB& bv)
template<>
void computeBV<AABB>(const Sphere& s, AABB& bv)
{
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
bv.max_ = T + Vec3f(s.radius, s.radius, s.radius);
bv.min_ = T + Vec3f(-s.radius, -s.radius, -s.radius);
@@ -68,9 +67,8 @@ void computeBV<AABB>(const Sphere& s, AABB& bv)
template<>
void computeBV<AABB>(const Capsule& s, AABB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.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;
@@ -83,9 +81,8 @@ void computeBV<AABB>(const Capsule& s, AABB& bv)
template<>
void computeBV<AABB>(const Cone& s, AABB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.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);
@@ -98,9 +95,8 @@ void computeBV<AABB>(const Cone& s, AABB& bv)
template<>
void computeBV<AABB>(const Cylinder& s, AABB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.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);
@@ -113,14 +109,13 @@ void computeBV<AABB>(const Cylinder& s, AABB& bv)
template<>
void computeBV<AABB>(const Convex& s, AABB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
AABB bv_;
for(int i = 0; i < s.num_points; ++i)
{
- Vec3f new_p = matMulVec(R, s.points[i]) + T;
+ Vec3f new_p = R * s.points[i] + T;
bv_ += new_p;
}
@@ -130,10 +125,9 @@ void computeBV<AABB>(const Convex& s, AABB& bv)
template<>
void computeBV<AABB>(const Plane& s, AABB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
- Vec3f n = matMulVec(R, n);
+ Vec3f n = R * n;
AABB bv_;
if(n[1] == (BVH_REAL)0.0 && n[2] == (BVH_REAL)0.0)
@@ -162,133 +156,97 @@ void computeBV<AABB>(const Plane& s, AABB& bv)
template<>
void computeBV<OBB>(const Box& s, OBB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
bv.To = T;
- bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
- bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
- bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
+ bv.axis[0] = R.getColumn(0);
+ bv.axis[1] = R.getColumn(1);
+ bv.axis[2] = R.getColumn(2);
bv.extent = s.side * (BVH_REAL)0.5;
}
template<>
void computeBV<OBB>(const Sphere& s, OBB& bv)
{
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
bv.To = T;
- bv.axis[0] = Vec3f(1, 0, 0);
- bv.axis[1] = Vec3f(0, 1, 0);
- bv.axis[2] = Vec3f(0, 0, 1);
- bv.extent = Vec3f(s.radius, s.radius, s.radius);
+ bv.axis[0].setValue(1, 0, 0);
+ bv.axis[1].setValue(0, 1, 0);
+ bv.axis[2].setValue(0, 0, 1);
+ bv.extent.setValue(s.radius);
}
template<>
void computeBV<OBB>(const Capsule& s, OBB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
bv.To = T;
- bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
- bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
- bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
- bv.extent = Vec3f(s.radius, s.radius, s.lz / 2 + s.radius);
+ bv.axis[0] = R.getColumn(0);
+ bv.axis[1] = R.getColumn(1);
+ bv.axis[2] = R.getColumn(2);
+ bv.extent.setValue(s.radius, s.radius, s.lz / 2 + s.radius);
}
template<>
void computeBV<OBB>(const Cone& s, OBB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
bv.To = T;
- bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
- bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
- bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
- bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
+ bv.axis[0] = R.getColumn(0);
+ bv.axis[1] = R.getColumn(1);
+ bv.axis[2] = R.getColumn(2);
+ bv.extent.setValue(s.radius, s.radius, s.lz / 2);
}
template<>
void computeBV<OBB>(const Cylinder& s, OBB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
bv.To = T;
- bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
- bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
- bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
- bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
+ bv.axis[0] = R.getColumn(0);
+ bv.axis[1] = R.getColumn(1);
+ bv.axis[2] = R.getColumn(2);
+ bv.extent.setValue(s.radius, s.radius, s.lz / 2);
}
template<>
void computeBV<OBB>(const Convex& s, OBB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
fit(s.points, s.num_points, bv);
- Vec3f axis[3];
- axis[0] = matMulVec(R, bv.axis[0]);
- axis[1] = matMulVec(R, bv.axis[1]);
- axis[2] = matMulVec(R, bv.axis[2]);
+ bv.axis[0] = R * bv.axis[0];
+ bv.axis[1] = R * bv.axis[1];
+ bv.axis[2] = R * bv.axis[2];
- bv.axis[0] = axis[0];
- bv.axis[1] = axis[1];
- bv.axis[2] = axis[2];
-
- bv.To = matMulVec(R, bv.To) + T;
+ bv.To = R * bv.To + T;
}
template<>
void computeBV<OBB>(const Plane& s, OBB& bv)
{
- Vec3f R[3];
- matMulMat(s.getRotation(), s.getLocalRotation(), R);
- Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
+ Matrix3f R = s.getRotation() * s.getLocalRotation();
+ Vec3f T = s.getRotation() * s.getLocalTranslation() + s.getTranslation();
// generate other two axes orthonormal to plane normal
- const Vec3f& w = s.n;
- Vec3f u, v;
- float inv_length;
- if(fabs(w[0]) >= fabs(w[1]))
- {
- inv_length = 1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = 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 = 1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = 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];
- }
-
- bv.axis[0] = w;
- bv.axis[1] = u;
- bv.axis[2] = v;
+ generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
+ bv.axis[0] = s.n;
- bv.extent = Vec3f(0, std::numeric_limits<BVH_REAL>::max(), std::numeric_limits<BVH_REAL>::max());
+ bv.extent.setValue(0, std::numeric_limits<BVH_REAL>::max(), std::numeric_limits<BVH_REAL>::max());
Vec3f p = s.n * s.d;
- bv.To = matMulVec(R, p) + T;
+ bv.To = R * p + T;
}
void Box::computeLocalAABB()
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 8270c5c47e52c5ab05258eddbd00d865f0fcced6..a58b6a9c72c9435df11750b41fe0ae2a381b7690 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -805,15 +805,15 @@ bool Intersect::intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Ve
bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Vec3f R[3], const Vec3f& T,
+ const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points,
unsigned int* num_contact_points,
BVH_REAL* penetration_depth,
Vec3f* normal)
{
- Vec3f Q1_ = Vec3f(R[0].dot(Q1), R[1].dot(Q1), R[2].dot(Q1)) + T;
- Vec3f Q2_ = Vec3f(R[0].dot(Q2), R[1].dot(Q2), R[2].dot(Q2)) + T;
- Vec3f Q3_ = Vec3f(R[0].dot(Q3), R[1].dot(Q3), R[2].dot(Q3)) + T;
+ Vec3f Q1_ = R * Q1 + T;
+ Vec3f Q2_ = R * Q2 + T;
+ Vec3f Q3_ = R * Q3 + T;
return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
}
@@ -1463,14 +1463,14 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
- double sigma = matMulVec(uc1[i].Sigma, fgrad).dot(fgrad);
+ double sigma = uc1[i].Sigma.quadraticForm(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
else
{
- double sigma = matMulVec(uc2[i - nPositiveExamples].Sigma, fgrad).dot(fgrad);
+ double sigma = uc2[i - nPositiveExamples].Sigma.quadraticForm(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1498,7 +1498,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
- const Vec3f R[3], const Vec3f& T, const CloudClassifierParam& solver, bool scaling)
+ const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling)
{
KERNEL_CACHE *kernel_cache;
LEARN_PARM learn_parm = solver.learn_parm;
@@ -1576,7 +1576,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
coord0[0] = cloud2[i][0];
coord0[1] = cloud2[i][1];
coord0[2] = cloud2[i][2];
- coord1 = matMulVec(R, coord0) + T; // rotate the coordinate
+ coord1 = R * coord0 + T; // rotate the coordinate
words[0].wnum = 1;
words[0].weight = coord1[0];
@@ -1651,16 +1651,15 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
- double sigma = matMulVec(uc1[i].Sigma, fgrad).dot(fgrad);
+ double sigma = uc1[i].Sigma.quadraticForm(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
else
{
- Vec3f rotatedSigma[3];
- tensorTransform(uc2[i - nPositiveExamples].Sigma, R, rotatedSigma);
- double sigma = matMulVec(rotatedSigma, fgrad).dot(fgrad);
+ Matrix3f rotatedSigma = R.tensorTransform(uc2[i - nPositiveExamples].Sigma);
+ double sigma = rotatedSigma.quadraticForm(fgrad);
BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
@@ -1724,26 +1723,19 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
return 0.0;
}
- Vec3f P[3];
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- P[i][j] = ((i == j) ? 1 : 0) - n[i] * n[j];
- }
- }
+ Matrix3f P(1 - n[0] * n[0], -n[0] * n[1], -n[0] * n[2],
+ -n[1] * n[0], 1 - n[1] * n[1], -n[1] * n[2],
+ -n[2] * n[0], -n[2] * n[1], 1 - n[2] * n[2]);
Vec3f delta = n * t;
BVH_REAL max_prob = 0;
for(int i = 0; i < size_cloud1; ++i)
{
- Vec3f projected_p = matMulVec(P, cloud1[i]) + delta;
+ Vec3f projected_p = P * cloud1[i] + delta;
// compute the projected uncertainty by P * S * P'
- const Vec3f* S = uc1[i].Sigma;
- Vec3f newS[3];
- tensorTransform(S, P, newS);
+ Matrix3f newS = P.tensorTransform(uc1[i].Sigma);
// check whether the point is inside or outside the triangle
@@ -1751,9 +1743,9 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(b_inside)
{
- BVH_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(quadraticForm(newS, edge_n[0])));
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(quadraticForm(newS, edge_n[1])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(quadraticForm(newS, edge_n[2])));
+ BVH_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(newS.quadraticForm(edge_n[0])));
+ BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(newS.quadraticForm(edge_n[1])));
+ BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(newS.quadraticForm(edge_n[2])));
BVH_REAL prob = 1.0 - prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
}
@@ -1772,12 +1764,12 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(pos_plane.size() == 1)
{
int pos_id = pos_plane[0];
- BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(quadraticForm(newS, edge_n[pos_id])));
+ BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(newS.quadraticForm(edge_n[pos_id])));
int neg_id1 = neg_plane[0];
int neg_id2 = neg_plane[1];
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
+ BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
+ BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
BVH_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
@@ -1786,13 +1778,13 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
else if(pos_plane.size() == 2)
{
int neg_id = neg_plane[0];
- BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(quadraticForm(newS, edge_n[neg_id])));
+ BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(newS.quadraticForm(edge_n[neg_id])));
int pos_id1 = pos_plane[0];
int pos_id2 = pos_plane[1];
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(quadraticForm(newS, edge_n[pos_id1])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(quadraticForm(newS, edge_n[pos_id2])));
+ BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(newS.quadraticForm(edge_n[pos_id1])));
+ BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(newS.quadraticForm(edge_n[pos_id2])));
BVH_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
@@ -1810,11 +1802,11 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Vec3f R[3], const Vec3f& T)
+ const Matrix3f& R, const Vec3f& T)
{
- Vec3f Q1_ = Vec3f(R[0].dot(Q1), R[1].dot(Q1), R[2].dot(Q1)) + T;
- Vec3f Q2_ = Vec3f(R[0].dot(Q2), R[1].dot(Q2), R[2].dot(Q2)) + T;
- Vec3f Q3_ = Vec3f(R[0].dot(Q3), R[1].dot(Q3), R[2].dot(Q3)) + T;
+ Vec3f Q1_ = R * Q1 + T;
+ Vec3f Q2_ = R * Q2 + T;
+ Vec3f Q3_ = R * Q3 + T;
return intersect_PointCloudsTriangle(cloud1, uc1, size_cloud1, Q1_, Q2_, Q3_);
}
@@ -2173,25 +2165,25 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const V
}
BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
- const Vec3f R[3], const Vec3f& Tl,
+ const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q)
{
Vec3f T_transformed[3];
- T_transformed[0] = matMulVec(R, T[0]) + Tl;
- T_transformed[1] = matMulVec(R, T[1]) + Tl;
- T_transformed[2] = matMulVec(R, T[2]) + Tl;
+ T_transformed[0] = R * T[0] + Tl;
+ T_transformed[1] = R * T[1] + Tl;
+ T_transformed[2] = R * T[2] + Tl;
return triDistance(S, T_transformed, P, Q);
}
BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
- const Vec3f R[3], const Vec3f& Tl,
+ const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q)
{
- Vec3f T1_transformed = matMulVec(R, T1) + Tl;
- Vec3f T2_transformed = matMulVec(R, T2) + Tl;
- Vec3f T3_transformed = matMulVec(R, T3) + Tl;
+ Vec3f T1_transformed = R * T1 + Tl;
+ Vec3f T2_transformed = R * T2 + Tl;
+ Vec3f T3_transformed = R * T3 + Tl;
return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
}
diff --git a/trunk/fcl/src/matrix_3f.cpp b/trunk/fcl/src/matrix_3f.cpp
index ab39f3855a0e9b60931f2dc80dec97ad60edd76d..75f89001c56771cd997a8efcf60e33c3f2ba286f 100644
--- a/trunk/fcl/src/matrix_3f.cpp
+++ b/trunk/fcl/src/matrix_3f.cpp
@@ -50,6 +50,15 @@ Matrix3f& Matrix3f::operator *= (const Matrix3f& other)
return *this;
}
+Matrix3f& Matrix3f::operator += (BVH_REAL c)
+{
+ setValue(v_[0][0] + c, v_[0][1] + c, v_[0][2] + c,
+ v_[1][0] + c, v_[1][1] + c, v_[1][2] + c,
+ v_[2][0] + c, v_[2][1] + c, v_[2][2] + c);
+ return *this;
+}
+
+
BVH_REAL Matrix3f::determinant() const
{
return triple(v_[0], v_[1], v_[2]);
@@ -62,6 +71,13 @@ Matrix3f Matrix3f::transpose() const
v_[0][2], v_[1][2], v_[2][2]);
}
+Matrix3f Matrix3f::abs() const
+{
+ return Matrix3f(fabs(v_[0][0]), fabs(v_[0][1]), fabs(v_[0][2]),
+ fabs(v_[1][0]), fabs(v_[1][1]), fabs(v_[1][2]),
+ fabs(v_[2][0]), fabs(v_[2][1]), fabs(v_[2][2]));
+}
+
Matrix3f Matrix3f::inverse() const
{
BVH_REAL det = determinant();
diff --git a/trunk/fcl/src/transform.cpp b/trunk/fcl/src/transform.cpp
index e43e513504493feaa5384726e66dba6fcc5dc664..31e7e3d005b018c7bd86e0ef01a343ecc8b9c0a4 100644
--- a/trunk/fcl/src/transform.cpp
+++ b/trunk/fcl/src/transform.cpp
@@ -40,7 +40,7 @@
namespace fcl
{
-void SimpleQuaternion::fromRotation(const Vec3f R[3])
+void SimpleQuaternion::fromRotation(const Matrix3f& R)
{
const int next[3] = {1, 2, 0};
@@ -82,7 +82,7 @@ void SimpleQuaternion::fromRotation(const Vec3f R[3])
}
}
-void SimpleQuaternion::toRotation(Vec3f R[3]) const
+void SimpleQuaternion::toRotation(Matrix3f& R) const
{
BVH_REAL twoX = 2.0*data[1];
BVH_REAL twoY = 2.0*data[2];
@@ -97,9 +97,9 @@ void SimpleQuaternion::toRotation(Vec3f R[3]) const
BVH_REAL twoYZ = twoZ*data[2];
BVH_REAL twoZZ = twoZ*data[3];
- R[0] = Vec3f(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY);
- R[1] = Vec3f(twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX);
- R[2] = Vec3f(twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
+ R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
+ twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
+ twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
}
@@ -163,9 +163,9 @@ void SimpleQuaternion::toAxes(Vec3f axis[3]) const
BVH_REAL twoYZ = twoZ*data[2];
BVH_REAL twoZZ = twoZ*data[3];
- axis[0] = Vec3f(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
- axis[1] = Vec3f(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX);
- axis[2] = Vec3f(twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY));
+ axis[0].setValue(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
+ axis[1].setValue(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX);
+ axis[2].setValue(twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY));
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index f8a278707071e49d101ca06a075504062460c88f..6c00681e02fa65baafc15fd9954effd475662d64 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -42,10 +42,7 @@ namespace fcl
MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode<OBB>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -107,10 +104,7 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode<RSS>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -173,10 +167,7 @@ void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
PointCloudCollisionTraversalNodeOBB::PointCloudCollisionTraversalNodeOBB() : PointCloudCollisionTraversalNode<OBB>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -211,10 +202,7 @@ void PointCloudCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
PointCloudCollisionTraversalNodeRSS::PointCloudCollisionTraversalNodeRSS() : PointCloudCollisionTraversalNode<RSS>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -249,10 +237,7 @@ void PointCloudCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
PointCloudMeshCollisionTraversalNodeOBB::PointCloudMeshCollisionTraversalNodeOBB() : PointCloudMeshCollisionTraversalNode<OBB>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -290,10 +275,7 @@ void PointCloudMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
PointCloudMeshCollisionTraversalNodeRSS::PointCloudMeshCollisionTraversalNodeRSS() : PointCloudMeshCollisionTraversalNode<RSS>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -332,10 +314,7 @@ void PointCloudMeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode<RSS>()
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -511,10 +490,7 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(BVH_REAL w_) : MeshConservativeAdvancementTraversalNode<RSS>(w_)
{
- R[0] = Vec3f(1, 0, 0);
- R[1] = Vec3f(0, 1, 0);
- R[2] = Vec3f(0, 0, 1);
-
+ R.setIdentity();
// default T is 0
}
@@ -573,9 +549,9 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
/** n is the local frame of object 1, pointing from object 1 to object2 */
Vec3f n = P2 - P1;
/** turn n into the global frame, pointing from object 1 to object 2 */
- Vec3f R0[3];
+ Matrix3f R0;
motion1->getCurrentRotation(R0);
- Vec3f n_transformed = matMulVec(R0, n);
+ Vec3f n_transformed = R0 * n;
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, -n_transformed);
@@ -619,9 +595,9 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
// n is in local frame of RSS c1, so we need to turn n into the global frame
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[2] + model1->getBV(c1).bv.axis[2] * n[2];
- Vec3f R0[3];
+ Matrix3f R0;
motion1->getCurrentRotation(R0);
- n_transformed = matMulVec(R0, n_transformed);
+ n_transformed = R0 * n_transformed;
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
diff --git a/trunk/fcl/src/vec_3f.cpp b/trunk/fcl/src/vec_3f.cpp
index f4bc98b01931166abf0466a74da4b419ea715a51..d95176794d1047da48442f54e6f281b3e9c59019 100644
--- a/trunk/fcl/src/vec_3f.cpp
+++ b/trunk/fcl/src/vec_3f.cpp
@@ -45,148 +45,29 @@ const float Vec3f::EPSILON = 1e-11;
const BVH_REAL Vec3f::EPSILON = 1e-11;
#endif
-Vec3f matMulVec(const Vec3f M[3], const Vec3f& v)
-{
- return Vec3f(M[0].dot(v), M[1].dot(v), M[2].dot(v));
-}
-
-Vec3f matTransMulVec(const Vec3f M[3], const Vec3f& v)
-{
- return M[0] * v[0] + M[1] * v[1] + M[2] * v[2];
-}
-
-BVH_REAL quadraticForm(const Vec3f M[3], const Vec3f& v)
-{
- return v.dot(Vec3f(M[0].dot(v), M[1].dot(v), M[2].dot(v)));
-}
-
-
-void tensorTransform(const Vec3f M[3], const Vec3f S[3], Vec3f newM[3])
-{
- Vec3f SMT_col[3] = {Vec3f(M[0].dot(S[0]), M[1].dot(S[0]), M[2].dot(S[0])),
- Vec3f(M[0].dot(S[1]), M[1].dot(S[1]), M[2].dot(S[1])),
- Vec3f(M[0].dot(S[2]), M[1].dot(S[2]), M[2].dot(S[2]))
- };
-
- newM[0] = Vec3f(S[0].dot(SMT_col[0]), S[1].dot(SMT_col[0]), S[2].dot(SMT_col[0]));
- newM[1] = Vec3f(S[0].dot(SMT_col[1]), S[1].dot(SMT_col[1]), S[2].dot(SMT_col[1]));
- newM[2] = Vec3f(S[0].dot(SMT_col[2]), S[1].dot(SMT_col[2]), S[2].dot(SMT_col[2]));
-}
-
-void relativeTransform(const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, Vec3f R[3], Vec3f& T)
-{
- R[0] = Vec3f(R1[0][0] * R2[0][0] + R1[1][0] * R2[1][0] + R1[2][0] * R2[2][0],
- R1[0][0] * R2[0][1] + R1[1][0] * R2[1][1] + R1[2][0] * R2[2][1],
- R1[0][0] * R2[0][2] + R1[1][0] * R2[1][2] + R1[2][0] * R2[2][2]);
- R[1] = Vec3f(R1[0][1] * R2[0][0] + R1[1][1] * R2[1][0] + R1[2][1] * R2[2][0],
- R1[0][1] * R2[0][1] + R1[1][1] * R2[1][1] + R1[2][1] * R2[2][1],
- R1[0][1] * R2[0][2] + R1[1][1] * R2[1][2] + R1[2][1] * R2[2][2]);
- R[2] = Vec3f(R1[0][2] * R2[0][0] + R1[1][2] * R2[1][0] + R1[2][2] * R2[2][0],
- R1[0][2] * R2[0][1] + R1[1][2] * R2[1][1] + R1[2][2] * R2[2][1],
- R1[0][2] * R2[0][2] + R1[1][2] * R2[1][2] + R1[2][2] * R2[2][2]);
- Vec3f temp = T2 - T1;
- T = Vec3f(R1[0][0] * temp[0] + R1[1][0] * temp[1] + R1[2][0] * temp[2],
- R1[0][1] * temp[0] + R1[1][1] * temp[1] + R1[2][1] * temp[2],
- R1[0][2] * temp[0] + R1[1][2] * temp[1] + R1[2][2] * temp[2]);
-}
-
-void matEigen(Vec3f a[3], BVH_REAL dout[3], Vec3f vout[3])
+void generateCoordinateSystem(const Vec3f& w, Vec3f& u, Vec3f& v)
{
- int n = 3;
- int j, iq, ip, i;
- BVH_REAL tresh, theta, tau, t, sm, s, h, g, c;
- int nrot;
- BVH_REAL b[3];
- BVH_REAL z[3];
- BVH_REAL v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- BVH_REAL d[3];
-
- for(ip = 0; ip < n; ++ip)
+ BVH_REAL inv_length;
+ if(fabs(w[0]) >= fabs(w[1]))
{
- b[ip] = a[ip][ip];
- d[ip] = a[ip][ip];
- z[ip] = 0.0;
+ inv_length = (BVH_REAL)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+ u[0] = -w[2] * inv_length;
+ u[1] = (BVH_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];
}
-
- nrot = 0;
-
- for(i = 0; i < 50; ++i)
- {
- sm = 0.0;
- for(ip = 0; ip < n; ++ip)
- for(iq = ip + 1; iq < n; ++iq)
- sm += fabs(a[ip][iq]);
- if(sm == 0.0)
- {
- vout[0] = Vec3f(v[0][0], v[0][1], v[0][2]);
- vout[1] = Vec3f(v[1][0], v[1][1], v[1][2]);
- vout[2] = Vec3f(v[2][0], v[2][1], v[2][2]);
- dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
- return;
- }
-
- if(i < 3) tresh = 0.2 * sm / (n * n);
- else tresh = 0.0;
-
- for(ip = 0; ip < n; ++ip)
- {
- for(iq= ip + 1; iq < n; ++iq)
- {
- g = 100.0 * fabs(a[ip][iq]);
- if(i > 3 &&
- fabs(d[ip]) + g == fabs(d[ip]) &&
- fabs(d[iq]) + g == fabs(d[iq]))
- a[ip][iq] = 0.0;
- else if(fabs(a[ip][iq]) > tresh)
- {
- h = d[iq] - d[ip];
- if(fabs(h) + g == fabs(h)) t = (a[ip][iq]) / h;
- else
- {
- theta = 0.5 * h / (a[ip][iq]);
- t = 1.0 /(fabs(theta) + sqrt(1.0 + theta * theta));
- if(theta < 0.0) t = -t;
- }
- c = 1.0 / sqrt(1 + t * t);
- s = t * c;
- tau = s / (1.0 + c);
- h = t * a[ip][iq];
- z[ip] -= h;
- z[iq] += h;
- d[ip] -= h;
- d[iq] += h;
- a[ip][iq] = 0.0;
- for(j = 0; j < ip; ++j) { g = a[j][ip]; h = a[j][iq]; a[j][ip] = g - s * (h + g * tau); a[j][iq] = h + s * (g - h * tau); }
- for(j = ip + 1; j < iq; ++j) { g = a[ip][j]; h = a[j][iq]; a[ip][j] = g - s * (h + g * tau); a[j][iq] = h + s * (g - h * tau); }
- for(j = iq + 1; j < n; ++j) { g = a[ip][j]; h = a[iq][j]; a[ip][j] = g - s * (h + g * tau); a[iq][j] = h + s * (g - h * tau); }
- for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
- nrot++;
- }
- }
- }
- for(ip = 0; ip < n; ++ip)
- {
- b[ip] += z[ip];
- d[ip] = b[ip];
- z[ip] = 0.0;
- }
- }
-
- std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
-
- return;
-}
-
-
-void matMulMat(const Vec3f M1[3], const Vec3f M2[3], Vec3f newM[3])
-{
- for(int i = 0; i < 3; ++i)
+ else
{
- for(int j = 0; j < 3; ++j)
- {
- newM[i][j] = M1[i][0] * M2[0][j] + M1[i][1] * M2[1][j] + M1[i][2] * M2[2][j];
- }
+ inv_length = (BVH_REAL)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+ u[0] = (BVH_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];
}
}
diff --git a/trunk/fcl/test/test_core_collision.cpp b/trunk/fcl/test/test_core_collision.cpp
index 2bd4ea8ba475c898ecc1601fb0c4e94a425d8397..d198d50d411542a056ec92578c55d59347a6275d 100644
--- a/trunk/fcl/test/test_core_collision.cpp
+++ b/trunk/fcl/test/test_core_collision.cpp
@@ -550,7 +550,7 @@ bool collide_Test2(const Transform& tf,
std::vector<Vec3f> vertices1_new(vertices1.size());
for(unsigned int i = 0; i < vertices1_new.size(); ++i)
{
- vertices1_new[i] = matMulVec(tf.R, vertices1[i]) + tf.T;
+ vertices1_new[i] = tf.R * vertices1[i] + tf.T;
}
@@ -629,10 +629,8 @@ bool collide_Test(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -702,10 +700,8 @@ bool collide_Test_OBB(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -774,10 +770,8 @@ bool collide_Test_RSS(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -847,10 +841,8 @@ bool test_collide_func(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setRotation(tf.R);
diff --git a/trunk/fcl/test/test_core_collision_point.cpp b/trunk/fcl/test/test_core_collision_point.cpp
index 4f313cfe0402ef08bde98068b0973901a9b56526..ac7bbe08bfbf47144cb07cd8b4510494e4033a40 100644
--- a/trunk/fcl/test/test_core_collision_point.cpp
+++ b/trunk/fcl/test/test_core_collision_point.cpp
@@ -161,10 +161,8 @@ bool collide_Test_PP(const Transform& tf,
m2.addSubModel(vertices2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -226,10 +224,8 @@ bool collide_Test_PP_OBB(const Transform& tf,
m2.addSubModel(vertices2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -295,10 +291,8 @@ bool collide_Test_MP(const Transform& tf,
m2.addSubModel(vertices2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -363,10 +357,8 @@ bool collide_Test_MP_OBB(const Transform& tf,
m2.addSubModel(vertices2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -432,10 +424,8 @@ bool collide_Test_PM(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -499,10 +489,8 @@ bool collide_Test_PM_OBB(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
diff --git a/trunk/fcl/test/test_core_conservative_advancement.cpp b/trunk/fcl/test/test_core_conservative_advancement.cpp
index 2525e1bf77eed17cd86c47e51166672d8d08db4b..b90d1a9fb22eb102f459f6aa0a52dfd102593156 100644
--- a/trunk/fcl/test/test_core_conservative_advancement.cpp
+++ b/trunk/fcl/test/test_core_conservative_advancement.cpp
@@ -178,11 +178,9 @@ bool CA_linear_Test(const Transform& tf1, const Transform& tf2,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
std::vector<Contact> contacts;
@@ -230,11 +228,9 @@ bool CA_screw_Test(const Transform& tf1, const Transform& tf2,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
std::vector<Contact> contacts;
@@ -275,11 +271,9 @@ bool linear_interp_Test(const Transform& tf1, const Transform& tf2,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
Vec3f m1_ref;
Vec3f m2_ref;
@@ -301,7 +295,7 @@ bool linear_interp_Test(const Transform& tf1, const Transform& tf2,
{
BVH_REAL curt = i / (BVH_REAL)nsamples;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
motion1.integrate(curt);
motion1.getCurrentTransform(R, T);
@@ -352,11 +346,10 @@ bool screw_interp_Test(const Transform& tf1, const Transform& tf2,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+
Vec3f m1_ref;
Vec3f m2_ref;
@@ -368,7 +361,7 @@ bool screw_interp_Test(const Transform& tf1, const Transform& tf2,
{
BVH_REAL curt = i / (BVH_REAL)nsamples;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
motion1.integrate(curt);
motion1.getCurrentTransform(R, T);
@@ -428,7 +421,7 @@ bool CA_spline_Test(const Transform& tf1, const Transform& tf2,
for(int i = 0; i < 4; ++i)
{
motion.integrate(i / 4.0);
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
motion.getCurrentTransform(R, T);
SimpleQuaternion q;
@@ -494,7 +487,7 @@ bool spline_interp_Test(const Transform& tf1, const Transform& tf2,
for(int i = 0; i < 4; ++i)
{
motion.integrate(i / 4.0);
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
motion.getCurrentTransform(R, T);
SimpleQuaternion q;
@@ -523,7 +516,7 @@ bool spline_interp_Test(const Transform& tf1, const Transform& tf2,
{
BVH_REAL curt = i / (BVH_REAL)nsamples;
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
motion1.integrate(curt);
motion2.integrate(curt);
diff --git a/trunk/fcl/test/test_core_continuous_collision.cpp b/trunk/fcl/test/test_core_continuous_collision.cpp
index e8f536d734a145a20681ef12b3468ed1b07bca38..45dee43a9f03338168a82325c7c22b7c1ae21012 100644
--- a/trunk/fcl/test/test_core_continuous_collision.cpp
+++ b/trunk/fcl/test/test_core_continuous_collision.cpp
@@ -173,7 +173,7 @@ bool continuous_collide_Test(const Transform& tf1, const Transform& tf2,
std::vector<Vec3f> vertices1_new(vertices1.size());
for(unsigned int i = 0; i < vertices1_new.size(); ++i)
{
- vertices1_new[i] = matMulVec(tf1.R, vertices1[i]) + tf1.T;
+ vertices1_new[i] = tf1.R * vertices1[i] + tf1.T;
}
m1.beginModel();
@@ -201,7 +201,7 @@ bool continuous_collide_Test(const Transform& tf1, const Transform& tf2,
// update
for(unsigned int i = 0; i < vertices1_new.size(); ++i)
{
- vertices1_new[i] = matMulVec(tf2.R, vertices1[i]) + tf2.T;
+ vertices1_new[i] = tf2.R * vertices1[i] + tf2.T;
}
m1.beginUpdateModel();
@@ -241,13 +241,13 @@ bool discrete_continuous_collide_Test(const Transform& tf1, const Transform& tf2
std::vector<Vec3f> vertices1_t1(vertices1.size());
for(unsigned int i = 0; i < vertices1_t1.size(); ++i)
{
- vertices1_t1[i] = matMulVec(tf1.R, vertices1[i]) + tf1.T;
+ vertices1_t1[i] = tf1.R * vertices1[i] + tf1.T;
}
std::vector<Vec3f> vertices1_t2(vertices1.size());
for(unsigned int i = 0; i < vertices1_t2.size(); ++i)
{
- vertices1_t2[i] = matMulVec(tf2.R, vertices1[i]) + tf2.T;
+ vertices1_t2[i] = tf2.R * vertices1[i] + tf2.T;
}
std::vector<Vec3f> vertices1_t(vertices1.size());
diff --git a/trunk/fcl/test/test_core_distance.cpp b/trunk/fcl/test/test_core_distance.cpp
index add5b59317328d581c6dde48a58c4518e563e399..87870aab92ba98894887b4324c3bf18381e83ec0 100644
--- a/trunk/fcl/test/test_core_distance.cpp
+++ b/trunk/fcl/test/test_core_distance.cpp
@@ -217,10 +217,8 @@ void distance_Test(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -236,8 +234,8 @@ void distance_Test(const Transform& tf,
distance(&node, NULL, qsize);
// points are in local coordinate, to global coordinate
- Vec3f p1 = matMulVec(tf.R, node.p1) + tf.T;
- Vec3f p2 = matMulVec(R2, node.p2) + T2;
+ Vec3f p1 = tf.R * node.p1 + tf.T;
+ Vec3f p2 = R2 * node.p2 + T2;
distance_result.distance = node.min_distance;
@@ -276,10 +274,8 @@ void distance_Test2(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
@@ -326,10 +322,8 @@ bool collide_Test_OBB(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
diff --git a/trunk/fcl/test/test_core_front_list.cpp b/trunk/fcl/test/test_core_front_list.cpp
index 1cf230398947a40f959609cd5f0f37e938d4fbab..d133ff83aeb3793813848a4956faf3aebc803814 100644
--- a/trunk/fcl/test/test_core_front_list.cpp
+++ b/trunk/fcl/test/test_core_front_list.cpp
@@ -215,7 +215,7 @@ bool collide_front_list_Test(const Transform& tf1, const Transform& tf2,
std::vector<Vec3f> vertices1_new(vertices1.size());
for(unsigned int i = 0; i < vertices1_new.size(); ++i)
{
- vertices1_new[i] = matMulVec(tf1.R, vertices1[i]) + tf1.T;
+ vertices1_new[i] = tf1.R * vertices1[i] + tf1.T;
}
m1.beginModel();
@@ -244,7 +244,7 @@ bool collide_front_list_Test(const Transform& tf1, const Transform& tf2,
// update the mesh
for(unsigned int i = 0; i < vertices1.size(); ++i)
{
- vertices1_new[i] = matMulVec(tf2.R, vertices1[i]) + tf2.T;
+ vertices1_new[i] = tf2.R * vertices1[i] + tf2.T;
}
m1.beginReplaceModel();
@@ -285,10 +285,8 @@ bool collide_front_list_OBB_Test(const Transform& tf1, const Transform& tf2,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf1.R, tf1.T);
@@ -344,10 +342,8 @@ bool collide_front_list_RSS_Test(const Transform& tf1, const Transform& tf2,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf1.R, tf1.T);
@@ -401,10 +397,8 @@ bool collide_Test(const Transform& tf,
m2.addSubModel(vertices2, triangles2);
m2.endModel();
- Vec3f R2[3];
- R2[0] = Vec3f(1, 0, 0);
- R2[1] = Vec3f(0, 1, 0);
- R2[2] = Vec3f(0, 0, 1);
+ Matrix3f R2;
+ R2.setIdentity();
Vec3f T2;
m1.setTransform(tf.R, tf.T);
diff --git a/trunk/fcl/test/test_core_geometric_shapes.cpp b/trunk/fcl/test/test_core_geometric_shapes.cpp
index 881ee17ed85c5c9e24357892853ddaabfee712f2..9473498bcc0e531b08e7a686997f65a20e46ce3f 100644
--- a/trunk/fcl/test/test_core_geometric_shapes.cpp
+++ b/trunk/fcl/test/test_core_geometric_shapes.cpp
@@ -209,7 +209,7 @@ TEST(shapeIntersection, boxbox)
SimpleQuaternion q;
q.fromAxisAngle(Vec3f(0, 0, 1), (BVH_REAL)3.140 / 6);
- Vec3f R[3];
+ Matrix3f R;
q.toRotation(R);
s2.setLocalRotation(R);
res = shapeIntersect(s1, s2);
diff --git a/trunk/fcl/test/test_core_utility.h b/trunk/fcl/test/test_core_utility.h
index 3fb04a29e75173c27b79584c92c6dccadccd5ec6..fda9b5246c12e0c747ab6f48eb5fa7079020590e 100644
--- a/trunk/fcl/test/test_core_utility.h
+++ b/trunk/fcl/test/test_core_utility.h
@@ -39,6 +39,7 @@
#define FCL_TEST_CORE_UTILITY_H
#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
#include "fcl/primitive.h"
#include <cstdio>
#include <vector>
@@ -52,14 +53,12 @@ using namespace fcl;
struct Transform
{
- Vec3f R[3];
+ Matrix3f R;
Vec3f T;
Transform()
{
- R[0][0] = 1;
- R[1][1] = 1;
- R[2][2] = 1;
+ R.setIdentity();
}
};
@@ -104,9 +103,9 @@ inline bool collide_PQP(const Transform& tf,
const Vec3f& p2_ = vertices1[t[1]];
const Vec3f& p3_ = vertices1[t[2]];
- Vec3f p1 = matMulVec(tf.R, p1_) + tf.T;
- Vec3f p2 = matMulVec(tf.R, p2_) + tf.T;
- Vec3f p3 = matMulVec(tf.R, p3_) + tf.T;
+ Vec3f p1 = tf.R * p1_ + tf.T;
+ Vec3f p2 = tf.R * p2_ + tf.T;
+ Vec3f p3 = tf.R * p3_ + tf.T;
PQP_REAL q1[3];
PQP_REAL q2[3];
@@ -292,9 +291,9 @@ inline bool distance_PQP(const Transform& tf,
const Vec3f& p2_ = vertices1[t[1]];
const Vec3f& p3_ = vertices1[t[2]];
- Vec3f p1 = matMulVec(tf.R, p1_) + tf.T;
- Vec3f p2 = matMulVec(tf.R, p2_) + tf.T;
- Vec3f p3 = matMulVec(tf.R, p3_) + tf.T;
+ Vec3f p1 = tf.R * p1_ + tf.T;
+ Vec3f p2 = tf.R * p2_ + tf.T;
+ Vec3f p3 = tf.R * p3_ + tf.T;
PQP_REAL q1[3];
PQP_REAL q2[3];
@@ -431,7 +430,7 @@ inline bool distance_PQP2(const Transform& tf,
Vec3f p1_temp(res.p1[0], res.p1[1], res.p1[2]);
Vec3f p2_temp(res.p2[0], res.p2[1], res.p2[2]);
- Vec3f p1 = matMulVec(tf.R, p1_temp) + tf.T;
+ Vec3f p1 = tf.R * p1_temp + tf.T;
Vec3f p2 = p2_temp;
@@ -554,7 +553,7 @@ inline void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::v
}
-inline void eulerToMatrix(BVH_REAL a, BVH_REAL b, BVH_REAL c, Vec3f R[3])
+inline void eulerToMatrix(BVH_REAL a, BVH_REAL b, BVH_REAL c, Matrix3f& R)
{
BVH_REAL c1 = cos(a);
BVH_REAL c2 = cos(b);
@@ -563,9 +562,9 @@ inline void eulerToMatrix(BVH_REAL a, BVH_REAL b, BVH_REAL c, Vec3f R[3])
BVH_REAL s2 = sin(b);
BVH_REAL s3 = sin(c);
- R[0] = Vec3f(c1 * c2, - c2 * s1, s2);
- R[1] = Vec3f(c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3);
- R[2] = Vec3f(s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3);
+ R.setValue(c1 * c2, - c2 * s1, s2,
+ c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3,
+ s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3);
}
inline void generateRandomTransform(BVH_REAL extents[6], Transform& transform)
@@ -580,7 +579,7 @@ inline void generateRandomTransform(BVH_REAL extents[6], Transform& transform)
BVH_REAL c = rand_interval(0, 2 * pi);
eulerToMatrix(a, b, c, transform.R);
- transform.T = Vec3f(x, y, z);
+ transform.T.setValue(x, y, z);
}
inline void generateRandomTransform(BVH_REAL extents[6], std::vector<Transform>& transforms, std::vector<Transform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n)
@@ -599,7 +598,7 @@ inline void generateRandomTransform(BVH_REAL extents[6], std::vector<Transform>&
BVH_REAL c = rand_interval(0, 2 * pi);
eulerToMatrix(a, b, c, transforms[i].R);
- transforms[i].T = Vec3f(x, y, z);
+ transforms[i].T.setValue(x, y, z);
BVH_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
BVH_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
@@ -610,7 +609,7 @@ inline void generateRandomTransform(BVH_REAL extents[6], std::vector<Transform>&
BVH_REAL deltac = rand_interval(-delta_rot, delta_rot);
eulerToMatrix(a + deltaa, b + deltab, c + deltac, transforms2[i].R);
- transforms2[i].T = Vec3f(x + deltax, y + deltay, z + deltaz);
+ transforms2[i].T.setValue(x + deltax, y + deltay, z + deltaz);
}
}
@@ -636,7 +635,7 @@ inline void generateRandomTransform_ccd(BVH_REAL extents[6], std::vector<Transfo
Transform tf;
eulerToMatrix(a, b, c, tf.R);
- tf.T = Vec3f(x, y, z);
+ tf.T.setValue(x, y, z);
std::vector<std::pair<int, int> > results;
#if USE_PQP
@@ -654,7 +653,7 @@ inline void generateRandomTransform_ccd(BVH_REAL extents[6], std::vector<Transfo
BVH_REAL deltac = rand_interval(-delta_rot, delta_rot);
eulerToMatrix(a + deltaa, b + deltab, c + deltac, transforms2[i].R);
- transforms2[i].T = Vec3f(x + deltax, y + deltay, z + deltaz);
+ transforms2[i].T.setValue(x + deltax, y + deltay, z + deltaz);
++i;
}
}