mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-01-24 14:45:14 +08:00
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).
This commit is contained in:
Gael Guennebaud
parent
0ee6b08128
commit
bc0c7c57ed
@ -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
|
||||
|
||||
@ -61,6 +61,9 @@ template<typename MatrixType, unsigned int Mode> class Extract;
|
||||
template<typename Derived, bool HasArrayFlag = int(ei_traits<Derived>::Flags) & ArrayBit> class ArrayBase {};
|
||||
template<typename Lhs, typename Rhs> class Cross;
|
||||
template<typename Scalar> class Quaternion;
|
||||
template<typename Scalar> class Rotation2D;
|
||||
template<typename Scalar> class AngleAxis;
|
||||
template<typename Scalar,int Dim> class Transform;
|
||||
|
||||
|
||||
template<typename Scalar> struct ei_scalar_sum_op;
|
||||
|
||||
@ -228,15 +228,15 @@ Quaternion<Scalar>::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<Scalar>& Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Deri
|
||||
{
|
||||
// FIXME maybe this function could accept 4x4 and 3x4 matrices as well ? (simply update the assert)
|
||||
// FIXME this function could also be static and returns a temporary ?
|
||||
ei_assert(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3);
|
||||
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3,you_did_a_programming_error);
|
||||
// This algorithm comes from "Quaternion Calculus and Fast Animation",
|
||||
// Ken Shoemake, 1987 SIGGRAPH course notes
|
||||
Scalar t = mat.trace();
|
||||
|
||||
256
Eigen/src/Geometry/Rotation.h
Normal file
256
Eigen/src/Geometry/Rotation.h
Normal file
@ -0,0 +1,256 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra. Eigen itself is part of the KDE project.
|
||||
//
|
||||
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
|
||||
//
|
||||
// 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 <http://www.gnu.org/licenses/>.
|
||||
|
||||
#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 <MatrixExpression> convert(const RotationType& r);
|
||||
* \endcode
|
||||
* where \c <MatrixExpression> 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<typename Scalar, int Dim, typename RotationType>
|
||||
struct ToRotationMatrix;
|
||||
|
||||
// 2D rotation to matrix
|
||||
template<typename Scalar, typename OtherScalarType>
|
||||
struct ToRotationMatrix<Scalar, 2, OtherScalarType>
|
||||
{
|
||||
inline static Matrix<Scalar,2,2> convert(const OtherScalarType& r)
|
||||
{ return Rotation2D<Scalar>(r).toRotationMatrix(); }
|
||||
};
|
||||
|
||||
// 2D rotation to matrix
|
||||
template<typename Scalar, typename OtherScalarType>
|
||||
struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> >
|
||||
{
|
||||
inline static Matrix<Scalar,2,2> convert(const Rotation2D<OtherScalarType>& r)
|
||||
{ return Rotation2D<Scalar>(r).toRotationMatrix(); }
|
||||
};
|
||||
|
||||
// quaternion to matrix
|
||||
template<typename Scalar, typename OtherScalarType>
|
||||
struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> >
|
||||
{
|
||||
inline static Matrix<Scalar,3,3> convert(const Quaternion<OtherScalarType>& q)
|
||||
{ return q.toRotationMatrix(); }
|
||||
};
|
||||
|
||||
// angle axis to matrix
|
||||
template<typename Scalar, typename OtherScalarType>
|
||||
struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> >
|
||||
{
|
||||
inline static Matrix<Scalar,3,3> convert(const AngleAxis<OtherScalarType>& aa)
|
||||
{ return aa.toRotationMatrix(); }
|
||||
};
|
||||
|
||||
// matrix xpr to matrix xpr
|
||||
template<typename Scalar, int Dim, typename OtherDerived>
|
||||
struct ToRotationMatrix<Scalar, Dim, MatrixBase<OtherDerived> >
|
||||
{
|
||||
inline static const MatrixBase<OtherDerived>& convert(const MatrixBase<OtherDerived>& 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<typename _Scalar>
|
||||
class Rotation2D
|
||||
{
|
||||
public:
|
||||
enum { Dim = 2 };
|
||||
/** the scalar type of the coefficients */
|
||||
typedef _Scalar Scalar;
|
||||
typedef Matrix<Scalar,2,2> 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<typename Derived>
|
||||
Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& 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<typename Scalar>
|
||||
template<typename Derived>
|
||||
Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& 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 Scalar>
|
||||
typename Rotation2D<Scalar>::Matrix2
|
||||
Rotation2D<Scalar>::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<typename _Scalar>
|
||||
class AngleAxis
|
||||
{
|
||||
public:
|
||||
enum { Dim = 3 };
|
||||
/** the scalar type of the coefficients */
|
||||
typedef _Scalar Scalar;
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
|
||||
protected:
|
||||
|
||||
Vector3 m_axis;
|
||||
Scalar m_angle;
|
||||
|
||||
public:
|
||||
|
||||
AngleAxis() {}
|
||||
template<typename Derived>
|
||||
inline AngleAxis(Scalar angle, const MatrixBase<Derived>& 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<typename Derived>
|
||||
AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& 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<typename Scalar>
|
||||
template<typename Derived>
|
||||
AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
|
||||
{
|
||||
// Since a direct conversion would not be really faster,
|
||||
// let's use the robust Quaternion implementation:
|
||||
Quaternion<Scalar>().fromRotationMatrix(mat).toAngleAxis(m_angle, m_axis);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Constructs and \returns an equivalent 2x2 rotation matrix.
|
||||
*/
|
||||
template<typename Scalar>
|
||||
typename AngleAxis<Scalar>::Matrix3
|
||||
AngleAxis<Scalar>::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
|
||||
@ -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<typename _Scalar>
|
||||
class Orientation2D
|
||||
{
|
||||
public:
|
||||
enum { Dim = 2 };
|
||||
/** the scalar type of the coefficients */
|
||||
typedef _Scalar Scalar;
|
||||
typedef Matrix<Scalar,2,2> 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<typename Derived>
|
||||
Orientation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
|
||||
Matrix2 toRotationMatrix(void) const;
|
||||
|
||||
Orientation2D slerp(Scalar t, const Orientation2D& other) const;
|
||||
};
|
||||
|
||||
/** returns the default type used to represent an orientation.
|
||||
*/
|
||||
template<typename Scalar, int Dim>
|
||||
struct ei_get_orientation_type;
|
||||
|
||||
template<typename Scalar>
|
||||
struct ei_get_orientation_type<Scalar,2>
|
||||
{ typedef Orientation2D<Scalar> type; };
|
||||
|
||||
template<typename Scalar>
|
||||
struct ei_get_orientation_type<Scalar,3>
|
||||
{ typedef Quaternion<Scalar> 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<typename Scalar>
|
||||
template<typename Derived>
|
||||
Orientation2D<Scalar>& Orientation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& 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 Scalar>
|
||||
typename Orientation2D<Scalar>::Matrix2
|
||||
Orientation2D<Scalar>::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<typename Scalar>
|
||||
Orientation2D<Scalar>
|
||||
Orientation2D<Scalar>::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<MatrixType,Dim,Dim> AffineMatrixRef;
|
||||
typedef Matrix<Scalar,Dim,1> VectorType;
|
||||
typedef Block<MatrixType,Dim,1> VectorRef;
|
||||
typedef typename ei_get_orientation_type<Scalar,Dim>::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<typename OtherDerived>
|
||||
inline explicit Transform(const MatrixBase<OtherDerived>& other)
|
||||
@ -168,7 +79,7 @@ public:
|
||||
|
||||
template<typename OtherDerived>
|
||||
inline Transform& operator=(const MatrixBase<OtherDerived>& 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<Dim,Dim>(0,0); }
|
||||
@ -202,7 +113,7 @@ public:
|
||||
operator * (const MatrixBase<OtherDerived> &other) const;
|
||||
|
||||
/** Contatenates two transformations */
|
||||
Product<MatrixType,MatrixType>
|
||||
const Product<MatrixType,MatrixType>
|
||||
operator * (const Transform& other) const
|
||||
{ return m_matrix * other.matrix(); }
|
||||
|
||||
@ -220,6 +131,12 @@ public:
|
||||
template<typename OtherDerived>
|
||||
Transform& pretranslate(const MatrixBase<OtherDerived> &other);
|
||||
|
||||
template<typename RotationType>
|
||||
Transform& rotate(const RotationType& rotation);
|
||||
|
||||
template<typename RotationType>
|
||||
Transform& prerotate(const RotationType& rotation);
|
||||
|
||||
template<typename OtherDerived>
|
||||
Transform& shear(Scalar sx, Scalar sy);
|
||||
|
||||
@ -229,9 +146,9 @@ public:
|
||||
AffineMatrixType extractRotation() const;
|
||||
AffineMatrixType extractRotationNoShear() const;
|
||||
|
||||
template<typename PositionDerived, typename ScaleDerived>
|
||||
template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
|
||||
Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
|
||||
const Orientation& orientation, const MatrixBase<ScaleDerived> &scale);
|
||||
const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale);
|
||||
|
||||
const Inverse<MatrixType, false> inverse() const
|
||||
{ return m_matrix.inverse(); }
|
||||
@ -258,6 +175,7 @@ Transform<Scalar,Dim>& Transform<Scalar,Dim>::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<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &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<Dim,HDim>(0,0) = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(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<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &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<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &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<typename Scalar, int Dim>
|
||||
template<typename RotationType>
|
||||
Transform<Scalar,Dim>&
|
||||
Transform<Scalar,Dim>::rotate(const RotationType& rotation)
|
||||
{
|
||||
affine() *= ToRotationMatrix<Scalar,Dim,RotationType>::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<typename Scalar, int Dim>
|
||||
template<typename RotationType>
|
||||
Transform<Scalar,Dim>&
|
||||
Transform<Scalar,Dim>::prerotate(const RotationType& rotation)
|
||||
{
|
||||
m_matrix.template block<Dim,HDim>(0,0) = ToRotationMatrix<Scalar,Dim,RotationType>::convert(rotation)
|
||||
* m_matrix.template block<Dim,HDim>(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<Scalar,Dim>::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<Dim,HDim>(0,0) = AffineMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
|
||||
return *this;
|
||||
}
|
||||
|
||||
@ -399,16 +360,17 @@ Transform<Scalar,Dim>::extractRotationNoShear() const
|
||||
* of a 3D object.
|
||||
*/
|
||||
template<typename Scalar, int Dim>
|
||||
template<typename PositionDerived, typename ScaleDerived>
|
||||
template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
|
||||
Transform<Scalar,Dim>&
|
||||
Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
|
||||
const Orientation& orientation, const MatrixBase<ScaleDerived> &scale)
|
||||
const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
|
||||
{
|
||||
affine() = orientation.toRotationMatrix();
|
||||
affine() = ToRotationMatrix<Scalar,Dim,OrientationType>::convert(orientation);
|
||||
translation() = position;
|
||||
m_matrix(Dim,Dim) = 1.;
|
||||
m_matrix.template block<1,Dim>(Dim,0).setZero();
|
||||
affine() *= scale.asDiagonal();
|
||||
return *this;
|
||||
}
|
||||
|
||||
//----------
|
||||
|
||||
@ -24,6 +24,7 @@
|
||||
|
||||
#include "main.h"
|
||||
#include <Eigen/Geometry>
|
||||
#include <Eigen/LU>
|
||||
|
||||
template<typename Scalar> void geometry(void)
|
||||
{
|
||||
@ -31,8 +32,10 @@ template<typename Scalar> void geometry(void)
|
||||
Cross.h Quaternion.h, Transform.cpp
|
||||
*/
|
||||
|
||||
typedef Matrix<Scalar,2,2> Matrix2;
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
typedef Matrix<Scalar,4,4> Matrix4;
|
||||
typedef Matrix<Scalar,2,1> Vector2;
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
typedef Matrix<Scalar,4,1> Vector4;
|
||||
typedef Quaternion<Scalar> Quaternion;
|
||||
@ -42,38 +45,38 @@ template<typename Scalar> void geometry(void)
|
||||
v1 = Vector3::random(),
|
||||
v2 = Vector3::random();
|
||||
|
||||
Scalar a;
|
||||
Scalar a = ei_random<Scalar>(-M_PI, M_PI);
|
||||
|
||||
q1.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized());
|
||||
q2.fromAngleAxis(ei_random<Scalar>(-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<typename Scalar> void geometry(void)
|
||||
(v0.cross(v1).cross(v0)).normalized();
|
||||
VERIFY(m.isOrtho());
|
||||
|
||||
// AngleAxis
|
||||
VERIFY_IS_APPROX(AngleAxis<Scalar>(a,v1.normalized()).toRotationMatrix(),
|
||||
q2.fromAngleAxis(a,v1.normalized()).toRotationMatrix());
|
||||
|
||||
AngleAxis<Scalar> 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<Scalar,2> Transform2;
|
||||
@ -119,6 +134,19 @@ template<typename Scalar> 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<Scalar>(a).toRotationMatrix();
|
||||
VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(),
|
||||
t21.pretranslate(v20).scale(v21).matrix());
|
||||
|
||||
t21.setIdentity();
|
||||
t21.affine() = Rotation2D<Scalar>(-a).toRotationMatrix();
|
||||
VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) * (t21.prescale(v21.cwiseInverse()).translate(-v20))).isIdentity() );
|
||||
}
|
||||
|
||||
void test_geometry()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user