diff options
Diffstat (limited to 'Eigen')
-rw-r--r-- | Eigen/src/Core/MatrixBase.h | 7 | ||||
-rw-r--r-- | Eigen/src/Core/util/ForwardDeclarations.h | 2 | ||||
-rw-r--r-- | Eigen/src/Geometry/Quaternion.h | 60 |
3 files changed, 38 insertions, 31 deletions
diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index f02aac36a..1ae2c4562 100644 --- a/Eigen/src/Core/MatrixBase.h +++ b/Eigen/src/Core/MatrixBase.h @@ -555,10 +555,15 @@ template<typename Derived> class MatrixBase : public ArrayBase<Derived> /////////// QR module /////////// const QR<typename ei_eval<Derived>::type> qr() const; - + EigenvaluesReturnType eigenvalues() const; RealScalar matrixNorm() const; +/////////// Geometry module /////////// + + template<typename OtherDerived> + const Cross<Derived,OtherDerived> cross(const MatrixBase<OtherDerived>& other) const; + }; #endif // EIGEN_MATRIXBASE_H diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h index a9974c38b..f9370ada9 100644 --- a/Eigen/src/Core/util/ForwardDeclarations.h +++ b/Eigen/src/Core/util/ForwardDeclarations.h @@ -51,6 +51,8 @@ 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 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> struct ei_scalar_sum_op; diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 4df490ea3..33c5498db 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -89,10 +89,10 @@ public: // 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) { - m_data[0] = _x; - m_data[1] = _y; - m_data[2] = _z; - m_data[3] = _w; + m_data[0] = x; + m_data[1] = y; + m_data[2] = z; + m_data[3] = w; } /** Constructor copying the value of the expression \a other */ @@ -126,8 +126,10 @@ public: Matrix3 toRotationMatrix(void) const; template<typename Derived> void fromRotationMatrix(const MatrixBase<Derived>& m); + template<typename Derived> - void fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis); + Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis); + template<typename Derived1, typename Derived2> Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); @@ -158,10 +160,10 @@ inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other { return Quaternion ( - this->w() * other.w() - this->x() * other.x() - this->y() * other.y() - this->z() * rkQ.z(), - this->w() * other.x() + this->x() * other.w() + this->y() * other.z() - this->z() * rkQ.y(), - this->w() * other.y() + this->y() * other.w() + this->z() * other.x() - this->x() * rkQ.z(), - this->w() * other.z() + this->z() * other.w() + this->x() * other.y() - this->y() * rkQ.x() + this->w() * other.w() - this->x() * other.x() - this->y() * other.y() - this->z() * other.z(), + this->w() * other.x() + this->x() * other.w() + this->y() * other.z() - this->z() * other.y(), + this->w() * other.y() + this->y() * other.w() + this->z() * other.x() - this->x() * other.z(), + this->w() * other.z() + this->z() * other.w() + this->x() * other.y() - this->y() * other.x() ); } @@ -172,8 +174,9 @@ inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& oth } template <typename Scalar> +template<typename Derived> inline typename Quaternion<Scalar>::Vector3 -Quaternion<Scalar>::operator* (const Vector3& v) const +Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const { // Note that this algorithm comes from the optimization by hand // of the conversion to a Matrix followed by a Matrix/Vector product. @@ -181,8 +184,8 @@ Quaternion<Scalar>::operator* (const Vector3& v) const // in the litterature (30 versus 39 flops). On the other hand it // requires two Vector3 as temporaries. Vector3 uv; - uv = 2 * start<3>().cross(v); - return v + this->w() * uv + start<3>().cross(uv); + uv = 2 * this->template start<3>().cross(v); + return v + this->w() * uv + this->template start<3>().cross(uv); } template<typename Scalar> @@ -205,9 +208,9 @@ Quaternion<Scalar>::toRotationMatrix(void) const Scalar tzz = tz*this->z(); res(0,0) = 1-(tyy+tzz); - res(0,1) = fTxy-twz; - res(0,2) = fTxz+twy; - res(1,0) = fTxy+twz; + 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; @@ -255,11 +258,13 @@ void Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& m) template<typename Scalar> template<typename Derived> -inline void Quaternion<Scalar>::fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis) +inline Quaternion<Scalar>& Quaternion<Scalar> +::fromAngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) { Scalar ha = 0.5*angle; this->w() = ei_cos(ha); - this->start<3>() = ei_sin(ha) * axis; + this->template start<3>() = ei_sin(ha) * axis; + return *this; } /** Makes a quaternion representing the rotation between two vectors \a a and \a b. @@ -268,26 +273,22 @@ inline void Quaternion<Scalar>::fromAngleAxis (const Scalar& angle, const Matrix */ template<typename Scalar> template<typename Derived1, typename Derived2> -Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) +inline Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) { Vector3 v0 = a.normalized(); - Vector3 v1 = a.normalized(); - Vector3 c = v0.cross(v1); - - // if the magnitude of the cross product approaches zero, - // we get unstable because ANY axis will do when a == +/- b - Scalar d = v0.dot(v1); + Vector3 v1 = b.normalized(); + Vector3 axis = v0.cross(v1); + Scalar c = v0.dot(v1); // if dot == 1, vectors are the same - if (ei_isApprox(d,1)) + if (ei_isApprox(c,Scalar(1))) { // set to identity - this->w() = 1; this->start<3>().setZero(); + this->w() = 1; this->template start<3>().setZero(); } - Scalar s = ei_sqrt((1+d)*2); + Scalar s = ei_sqrt((1+c)*2); Scalar invs = 1./s; - - this->start<3>() = c * invs; + this->template start<3>() = axis * invs; this->w() = s * 0.5; return *this; @@ -299,7 +300,6 @@ inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const Scalar n2 = this->norm2(); if (n2 > 0) return (*this) / norm; - } else { // return an invalid result to flag the error |