devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added MatrixBase::Unit*() static function to easily create unit/basis vectors.

Browse Source 
Removed EulerAngles, addes typdefs for Quaternion and AngleAxis,
and added automatic conversions from Quaternion/AngleAxis to Matrix3 such that:
 Matrix3f m = AngleAxisf(0.2,Vector3f::UnitX) * AngleAxisf(0.2,Vector3f::UnitY);
just works.
This commit is contained in:
Gael Guennebaud
2008-07-19 13:03:23 +00:00
parent 7245c63067
commit 05ad083467
11 changed files with 154 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
Eigen/src/Geometry/AngleAxis.h
View File

@@ -31,6 +31,10 @@
*
* \param _Scalar the scalar type, i.e., the type of the coefficients.
*
* The following two typedefs are provided for convenience:
* \li \c AngleAxisf for \c float
* \li \c AngleAxisd for \c double
*
* \sa class Quaternion, class EulerAngles, class Transform
*/
template<typename _Scalar>
@@ -43,7 +47,6 @@ public:
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Quaternion<Scalar> QuaternionType;
typedef EulerAngles<Scalar> EulerAnglesType;
protected:
@@ -56,7 +59,6 @@ public:
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; }
@@ -66,8 +68,26 @@ public:
const Vector3& axis() const { return m_axis; }
Vector3& axis() { return m_axis; }
operator Matrix3 () const { return toRotationMatrix(); }
inline QuaternionType operator* (const AngleAxis& other) const
{ return QuaternionType(*this) * QuaternionType(other); }
inline QuaternionType operator* (const QuaternionType& other) const
{ return QuaternionType(*this) * other; }
friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
{ return a * QuaternionType(b); }
inline typename ProductReturnType<Matrix3,Matrix3>::Type
operator* (const Matrix3& other) const
{ return toRotationMatrix() * other; }
inline friend typename ProductReturnType<Matrix3,Matrix3>::Type
operator* (const Matrix3& a, const AngleAxis& b)
{ return a * b.toRotationMatrix(); }
AngleAxis& operator=(const QuaternionType& q);
AngleAxis& operator=(const EulerAnglesType& ea);
template<typename Derived>
AngleAxis& operator=(const MatrixBase<Derived>& m);
@@ -76,6 +96,9 @@ public:
Matrix3 toRotationMatrix(void) const;
};
typedef AngleAxis<float> AngleAxisf;
typedef AngleAxis<double> AngleAxisd;
/** Set \c *this from a quaternion.
* The axis is normalized.
*/
@@ -96,14 +119,6 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
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>

157
Eigen/src/Geometry/EulerAngles.h
View File

@@ -1,157 +0,0 @@
// 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() {}
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.cwise().cos();
Vector3 s = m_angles.cwise().sin();
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

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

@@ -40,9 +40,13 @@ struct ei_quaternion_assign_impl;
* orientations and rotations of objects in three dimensions. Compared to other
* representations like Euler angles or 3x3 matrices, quatertions offer the
* following advantages:
* - compact storage (4 scalars)
* - efficient to compose (28 flops),
* - stable spherical interpolation
* \li \c compact storage (4 scalars)
* \li \c efficient to compose (28 flops),
* \li \c stable spherical interpolation
*
* The following two typedefs are provided for convenience:
* \li \c Quaternionf for \c float
* \li \c Quaterniond for \c double
*
* \sa class AngleAxis, class EulerAngles, class Transform
*/
@@ -60,7 +64,6 @@ 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); }
@@ -97,16 +100,16 @@ public:
inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
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);
operator Matrix3 () const { return toRotationMatrix(); }
/** \returns a quaternion representing an identity rotation
* \sa MatrixBase::identity()
*/
@@ -144,6 +147,9 @@ public:
};
typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;
/** \returns the concatenation of two rotations as a quaternion-quaternion product */
template <typename Scalar>
inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
@@ -204,30 +210,6 @@ inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa
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().cwise().cos();
Vector3 sines = ea.coeffs().cwise().sin();
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

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

@@ -89,14 +89,6 @@ 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> >