[136] | 1 | // This file is part of Eigen, a lightweight C++ template library
|
---|
| 2 | // for linear algebra.
|
---|
| 3 | //
|
---|
| 4 | // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
---|
| 5 | //
|
---|
| 6 | // This Source Code Form is subject to the terms of the Mozilla
|
---|
| 7 | // Public License v. 2.0. If a copy of the MPL was not distributed
|
---|
| 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
---|
| 9 |
|
---|
| 10 | #include "main.h"
|
---|
| 11 | #include <Eigen/Geometry>
|
---|
| 12 | #include <Eigen/LU>
|
---|
| 13 | #include <Eigen/SVD>
|
---|
| 14 |
|
---|
| 15 | /* this test covers the following files:
|
---|
| 16 | Geometry/OrthoMethods.h
|
---|
| 17 | */
|
---|
| 18 |
|
---|
| 19 | template<typename Scalar> void orthomethods_3()
|
---|
| 20 | {
|
---|
| 21 | typedef typename NumTraits<Scalar>::Real RealScalar;
|
---|
| 22 | typedef Matrix<Scalar,3,3> Matrix3;
|
---|
| 23 | typedef Matrix<Scalar,3,1> Vector3;
|
---|
| 24 |
|
---|
| 25 | typedef Matrix<Scalar,4,1> Vector4;
|
---|
| 26 |
|
---|
| 27 | Vector3 v0 = Vector3::Random(),
|
---|
| 28 | v1 = Vector3::Random(),
|
---|
| 29 | v2 = Vector3::Random();
|
---|
| 30 |
|
---|
| 31 | // cross product
|
---|
| 32 | VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1));
|
---|
| 33 | VERIFY_IS_MUCH_SMALLER_THAN(v1.dot(v1.cross(v2)), Scalar(1));
|
---|
| 34 | VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v2), Scalar(1));
|
---|
| 35 | VERIFY_IS_MUCH_SMALLER_THAN(v2.dot(v1.cross(v2)), Scalar(1));
|
---|
| 36 | Matrix3 mat3;
|
---|
| 37 | mat3 << v0.normalized(),
|
---|
| 38 | (v0.cross(v1)).normalized(),
|
---|
| 39 | (v0.cross(v1).cross(v0)).normalized();
|
---|
| 40 | VERIFY(mat3.isUnitary());
|
---|
| 41 |
|
---|
| 42 |
|
---|
| 43 | // colwise/rowwise cross product
|
---|
| 44 | mat3.setRandom();
|
---|
| 45 | Vector3 vec3 = Vector3::Random();
|
---|
| 46 | Matrix3 mcross;
|
---|
| 47 | int i = internal::random<int>(0,2);
|
---|
| 48 | mcross = mat3.colwise().cross(vec3);
|
---|
| 49 | VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3));
|
---|
| 50 | mcross = mat3.rowwise().cross(vec3);
|
---|
| 51 | VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3));
|
---|
| 52 |
|
---|
| 53 | // cross3
|
---|
| 54 | Vector4 v40 = Vector4::Random(),
|
---|
| 55 | v41 = Vector4::Random(),
|
---|
| 56 | v42 = Vector4::Random();
|
---|
| 57 | v40.w() = v41.w() = v42.w() = 0;
|
---|
| 58 | v42.template head<3>() = v40.template head<3>().cross(v41.template head<3>());
|
---|
| 59 | VERIFY_IS_APPROX(v40.cross3(v41), v42);
|
---|
| 60 |
|
---|
| 61 | // check mixed product
|
---|
| 62 | typedef Matrix<RealScalar, 3, 1> RealVector3;
|
---|
| 63 | RealVector3 rv1 = RealVector3::Random();
|
---|
| 64 | VERIFY_IS_APPROX(v1.cross(rv1.template cast<Scalar>()), v1.cross(rv1));
|
---|
| 65 | VERIFY_IS_APPROX(rv1.template cast<Scalar>().cross(v1), rv1.cross(v1));
|
---|
| 66 | }
|
---|
| 67 |
|
---|
| 68 | template<typename Scalar, int Size> void orthomethods(int size=Size)
|
---|
| 69 | {
|
---|
| 70 | typedef typename NumTraits<Scalar>::Real RealScalar;
|
---|
| 71 | typedef Matrix<Scalar,Size,1> VectorType;
|
---|
| 72 | typedef Matrix<Scalar,3,Size> Matrix3N;
|
---|
| 73 | typedef Matrix<Scalar,Size,3> MatrixN3;
|
---|
| 74 | typedef Matrix<Scalar,3,1> Vector3;
|
---|
| 75 |
|
---|
| 76 | VectorType v0 = VectorType::Random(size);
|
---|
| 77 |
|
---|
| 78 | // unitOrthogonal
|
---|
| 79 | VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().dot(v0), Scalar(1));
|
---|
| 80 | VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), RealScalar(1));
|
---|
| 81 |
|
---|
| 82 | if (size>=3)
|
---|
| 83 | {
|
---|
| 84 | v0.template head<2>().setZero();
|
---|
| 85 | v0.tail(size-2).setRandom();
|
---|
| 86 |
|
---|
| 87 | VERIFY_IS_MUCH_SMALLER_THAN(v0.unitOrthogonal().dot(v0), Scalar(1));
|
---|
| 88 | VERIFY_IS_APPROX(v0.unitOrthogonal().norm(), RealScalar(1));
|
---|
| 89 | }
|
---|
| 90 |
|
---|
| 91 | // colwise/rowwise cross product
|
---|
| 92 | Vector3 vec3 = Vector3::Random();
|
---|
| 93 | int i = internal::random<int>(0,size-1);
|
---|
| 94 |
|
---|
| 95 | Matrix3N mat3N(3,size), mcross3N(3,size);
|
---|
| 96 | mat3N.setRandom();
|
---|
| 97 | mcross3N = mat3N.colwise().cross(vec3);
|
---|
| 98 | VERIFY_IS_APPROX(mcross3N.col(i), mat3N.col(i).cross(vec3));
|
---|
| 99 |
|
---|
| 100 | MatrixN3 matN3(size,3), mcrossN3(size,3);
|
---|
| 101 | matN3.setRandom();
|
---|
| 102 | mcrossN3 = matN3.rowwise().cross(vec3);
|
---|
| 103 | VERIFY_IS_APPROX(mcrossN3.row(i), matN3.row(i).cross(vec3));
|
---|
| 104 | }
|
---|
| 105 |
|
---|
| 106 | void test_geo_orthomethods()
|
---|
| 107 | {
|
---|
| 108 | for(int i = 0; i < g_repeat; i++) {
|
---|
| 109 | CALL_SUBTEST_1( orthomethods_3<float>() );
|
---|
| 110 | CALL_SUBTEST_2( orthomethods_3<double>() );
|
---|
| 111 | CALL_SUBTEST_4( orthomethods_3<std::complex<double> >() );
|
---|
| 112 | CALL_SUBTEST_1( (orthomethods<float,2>()) );
|
---|
| 113 | CALL_SUBTEST_2( (orthomethods<double,2>()) );
|
---|
| 114 | CALL_SUBTEST_1( (orthomethods<float,3>()) );
|
---|
| 115 | CALL_SUBTEST_2( (orthomethods<double,3>()) );
|
---|
| 116 | CALL_SUBTEST_3( (orthomethods<float,7>()) );
|
---|
| 117 | CALL_SUBTEST_4( (orthomethods<std::complex<double>,8>()) );
|
---|
| 118 | CALL_SUBTEST_5( (orthomethods<float,Dynamic>(36)) );
|
---|
| 119 | CALL_SUBTEST_6( (orthomethods<double,Dynamic>(35)) );
|
---|
| 120 | }
|
---|
| 121 | }
|
---|