diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h index db0792b850145fb2e54bf2a142229af46ac3b549..3f08e0b9b6a3df2a39a092575d8cb5d78cab134b 100644 --- a/include/hpp/fcl/math/transform.h +++ b/include/hpp/fcl/math/transform.h @@ -100,8 +100,8 @@ public: /// @brief Construct transform from rotation Transform3f(const Matrix3f& R_) : matrix_set(true), - T(Vec3f::Zero()), - R(R_) + R(R_), + T(Vec3f::Zero()) { q = Quaternion3f(R_); } diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 30334bca3e4be70d42ceaa87630ed8daa4b0badf..a2ed4d32d7f68a2b063733a24c19943aab73639b 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -142,7 +142,7 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { - Vec3f diff = tf2.transform(Vec3f()) - tf1.transform(Vec3f()); + Vec3f diff = tf2.getTranslation() - tf1.getTranslation(); FCL_REAL len = diff.norm(); if(len > s1.radius + s2.radius) return false; @@ -161,7 +161,7 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, } if(contact_points) - *contact_points = tf1.transform(Vec3f(0,0,0)) + diff * s1.radius / (s1.radius + s2.radius); + *contact_points = tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius); return true; }