Commit a39dfbcc authored by jpan's avatar jpan
Browse files

use a better 3x3 matrix, avoid the previous bad one


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@48 253336fb-580f-4252-a368-f3cef5a2a82b
parent 97f6b180
......@@ -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.
......
......@@ -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);
}
......
......@@ -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);
}
......
......@@ -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);
}
......
......@@ -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;
}
......
......@@ -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;
};
......
......@@ -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);
......
......@@ -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);
};
......
......@@ -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);
}
};
......
......@@ -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
......
......@@ -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;
};
......
......@@ -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];
......
......@@ -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()