Commit a39dfbcc by jpan

### 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& 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& 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& 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& 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& 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& 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& 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 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::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& node, BVHModel& 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& 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& node, const BVHModel 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& 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& node, BVHModel& 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& node, BVHModel& model1, BVHM for(int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i];