Commit 2aafd003 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Do not store a Quaternion3f in Transform3f.

parent bea72583
......@@ -203,7 +203,7 @@ public:
/// @brief compute the AABB in world space
inline void computeAABB()
{
if(isQuatIdentity(t.getQuatRotation()))
if(t.getRotation().isIdentity())
{
aabb = translate(cgeom->aabb_local, t.getTranslation());
}
......@@ -240,8 +240,6 @@ public:
return t.getRotation();
}
/// @brief get object's transform
inline const Transform3f& getTransform() const
{
......
......@@ -69,18 +69,11 @@ inline bool areQuatEquals (const Quaternion3f& q1, const Quaternion3f& q2)
/// @brief Simple transform class used locally by InterpMotion
class Transform3f
{
/// @brief Whether matrix cache is set
mutable bool matrix_set;
/// @brief Matrix cache
mutable Matrix3f R;
Matrix3f R;
/// @brief Tranlation vector
Vec3f T;
/// @brief Rotation
Quaternion3f q;
const Matrix3f& getRotationInternal() const;
public:
/// @brief Default transform is no movement
......@@ -90,49 +83,40 @@ public:
}
/// @brief Construct transform from rotation and translation
Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
R(R_),
T(T_)
{
q = Quaternion3f(R_);
}
template <typename Matrixx3Like, typename Vector3Like>
Transform3f(const Eigen::MatrixBase<Matrixx3Like>& R_,
const Eigen::MatrixBase<Vector3Like>& T_) :
R(R_),
T(T_)
{}
/// @brief Construct transform from rotation and translation
Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false),
T(T_),
q(q_)
{
}
template <typename Vector3Like>
Transform3f(const Quaternion3f& q_,
const Eigen::MatrixBase<Vector3Like>& T_) :
R(q_.toRotationMatrix()),
T(T_)
{}
/// @brief Construct transform from rotation
Transform3f(const Matrix3f& R_) : matrix_set(true),
R(R_),
Transform3f(const Matrix3f& R_) : R(R_),
T(Vec3f::Zero())
{
q = Quaternion3f(R_);
}
{}
/// @brief Construct transform from rotation
Transform3f(const Quaternion3f& q_) : matrix_set(false),
T(Vec3f::Zero()),
q(q_)
{
}
Transform3f(const Quaternion3f& q_) : R(q_),
T(Vec3f::Zero())
{}
/// @brief Construct transform from translation
Transform3f(const Vec3f& T_) : matrix_set(true),
T(T_),
q(Quaternion3f::Identity())
{
R.setIdentity();
}
Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()),
T(T_)
{}
/// @brief operator =
Transform3f& operator = (const Transform3f& tf)
{
matrix_set = tf.matrix_set;
R = tf.R;
q = tf.q;
T = tf.T;
return *this;
}
......@@ -146,31 +130,28 @@ public:
/// @brief get rotation
inline const Matrix3f& getRotation() const
{
if(matrix_set) return R;
return getRotationInternal();
return R;
}
/// @brief get quaternion
inline const Quaternion3f& getQuatRotation() const
inline Quaternion3f getQuatRotation() const
{
return q;
return Quaternion3f (R);
}
/// @brief set transform from rotation and translation
inline void setTransform(const Matrix3f& R_, const Vec3f& T_)
template <typename Matrixx3Like, typename Vector3Like>
inline void setTransform(const Eigen::MatrixBase<Matrixx3Like>& R_, const Eigen::MatrixBase<Vector3Like>& T_)
{
R.noalias() = R_;
T.noalias() = T_;
q = Quaternion3f(R_);
matrix_set = true;
}
/// @brief set transform from rotation and translation
inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
{
matrix_set = false;
q = q_;
T.noalias() = T_;
R = q_.toRotationMatrix();
T = T_;
}
/// @brief set transform from rotation
......@@ -178,8 +159,6 @@ public:
inline void setRotation(const Eigen::MatrixBase<Derived>& R_)
{
R.noalias() = R_;
matrix_set = true;
q = Quaternion3f(R);
}
/// @brief set transform from translation
......@@ -192,23 +171,21 @@ public:
/// @brief set transform from rotation
inline void setQuatRotation(const Quaternion3f& q_)
{
matrix_set = false;
q = q_;
R = q_.toRotationMatrix();
}
/// @brief transform a given vector by the transform
template <typename Derived>
inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const
{
return q * v + T;
return R * v + T;
}
/// @brief inverse transform
inline Transform3f& inverseInPlace()
{
matrix_set = false;
q = q.conjugate();
T = q * (-T);
R.transposeInPlace();
T = - R * T;
return *this;
}
......@@ -221,30 +198,27 @@ public:
/// @brief inverse the transform and multiply with another
inline Transform3f inverseTimes(const Transform3f& other) const
{
const Quaternion3f& q_inv = q.conjugate();
return Transform3f(q_inv * other.q, q_inv * (other.T - T));
return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T));
}
/// @brief multiply with another transform
inline const Transform3f& operator *= (const Transform3f& other)
{
matrix_set = false;
T += q * other.T;
q *= other.q;
T += R * other.T;
R *= other.R;
return *this;
}
/// @brief multiply with another transform
inline Transform3f operator * (const Transform3f& other) const
{
Quaternion3f q_new = q * other.q;
return Transform3f(q_new, q * other.T + T);
return Transform3f(R * other.R, R * other.T + T);
}
/// @brief check whether the transform is identity
inline bool isIdentity() const
{
return isQuatIdentity(q) && T.isZero();
return R.isIdentity() && T.isZero();
}
/// @brief set the transform to be identity transform
......@@ -252,13 +226,11 @@ public:
{
R.setIdentity();
T.setZero();
q.setIdentity();
matrix_set = true;
}
bool operator == (const Transform3f& other) const
{
return areQuatEquals(q, other.getQuatRotation()) && (T == other.getTranslation());
return R == other.R && (T == other.getTranslation());
}
bool operator != (const Transform3f& other) const
......@@ -267,8 +239,6 @@ public:
}
};
using namespace hpp::fcl;
template<typename Derived>
inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_REAL angle)
{
......
......@@ -111,7 +111,7 @@ template<typename BV>
BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, const AABB& _aabb)
{
assert(model.getModelType() == BVH_MODEL_TRIANGLES);
const Quaternion3f& q = pose.getQuatRotation();
const Matrix3f& q = pose.getRotation();
AABB aabb = translate (_aabb, - pose.getTranslation());
Transform3f box_pose; Box box;
......
......@@ -44,30 +44,17 @@ namespace hpp
namespace fcl
{
const Matrix3f& Transform3f::getRotationInternal() const
{
if(!matrix_set)
{
R = q.toRotationMatrix();
matrix_set = true;
}
return R;
}
void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
const Quaternion3f& q1_inv = tf1.getQuatRotation().conjugate();
tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv * (tf2.getTranslation() - tf1.getTranslation()));
tf = tf1.inverseTimes (tf2);
}
void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
const Quaternion3f& q1inv = tf1.getQuatRotation().conjugate();
const Quaternion3f& q2_q1inv = tf2.getQuatRotation() * q1inv;
tf = Transform3f(q2_q1inv, tf2.getTranslation() - q2_q1inv * tf1.getTranslation());
Matrix3f R (tf2.getRotation() * tf1.getRotation().transpose());
tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation());
}
......
......@@ -227,7 +227,7 @@ Halfspace transform(const Halfspace& a, const Transform3f& tf)
/// where n' = R * n
/// and d' = d + n' * T
Vec3f n = tf.getQuatRotation() * a.n;
Vec3f n = tf.getRotation() * a.n;
FCL_REAL d = a.d + n.dot(tf.getTranslation());
return Halfspace(n, d);
......@@ -242,7 +242,7 @@ Plane transform(const Plane& a, const Transform3f& tf)
/// where n' = R * n
/// and d' = d + n' * T
Vec3f n = tf.getQuatRotation() * a.n;
Vec3f n = tf.getRotation() * a.n;
FCL_REAL d = a.d + n.dot(tf.getTranslation());
return Plane(n, d);
......@@ -698,7 +698,7 @@ void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, K
template<>
void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
{
Vec3f n = tf.getQuatRotation() * s.n;
Vec3f n = tf.getRotation() * s.n;
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
bv.axes.col(0).noalias() = n;
......@@ -711,7 +711,7 @@ void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
template<>
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
{
Vec3f n = tf.getQuatRotation() * s.n;
Vec3f n = tf.getRotation() * s.n;
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
bv.axes.col(0).noalias() = n;
......
......@@ -879,7 +879,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
tf2 = transform;
contact = transform.transform(Vec3f(-5, 0, 0));
depth = 10;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
tf1 = Transform3f();
......@@ -893,7 +893,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
tf2 = transform * Transform3f(Vec3f(5, 0, 0));
contact = transform.transform(Vec3f(-2.5, 0, 0));
depth = 15;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
tf1 = Transform3f();
......@@ -907,7 +907,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
contact = transform.transform(Vec3f(-7.5, 0, 0));
depth = 5;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
tf1 = Transform3f();
......@@ -929,7 +929,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
contact = transform.transform(Vec3f(0.05, 0, 0));
depth = 20.1;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
}
......@@ -1033,7 +1033,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
tf2 = transform;
contact = transform.transform(Vec3f(-1.25, 0, 0));
depth = 2.5;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
tf1 = Transform3f();
......@@ -1047,7 +1047,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
contact = transform.transform(Vec3f(-0.625, 0, 0));
depth = 3.75;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
tf1 = Transform3f();
......@@ -1061,7 +1061,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
contact = transform.transform(Vec3f(-1.875, 0, 0));
depth = 1.25;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
tf1 = Transform3f();
......@@ -1075,7 +1075,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
contact = transform.transform(Vec3f(0.005, 0, 0));
depth = 5.01;
normal = transform.getQuatRotation() * Vec3f(-1, 0, 0);
normal = transform.getRotation() * Vec3f(-1, 0, 0);
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true, &contact, &depth, &normal);
tf1 = Transform3f();
......@@ -1086,7 +1086,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
tf1 = Transform3f(transform.getQuatRotation());
tf1 = Transform3f(transform.getRotation());
tf2 = Transform3f();
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true);
}
......@@ -1164,7 +1164,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, false);
tf1 = Transform3f(transform.getQuatRotation());
tf1 = Transform3f(transform.getRotation());
tf2 = Transform3f();
testShapeIntersection(s, tf1, hs, tf2, GST_INDEP, true);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment