From bc0c7c57edd8b4022deeb6d30edf202c2e8609b4 Mon Sep 17 00:00:00 2001 From: Gael Guennebaud Date: Sun, 15 Jun 2008 17:22:41 +0000 Subject: [PATCH] Added an extensible mechanism to support any kind of rotation representation in Transform via the template static class ToRotationMatrix. Added a lightweight AngleAxis class (similar to Rotation2D). --- Eigen/Geometry | 1 + Eigen/src/Core/util/ForwardDeclarations.h | 3 + Eigen/src/Geometry/Quaternion.h | 20 +- Eigen/src/Geometry/Rotation.h | 256 ++++++++++++++++++++++ Eigen/src/Geometry/Transform.h | 168 ++++++-------- test/geometry.cpp | 82 ++++--- 6 files changed, 390 insertions(+), 140 deletions(-) create mode 100644 Eigen/src/Geometry/Rotation.h diff --git a/Eigen/Geometry b/Eigen/Geometry index fe241b5c9..1452e9752 100644 --- a/Eigen/Geometry +++ b/Eigen/Geometry @@ -7,6 +7,7 @@ namespace Eigen { #include "src/Geometry/Cross.h" #include "src/Geometry/Quaternion.h" +#include "src/Geometry/Rotation.h" #include "src/Geometry/Transform.h" // the Geometry module use cwiseCos and cwiseSin which are defined in the Array module diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h index 1ea58d483..3e2b504c5 100644 --- a/Eigen/src/Core/util/ForwardDeclarations.h +++ b/Eigen/src/Core/util/ForwardDeclarations.h @@ -61,6 +61,9 @@ template class Extract; template::Flags) & ArrayBit> class ArrayBase {}; template class Cross; template class Quaternion; +template class Rotation2D; +template class AngleAxis; +template class Transform; template struct ei_scalar_sum_op; diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 966eca415..354d23e89 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -228,15 +228,15 @@ Quaternion::toRotationMatrix(void) const Scalar tyz = tz*this->y(); Scalar tzz = tz*this->z(); - res(0,0) = 1-(tyy+tzz); - res(0,1) = txy-twz; - res(0,2) = txz+twy; - res(1,0) = txy+twz; - res(1,1) = 1-(txx+tzz); - res(1,2) = tyz-twx; - res(2,0) = txz-twy; - res(2,1) = tyz+twx; - res(2,2) = 1-(txx+tyy); + res.coeffRef(0,0) = 1-(tyy+tzz); + res.coeffRef(0,1) = txy-twz; + res.coeffRef(0,2) = txz+twy; + res.coeffRef(1,0) = txy+twz; + res.coeffRef(1,1) = 1-(txx+tzz); + res.coeffRef(1,2) = tyz-twx; + res.coeffRef(2,0) = txz-twy; + res.coeffRef(2,1) = tyz+twx; + res.coeffRef(2,2) = 1-(txx+tyy); return res; } @@ -250,7 +250,7 @@ Quaternion& Quaternion::fromRotationMatrix(const MatrixBase +// +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see . + +#ifndef EIGEN_ROTATION_H +#define EIGEN_ROTATION_H + +// this file aims to contains the various representations of rotation/orientation +// in 2D and 3D space excepted Matrix and Quaternion. + +/** \class ToRotationMatrix + * + * \brief Template static struct to convert any rotation representation to a matrix form + * + * \param Scalar the numeric type of the matrix coefficients + * \param Dim the dimension of the current space + * \param RotationType the input type of the rotation + * + * This class defines a single static member with the following prototype: + * \code + * static convert(const RotationType& r); + * \endcode + * where \c must be a fixed-size matrix expression of size Dim x Dim and + * coefficient type Scalar. + * + * Default specializations are provided for: + * - any scalar type (2D), + * - any matrix expression, + * - Quaternion, + * - AngleAxis. + * + * Currently ToRotationMatrix is only used by Transform. + * + * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis + * + */ +template +struct ToRotationMatrix; + +// 2D rotation to matrix +template +struct ToRotationMatrix +{ + inline static Matrix convert(const OtherScalarType& r) + { return Rotation2D(r).toRotationMatrix(); } +}; + +// 2D rotation to matrix +template +struct ToRotationMatrix > +{ + inline static Matrix convert(const Rotation2D& r) + { return Rotation2D(r).toRotationMatrix(); } +}; + +// quaternion to matrix +template +struct ToRotationMatrix > +{ + inline static Matrix convert(const Quaternion& q) + { return q.toRotationMatrix(); } +}; + +// angle axis to matrix +template +struct ToRotationMatrix > +{ + inline static Matrix convert(const AngleAxis& aa) + { return aa.toRotationMatrix(); } +}; + +// matrix xpr to matrix xpr +template +struct ToRotationMatrix > +{ + inline static const MatrixBase& convert(const MatrixBase& mat) + { + EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim, + you_did_a_programming_error); + return mat; + } +}; + +/** \class Rotation2D + * + * \brief Represents a rotation/orientation in a 2 dimensional space. + * + * \param _Scalar the scalar type, i.e., the type of the coefficients + * + * This class is equivalent to a single scalar representating the rotation angle + * in radian with some additional features such as the conversion from/to + * rotation matrix. Moreover this class aims to provide a similar interface + * to Quaternion in order to facilitate the writting of generic algorithm + * dealing with rotations. + * + * \sa class Quaternion, class Transform + */ +template +class Rotation2D +{ +public: + enum { Dim = 2 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix Matrix2; + +protected: + + Scalar m_angle; + +public: + + inline Rotation2D(Scalar a) : m_angle(a) {} + inline operator Scalar& () { return m_angle; } + inline operator Scalar () const { return m_angle; } + + template + Rotation2D& fromRotationMatrix(const MatrixBase& m); + Matrix2 toRotationMatrix(void) const; + + /** \returns the spherical interpolation between \c *this and \a other using + * parameter \a t. It is equivalent to a linear interpolation. + */ + inline Rotation2D slerp(Scalar t, const Rotation2D& other) const + { return m_angle * (1-t) + t * other; } +}; + +/** Set \c *this from a 2x2 rotation matrix \a mat. + * In other words, this function extract the rotation angle + * from the rotation matrix. + */ +template +template +Rotation2D& Rotation2D::fromRotationMatrix(const MatrixBase& mat) +{ + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,you_did_a_programming_error); + m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0)); + return *this; +} + +/** Constructs and \returns an equivalent 2x2 rotation matrix. + */ +template +typename Rotation2D::Matrix2 +Rotation2D::toRotationMatrix(void) const +{ + Scalar sinA = ei_sin(m_angle); + Scalar cosA = ei_cos(m_angle); + return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); +} + +/** \class AngleAxis + * + * \brief Represents a rotation in a 3 dimensional space as a rotation angle around a 3D axis + * + * \param _Scalar the scalar type, i.e., the type of the coefficients. + * + * \sa class Quaternion, class Transform + */ +template +class AngleAxis +{ +public: + enum { Dim = 3 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix Matrix3; + typedef Matrix Vector3; + +protected: + + Vector3 m_axis; + Scalar m_angle; + +public: + + AngleAxis() {} + template + inline AngleAxis(Scalar angle, const MatrixBase& axis) : m_axis(axis), m_angle(angle) {} + + Scalar angle() const { return m_angle; } + Scalar& angle() { return m_angle; } + + const Vector3& axis() const { return m_axis; } + Vector3& axis() { return m_axis; } + + template + AngleAxis& fromRotationMatrix(const MatrixBase& m); + Matrix3 toRotationMatrix(void) const; +}; + +/** Set \c *this from a 3x3 rotation matrix \a mat. + * In other words, this function extract the rotation angle + * from the rotation matrix. + */ +template +template +AngleAxis& AngleAxis::fromRotationMatrix(const MatrixBase& mat) +{ + // Since a direct conversion would not be really faster, + // let's use the robust Quaternion implementation: + Quaternion().fromRotationMatrix(mat).toAngleAxis(m_angle, m_axis); + + return *this; +} + +/** Constructs and \returns an equivalent 2x2 rotation matrix. + */ +template +typename AngleAxis::Matrix3 +AngleAxis::toRotationMatrix(void) const +{ + Matrix3 res; + Vector3 sin_axis = ei_sin(m_angle) * m_axis; + Scalar c = ei_cos(m_angle); + Vector3 cos1_axis = (Scalar(1)-c) * m_axis; + + Scalar tmp; + tmp = cos1_axis.x() * m_axis.y(); + res.coeffRef(0,1) = tmp - sin_axis.z(); + res.coeffRef(1,0) = tmp + sin_axis.z(); + + tmp = cos1_axis.x() * m_axis.z(); + res.coeffRef(0,2) = tmp + sin_axis.y(); + res.coeffRef(2,0) = tmp - sin_axis.y(); + + tmp = cos1_axis.y() * m_axis.z(); + res.coeffRef(1,2) = tmp - sin_axis.x(); + res.coeffRef(2,1) = tmp + sin_axis.x(); + + res.diagonal() = Vector3::constant(c) + cos1_axis.cwiseProduct(m_axis); + + return res; +} + +#endif // EIGEN_ROTATION_H diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index f6a525ead..cdc24e772 100644 --- a/Eigen/src/Geometry/Transform.h +++ b/Eigen/src/Geometry/Transform.h @@ -25,94 +25,6 @@ #ifndef EIGEN_TRANSFORM_H #define EIGEN_TRANSFORM_H -/** \class Orientation2D - * - * \brief Represents an orientation/rotation in a 2 dimensional space. - * - * \param _Scalar the scalar type, i.e., the type of the coefficients - * - * This class is equivalent to a single scalar representating the rotation angle - * in radian with some additional features such as the conversion from/to - * rotation matrix. Moreover this class aims to provide a similar interface - * to Quaternion in order to facilitate the writting of generic algorithm - * dealing with rotations. - * - * \sa class Quaternion, class Transform - */ -template -class Orientation2D -{ -public: - enum { Dim = 2 }; - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; - typedef Matrix Matrix2; - -protected: - - Scalar m_angle; - -public: - - inline Orientation2D(Scalar a) : m_angle(a) {} - inline operator Scalar& () { return m_angle; } - inline operator Scalar () const { return m_angle; } - - template - Orientation2D& fromRotationMatrix(const MatrixBase& m); - Matrix2 toRotationMatrix(void) const; - - Orientation2D slerp(Scalar t, const Orientation2D& other) const; -}; - -/** returns the default type used to represent an orientation. - */ -template -struct ei_get_orientation_type; - -template -struct ei_get_orientation_type -{ typedef Orientation2D type; }; - -template -struct ei_get_orientation_type -{ typedef Quaternion type; }; - -/** Set \c *this from a 2x2 rotation matrix \a mat. - * In other words, this function extract the rotation angle - * from the rotation matrix. - */ -template -template -Orientation2D& Orientation2D::fromRotationMatrix(const MatrixBase& mat) -{ - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,you_did_a_programming_error); - m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0)); - return *this; -} - -/** Constructs and \returns an equivalent 2x2 rotation matrix. - */ -template -typename Orientation2D::Matrix2 -Orientation2D::toRotationMatrix(void) const -{ - Scalar sinA = ei_sin(m_angle); - Scalar cosA = ei_cos(m_angle); - return Matrix2(cosA, -sinA, sinA, cosA); -} - -/** \returns the spherical interpolation between \c *this and \a other using - * parameter \a t. It is equivalent to a linear interpolation. - */ -template -Orientation2D -Orientation2D::slerp(Scalar t, const Orientation2D& other) const -{ - return m_angle * (1-t) + t * other; -} - - /** \class Transform * * \brief Represents an homogeneous transformation in a N dimensional space @@ -140,7 +52,6 @@ public: typedef Block AffineMatrixRef; typedef Matrix VectorType; typedef Block VectorRef; - typedef typename ei_get_orientation_type::type Orientation; protected: @@ -160,7 +71,7 @@ public: { m_matrix = other.m_matrix; } inline Transform& operator=(const Transform& other) - { m_matrix = other.m_matrix; } + { m_matrix = other.m_matrix; return *this; } template inline explicit Transform(const MatrixBase& other) @@ -168,7 +79,7 @@ public: template inline Transform& operator=(const MatrixBase& other) - { m_matrix = other; } + { m_matrix = other; return *this; } #ifdef EIGEN_QT_SUPPORT inline Transform(const QMatrix& other); @@ -177,9 +88,9 @@ public: #endif /** \returns a read-only expression of the transformation matrix */ - inline const MatrixType matrix() const { return m_matrix; } + inline const MatrixType& matrix() const { return m_matrix; } /** \returns a writable expression of the transformation matrix */ - inline MatrixType matrix() { return m_matrix; } + inline MatrixType& matrix() { return m_matrix; } /** \returns a read-only expression of the affine (linear) part of the transformation */ inline const AffineMatrixRef affine() const { return m_matrix.template block(0,0); } @@ -202,7 +113,7 @@ public: operator * (const MatrixBase &other) const; /** Contatenates two transformations */ - Product + const Product operator * (const Transform& other) const { return m_matrix * other.matrix(); } @@ -220,6 +131,12 @@ public: template Transform& pretranslate(const MatrixBase &other); + template + Transform& rotate(const RotationType& rotation); + + template + Transform& prerotate(const RotationType& rotation); + template Transform& shear(Scalar sx, Scalar sy); @@ -229,9 +146,9 @@ public: AffineMatrixType extractRotation() const; AffineMatrixType extractRotationNoShear() const; - template + template Transform& fromPositionOrientationScale(const MatrixBase &position, - const Orientation& orientation, const MatrixBase &scale); + const OrientationType& orientation, const MatrixBase &scale); const Inverse inverse() const { return m_matrix.inverse(); } @@ -258,6 +175,7 @@ Transform& Transform::operator=(const QMatrix& other) m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(), 0, 0, 1; + return *this; } /** \returns a QMatrix from \c *this assuming the dimension is 2. @@ -306,11 +224,11 @@ Transform::prescale(const MatrixBase &other) { EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime) && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error); - m_matrix.template block<3,4>(0,0) = (other.asDiagonal() * m_matrix.template block<3,4>(0,0)).lazy(); + m_matrix.template block(0,0) = (other.asDiagonal() * m_matrix.template block(0,0)).lazy(); return *this; } -/** Applies on the right translation matrix represented by the vector \a other +/** Applies on the right the translation matrix represented by the vector \a other * to \c *this and returns a reference to \c *this. * \sa pretranslate() */ @@ -325,7 +243,7 @@ Transform::translate(const MatrixBase &other) return *this; } -/** Applies on the left translation matrix represented by the vector \a other +/** Applies on the left the translation matrix represented by the vector \a other * to \c *this and returns a reference to \c *this. * \sa translate() */ @@ -340,6 +258,49 @@ Transform::pretranslate(const MatrixBase &other) return *this; } +/** Applies on the right the rotation represented by the rotation \a rotation + * to \c *this and returns a reference to \c *this. + * + * The template parameter \a RotationType is the type of the rotation which + * must be registered by ToRotationMatrix<>. + * + * Natively supported types includes: + * - any scalar (2D), + * - a Dim x Dim matrix expression, + * - Quaternion (3D), + * - AngleAxis (3D) + * + * This mechanism is easily extendable to support user types such as Euler angles, + * or a pair of Quaternion for 4D rotations. + * + * \sa rotate(Scalar), class Quaternion, class AngleAxis, class ToRotationMatrix, prerotate(RotationType) + */ +template +template +Transform& +Transform::rotate(const RotationType& rotation) +{ + affine() *= ToRotationMatrix::convert(rotation); + return *this; +} + +/** Applies on the left the rotation represented by the rotation \a rotation + * to \c *this and returns a reference to \c *this. + * + * See rotate(RotationType) for further details. + * + * \sa rotate(RotationType), rotate(Scalar) + */ +template +template +Transform& +Transform::prerotate(const RotationType& rotation) +{ + m_matrix.template block(0,0) = ToRotationMatrix::convert(rotation) + * m_matrix.template block(0,0); + return *this; +} + /** Applies on the right the shear transformation represented * by the vector \a other to \c *this and returns a reference to \c *this. * \warning 2D only. @@ -369,7 +330,7 @@ Transform::preshear(Scalar sx, Scalar sy) { EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime) && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error); - m_matrix.template block<3,4>(0,0) = AffineMatrixType(1, sx, sy, 1) * m_matrix.template block<3,4>(0,0); + m_matrix.template block(0,0) = AffineMatrixType(1, sx, sy, 1) * m_matrix.template block(0,0); return *this; } @@ -399,16 +360,17 @@ Transform::extractRotationNoShear() const * of a 3D object. */ template -template +template Transform& Transform::fromPositionOrientationScale(const MatrixBase &position, - const Orientation& orientation, const MatrixBase &scale) + const OrientationType& orientation, const MatrixBase &scale) { - affine() = orientation.toRotationMatrix(); + affine() = ToRotationMatrix::convert(orientation); translation() = position; m_matrix(Dim,Dim) = 1.; m_matrix.template block<1,Dim>(Dim,0).setZero(); affine() *= scale.asDiagonal(); + return *this; } //---------- diff --git a/test/geometry.cpp b/test/geometry.cpp index f41d086de..c4d8326a8 100644 --- a/test/geometry.cpp +++ b/test/geometry.cpp @@ -24,6 +24,7 @@ #include "main.h" #include +#include template void geometry(void) { @@ -31,8 +32,10 @@ template void geometry(void) Cross.h Quaternion.h, Transform.cpp */ + typedef Matrix Matrix2; typedef Matrix Matrix3; typedef Matrix Matrix4; + typedef Matrix Vector2; typedef Matrix Vector3; typedef Matrix Vector4; typedef Quaternion Quaternion; @@ -42,38 +45,38 @@ template void geometry(void) v1 = Vector3::random(), v2 = Vector3::random(); - Scalar a; + Scalar a = ei_random(-M_PI, M_PI); q1.fromAngleAxis(ei_random(-M_PI, M_PI), v0.normalized()); q2.fromAngleAxis(ei_random(-M_PI, M_PI), v1.normalized()); // rotation matrix conversion -// VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2); -// VERIFY_IS_APPROX(q1 * q2 * v2, -// q1.toRotationMatrix() * q2.toRotationMatrix() * v2); -// VERIFY_IS_NOT_APPROX(q2 * q1 * v2, -// q1.toRotationMatrix() * q2.toRotationMatrix() * v2); -// q2.fromRotationMatrix(q1.toRotationMatrix()); -// VERIFY_IS_APPROX(q1*v1,q2*v1); -// -// // Euler angle conversion -// VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1); -// v2 = q2.toEulerAngles(); -// VERIFY_IS_APPROX(q2.fromEulerAngles(v2).toEulerAngles(), v2); -// VERIFY_IS_NOT_APPROX(q2.fromEulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))).toEulerAngles(), v2); -// -// // angle-axis conversion -// q1.toAngleAxis(a, v2); -// VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1); -// VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1); -// -// // from two vector creation -// VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized()); -// VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized()); -// -// // inverse and conjugate -// VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1); -// VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1); + VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2); + VERIFY_IS_APPROX(q1 * q2 * v2, + q1.toRotationMatrix() * q2.toRotationMatrix() * v2); + VERIFY_IS_NOT_APPROX(q2 * q1 * v2, + q1.toRotationMatrix() * q2.toRotationMatrix() * v2); + q2.fromRotationMatrix(q1.toRotationMatrix()); + VERIFY_IS_APPROX(q1*v1,q2*v1); + + // Euler angle conversion + VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1); + v2 = q2.toEulerAngles(); + VERIFY_IS_APPROX(q2.fromEulerAngles(v2).toEulerAngles(), v2); + VERIFY_IS_NOT_APPROX(q2.fromEulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))).toEulerAngles(), v2); + + // angle-axis conversion + q1.toAngleAxis(a, v2); + VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1); + VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1); + + // from two vector creation + VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized()); + VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized()); + + // inverse and conjugate + VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1); + VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1); // cross product VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1)); @@ -83,6 +86,18 @@ template void geometry(void) (v0.cross(v1).cross(v0)).normalized(); VERIFY(m.isOrtho()); + // AngleAxis + VERIFY_IS_APPROX(AngleAxis(a,v1.normalized()).toRotationMatrix(), + q2.fromAngleAxis(a,v1.normalized()).toRotationMatrix()); + + AngleAxis aa1; + m = q1.toRotationMatrix(); + Vector3 tax; Scalar tan; + q2.fromRotationMatrix(m).toAngleAxis(tan,tax); + VERIFY_IS_APPROX(aa1.fromRotationMatrix(m).toRotationMatrix(), + q2.fromRotationMatrix(m).toRotationMatrix()); + + // Transform // TODO complete the tests ! typedef Transform Transform2; @@ -119,6 +134,19 @@ template void geometry(void) t1.fromPositionOrientationScale(v0, q1, v1); VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); + + // 2D transformation + Transform2 t20, t21; + Vector2 v20 = Vector2::random(); + Vector2 v21 = Vector2::random(); + t21.setIdentity(); + t21.affine() = Rotation2D(a).toRotationMatrix(); + VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(), + t21.pretranslate(v20).scale(v21).matrix()); + + t21.setIdentity(); + t21.affine() = Rotation2D(-a).toRotationMatrix(); + VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) * (t21.prescale(v21.cwiseInverse()).translate(-v20))).isIdentity() ); } void test_geometry()