1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 47faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright (C) 2008-2012 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 157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename Scalar> 177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid verify_euler(const Matrix<Scalar,3,1>& ea, int i, int j, int k) 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,3> Matrix3; 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,1> Vector3; 217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef AngleAxis<Scalar> AngleAxisx; 227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::abs; 237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); 247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Vector3 eabis = m.eulerAngles(i, j, k); 257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); 267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX(m, mbis); 277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* If I==K, and ea[1]==0, then there no unique solution. */ 287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ 297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if( (i!=k || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(M_PI/2),test_precision<Scalar>())) ) 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY((ea-eabis).norm() <= test_precision<Scalar>()); 317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // approx_or_less_than does not work for 0 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); 347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(M_PI)); 357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[1]); 367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(M_PI)); 377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(M_PI), eabis[2]); 387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(M_PI)); 397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea) 427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 0,1,2); 447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 0,1,0); 457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 0,2,1); 467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 0,2,0); 477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 1,2,0); 497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 1,2,1); 507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 1,0,2); 517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 1,0,1); 527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 2,0,1); 547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 2,0,2); 557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 2,1,0); 567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez verify_euler(ea, 2,1,2); 577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename Scalar> void eulerangles() 607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,3,3> Matrix3; 627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,3,1> Vector3; 637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Array<Scalar,3,1> Array3; 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Quaternion<Scalar> Quaternionx; 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef AngleAxis<Scalar> AngleAxisx; 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)); 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Quaternionx q1; 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath q1 = AngleAxisx(a, Vector3::Random().normalized()); 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 m; 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m = q1; 727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Vector3 ea = m.eulerAngles(0,1,2); 747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea = m.eulerAngles(0,1,0); 767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Check with purely random Quaternion: 797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez q1.coeffs() = Quaternionx::Coefficients::Random().normalized(); 807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m = q1; 817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea = m.eulerAngles(0,1,2); 827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea = m.eulerAngles(0,1,0); 847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. 877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea = (Array3::Random() + Array3(1,0,0))*Scalar(M_PI)*Array3(0.5,1,1); 887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(M_PI)); 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(M_PI)); 947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea[1] = 0; 977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea.head(2).setZero(); 1007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 1017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ea.setZero(); 1037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez check_all_var(ea); 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathvoid test_geo_eulerangles() 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath for(int i = 0; i < g_repeat; i++) { 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_1( eulerangles<float>() ); 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath CALL_SUBTEST_2( eulerangles<double>() ); 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 113