1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include "main.h" 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <Eigen/Geometry> 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <Eigen/LU> 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#include <Eigen/SVD> 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 152b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wangtemplate<typename T> 162b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao WangMatrix<T,2,1> angleToVec(T a) 172b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang{ 182b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang return Matrix<T,2,1>(std::cos(a), std::sin(a)); 192b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang} 202b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 212b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang// This permits to workaround a bug in clang/llvm code generation. 222b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wangtemplate<typename T> 232b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao WangEIGEN_DONT_INLINE 242b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wangvoid dont_over_optimize(T& x) { volatile typename T::Scalar tmp = x(0); x(0) = tmp; } 252b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar, int Mode, int Options> void non_projective_only() 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* this test covers the following files: 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Cross.h Quaternion.h, Transform.cpp 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,1> Vector3; 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Quaternion<Scalar> Quaternionx; 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef AngleAxis<Scalar> AngleAxisx; 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,3,Mode,Options> Transform3; 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef DiagonalMatrix<Scalar,3> AlignedScaling3; 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Translation<Scalar,3> Translation3; 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 v0 = Vector3::Random(), 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v1 = Vector3::Random(); 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform3 t0, t1, t2; 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 432b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternionx q1, q2; 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q1 = AngleAxisx(a, v0.normalized()); 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0 = Transform3::Identity(); 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.linear() = q1.toRotationMatrix(); 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v0 << 50, 2, 1; 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(v0); 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template head<3>().norm(), v0.x()); 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.setIdentity(); 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v1 << 1, 2, 3; 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.linear() = q1.toRotationMatrix(); 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.pretranslate(v0); 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(v1); 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.linear() = q1.conjugate().toRotationMatrix(); 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.prescale(v1.cwiseInverse()); 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.translate(-v0); 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>())); 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.fromPositionOrientationScale(v0, q1, v1); 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t1*v1, t0*v1); 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // translation * vector 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.translate(v0); 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1); 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // AlignedScaling * vector 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(v0); 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((t0 * v1).template head<3>(), AlignedScaling3(v0) * v1); 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar, int Mode, int Options> void transformations() 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /* this test covers the following files: 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Cross.h Quaternion.h, Transform.cpp 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::cos; 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,3> Matrix3; 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,4,4> Matrix4; 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,2,1> Vector2; 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,1> Vector3; 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,4,1> Vector4; 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Quaternion<Scalar> Quaternionx; 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef AngleAxis<Scalar> AngleAxisx; 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,2,Mode,Options> Transform2; 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,3,Mode,Options> Transform3; 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef typename Transform3::MatrixType MatrixType; 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef DiagonalMatrix<Scalar,3> AlignedScaling3; 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Translation<Scalar,2> Translation2; 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Translation<Scalar,3> Translation3; 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 v0 = Vector3::Random(), 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v1 = Vector3::Random(); 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 matrot1, m; 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 1112b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); 1122b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar s0 = internal::random<Scalar>(), s1 = internal::random<Scalar>(); 113615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray 114615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray while(v0.norm() < test_precision<Scalar>()) v0 = Vector3::Random(); 115615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray while(v1.norm() < test_precision<Scalar>()) v1 = Vector3::Random(); 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); 1182b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(EIGEN_PI), v0.unitOrthogonal()) * v0); 119615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray if(abs(cos(a)) > test_precision<Scalar>()) 120615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray { 121615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); 122615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray } 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint(); 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized())); 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m); 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternionx q1, q2; 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q1 = AngleAxisx(a, v0.normalized()); 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q2 = AngleAxisx(a, v1.normalized()); 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // rotation matrix conversion 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX()) 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * AngleAxisx(Scalar(0.2), Vector3::UnitY()) 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * AngleAxisx(Scalar(0.3), Vector3::UnitZ()); 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(matrot1 * v1, 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix() 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix() 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1))); 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // angle-axis conversion 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxisx aa = AngleAxisx(q1); 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); 143615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray 1442b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang // The following test is stable only if 2*angle != angle and v1 is not colinear with axis 1452b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang if( (abs(aa.angle()) > test_precision<Scalar>()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision<Scalar>())) ) 146615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray { 147615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); 148615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray } 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath aa.fromRotationMatrix(aa.toRotationMatrix()); 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); 1522b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang // The following test is stable only if 2*angle != angle and v1 is not colinear with axis 1532b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang if( (abs(aa.angle()) > test_precision<Scalar>()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision<Scalar>())) ) 154615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray { 155615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); 156615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray } 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // AngleAxis 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(), 160c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix()); 161c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 162c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxisx aa1; 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = q1.toRotationMatrix(); 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath aa1 = m; 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(), 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternionx(m).toRotationMatrix()); 167c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 168c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Transform 169c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // TODO complete the tests ! 170c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath a = 0; 1717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez while (abs(a)<Scalar(0.1)) 1722b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang a = internal::random<Scalar>(-Scalar(0.4)*Scalar(EIGEN_PI), Scalar(0.4)*Scalar(EIGEN_PI)); 173c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q1 = AngleAxisx(a, v0.normalized()); 174c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform3 t0, t1, t2; 175c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 176c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // first test setIdentity() and Identity() 177c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 178c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); 179c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.matrix().setZero(); 180c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0 = Transform3::Identity(); 181c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); 182c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 183c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 184c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.setIdentity(); 185c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath v1 << 1, 2, 3; 186c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.linear() = q1.toRotationMatrix(); 187c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.pretranslate(v0); 188c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(v1); 189c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.linear() = q1.conjugate().toRotationMatrix(); 190c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.prescale(v1.cwiseInverse()); 191c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.translate(-v0); 192c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 193c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>())); 194c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 195c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.fromPositionOrientationScale(v0, q1, v1); 196c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); 197c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 198c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix()); 199c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1.setIdentity(); t1.scale(v0).rotate(q1); 200c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 201c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 202c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1)); 203c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 204c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 205c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix()); 206c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix()); 207c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 208c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // More transform constructors, operator=, operator*= 209c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 210c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 mat3 = Matrix3::Random(); 211c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix4 mat4; 212c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose(); 213c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform3 tmat3(mat3), tmat4(mat4); 214c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(Mode!=int(AffineCompact)) 215c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmat4.matrix()(3,3) = Scalar(1); 216c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix()); 217c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 2182b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar a3 = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); 219c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 v3 = Vector3::Random().normalized(); 220c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxisx aa3(a3, v3); 221c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform3 t3(aa3); 222c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform3 t4; 223c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4 = aa3; 224c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); 225c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4.rotate(AngleAxisx(-a3,v3)); 226c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); 227c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4 *= aa3; 228c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); 229c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 2302b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang do { 2312b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang v3 = Vector3::Random(); 2322b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang dont_over_optimize(v3); 2332b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang } while (v3.cwiseAbs().minCoeff()<NumTraits<Scalar>::epsilon()); 234c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Translation3 tv3(v3); 235c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform3 t5(tv3); 236c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4 = tv3; 237c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); 2382b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang t4.translate((-v3).eval()); 239c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); 240c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4 *= tv3; 241c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); 242c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 243c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AlignedScaling3 sv3(v3); 244c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform3 t6(sv3); 245c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4 = sv3; 246c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); 247c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4.scale(v3.cwiseInverse()); 248c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); 249c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t4 *= sv3; 250c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); 251c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 252c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // matrix * transform 253c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((t3.matrix()*t4).matrix(), (t3*t4).matrix()); 254c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 255c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // chained Transform product 256c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix()); 257c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 258c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // check that Transform product doesn't have aliasing problems 259c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t5 = t4; 260c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t5 = t5*t5; 261c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t5, t4*t4); 262c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 263c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 2D transformation 264c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform2 t20, t21; 265c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector2 v20 = Vector2::Random(); 266c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector2 v21 = Vector2::Random(); 267c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for (int k=0; k<2; ++k) 2687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (abs(v21[k])<Scalar(1e-3)) v21[k] = Scalar(1e-3); 269c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t21.setIdentity(); 270c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t21.linear() = Rotation2D<Scalar>(a).toRotationMatrix(); 271c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(), 272c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t21.pretranslate(v20).scale(v21).matrix()); 273c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 274c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t21.setIdentity(); 275c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t21.linear() = Rotation2D<Scalar>(-a).toRotationMatrix(); 276c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) 277c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * (t21.prescale(v21.cwiseInverse()).translate(-v20))).matrix().isIdentity(test_precision<Scalar>()) ); 278c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 279c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Transform - new API 280c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // 3D 281c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 282c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.rotate(q1).scale(v0).translate(v0); 283c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // mat * aligned scaling and mat * translation 284c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = (Matrix3(q1) * AlignedScaling3(v0)) * Translation3(v0); 285c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 286c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = (Matrix3(q1) * Eigen::Scaling(v0)) * Translation3(v0); 287c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 288c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = (q1 * Eigen::Scaling(v0)) * Translation3(v0); 289c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 290c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // mat * transformation and aligned scaling * translation 291c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = Matrix3(q1) * (AlignedScaling3(v0) * Translation3(v0)); 292c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 293c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 294c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 295c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 296c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(s0).translate(v0); 297c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = Eigen::Scaling(s0) * Translation3(v0); 298c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 299c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.prescale(s0); 300c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = Eigen::Scaling(s0) * t1; 301c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 302c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 303c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0 = t3; 304c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(s0); 305c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = t3 * Eigen::Scaling(s0,s0,s0); 306c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 307c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.prescale(s0); 308c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = Eigen::Scaling(s0,s0,s0) * t1; 309c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 310c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 3117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez t0 = t3; 3127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez t0.scale(s0); 3137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez t1 = t3 * Eigen::Scaling(s0); 3147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 3157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez t0.prescale(s0); 3167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez t1 = Eigen::Scaling(s0) * t1; 3177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 318c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 319c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 320c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.prerotate(q1).prescale(v0).pretranslate(v0); 321c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // translation * aligned scaling and transformation * mat 322c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = (Translation3(v0) * AlignedScaling3(v0)) * Transform3(q1); 323c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 324c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // scaling * mat and translation * mat 325c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = Translation3(v0) * (AlignedScaling3(v0) * Transform3(q1)); 326c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 327c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 328c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 329c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(v0).translate(v0).rotate(q1); 330c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // translation * mat and aligned scaling * transformation 331c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); 332c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 333c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // transformation * aligned scaling 334c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(v0); 335c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 *= AlignedScaling3(v0); 336c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 337353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); 338353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk t1 = t1 * v0.asDiagonal(); 339353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 340c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // transformation * translation 341c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.translate(v0); 342c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = t1 * Translation3(v0); 343c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 344c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // translation * transformation 345c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.pretranslate(v0); 346c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = Translation3(v0) * t1; 347c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 348c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 349c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // transform * quaternion 350c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.rotate(q1); 351c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = t1 * q1; 352c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 353c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 354c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // translation * quaternion 355c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.translate(v1).rotate(q1); 356c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = t1 * (Translation3(v1) * q1); 357c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 358c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 359c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // aligned scaling * quaternion 360c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.scale(v1).rotate(q1); 361c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = t1 * (AlignedScaling3(v1) * q1); 362c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 363c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 364c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // quaternion * transform 365c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.prerotate(q1); 366c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = q1 * t1; 367c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 368c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 369c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // quaternion * translation 370c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.rotate(q1).translate(v1); 371c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = t1 * (q1 * Translation3(v1)); 372c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 373c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 374c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // quaternion * aligned scaling 375c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.rotate(q1).scale(v1); 376c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t1 = t1 * (q1 * AlignedScaling3(v1)); 377c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 378c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 379c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // test transform inversion 380c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 381c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.translate(v0); 382615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray do { 383615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray t0.linear().setRandom(); 384615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray } while(t0.linear().jacobiSvd().singularValues()(2)<test_precision<Scalar>()); 385c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix4 t044 = Matrix4::Zero(); 386c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t044(3,3) = 1; 387c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); 388c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.inverse(Affine).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4)); 389c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 390c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.translate(v0).rotate(q1); 391c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t044 = Matrix4::Zero(); 392c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t044(3,3) = 1; 393c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); 394c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.inverse(Isometry).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4)); 395c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 396c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 mat_rotation, mat_scaling; 397c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.setIdentity(); 398c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.translate(v0).rotate(q1).scale(v1); 399c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.computeRotationScaling(&mat_rotation, &mat_scaling); 400c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling); 401c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); 402c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); 403c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath t0.computeScalingRotation(&mat_scaling, &mat_rotation); 404c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation); 405c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); 406c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); 407c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 408c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // test casting 409c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform<float,3,Mode> t1f = t1.template cast<float>(); 410c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t1f.template cast<Scalar>(),t1); 411c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Transform<double,3,Mode> t1d = t1.template cast<double>(); 412c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(t1d.template cast<Scalar>(),t1); 413c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 414c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Translation3 tr1(v0); 415c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Translation<float,3> tr1f = tr1.template cast<float>(); 416c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(tr1f.template cast<Scalar>(),tr1); 417c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Translation<double,3> tr1d = tr1.template cast<double>(); 418c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(tr1d.template cast<Scalar>(),tr1); 419c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 420c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxis<float> aa1f = aa1.template cast<float>(); 421c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(aa1f.template cast<Scalar>(),aa1); 422c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxis<double> aa1d = aa1.template cast<double>(); 423c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(aa1d.template cast<Scalar>(),aa1); 424c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 425c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rotation2D<Scalar> r2d1(internal::random<Scalar>()); 426c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rotation2D<float> r2d1f = r2d1.template cast<float>(); 427c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1); 428c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rotation2D<double> r2d1d = r2d1.template cast<double>(); 429c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1); 430615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray 4312b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang for(int k=0; k<100; ++k) 4322b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang { 4332b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar angle = internal::random<Scalar>(-100,100); 4342b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Rotation2D<Scalar> rot2(angle); 4352b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY( rot2.smallestPositiveAngle() >= 0 ); 4362b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY( rot2.smallestPositiveAngle() <= Scalar(2)*Scalar(EIGEN_PI) ); 4372b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_APPROX( angleToVec(rot2.smallestPositiveAngle()), angleToVec(rot2.angle()) ); 4382b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 4392b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY( rot2.smallestAngle() >= -Scalar(EIGEN_PI) ); 4402b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY( rot2.smallestAngle() <= Scalar(EIGEN_PI) ); 4412b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_APPROX( angleToVec(rot2.smallestAngle()), angleToVec(rot2.angle()) ); 4422b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 4432b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Matrix<Scalar,2,2> rot2_as_mat(rot2); 4442b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Rotation2D<Scalar> rot3(rot2_as_mat); 4452b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_APPROX( angleToVec(rot2.smallestAngle()), angleToVec(rot3.angle()) ); 4462b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang } 4472b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 4482b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang s0 = internal::random<Scalar>(-100,100); 4492b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang s1 = internal::random<Scalar>(-100,100); 450615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray Rotation2D<Scalar> R0(s0), R1(s1); 4512b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 452615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); 453615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray t21 = Translation2(v20) * R0 * Eigen::Scaling(s0); 454615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY_IS_APPROX(t20,t21); 455615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray 456615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0)); 457615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray t21 = Translation2(v20) * Eigen::Scaling(s0); 458615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY_IS_APPROX(t20,t21); 459615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray 460615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); 4612b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_APPROX( angleToVec(R1.smallestPositiveAngle()), angleToVec((R0.slerp(1, R1)).smallestPositiveAngle()) ); 4622b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_APPROX(R0.smallestPositiveAngle(), (R0.slerp(0.5, R0)).smallestPositiveAngle()); 4632b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 4642b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang if(std::cos(s0)>0) 4652b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_MUCH_SMALLER_THAN((R0.slerp(0.5, R0.inverse())).smallestAngle(), Scalar(1)); 4662b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang else 4672b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY_IS_APPROX(Scalar(EIGEN_PI), (R0.slerp(0.5, R0.inverse())).smallestPositiveAngle()); 4682b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang 4692b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang // Check path length 4702b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar l = 0; 4712b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang int path_steps = 100; 4722b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang for(int k=0; k<path_steps; ++k) 4732b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang { 4742b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar a1 = R0.slerp(Scalar(k)/Scalar(path_steps), R1).angle(); 4752b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang Scalar a2 = R0.slerp(Scalar(k+1)/Scalar(path_steps), R1).angle(); 4762b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang l += std::abs(a2-a1); 4772b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang } 4782b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang VERIFY(l<=Scalar(EIGEN_PI)*(Scalar(1)+NumTraits<Scalar>::epsilon()*Scalar(path_steps/2))); 479615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray 480615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray // check basic features 481615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray { 482615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray Rotation2D<Scalar> r1; // default ctor 483615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray r1 = Rotation2D<Scalar>(s0); // copy assignment 484615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY_IS_APPROX(r1.angle(),s0); 485615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray Rotation2D<Scalar> r2(r1); // copy ctor 486615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray VERIFY_IS_APPROX(r2.angle(),s0); 487615d816d068b4d0f5e8df601930b5f160bf7eda1Tim Murray } 488353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 489353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk { 490353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk Transform3 t32(Matrix4::Random()), t33, t34; 491353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk t34 = t33 = t32; 492353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk t32.scale(v0); 493353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk t33*=AlignedScaling3(v0); 494353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); 495353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk t33 = t34 * AlignedScaling3(v0); 496353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); 497353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk } 498353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 499353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk} 500353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 501353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyktemplate<typename A1, typename A2, typename P, typename Q, typename V, typename H> 502353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazykvoid transform_associativity_left(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) 503353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk{ 504353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( q*(a1*v), (q*a1)*v ); 505353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( q*(a2*v), (q*a2)*v ); 506353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( q*(p*h).hnormalized(), ((q*p)*h).hnormalized() ); 507353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk} 508353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 509353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyktemplate<typename A1, typename A2, typename P, typename Q, typename V, typename H> 510353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazykvoid transform_associativity2(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) 511353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk{ 512353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( a1*(q*v), (a1*q)*v ); 513353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( a2*(q*v), (a2*q)*v ); 514353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( p *(q*v).homogeneous(), (p *q)*v.homogeneous() ); 515353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 516353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk transform_associativity_left(a1, a2,p, q, v, h); 517353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk} 518353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 519353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyktemplate<typename Scalar, int Dim, int Options,typename RotationType> 520353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazykvoid transform_associativity(const RotationType& R) 521353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk{ 522353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Matrix<Scalar,Dim,1> VectorType; 523353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Matrix<Scalar,Dim+1,1> HVectorType; 524353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Matrix<Scalar,Dim,Dim> LinearType; 525353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Matrix<Scalar,Dim+1,Dim+1> MatrixType; 526353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Transform<Scalar,Dim,AffineCompact,Options> AffineCompactType; 527353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Transform<Scalar,Dim,Affine,Options> AffineType; 528353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Transform<Scalar,Dim,Projective,Options> ProjectiveType; 529353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef DiagonalMatrix<Scalar,Dim> ScalingType; 530353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk typedef Translation<Scalar,Dim> TranslationType; 531353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 532353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk AffineCompactType A1c; A1c.matrix().setRandom(); 533353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk AffineCompactType A2c; A2c.matrix().setRandom(); 534353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk AffineType A1(A1c); 535353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk AffineType A2(A2c); 536353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk ProjectiveType P1; P1.matrix().setRandom(); 537353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VectorType v1 = VectorType::Random(); 538353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VectorType v2 = VectorType::Random(); 539353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk HVectorType h1 = HVectorType::Random(); 540353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk Scalar s1 = internal::random<Scalar>(); 541353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk LinearType L = LinearType::Random(); 542353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk MatrixType M = MatrixType::Random(); 543353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 544353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2, v2, h1) ); 545353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2c, v2, h1) ); 546353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, v1.asDiagonal(), v2, h1) ); 547353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, ScalingType(v1), v2, h1) ); 548353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(v1), v2, h1) ); 549353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(s1), v2, h1) ); 550353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, TranslationType(v1), v2, h1) ); 551353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity_left(A1c, A1, P1, L, v2, h1) ); 552353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST( transform_associativity2(A1c, A1, P1, R, v2, h1) ); 553353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 554353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( A1*(M*h1), (A1*M)*h1 ); 555353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( A1c*(M*h1), (A1c*M)*h1 ); 556353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( P1*(M*h1), (P1*M)*h1 ); 557353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 558353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( M*(A1*h1), (M*A1)*h1 ); 559353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( M*(A1c*h1), (M*A1c)*h1 ); 560353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk VERIFY_IS_APPROX( M*(P1*h1), ((M*P1)*h1) ); 561c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 562c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 563c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> void transform_alignment() 564c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 565c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,3,Projective,AutoAlign> Projective3a; 566c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,3,Projective,DontAlign> Projective3u; 567c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 5682b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang EIGEN_ALIGN_MAX Scalar array1[16]; 5692b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang EIGEN_ALIGN_MAX Scalar array2[16]; 5702b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang EIGEN_ALIGN_MAX Scalar array3[16+1]; 571c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar* array3u = array3+1; 572c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 573c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Projective3a *p1 = ::new(reinterpret_cast<void*>(array1)) Projective3a; 574c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Projective3u *p2 = ::new(reinterpret_cast<void*>(array2)) Projective3u; 575c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Projective3u *p3 = ::new(reinterpret_cast<void*>(array3u)) Projective3u; 576c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 577c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath p1->matrix().setRandom(); 578c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath *p2 = *p1; 579c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath *p3 = *p1; 580c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 581c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(p1->matrix(), p2->matrix()); 582c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX(p1->matrix(), p3->matrix()); 583c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 584c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX( (*p1) * (*p1), (*p2)*(*p3)); 585c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 5862b8756b6f1de65d3f8bffab45be6c44ceb7411fcMiao Wang #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0 587c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if(internal::packet_traits<Scalar>::Vectorizable) 588c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Projective3a)); 589c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath #endif 590c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 591c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 592c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar, int Dim, int Options> void transform_products() 593c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 594c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,Dim+1,Dim+1> Mat; 595c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,Dim,Projective,Options> Proj; 596c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,Dim,Affine,Options> Aff; 597c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Transform<Scalar,Dim,AffineCompact,Options> AffC; 598c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 599c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Proj p; p.matrix().setRandom(); 600c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Aff a; a.linear().setRandom(); a.translation().setRandom(); 601c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AffC ac = a; 602c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 603c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Mat p_m(p.matrix()), a_m(a.matrix()); 604c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 605c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((p*p).matrix(), p_m*p_m); 606c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((a*a).matrix(), a_m*a_m); 607c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((p*a).matrix(), p_m*a_m); 608c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((a*p).matrix(), a_m*p_m); 609c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((ac*a).matrix(), a_m*a_m); 610c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((a*ac).matrix(), a_m*a_m); 611c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((p*ac).matrix(), p_m*a_m); 612c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m); 613c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 614c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 615c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_geo_transformations() 616c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 617c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for(int i = 0; i < g_repeat; i++) { 618c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_1(( transformations<double,Affine,AutoAlign>() )); 619c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_1(( non_projective_only<double,Affine,AutoAlign>() )); 620c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 621c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_2(( transformations<float,AffineCompact,AutoAlign>() )); 622c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_2(( non_projective_only<float,AffineCompact,AutoAlign>() )); 623c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_2(( transform_alignment<float>() )); 624c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 625c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_3(( transformations<double,Projective,AutoAlign>() )); 626c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_3(( transformations<double,Projective,DontAlign>() )); 627c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_3(( transform_alignment<double>() )); 628c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 629c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_4(( transformations<float,Affine,RowMajor|AutoAlign>() )); 630c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_4(( non_projective_only<float,Affine,RowMajor>() )); 631c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 632c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_5(( transformations<double,AffineCompact,RowMajor|AutoAlign>() )); 633c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_5(( non_projective_only<double,AffineCompact,RowMajor>() )); 634c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 635c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|AutoAlign>() )); 636c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|DontAlign>() )); 637c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 638c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 639c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_7(( transform_products<double,3,RowMajor|AutoAlign>() )); 640c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_7(( transform_products<float,2,AutoAlign>() )); 641353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk 642353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST_8(( transform_associativity<double,2,ColMajor>(Rotation2D<double>(internal::random<double>()*double(EIGEN_PI))) )); 643353bba589de58014a35f8f3666b7b96353c300f8Stephen Kiazyk CALL_SUBTEST_8(( transform_associativity<double,3,ColMajor>(Quaterniond::UnitRandom()) )); 644c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 645c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 646