GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-09-24 15:23:13 +08:00
Code Issues Packages Projects Releases Wiki Activity

work on rotations in the Geometry module:

Browse Source 
- convertions are done trough constructors and operator=
 - added a EulerAngles class
This commit is contained in:
Gael Guennebaud 2008-06-21 15:01:49 +00:00
parent 574416b842
commit 32c5ea388e
8 changed files with 491 additions and 288 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Eigen/Geometry
View File

@ -7,6 +7,8 @@ namespace Eigen {
#include "src/Geometry/Cross.h"
#include "src/Geometry/Quaternion.h"
#include "src/Geometry/AngleAxis.h"
#include "src/Geometry/EulerAngles.h"
#include "src/Geometry/Rotation.h"
#include "src/Geometry/Transform.h"

13
Eigen/src/Core/util/ForwardDeclarations.h
View File

@ -56,11 +56,6 @@ template<int Direction, typename UnaryOp, typename MatrixType> class PartialRedu
template<typename MatrixType, unsigned int Mode> class Part;
template<typename MatrixType, unsigned int Mode> class Extract;
template<typename MatrixType> class Array;
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 Lhs, typename Rhs> struct ei_product_mode;
template<typename Lhs, typename Rhs, int ProductMode = ei_product_mode<Lhs,Rhs>::value> struct ProductReturnType;
@ -98,4 +93,12 @@ void ei_cache_friendly_product(
template<typename ExpressionType, bool CheckExistence = true> class Inverse;
template<typename MatrixType> class QR;
// Geometry module:
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> class EulerAngles;
template<typename Scalar,int Dim> class Transform;
#endif // EIGEN_FORWARDDECLARATIONS_H

147
Eigen/src/Geometry/AngleAxis.h Normal file
View File

@ -0,0 +1,147 @@
// 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_ANGLEAXIS_H
#define EIGEN_ANGLEAXIS_H
/** \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 EulerAngles, 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;
typedef Quaternion<Scalar> QuaternionType;
typedef EulerAngles<Scalar> EulerAnglesType;
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) {}
inline AngleAxis(const QuaternionType& q) { *this = q; }
inline AngleAxis(const EulerAnglesType& ea) { *this = ea; }
template<typename Derived>
inline AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
Scalar angle() const { return m_angle; }
Scalar& angle() { return m_angle; }
const Vector3& axis() const { return m_axis; }
Vector3& axis() { return m_axis; }
AngleAxis& operator=(const QuaternionType& q);
AngleAxis& operator=(const EulerAnglesType& ea);
template<typename Derived>
AngleAxis& operator=(const MatrixBase<Derived>& m);
template<typename Derived>
AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix3 toRotationMatrix(void) const;
};
/** Set \c *this from a quaternion.
* The axis is normalized.
*/
template<typename Scalar>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
{
Scalar n2 = q.vec().norm2();
if (ei_isMuchSmallerThan(n2,Scalar(1)))
{
m_angle = 0;
m_axis << 1, 0, 0;
}
else
{
m_angle = 2*std::acos(q.w());
m_axis = q.vec() / ei_sqrt(n2);
}
return *this;
}
/** Set \c *this from Euler angles \a ea.
*/
template<typename Scalar>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const EulerAnglesType& ea)
{
return *this = QuaternionType(ea);
}
/** Set \c *this from a 3x3 rotation matrix \a mat.
*/
template<typename Scalar>
template<typename Derived>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
{
// Since a direct conversion would not be really faster,
// let's use the robust Quaternion implementation:
return *this = QuaternionType(mat);
}
/** Constructs and \returns an equivalent 3x3 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_ANGLEAXIS_H

158
Eigen/src/Geometry/EulerAngles.h Normal file
View File

@ -0,0 +1,158 @@
// 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_EULERANGLES_H
#define EIGEN_EULERANGLES_H
template<typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct ei_eulerangles_assign_impl;
/** \class EulerAngles
*
* \brief Represents a rotation in a 3 dimensional space as three Euler angles
*
* \param _Scalar the scalar type, i.e., the type of the angles.
*
* \sa class Quaternion, class AngleAxis, class Transform
*/
template<typename _Scalar>
class EulerAngles
{
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;
typedef Quaternion<Scalar> QuaternionType;
typedef AngleAxis<Scalar> AngleAxisType;
protected:
Vector3 m_angles;
public:
EulerAngles() {}
template<typename Derived>
inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
inline EulerAngles(const QuaternionType& q) { *this = q; }
inline EulerAngles(const AngleAxisType& aa) { *this = aa; }
template<typename Derived>
inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
Scalar angle(int i) const { return m_angles.coeff(i); }
Scalar& angle(int i) { return m_angles.coeffRef(i); }
const Vector3& coeffs() const { return m_angles; }
Vector3& coeffs() { return m_angles; }
EulerAngles& operator=(const QuaternionType& q);
EulerAngles& operator=(const AngleAxisType& ea);
template<typename Derived>
EulerAngles& operator=(const MatrixBase<Derived>& m);
template<typename Derived>
EulerAngles& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix3 toRotationMatrix(void) const;
};
/** Set \c *this from a quaternion.
* The axis is normalized.
*/
template<typename Scalar>
EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const QuaternionType& q)
{
Scalar y2 = q.y() * q.y();
m_angles.coeffRef(0) = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2)));
m_angles.coeffRef(1) = std::asin( 2*(q.w()*q.y() - q.z()*q.x()));
m_angles.coeffRef(2) = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z())));
return *this;
}
/** Set \c *this from Euler angles \a ea.
*/
template<typename Scalar>
EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const AngleAxisType& aa)
{
return *this = QuaternionType(aa);
}
/** Set \c *this from the expression \a xpr:
* - if \a xpr is a 3x1 vector, then \a xpr is assumed to be a vector of angles
* - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
* and \a xpr is converted to Euler angles
*/
template<typename Scalar>
template<typename Derived>
EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const MatrixBase<Derived>& other)
{
ei_eulerangles_assign_impl<Derived>::run(*this,other.derived());
return *this;
}
/** Constructs and \returns an equivalent 3x3 rotation matrix.
*/
template<typename Scalar>
typename EulerAngles<Scalar>::Matrix3
EulerAngles<Scalar>::toRotationMatrix(void) const
{
Vector3 c = m_angles.cwiseCos();
Vector3 s = m_angles.cwiseSin();
return Matrix3() <<
c.y()*c.z(), -c.y()*s.z(), s.y(),
c.z()*s.x()*s.y()+c.x()*s.z(), c.x()*c.z()-s.x()*s.y()*s.z(), -c.y()*s.x(),
-c.x()*c.z()*s.y()+s.x()*s.z(), c.z()*s.x()+c.x()*s.y()*s.z(), c.x()*c.y();
}
// set from a rotation matrix
template<typename Other>
struct ei_eulerangles_assign_impl<Other,3,3>
{
typedef typename Other::Scalar Scalar;
inline static void run(EulerAngles<Scalar>& ea, const Other& mat)
{
// mat = cy*cz -cy*sz sy
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
ea.angle(1) = std::asin(mat.coeff(0,2));
ea.angle(0) = std::atan2(-mat.coeff(1,2),mat.coeff(2,2));
ea.angle(2) = std::atan2(-mat.coeff(0,1),mat.coeff(0,0));
}
};
// set from a vector of angles
template<typename Other>
struct ei_eulerangles_assign_impl<Other,3,1>
{
typedef typename Other::Scalar Scalar;
inline static void run(EulerAngles<Scalar>& ea, const Other& vec)
{
ea.coeffs() = vec;
}
};
#endif // EIGEN_EULERANGLES_H

300
Eigen/src/Geometry/Quaternion.h
View File

@ -25,6 +25,11 @@
#ifndef EIGEN_QUATERNION_H
#define EIGEN_QUATERNION_H
template<typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct ei_quaternion_assign_impl;
/** \class Quaternion
*
* \brief The quaternion class used to represent 3D orientations and rotations
@ -39,6 +44,7 @@
* - efficient to compose (28 flops),
* - stable spherical interpolation
*
* \sa class AngleAxis, class EulerAngles, class Transform
*/
template<typename _Scalar>
class Quaternion
@ -53,6 +59,8 @@ public:
typedef Matrix<Scalar,3,1> Vector3;
typedef Matrix<Scalar,3,3> Matrix3;
typedef AngleAxis<Scalar> AngleAxisType;
typedef EulerAngles<Scalar> EulerAnglesType;
inline Scalar x() const { return m_coeffs.coeff(0); }
inline Scalar y() const { return m_coeffs.coeff(1); }
@ -71,10 +79,10 @@ public:
inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
/** \returns a read-only vector expression of the coefficients */
inline const Coefficients& _coeffs() const { return m_coeffs; }
inline const Coefficients& coeffs() const { return m_coeffs; }
/** \returns a vector expression of the coefficients */
inline Coefficients& _coeffs() { return m_coeffs; }
inline Coefficients& coeffs() { return m_coeffs; }
// FIXME what is the prefered order: w x,y,z or x,y,z,w ?
inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0)
@ -88,14 +96,16 @@ public:
/** Copy constructor */
inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
/** This is a special case of the templated operator=. Its purpose is to
* prevent a default operator= from hiding the templated operator=.
*/
inline Quaternion& operator=(const Quaternion& other)
{
m_coeffs = other.m_coeffs;
return *this;
}
explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
explicit inline Quaternion(const EulerAnglesType& ea) { *this = ea; }
template<typename Derived>
explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
Quaternion& operator=(const Quaternion& other);
Quaternion& operator=(const AngleAxisType& aa);
Quaternion& operator=(EulerAnglesType ea);
template<typename Derived>
Quaternion& operator=(const MatrixBase<Derived>& m);
/** \returns a quaternion representing an identity rotation
* \sa MatrixBase::identity()
@ -116,20 +126,10 @@ public:
*/
inline Scalar norm() const { return m_coeffs.norm(); }
template<typename Derived>
Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix3 toRotationMatrix(void) const;
template<typename Derived>
Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis);
void toAngleAxis(Scalar& angle, Vector3& axis) const;
Quaternion& fromEulerAngles(Vector3 eulerAngles);
Vector3 toEulerAngles(void) const;
template<typename Derived1, typename Derived2>
Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
inline Quaternion operator* (const Quaternion& q) const;
inline Quaternion& operator*= (const Quaternion& q);
@ -142,24 +142,6 @@ public:
template<typename Derived>
Vector3 operator* (const MatrixBase<Derived>& vec) const;
protected:
/** Constructor copying the value of the expression \a other */
template<typename OtherDerived>
inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other)
{
m_coeffs = other;
}
/** Copies the value of the expression \a other into \c *this.
*/
template<typename OtherDerived>
inline Quaternion& operator=(const MatrixBase<OtherDerived>& other)
{
m_coeffs = other.derived();
return *this;
}
};
/** \returns the concatenation of two rotations as a quaternion-quaternion product */
@ -203,16 +185,71 @@ Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
return v + this->w() * uv + this->vec().cross(uv);
}
template<typename Scalar>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
{
m_coeffs = other.m_coeffs;
return *this;
}
/** Set \c *this from an angle-axis \a aa
* and returns a reference to \c *this
*/
template<typename Scalar>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
{
Scalar ha = 0.5*aa.angle();
this->w() = ei_cos(ha);
this->vec() = ei_sin(ha) * aa.axis();
return *this;
}
/** Set \c *this from the rotation defined by the Euler angles \a ea,
* and returns a reference to \c *this
*/
template<typename Scalar>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(EulerAnglesType ea)
{
ea.coeffs() *= 0.5;
Vector3 cosines = ea.coeffs().cwiseCos();
Vector3 sines = ea.coeffs().cwiseSin();
Scalar cYcZ = cosines.y() * cosines.z();
Scalar sYsZ = sines.y() * sines.z();
Scalar sYcZ = sines.y() * cosines.z();
Scalar cYsZ = cosines.y() * sines.z();
this->w() = cosines.x() * cYcZ + sines.x() * sYsZ;
this->x() = sines.x() * cYcZ - cosines.x() * sYsZ;
this->y() = cosines.x() * sYcZ + sines.x() * cYsZ;
this->z() = cosines.x() * cYsZ - sines.x() * sYcZ;
return *this;
}
/** Set \c *this from the expression \a xpr:
* - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
* - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
* and \a xpr is converted to a quaternion
*/
template<typename Scalar>
template<typename Derived>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
{
ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
return *this;
}
/** Convert the quaternion to a 3x3 rotation matrix */
template<typename Scalar>
inline typename Quaternion<Scalar>::Matrix3
Quaternion<Scalar>::toRotationMatrix(void) const
{
// FIXME another option would be to declare toRotationMatrix like that:
// OtherDerived& toRotationMatrix(MatrixBase<OtherDerived>& m)
// it would fill m and returns a ref to m.
// the advantages is that way we can accept 4x4 and 3x4 matrices filling the rest of the
// matrix with I... ??
// NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
// if not inlined then the cost of the return by value is huge ~ +35%,
// however, not inlining this function is an order of magnitude slower, so
// it has to be inlined, and so the return by value is not an issue
Matrix3 res;
Scalar tx = 2*this->x();
@ -241,132 +278,13 @@ Quaternion<Scalar>::toRotationMatrix(void) const
return res;
}
/** updates \c *this from the rotation matrix \a m and returns a reference to \c *this
* \warning the size of the input matrix expression \a m must be 3x3 at compile time
*/
template<typename Scalar>
template<typename Derived>
Quaternion<Scalar>& Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
{
// 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 ?
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();
if (t > 0)
{
t = ei_sqrt(t + 1.0);
this->w() = 0.5*t;
t = 0.5/t;
this->x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
this->y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
this->z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
}
else
{
int i = 0;
if (mat.coeff(1,1) > mat.coeff(0,0))
i = 1;
if (mat.coeff(2,2) > mat.coeff(i,i))
i = 2;
int j = (i+1)%3;
int k = (j+1)%3;
t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + 1.0);
m_coeffs.coeffRef(i) = 0.5 * t;
t = 0.5/t;
this->w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
m_coeffs.coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
m_coeffs.coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
}
return *this;
}
/** updates \c *this from the rotation defined by axis \a axis and angle \a angle
* and returns a reference to \c *this
* \warning the size of the input vector expression \a axis must be 3 at compile time
*/
template<typename Scalar>
template<typename Derived>
inline Quaternion<Scalar>& Quaternion<Scalar>
::fromAngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis)
{
ei_assert(Derived::SizeAtCompileTime==3);
Scalar ha = 0.5*angle;
this->w() = ei_cos(ha);
this->vec() = ei_sin(ha) * axis;
return *this;
}
/** Computes and returns the angle and axis of the rotation represented by the quaternion.
* The values are returned in the arguments \a angle and \a axis respectively.
* The returned axis is normalized.
*/
template <typename Scalar>
void Quaternion<Scalar>::toAngleAxis(Scalar& angle, Vector3& axis) const
{
// FIXME should we split this function to an "angle" and an "axis" functions ?
// the drawbacks is that this approach would require to compute twice the norm of (x,y,z)...
// or we returns a Vector4, or a small AngleAxis object... ???
Scalar n2 = this->vec().norm2();
if (ei_isMuchSmallerThan(n2,Scalar(1)))
{
angle = 0;
axis << 1, 0, 0;
}
else
{
angle = 2*std::acos(this->w());
axis = this->vec() / ei_sqrt(n2);
}
}
/** updates \c *this from the rotation defined by the Euler angles \a eulerAngles,
* and returns a reference to \c *this
*/
template <typename Scalar>
Quaternion<Scalar>& Quaternion<Scalar>::fromEulerAngles(Vector3 eulerAngles)
{
// FIXME should the arguments be 3 scalars or a single Vector3 ?
eulerAngles *= 0.5;
Vector3 cosines = eulerAngles.cwiseCos();
Vector3 sines = eulerAngles.cwiseSin();
Scalar cYcZ = cosines.y() * cosines.z();
Scalar sYsZ = sines.y() * sines.z();
Scalar sYcZ = sines.y() * cosines.z();
Scalar cYsZ = cosines.y() * sines.z();
this->w() = cosines.x() * cYcZ + sines.x() * sYsZ;
this->x() = sines.x() * cYcZ - cosines.x() * sYsZ;
this->y() = cosines.x() * sYcZ + sines.x() * cYsZ;
this->z() = cosines.x() * cYsZ - sines.x() * sYcZ;
return *this;
}
/** Computes and returns the Euler angles corresponding to the quaternion \c *this.
*/
template <typename Scalar>
typename Quaternion<Scalar>::Vector3 Quaternion<Scalar>::toEulerAngles(void) const
{
Scalar y2 = this->y() * this->y();
return Vector3(
std::atan2(2*(this->w()*this->x() + this->y()*this->z()), (1 - 2*(this->x()*this->x() + y2))),
std::asin( 2*(this->w()*this->y() - this->z()*this->x())),
std::atan2(2*(this->w()*this->z() + this->x()*this->y()), (1 - 2*(y2 + this->z()*this->z()))));
}
/** Makes a quaternion representing the rotation between two vectors \a a and \a b.
* \returns a reference to the actual quaternion
* Note that the two input vectors have \b not to be normalized.
*/
template<typename Scalar>
template<typename Derived1, typename Derived2>
inline Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
{
Vector3 v0 = a.normalized();
Vector3 v1 = b.normalized();
@ -399,11 +317,11 @@ inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
// FIXME should this funtion be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
Scalar n2 = this->norm2();
if (n2 > 0)
return conjugate()._coeffs() / n2;
return Quaternion(conjugate().coeffs() / n2);
else
{
// return an invalid result to flag the error
return Coefficients::zero();
return Quaternion(Coefficients::zero());
}
}
@ -448,4 +366,54 @@ Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other)
return scale0 * (*this) + scale1 * other;
}
// set from a rotation matrix
template<typename Other>
struct ei_quaternion_assign_impl<Other,3,3>
{
typedef typename Other::Scalar Scalar;
inline static void run(Quaternion<Scalar>& q, const Other& mat)
{
// This algorithm comes from "Quaternion Calculus and Fast Animation",
// Ken Shoemake, 1987 SIGGRAPH course notes
Scalar t = mat.trace();
if (t > 0)
{
t = ei_sqrt(t + 1.0);
q.w() = 0.5*t;
t = 0.5/t;
q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
}
else
{
int i = 0;
if (mat.coeff(1,1) > mat.coeff(0,0))
i = 1;
if (mat.coeff(2,2) > mat.coeff(i,i))
i = 2;
int j = (i+1)%3;
int k = (j+1)%3;
t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + 1.0);
q.coeffs().coeffRef(i) = 0.5 * t;
t = 0.5/t;
q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
}
}
};
// set from a vector of coefficients assumed to be a quaternion
template<typename Other>
struct ei_quaternion_assign_impl<Other,4,1>
{
typedef typename Other::Scalar Scalar;
inline static void run(Quaternion<Scalar>& q, const Other& vec)
{
q.coeffs() = vec;
}
};
#endif // EIGEN_QUATERNION_H

98
Eigen/src/Geometry/Rotation.h
View File

@ -65,7 +65,7 @@ struct ToRotationMatrix<Scalar, 2, OtherScalarType>
{ return Rotation2D<Scalar>(r).toRotationMatrix(); }
};
// 2D rotation to matrix
// 2D rotation to rotation matrix
template<typename Scalar, typename OtherScalarType>
struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> >
{
@ -73,7 +73,7 @@ struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> >
{ return Rotation2D<Scalar>(r).toRotationMatrix(); }
};
// quaternion to matrix
// quaternion to rotation matrix
template<typename Scalar, typename OtherScalarType>
struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> >
{
@ -81,7 +81,7 @@ struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> >
{ return q.toRotationMatrix(); }
};
// angle axis to matrix
// angle axis to rotation matrix
template<typename Scalar, typename OtherScalarType>
struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> >
{
@ -89,6 +89,14 @@ struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> >
{ return aa.toRotationMatrix(); }
};
// euler angles to rotation matrix
template<typename Scalar, typename OtherScalarType>
struct ToRotationMatrix<Scalar, 3, EulerAngles<OtherScalarType> >
{
inline static Matrix<Scalar,3,3> convert(const EulerAngles<OtherScalarType>& ea)
{ return ea.toRotationMatrix(); }
};
// matrix xpr to matrix xpr
template<typename Scalar, int Dim, typename OtherDerived>
struct ToRotationMatrix<Scalar, Dim, MatrixBase<OtherDerived> >
@ -169,88 +177,4 @@ Rotation2D<Scalar>::toRotationMatrix(void) const
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

20
Eigen/src/Geometry/Transform.h
View File

@ -384,20 +384,20 @@ Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDer
template<typename Other, int Dim, int HDim>
struct ei_transform_product_impl<Other,Dim,HDim, HDim,HDim>
{
typedef Transform<typename Other::Scalar,Dim> Transform;
typedef typename Transform::MatrixType MatrixType;
typedef Transform<typename Other::Scalar,Dim> TransformType;
typedef typename TransformType::MatrixType MatrixType;
typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
static ResultType run(const Transform& tr, const Other& other)
static ResultType run(const TransformType& tr, const Other& other)
{ return tr.matrix() * other; }
};
template<typename Other, int Dim, int HDim>
struct ei_transform_product_impl<Other,Dim,HDim, HDim,1>
{
typedef Transform<typename Other::Scalar,Dim> Transform;
typedef typename Transform::MatrixType MatrixType;
typedef Transform<typename Other::Scalar,Dim> TransformType;
typedef typename TransformType::MatrixType MatrixType;
typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
static ResultType run(const Transform& tr, const Other& other)
static ResultType run(const TransformType& tr, const Other& other)
{ return tr.matrix() * other; }
};
@ -405,17 +405,17 @@ template<typename Other, int Dim, int HDim>
struct ei_transform_product_impl<Other,Dim,HDim, Dim,1>
{
typedef typename Other::Scalar Scalar;
typedef Transform<Scalar,Dim> Transform;
typedef typename Transform::AffineMatrixRef MatrixType;
typedef Transform<Scalar,Dim> TransformType;
typedef typename TransformType::AffineMatrixRef MatrixType;
typedef const CwiseUnaryOp<
ei_scalar_multiple_op<Scalar>,
NestByValue<CwiseBinaryOp<
ei_scalar_sum_op<Scalar>,
NestByValue<typename ProductReturnType<NestByValue<MatrixType>,Other>::Type >,
NestByValue<typename Transform::VectorRef> > >
NestByValue<typename TransformType::VectorRef> > >
> ResultType;
// FIXME shall we offer an optimized version when the last row is known to be 0,0...,0,1 ?
static ResultType run(const Transform& tr, const Other& other)
static ResultType run(const TransformType& tr, const Other& other)
{ return ((tr.affine().nestByValue() * other).nestByValue() + tr.translation().nestByValue()).nestByValue()
* (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); }
};

41
test/geometry.cpp
View File

@ -39,6 +39,8 @@ template<typename Scalar> void geometry(void)
typedef Matrix<Scalar,3,1> Vector3;
typedef Matrix<Scalar,4,1> Vector4;
typedef Quaternion<Scalar> Quaternion;
typedef AngleAxis<Scalar> AngleAxis;
typedef EulerAngles<Scalar> EulerAngles;
Quaternion q1, q2;
Vector3 v0 = Vector3::random(),
@ -47,8 +49,8 @@ template<typename Scalar> void geometry(void)
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());
q1 = AngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized());
q2 = AngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized());
// rotation matrix conversion
VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
@ -56,23 +58,23 @@ template<typename Scalar> void geometry(void)
q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
VERIFY_IS_NOT_APPROX(q2 * q1 * v2,
q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
q2.fromRotationMatrix(q1.toRotationMatrix());
q2 = 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);
VERIFY_IS_APPROX(Quaternion(EulerAngles(q1)) * v1, q1 * v1);
EulerAngles ea = q2;
VERIFY_IS_APPROX(EulerAngles(Quaternion(ea)).coeffs(), ea.coeffs());
VERIFY_IS_NOT_APPROX(EulerAngles(Quaternion(EulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))))).coeffs(), 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);
AngleAxis aa = q1;
VERIFY_IS_APPROX(q1 * v1, Quaternion(aa) * v1);
VERIFY_IS_NOT_APPROX(q1 * v1, Quaternion(AngleAxis(aa.angle()*2,aa.axis())) * 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());
VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());
VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized());
// inverse and conjugate
VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
@ -87,15 +89,14 @@ template<typename Scalar> void geometry(void)
VERIFY(m.isOrtho());
// AngleAxis
VERIFY_IS_APPROX(AngleAxis<Scalar>(a,v1.normalized()).toRotationMatrix(),
q2.fromAngleAxis(a,v1.normalized()).toRotationMatrix());
VERIFY_IS_APPROX(AngleAxis(a,v1.normalized()).toRotationMatrix(),
Quaternion(AngleAxis(a,v1.normalized())).toRotationMatrix());
AngleAxis<Scalar> aa1;
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());
aa1 = m;
VERIFY_IS_APPROX(AngleAxis(m).toRotationMatrix(),
Quaternion(m).toRotationMatrix());
// Transform
@ -106,7 +107,7 @@ template<typename Scalar> void geometry(void)
a = 0;
while (ei_abs(a)<0.1)
a = ei_random<Scalar>(-0.4*M_PI, 0.4*M_PI);
q1.fromAngleAxis(a, v0.normalized());
q1 = AngleAxis(a, v0.normalized());
Transform3 t0, t1, t2;
t0.setIdentity();
t0.affine() = q1.toRotationMatrix();