GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-14 04:35:57 +08:00
Code Issues Packages Projects Releases Wiki Activity

Started a Transform class in the Geometry module to represent

Browse Source 
homography.
Fix indentation in Quaternion.h
This commit is contained in:
Gael Guennebaud 2008-06-15 08:33:44 +00:00
parent 4af7089ab8
commit fbbd8afe30
7 changed files with 385 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Eigen/Geometry
View File

@ -7,6 +7,7 @@ namespace Eigen {
#include "src/Geometry/Cross.h" #include "src/Geometry/Cross.h"
#include "src/Geometry/Quaternion.h" #include "src/Geometry/Quaternion.h"
#include "src/Geometry/Transform.h"
// the Geometry module use cwiseCos and cwiseSin which are defined in the Array module // the Geometry module use cwiseCos and cwiseSin which are defined in the Array module
#include "src/Array/CwiseOperators.h" #include "src/Array/CwiseOperators.h"

6
Eigen/src/Core/Product.h
View File

@ -269,6 +269,10 @@ template<typename Lhs, typename Rhs, int EvalMode> class Product : ei_no_assignm
{ {
return m_lhs.coeff(row, col) * m_rhs.coeff(col, col); return m_lhs.coeff(row, col) * m_rhs.coeff(col, col);
} }
else if ((Lhs::Flags&Diagonal)==Diagonal)
{
return m_lhs.coeff(row, row) * m_rhs.coeff(row, col);
}
else else
{ {
Scalar res; Scalar res;
@ -286,7 +290,7 @@ template<typename Lhs, typename Rhs, int EvalMode> class Product : ei_no_assignm
{ {
if ((Rhs::Flags&Diagonal)==Diagonal) if ((Rhs::Flags&Diagonal)==Diagonal)
{ {
assert((_LhsNested::Flags&RowMajorBit)==0); ei_assert((_LhsNested::Flags&RowMajorBit)==0);
return ei_pmul(m_lhs.template packetCoeff<LoadMode>(row, col), ei_pset1(m_rhs.coeff(col, col))); return ei_pmul(m_lhs.template packetCoeff<LoadMode>(row, col), ei_pset1(m_rhs.coeff(col, col)));
} }
else else

3
Eigen/src/Core/util/StaticAssert.h
View File

@ -57,7 +57,8 @@
enum { enum {
you_tried_calling_a_vector_method_on_a_matrix, you_tried_calling_a_vector_method_on_a_matrix,
you_mixed_vectors_of_different_sizes, you_mixed_vectors_of_different_sizes,
you_mixed_matrices_of_different_sizes you_mixed_matrices_of_different_sizes,
you_did_a_programming_error
}; };
}; };

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

@ -43,122 +43,122 @@
template<typename _Scalar> template<typename _Scalar>
class Quaternion class Quaternion
{ {
typedef Matrix<_Scalar, 4, 1> Coefficients; typedef Matrix<_Scalar, 4, 1> Coefficients;
Coefficients m_coeffs; Coefficients m_coeffs;
public: public:
/** the scalar type of the coefficients */ /** the scalar type of the coefficients */
typedef _Scalar Scalar; typedef _Scalar Scalar;
typedef Matrix<Scalar,3,1> Vector3; typedef Matrix<Scalar,3,1> Vector3;
typedef Matrix<Scalar,3,3> Matrix3; typedef Matrix<Scalar,3,3> Matrix3;
inline Scalar x() const { return m_coeffs.coeff(0); } inline Scalar x() const { return m_coeffs.coeff(0); }
inline Scalar y() const { return m_coeffs.coeff(1); } inline Scalar y() const { return m_coeffs.coeff(1); }
inline Scalar z() const { return m_coeffs.coeff(2); } inline Scalar z() const { return m_coeffs.coeff(2); }
inline Scalar w() const { return m_coeffs.coeff(3); } inline Scalar w() const { return m_coeffs.coeff(3); }
inline Scalar& x() { return m_coeffs.coeffRef(0); } inline Scalar& x() { return m_coeffs.coeffRef(0); }
inline Scalar& y() { return m_coeffs.coeffRef(1); } inline Scalar& y() { return m_coeffs.coeffRef(1); }
inline Scalar& z() { return m_coeffs.coeffRef(2); } inline Scalar& z() { return m_coeffs.coeffRef(2); }
inline Scalar& w() { return m_coeffs.coeffRef(3); } inline Scalar& w() { return m_coeffs.coeffRef(3); }
/** \returns a read-only vector expression of the imaginary part (x,y,z) */ /** \returns a read-only vector expression of the imaginary part (x,y,z) */
inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); } inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
/** \returns a vector expression of the imaginary part (x,y,z) */ /** \returns a vector expression of the imaginary part (x,y,z) */
inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); } inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
/** \returns a read-only vector expression of the coefficients */ /** \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 */ /** \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 ? // 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) inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0)
{ {
m_coeffs.coeffRef(0) = x; m_coeffs.coeffRef(0) = x;
m_coeffs.coeffRef(1) = y; m_coeffs.coeffRef(1) = y;
m_coeffs.coeffRef(2) = z; m_coeffs.coeffRef(2) = z;
m_coeffs.coeffRef(3) = w; m_coeffs.coeffRef(3) = w;
} }
/** Copy constructor */ /** Copy constructor */
inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; } inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
/** This is a special case of the templated operator=. Its purpose is to /** This is a special case of the templated operator=. Its purpose is to
* prevent a default operator= from hiding the templated operator=. * prevent a default operator= from hiding the templated operator=.
*/ */
inline Quaternion& operator=(const Quaternion& other) inline Quaternion& operator=(const Quaternion& other)
{ {
m_coeffs = other.m_coeffs; m_coeffs = other.m_coeffs;
return *this; return *this;
} }
/** \returns a quaternion representing an identity rotation /** \returns a quaternion representing an identity rotation
* \sa MatrixBase::identity() * \sa MatrixBase::identity()
*/ */
inline static Quaternion identity() { return Quaternion(1, 0, 0, 0); } inline static Quaternion identity() { return Quaternion(1, 0, 0, 0); }
/** \sa Quaternion::identity(), MatrixBase::setIdentity() /** \sa Quaternion::identity(), MatrixBase::setIdentity()
*/ */
inline Quaternion& setIdentity() { m_coeffs << 1, 0, 0, 0; return *this; } inline Quaternion& setIdentity() { m_coeffs << 1, 0, 0, 0; return *this; }
/** \returns the squared norm of the quaternion's coefficients /** \returns the squared norm of the quaternion's coefficients
* \sa Quaternion::norm(), MatrixBase::norm2() * \sa Quaternion::norm(), MatrixBase::norm2()
*/ */
inline Scalar norm2() const { return m_coeffs.norm2(); } inline Scalar norm2() const { return m_coeffs.norm2(); }
/** \returns the norm of the quaternion's coefficients /** \returns the norm of the quaternion's coefficients
* \sa Quaternion::norm2(), MatrixBase::norm() * \sa Quaternion::norm2(), MatrixBase::norm()
*/ */
inline Scalar norm() const { return m_coeffs.norm(); } inline Scalar norm() const { return m_coeffs.norm(); }
template<typename Derived> template<typename Derived>
Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m); Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix3 toRotationMatrix(void) const; Matrix3 toRotationMatrix(void) const;
template<typename Derived> template<typename Derived>
Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis); Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis);
void toAngleAxis(Scalar& angle, Vector3& axis) const; void toAngleAxis(Scalar& angle, Vector3& axis) const;
Quaternion& fromEulerAngles(Vector3 eulerAngles); Quaternion& fromEulerAngles(Vector3 eulerAngles);
Vector3 toEulerAngles(void) const; Vector3 toEulerAngles(void) const;
template<typename Derived1, typename Derived2> template<typename Derived1, typename Derived2>
Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
inline Quaternion operator* (const Quaternion& q) const; inline Quaternion operator* (const Quaternion& q) const;
inline Quaternion& operator*= (const Quaternion& q); inline Quaternion& operator*= (const Quaternion& q);
Quaternion inverse(void) const; Quaternion inverse(void) const;
Quaternion conjugate(void) const; Quaternion conjugate(void) const;
Quaternion slerp(Scalar t, const Quaternion& other) const; Quaternion slerp(Scalar t, const Quaternion& other) const;
template<typename Derived> template<typename Derived>
Vector3 operator* (const MatrixBase<Derived>& vec) const; Vector3 operator* (const MatrixBase<Derived>& vec) const;
protected: protected:
/** Constructor copying the value of the expression \a other */ /** Constructor copying the value of the expression \a other */
template<typename OtherDerived> template<typename OtherDerived>
inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other) inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other)
{ {
m_coeffs = other; m_coeffs = other;
} }
/** Copies the value of the expression \a other into \c *this. /** Copies the value of the expression \a other into \c *this.
*/ */
template<typename OtherDerived> template<typename OtherDerived>
inline Quaternion& operator=(const MatrixBase<OtherDerived>& other) inline Quaternion& operator=(const MatrixBase<OtherDerived>& other)
{ {
m_coeffs = other.derived(); m_coeffs = other.derived();
return *this; return *this;
} }
}; };

227
Eigen/src/Geometry/Transform.h Normal file
View File

@ -0,0 +1,227 @@
// 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_TRANSFORM_H
#define EIGEN_TRANSFORM_H
/** \class Transform
*
* \brief Represents an homogeneous transformation in a N dimensional space
*
* \param _Scalar the scalar type, i.e., the type of the coefficients
* \param _Dim the dimension of the space
*
*
*/
template<typename _Scalar, int _Dim>
class Transform
{
public:
enum { Dim = _Dim, HDim = _Dim+1 };
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
typedef Matrix<Scalar,HDim,HDim> MatrixType;
typedef Matrix<Scalar,Dim,Dim> AffineMatrixType;
typedef Block<MatrixType,Dim,Dim> AffineMatrixRef;
typedef Matrix<Scalar,Dim,1> VectorType;
typedef Block<MatrixType,Dim,1> VectorRef;
protected:
MatrixType m_matrix;
template<typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct ei_transform_product_impl;
public:
inline const MatrixType matrix() const { return m_matrix; }
inline MatrixType matrix() { return m_matrix; }
inline const AffineMatrixRef affine() const { return m_matrix.template block<Dim,Dim>(0,0); }
inline AffineMatrixRef affine() { return m_matrix.template block<Dim,Dim>(0,0); }
inline const VectorRef translation() const { return m_matrix.template block<Dim,1>(0,Dim); }
inline VectorRef translation() { return m_matrix.template block<Dim,1>(0,Dim); }
template<typename OtherDerived>
struct ProductReturnType
{
typedef typename ei_transform_product_impl<OtherDerived>::ResultType Type;
};
template<typename OtherDerived>
const typename ProductReturnType<OtherDerived>::Type
operator * (const MatrixBase<OtherDerived> &other) const;
void setIdentity() { m_matrix.setIdentity(); }
template<typename OtherDerived>
Transform& scale(const MatrixBase<OtherDerived> &other);
template<typename OtherDerived>
Transform& prescale(const MatrixBase<OtherDerived> &other);
template<typename OtherDerived>
Transform& translate(const MatrixBase<OtherDerived> &other);
template<typename OtherDerived>
Transform& pretranslate(const MatrixBase<OtherDerived> &other);
AffineMatrixType extractRotation() const;
AffineMatrixType extractRotationNoShear() const;
protected:
};
template<typename Scalar, int Dim>
template<typename OtherDerived>
const typename Transform<Scalar,Dim>::template ProductReturnType<OtherDerived>::Type
Transform<Scalar,Dim>::operator*(const MatrixBase<OtherDerived> &other) const
{
return ei_transform_product_impl<OtherDerived>::run(*this,other.derived());
}
/** Applies on the right the non uniform scale transformation represented
* by the vector \a other to \c *this and returns a reference to \c *this.
* \sa prescale()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::scale(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime)
&& int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error);
affine() = (affine() * other.asDiagonal()).lazy();
return *this;
}
/** Applies on the left the non uniform scale transformation represented
* by the vector \a other to \c *this and returns a reference to \c *this.
* \sa scale()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime)
&& int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error);
m_matrix.template block<3,4>(0,0) = (other.asDiagonal().eval() * m_matrix.template block<3,4>(0,0)).lazy();
return *this;
}
/** Applies on the right translation matrix represented by the vector \a other
* to \c *this and returns a reference to \c *this.
* \sa pretranslate()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime)
&& int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error);
translation() += affine() * other;
return *this;
}
/** Applies on the left translation matrix represented by the vector \a other
* to \c *this and returns a reference to \c *this.
* \sa translate()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime)
&& int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error);
translation() += other;
return *this;
}
/** \returns the rotation part of the transformation using a QR decomposition.
* \sa extractRotationNoShear()
*/
template<typename Scalar, int Dim>
typename Transform<Scalar,Dim>::AffineMatrixType
Transform<Scalar,Dim>::extractRotation() const
{
return affine().qr().matrixQ();
}
/** \returns the rotation part of the transformation assuming no shear in
* the affine part.
* \sa extractRotation()
*/
template<typename Scalar, int Dim>
typename Transform<Scalar,Dim>::AffineMatrixType
Transform<Scalar,Dim>::extractRotationNoShear() const
{
return affine().cwiseAbs2()
.verticalRedux(ei_scalar_sum_op<Scalar>()).cwiseSqrt();
}
//----------
template<typename Scalar, int Dim>
template<typename Other>
struct Transform<Scalar,Dim>::ei_transform_product_impl<Other,Dim+1,Dim+1>
{
typedef typename Transform<Scalar,Dim>::MatrixType MatrixType;
typedef Product<MatrixType,Other> ResultType;
static ResultType run(const Transform<Scalar,Dim>& tr, const Other& other)
{ return tr.matrix() * other; }
};
template<typename Scalar, int Dim>
template<typename Other>
struct Transform<Scalar,Dim>::ei_transform_product_impl<Other,Dim+1,1>
{
typedef typename Transform<Scalar,Dim>::MatrixType MatrixType;
typedef Product<MatrixType,Other> ResultType;
static ResultType run(const Transform<Scalar,Dim>& tr, const Other& other)
{ return tr.matrix() * other; }
};
template<typename Scalar, int Dim>
template<typename Other>
struct Transform<Scalar,Dim>::ei_transform_product_impl<Other,Dim,1>
{
typedef typename Transform<Scalar,Dim>::AffineMatrixRef MatrixType;
typedef const CwiseBinaryOp<
ei_scalar_sum_op<Scalar>,
NestByValue<Product<NestByValue<MatrixType>,Other> >,
NestByValue<typename Transform<Scalar,Dim>::VectorRef> > ResultType;
static ResultType run(const Transform<Scalar,Dim>& tr, const Other& other)
{ return (tr.affine().nestByValue() * other).nestByValue() + tr.translation().nestByValue(); }
};
#endif // EIGEN_TRANSFORM_H

1
test/CMakeLists.txt
View File

@ -5,6 +5,7 @@ IF(CMAKE_COMPILER_IS_GNUCXX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O1 -g1") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O1 -g1")
SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} -O2 -g2") SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} -O2 -g2")
SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -fno-inline-functions") SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -fno-inline-functions")
SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0 -g2")
ENDIF(CMAKE_SYSTEM_NAME MATCHES Linux) ENDIF(CMAKE_SYSTEM_NAME MATCHES Linux)
ENDIF(CMAKE_COMPILER_IS_GNUCXX) ENDIF(CMAKE_COMPILER_IS_GNUCXX)

90
test/geometry.cpp
View File

@ -28,7 +28,7 @@
template<typename Scalar> void geometry(void) template<typename Scalar> void geometry(void)
{ {
/* this test covers the following files: /* this test covers the following files:
Cross.h Quaternion.h Cross.h Quaternion.h, Transform.cpp
*/ */
typedef Matrix<Scalar,3,3> Matrix3; typedef Matrix<Scalar,3,3> Matrix3;
@ -48,32 +48,32 @@ template<typename Scalar> void geometry(void)
q2.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized()); q2.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized());
// rotation matrix conversion // rotation matrix conversion
VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2); // VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
VERIFY_IS_APPROX(q1 * q2 * v2, // VERIFY_IS_APPROX(q1 * q2 * v2,
q1.toRotationMatrix() * q2.toRotationMatrix() * v2); // q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
VERIFY_IS_NOT_APPROX(q2 * q1 * v2, // VERIFY_IS_NOT_APPROX(q2 * q1 * v2,
q1.toRotationMatrix() * q2.toRotationMatrix() * v2); // q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
q2.fromRotationMatrix(q1.toRotationMatrix()); // q2.fromRotationMatrix(q1.toRotationMatrix());
VERIFY_IS_APPROX(q1*v1,q2*v1); // VERIFY_IS_APPROX(q1*v1,q2*v1);
//
// Euler angle conversion // // Euler angle conversion
VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1); // VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1);
v2 = q2.toEulerAngles(); // v2 = q2.toEulerAngles();
VERIFY_IS_APPROX(q2.fromEulerAngles(v2).toEulerAngles(), v2); // 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_NOT_APPROX(q2.fromEulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))).toEulerAngles(), v2);
//
// angle-axis conversion // // angle-axis conversion
q1.toAngleAxis(a, v2); // q1.toAngleAxis(a, v2);
VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1); // VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1);
VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1); // VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1);
//
// from two vector creation // // 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.fromTwoVectors(v1,v2)*v1).normalized()); // VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
//
// inverse and conjugate // // inverse and conjugate
VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1); // VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1);
VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1); // VERIFY_IS_APPROX(q1 * (q1.conjugate() * v1), v1);
// cross product // cross product
VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1)); VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1));
@ -82,12 +82,46 @@ template<typename Scalar> void geometry(void)
(v0.cross(v1)).normalized(), (v0.cross(v1)).normalized(),
(v0.cross(v1).cross(v0)).normalized(); (v0.cross(v1).cross(v0)).normalized();
VERIFY(m.isOrtho()); VERIFY(m.isOrtho());
// Transform
// TODO complete the tests !
typedef Transform<Scalar,2> Transform2;
typedef Transform<Scalar,3> Transform3;
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());
Transform3 t0, t1, t2;
t0.setIdentity();
t0.affine() = q1.toRotationMatrix();
t1.setIdentity();
t1.affine() = q1.toRotationMatrix();
v0 << 50, 2, 1;//= Vector3::random().cwiseProduct(Vector3(10,2,0.5));
t0.scale(v0);
t1.prescale(v0);
VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).norm(), v0.x());
VERIFY_IS_NOT_APPROX((t1 * Vector3(1,0,0)).norm(), v0.x());
t0.setIdentity();
t1.setIdentity();
v1 << 1, 2, 3;
t0.affine() = q1.toRotationMatrix();
t0.pretranslate(v0);
t0.scale(v1);
t1.affine() = q1.conjugate().toRotationMatrix();
t1.prescale(v1.cwiseInverse());
t1.translate(-v0);
VERIFY((t0.matrix() * t1.matrix()).isIdentity());
} }
void test_geometry() void test_geometry()
{ {
for(int i = 0; i < g_repeat; i++) { for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST( geometry<float>() ); CALL_SUBTEST( geometry<float>() );
CALL_SUBTEST( geometry<double>() ); // CALL_SUBTEST( geometry<double>() );
} }
} }