Commit 7f3e1fd4 by Joseph Mirabel Committed by Joseph Mirabel

### Use Eigen Matrix without FclMatrix

parent fdd9b54d
 ... ... @@ -62,8 +62,8 @@ public: } /// @brief Creating an AABB with two endpoints a and b AABB(const Vec3f& a, const Vec3f&b) : min_(min(a, b)), max_(max(a, b)) AABB(const Vec3f& a, const Vec3f&b) : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) { } ... ... @@ -74,8 +74,8 @@ public: } /// @brief Creating an AABB contains three points AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(min(min(a, b), c)), max_(max(max(a, b), c)) AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) { } ... ... @@ -124,8 +124,8 @@ public: return false; } overlap_part.min_ = max(min_, other.min_); overlap_part.max_ = min(max_, other.max_); overlap_part.min_ = min_.cwiseMax(other.min_); overlap_part.max_ = max_.cwiseMin(other.max_); return true; } ... ... @@ -214,7 +214,7 @@ public: /// @brief whether two AABB are equal inline bool equal(const AABB& other) const { return min_.equal(other.min_) && max_.equal(other.max_); return isEqual(min_, other.min_) && isEqual(max_, other.max_); } /// @brief expand the half size of the AABB by delta, and keep the center unchanged. ... ...
 ... ... @@ -105,31 +105,31 @@ struct BVNode : public BVNodeBase Vec3f getCenter() const { return bv.center(); } /// @brief Access the orientation of the BV Matrix3f getOrientation() const { return Matrix3f::getIdentity(); } Matrix3f getOrientation() const { return Matrix3f::Identity(); } }; template<> inline Matrix3f BVNode::getOrientation() const { return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], return (Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]); bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished(); } template<> inline Matrix3f BVNode::getOrientation() const { return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], return (Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]); bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished(); } template<> inline Matrix3f BVNode::getOrientation() const { return Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0], return (Matrix3f() << bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0], bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1], bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]); bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]).finished(); } ... ...
 ... ... @@ -222,13 +222,12 @@ public: Matrix3f computeMomentofInertia() const { Matrix3f C(0, 0, 0, 0, 0, 0, 0, 0, 0); Matrix3f C = Matrix3f::Zero(); Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0); Matrix3f C_canonical; C_canonical << 1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0; for(int i = 0; i < num_tris; ++i) { ... ... @@ -236,15 +235,11 @@ public: const Vec3f& v1 = vertices[tri[0]]; const Vec3f& v2 = vertices[tri[1]]; const Vec3f& v3 = vertices[tri[2]]; Matrix3f A(v1, v2, v3); Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); } FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2); return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2), -C(1, 0), trace_C - C(1, 1), -C(1, 2), -C(2, 0), -C(2, 1), trace_C - C(2, 2)); return C.trace() * Matrix3f::Identity() - C; } public: ... ...
 ... ... @@ -76,7 +76,7 @@ class BVSplitter : public BVSplitterBase { public: BVSplitter(SplitMethodType method) : split_method(method) BVSplitter(SplitMethodType method) : split_vector(0,0,0), split_method(method) { } ... ...
 ... ... @@ -164,13 +164,13 @@ protected: else return aabb->cached.min_; } inline Vec3f::U getVal(size_t i) const inline Vec3f::Scalar getVal(size_t i) const { if(minmax) return aabb->cached.max_[i]; else return aabb->cached.min_[i]; } inline Vec3f::U& getVal(size_t i) inline Vec3f::Scalar& getVal(size_t i) { if(minmax) return aabb->cached.max_[i]; else return aabb->cached.min_[i]; ... ...
 ... ... @@ -166,7 +166,6 @@ public: } // set tm Matrix3f I(1, 0, 0, 0, 1, 0, 0, 0, 1); // R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5 /// 1. compute M(1/2) Vec3f Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0); ... ... @@ -182,7 +181,7 @@ public: Matrix3f hatWt0; hat(hatWt0, Wt0); Matrix3f hatWt0_sqr = hatWt0 * hatWt0; Matrix3f Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0); Matrix3f Mt0 = Matrix3f::Identity() + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0); /// 2. compute M'(1/2) Vec3f dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0); ... ... @@ -340,7 +339,7 @@ public: TaylorModel sin_model(getTimeInterval()); generateTaylorModelForSinFunc(sin_model, angular_vel, 0); TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f::Identity(); TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval()); generateTaylorModelForLinearFunc(a, 0, linear_vel * axis[0]); ... ... @@ -494,7 +493,7 @@ public: TaylorModel sin_model(getTimeInterval()); generateTaylorModelForSinFunc(sin_model, angular_vel, 0); TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f::Identity(); TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval()); generateTaylorModelForLinearFunc(a, 0, linear_vel[0]); ... ...
 ... ... @@ -134,15 +134,15 @@ public: Vec3f com = computeCOM(); FCL_REAL V = computeVolume(); return Matrix3f(C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], C(1, 0) + V * com[1] * com[0], C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], C(2, 1) + V * com[2] * com[1], C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])); return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], C(1, 0) + V * com[1] * com[0], C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], C(2, 1) + V * com[2] * com[1], C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])).finished(); } }; ... ... @@ -207,7 +207,7 @@ public: else { Vec3f center = t.transform(cgeom->aabb_center); Vec3f delta(cgeom->aabb_radius); Vec3f delta(Vec3f::Constant(cgeom->aabb_radius)); aabb.min_ = center - delta; aabb.max_ = center + delta; } ... ...
 template IVector3& operator=(const FclType& other) { const Vec3f& tmp = other.fcl(); setValue (tmp); return *this; } template IVector3& operator=(const Eigen::MatrixBase& other) { const Vec3f& tmp (other); ... ...
 ... ... @@ -14,9 +14,9 @@ template<> struct TaylorReturnType<1> { typedef TVector3 type; typedef Vec3f template<> struct TaylorReturnType<3> { typedef TMatrix3 type; typedef Matrix3f eigen_type; }; template typename TaylorReturnType::type operator * (const FclType& v, const TaylorModel& a) typename TaylorReturnType::type operator * (const Eigen::MatrixBase& v, const TaylorModel& a) { const typename TaylorReturnType::eigen_type b = v.fcl(); const typename TaylorReturnType::eigen_type b = v.derived(); return b * a; } ... ...
 ... ... @@ -48,13 +48,14 @@ namespace fcl { #if FCL_HAVE_EIGEN typedef Eigen::FclMatrix Matrix3f; typedef Eigen::Matrix Matrix3f; #elif FCL_HAVE_SSE typedef Matrix3fX Matrix3f; #else typedef Matrix3fX > Matrix3f; #endif #if ! FCL_HAVE_EIGEN static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m) { o << "[(" << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << ")(" ... ... @@ -62,6 +63,7 @@ static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m) << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << ")]"; return o; } #endif ... ... @@ -73,7 +75,7 @@ public: Matrix3f Sigma; /// @brief Variations along the eign axes Matrix3f::U sigma[3]; Matrix3f::Scalar sigma[3]; /// @brief Eigen axes of the variation matrix Vec3f axis[3]; ... ...