aboutsummaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Eigen/Geometry2
-rw-r--r--Eigen/src/Core/util/ForwardDeclarations.h13
-rw-r--r--Eigen/src/Geometry/AngleAxis.h147
-rw-r--r--Eigen/src/Geometry/EulerAngles.h158
-rw-r--r--Eigen/src/Geometry/Quaternion.h296
-rw-r--r--Eigen/src/Geometry/Rotation.h98
-rw-r--r--Eigen/src/Geometry/Transform.h20
-rw-r--r--test/geometry.cpp41
8 files changed, 489 insertions, 286 deletions
diff --git a/Eigen/Geometry b/Eigen/Geometry
index 1452e9752..2dd44fb0b 100644
--- a/Eigen/Geometry
+++ b/Eigen/Geometry
@@ -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"
diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h
index 96949a12a..ea6478a04 100644
--- a/Eigen/src/Core/util/ForwardDeclarations.h
+++ b/Eigen/src/Core/util/ForwardDeclarations.h
@@ -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
diff --git a/Eigen/src/Geometry/AngleAxis.h b/Eigen/src/Geometry/AngleAxis.h
new file mode 100644
index 000000000..2b0ee6196
--- /dev/null
+++ b/Eigen/src/Geometry/AngleAxis.h
@@ -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
diff --git a/Eigen/src/Geometry/EulerAngles.h b/Eigen/src/Geometry/EulerAngles.h
new file mode 100644
index 000000000..1e12e50cc
--- /dev/null
+++ b/Eigen/src/Geometry/EulerAngles.h
@@ -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
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 354d23e89..f08f82151 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -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,137 +185,35 @@ Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
return v + this->w() * uv + this->vec().cross(uv);
}
-/** 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... ??
- Matrix3 res;
-
- Scalar tx = 2*this->x();
- Scalar ty = 2*this->y();
- Scalar tz = 2*this->z();
- Scalar twx = tx*this->w();
- Scalar twy = ty*this->w();
- Scalar twz = tz*this->w();
- Scalar txx = tx*this->x();
- Scalar txy = ty*this->x();
- Scalar txz = tz*this->x();
- Scalar tyy = ty*this->y();
- Scalar tyz = tz*this->y();
- Scalar tzz = tz*this->z();
-
- 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;
-}
-
-/** 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)
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
{
- // 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;
- }
-
+ m_coeffs = other.m_coeffs;
return *this;
}
-/** updates \c *this from the rotation defined by axis \a axis and angle \a angle
+/** Set \c *this from an angle-axis \a aa
* 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)
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
{
- ei_assert(Derived::SizeAtCompileTime==3);
- Scalar ha = 0.5*angle;
+ Scalar ha = 0.5*aa.angle();
this->w() = ei_cos(ha);
- this->vec() = ei_sin(ha) * axis;
+ this->vec() = ei_sin(ha) * aa.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,
+/** Set \c *this from the rotation defined by the Euler angles \a ea,
* and returns a reference to \c *this
*/
-template <typename Scalar>
-Quaternion<Scalar>& Quaternion<Scalar>::fromEulerAngles(Vector3 eulerAngles)
+template<typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(EulerAnglesType ea)
{
- // FIXME should the arguments be 3 scalars or a single Vector3 ?
- eulerAngles *= 0.5;
+ ea.coeffs() *= 0.5;
- Vector3 cosines = eulerAngles.cwiseCos();
- Vector3 sines = eulerAngles.cwiseSin();
+ Vector3 cosines = ea.coeffs().cwiseCos();
+ Vector3 sines = ea.coeffs().cwiseSin();
Scalar cYcZ = cosines.y() * cosines.z();
Scalar sYsZ = sines.y() * sines.z();
@@ -348,16 +228,54 @@ Quaternion<Scalar>& Quaternion<Scalar>::fromEulerAngles(Vector3 eulerAngles)
return *this;
}
-/** Computes and returns the Euler angles corresponding to the quaternion \c *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>
-typename Quaternion<Scalar>::Vector3 Quaternion<Scalar>::toEulerAngles(void) const
+template<typename Scalar>
+template<typename Derived>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
{
- 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()))));
+ 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
+{
+ // 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();
+ Scalar ty = 2*this->y();
+ Scalar tz = 2*this->z();
+ Scalar twx = tx*this->w();
+ Scalar twy = ty*this->w();
+ Scalar twz = tz*this->w();
+ Scalar txx = tx*this->x();
+ Scalar txy = ty*this->x();
+ Scalar txz = tz*this->x();
+ Scalar tyy = ty*this->y();
+ Scalar tyz = tz*this->y();
+ Scalar tzz = tz*this->z();
+
+ 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;
}
/** Makes a quaternion representing the rotation between two vectors \a a and \a b.
@@ -366,7 +284,7 @@ typename Quaternion<Scalar>::Vector3 Quaternion<Scalar>::toEulerAngles(void) con
*/
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
diff --git a/Eigen/src/Geometry/Rotation.h b/Eigen/src/Geometry/Rotation.h
index 7e07b48f5..ba533b9fc 100644
--- a/Eigen/src/Geometry/Rotation.h
+++ b/Eigen/src/Geometry/Rotation.h
@@ -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
diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h
index 5d092951e..ed4e6f200 100644
--- a/Eigen/src/Geometry/Transform.h
+++ b/Eigen/src/Geometry/Transform.h
@@ -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))); }
};
diff --git a/test/geometry.cpp b/test/geometry.cpp
index c4d8326a8..bf3268745 100644
--- a/test/geometry.cpp
+++ b/test/geometry.cpp
@@ -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();
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-07 02:17:49 +0000