From a9cf229e150f0a93a62376cddf030995171fd5d6 Mon Sep 17 00:00:00 2001 From: Gael Guennebaud Date: Tue, 3 Jun 2008 07:32:12 +0000 Subject: [PATCH] add a geometry unit test and fix a couple of typo in Quaternion.h --- Eigen/src/Core/MatrixBase.h | 7 ++- Eigen/src/Core/util/ForwardDeclarations.h | 2 + Eigen/src/Geometry/Quaternion.h | 60 +++++++++--------- test/CMakeLists.txt | 1 + test/geometry.cpp | 74 +++++++++++++++++++++++ test/main.h | 2 +- 6 files changed, 114 insertions(+), 32 deletions(-) create mode 100644 test/geometry.cpp 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 class MatrixBase : public ArrayBase /////////// QR module /////////// const QR::type> qr() const; - + EigenvaluesReturnType eigenvalues() const; RealScalar matrixNorm() const; +/////////// Geometry module /////////// + + template + const Cross cross(const MatrixBase& 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 class PartialRedu template class Part; template class Extract; template::Flags) & ArrayBit> class ArrayBase {}; +template class Cross; +template class Quaternion; template 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 void fromRotationMatrix(const MatrixBase& m); + template - void fromAngleAxis (const Scalar& angle, const MatrixBase& axis); + Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase& axis); + template Quaternion& fromTwoVectors(const MatrixBase& a, const MatrixBase& b); @@ -158,10 +160,10 @@ inline Quaternion Quaternion::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& Quaternion::operator*= (const Quaternion& oth } template +template inline typename Quaternion::Vector3 -Quaternion::operator* (const Vector3& v) const +Quaternion::operator* (const MatrixBase& 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::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 @@ -205,9 +208,9 @@ Quaternion::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::fromRotationMatrix(const MatrixBase& m) template template -inline void Quaternion::fromAngleAxis (const Scalar& angle, const MatrixBase& axis) +inline Quaternion& Quaternion +::fromAngleAxis(const Scalar& angle, const MatrixBase& 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::fromAngleAxis (const Scalar& angle, const Matrix */ template template -Quaternion& Quaternion::fromTwoVectors(const MatrixBase& a, const MatrixBase& b) +inline Quaternion& Quaternion::fromTwoVectors(const MatrixBase& a, const MatrixBase& 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 Quaternion::inverse() const Scalar n2 = this->norm2(); if (n2 > 0) return (*this) / norm; - } else { // return an invalid result to flag the error diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c53f55c5c..92d13c8dd 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -85,5 +85,6 @@ EI_ADD_TEST(cholesky) EI_ADD_TEST(inverse) EI_ADD_TEST(qr) EI_ADD_TEST(eigensolver) +EI_ADD_TEST(geometry) ENDIF(BUILD_TESTS) diff --git a/test/geometry.cpp b/test/geometry.cpp new file mode 100644 index 000000000..efdff969f --- /dev/null +++ b/test/geometry.cpp @@ -0,0 +1,74 @@ +// 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 +// +// 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 . + +#include "main.h" +#include + +template void geometry(void) +{ + /* this test covers the following files: + Cross.h Quaternion.h + */ + + typedef Matrix Matrix3; + typedef Matrix Matrix4; + typedef Matrix Vector3; + typedef Matrix Vector4; + typedef Quaternion Quaternion; + + Quaternion q1, q2, q3; + Vector3 v0 = Vector3::random(), + v1 = Vector3::random(), + v2 = Vector3::random(); + + q1.fromAngleAxis(ei_random(-M_PI, M_PI), v0.normalized()); + q2.fromAngleAxis(ei_random(-M_PI, M_PI), v1.normalized()); + + 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); + + VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized()); + + VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1)); + + Matrix3 m; + m << v0.normalized(), + (v0.cross(v1)).normalized(), + (v0.cross(v1).cross(v0)).normalized(); + VERIFY(m.isOrtho()); +} + +void test_geometry() +{ + for(int i = 0; i < g_repeat; i++) { +// CALL_SUBTEST( geometry() ); + CALL_SUBTEST( geometry() ); + } +} diff --git a/test/main.h b/test/main.h index 4a5af42c3..b19c19545 100644 --- a/test/main.h +++ b/test/main.h @@ -165,7 +165,7 @@ namespace Eigen { template inline typename NumTraits::Real test_precision(); template<> inline int test_precision() { return 0; } -template<> inline float test_precision() { return 1e-2f; } +template<> inline float test_precision() { return 1e-3f; } template<> inline double test_precision() { return 1e-5; } template<> inline float test_precision >() { return test_precision(); } template<> inline double test_precision >() { return test_precision(); }