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 (at) 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 template<typename T> 16 Matrix<T,2,1> angleToVec(T a) 17 { 18 return Matrix<T,2,1>(std::cos(a), std::sin(a)); 19 } 20 21 // This permits to workaround a bug in clang/llvm code generation. 22 template<typename T> 23 EIGEN_DONT_INLINE 24 void dont_over_optimize(T& x) { volatile typename T::Scalar tmp = x(0); x(0) = tmp; } 25 26 template<typename Scalar, int Mode, int Options> void non_projective_only() 27 { 28 /* this test covers the following files: 29 Cross.h Quaternion.h, Transform.cpp 30 */ 31 typedef Matrix<Scalar,3,1> Vector3; 32 typedef Quaternion<Scalar> Quaternionx; 33 typedef AngleAxis<Scalar> AngleAxisx; 34 typedef Transform<Scalar,3,Mode,Options> Transform3; 35 typedef DiagonalMatrix<Scalar,3> AlignedScaling3; 36 typedef Translation<Scalar,3> Translation3; 37 38 Vector3 v0 = Vector3::Random(), 39 v1 = Vector3::Random(); 40 41 Transform3 t0, t1, t2; 42 43 Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); 44 45 Quaternionx q1, q2; 46 47 q1 = AngleAxisx(a, v0.normalized()); 48 49 t0 = Transform3::Identity(); 50 VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); 51 52 t0.linear() = q1.toRotationMatrix(); 53 54 v0 << 50, 2, 1; 55 t0.scale(v0); 56 57 VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template head<3>().norm(), v0.x()); 58 59 t0.setIdentity(); 60 t1.setIdentity(); 61 v1 << 1, 2, 3; 62 t0.linear() = q1.toRotationMatrix(); 63 t0.pretranslate(v0); 64 t0.scale(v1); 65 t1.linear() = q1.conjugate().toRotationMatrix(); 66 t1.prescale(v1.cwiseInverse()); 67 t1.translate(-v0); 68 69 VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>())); 70 71 t1.fromPositionOrientationScale(v0, q1, v1); 72 VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); 73 VERIFY_IS_APPROX(t1*v1, t0*v1); 74 75 // translation * vector 76 t0.setIdentity(); 77 t0.translate(v0); 78 VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1); 79 80 // AlignedScaling * vector 81 t0.setIdentity(); 82 t0.scale(v0); 83 VERIFY_IS_APPROX((t0 * v1).template head<3>(), AlignedScaling3(v0) * v1); 84 } 85 86 template<typename Scalar, int Mode, int Options> void transformations() 87 { 88 /* this test covers the following files: 89 Cross.h Quaternion.h, Transform.cpp 90 */ 91 using std::cos; 92 using std::abs; 93 typedef Matrix<Scalar,3,3> Matrix3; 94 typedef Matrix<Scalar,4,4> Matrix4; 95 typedef Matrix<Scalar,2,1> Vector2; 96 typedef Matrix<Scalar,3,1> Vector3; 97 typedef Matrix<Scalar,4,1> Vector4; 98 typedef Quaternion<Scalar> Quaternionx; 99 typedef AngleAxis<Scalar> AngleAxisx; 100 typedef Transform<Scalar,2,Mode,Options> Transform2; 101 typedef Transform<Scalar,3,Mode,Options> Transform3; 102 typedef typename Transform3::MatrixType MatrixType; 103 typedef DiagonalMatrix<Scalar,3> AlignedScaling3; 104 typedef Translation<Scalar,2> Translation2; 105 typedef Translation<Scalar,3> Translation3; 106 107 Vector3 v0 = Vector3::Random(), 108 v1 = Vector3::Random(); 109 Matrix3 matrot1, m; 110 111 Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); 112 Scalar s0 = internal::random<Scalar>(), s1 = internal::random<Scalar>(); 113 114 while(v0.norm() < test_precision<Scalar>()) v0 = Vector3::Random(); 115 while(v1.norm() < test_precision<Scalar>()) v1 = Vector3::Random(); 116 117 VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0); 118 VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(EIGEN_PI), v0.unitOrthogonal()) * v0); 119 if(abs(cos(a)) > test_precision<Scalar>()) 120 { 121 VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0)); 122 } 123 m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint(); 124 VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized())); 125 VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m); 126 127 Quaternionx q1, q2; 128 q1 = AngleAxisx(a, v0.normalized()); 129 q2 = AngleAxisx(a, v1.normalized()); 130 131 // rotation matrix conversion 132 matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX()) 133 * AngleAxisx(Scalar(0.2), Vector3::UnitY()) 134 * AngleAxisx(Scalar(0.3), Vector3::UnitZ()); 135 VERIFY_IS_APPROX(matrot1 * v1, 136 AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix() 137 * (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix() 138 * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1))); 139 140 // angle-axis conversion 141 AngleAxisx aa = AngleAxisx(q1); 142 VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); 143 144 // The following test is stable only if 2*angle != angle and v1 is not colinear with axis 145 if( (abs(aa.angle()) > test_precision<Scalar>()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision<Scalar>())) ) 146 { 147 VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); 148 } 149 150 aa.fromRotationMatrix(aa.toRotationMatrix()); 151 VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1); 152 // The following test is stable only if 2*angle != angle and v1 is not colinear with axis 153 if( (abs(aa.angle()) > test_precision<Scalar>()) && (abs(aa.axis().dot(v1.normalized()))<(Scalar(1)-Scalar(4)*test_precision<Scalar>())) ) 154 { 155 VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) ); 156 } 157 158 // AngleAxis 159 VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(), 160 Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix()); 161 162 AngleAxisx aa1; 163 m = q1.toRotationMatrix(); 164 aa1 = m; 165 VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(), 166 Quaternionx(m).toRotationMatrix()); 167 168 // Transform 169 // TODO complete the tests ! 170 a = 0; 171 while (abs(a)<Scalar(0.1)) 172 a = internal::random<Scalar>(-Scalar(0.4)*Scalar(EIGEN_PI), Scalar(0.4)*Scalar(EIGEN_PI)); 173 q1 = AngleAxisx(a, v0.normalized()); 174 Transform3 t0, t1, t2; 175 176 // first test setIdentity() and Identity() 177 t0.setIdentity(); 178 VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); 179 t0.matrix().setZero(); 180 t0 = Transform3::Identity(); 181 VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); 182 183 t0.setIdentity(); 184 t1.setIdentity(); 185 v1 << 1, 2, 3; 186 t0.linear() = q1.toRotationMatrix(); 187 t0.pretranslate(v0); 188 t0.scale(v1); 189 t1.linear() = q1.conjugate().toRotationMatrix(); 190 t1.prescale(v1.cwiseInverse()); 191 t1.translate(-v0); 192 193 VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>())); 194 195 t1.fromPositionOrientationScale(v0, q1, v1); 196 VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); 197 198 t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix()); 199 t1.setIdentity(); t1.scale(v0).rotate(q1); 200 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 201 202 t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1)); 203 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 204 205 VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix()); 206 VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix()); 207 208 // More transform constructors, operator=, operator*= 209 210 Matrix3 mat3 = Matrix3::Random(); 211 Matrix4 mat4; 212 mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose(); 213 Transform3 tmat3(mat3), tmat4(mat4); 214 if(Mode!=int(AffineCompact)) 215 tmat4.matrix()(3,3) = Scalar(1); 216 VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix()); 217 218 Scalar a3 = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); 219 Vector3 v3 = Vector3::Random().normalized(); 220 AngleAxisx aa3(a3, v3); 221 Transform3 t3(aa3); 222 Transform3 t4; 223 t4 = aa3; 224 VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); 225 t4.rotate(AngleAxisx(-a3,v3)); 226 VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); 227 t4 *= aa3; 228 VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); 229 230 do { 231 v3 = Vector3::Random(); 232 dont_over_optimize(v3); 233 } while (v3.cwiseAbs().minCoeff()<NumTraits<Scalar>::epsilon()); 234 Translation3 tv3(v3); 235 Transform3 t5(tv3); 236 t4 = tv3; 237 VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); 238 t4.translate((-v3).eval()); 239 VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); 240 t4 *= tv3; 241 VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); 242 243 AlignedScaling3 sv3(v3); 244 Transform3 t6(sv3); 245 t4 = sv3; 246 VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); 247 t4.scale(v3.cwiseInverse()); 248 VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); 249 t4 *= sv3; 250 VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); 251 252 // matrix * transform 253 VERIFY_IS_APPROX((t3.matrix()*t4).matrix(), (t3*t4).matrix()); 254 255 // chained Transform product 256 VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix()); 257 258 // check that Transform product doesn't have aliasing problems 259 t5 = t4; 260 t5 = t5*t5; 261 VERIFY_IS_APPROX(t5, t4*t4); 262 263 // 2D transformation 264 Transform2 t20, t21; 265 Vector2 v20 = Vector2::Random(); 266 Vector2 v21 = Vector2::Random(); 267 for (int k=0; k<2; ++k) 268 if (abs(v21[k])<Scalar(1e-3)) v21[k] = Scalar(1e-3); 269 t21.setIdentity(); 270 t21.linear() = Rotation2D<Scalar>(a).toRotationMatrix(); 271 VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(), 272 t21.pretranslate(v20).scale(v21).matrix()); 273 274 t21.setIdentity(); 275 t21.linear() = Rotation2D<Scalar>(-a).toRotationMatrix(); 276 VERIFY( (t20.fromPositionOrientationScale(v20,a,v21) 277 * (t21.prescale(v21.cwiseInverse()).translate(-v20))).matrix().isIdentity(test_precision<Scalar>()) ); 278 279 // Transform - new API 280 // 3D 281 t0.setIdentity(); 282 t0.rotate(q1).scale(v0).translate(v0); 283 // mat * aligned scaling and mat * translation 284 t1 = (Matrix3(q1) * AlignedScaling3(v0)) * Translation3(v0); 285 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 286 t1 = (Matrix3(q1) * Eigen::Scaling(v0)) * Translation3(v0); 287 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 288 t1 = (q1 * Eigen::Scaling(v0)) * Translation3(v0); 289 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 290 // mat * transformation and aligned scaling * translation 291 t1 = Matrix3(q1) * (AlignedScaling3(v0) * Translation3(v0)); 292 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 293 294 295 t0.setIdentity(); 296 t0.scale(s0).translate(v0); 297 t1 = Eigen::Scaling(s0) * Translation3(v0); 298 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 299 t0.prescale(s0); 300 t1 = Eigen::Scaling(s0) * t1; 301 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 302 303 t0 = t3; 304 t0.scale(s0); 305 t1 = t3 * Eigen::Scaling(s0,s0,s0); 306 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 307 t0.prescale(s0); 308 t1 = Eigen::Scaling(s0,s0,s0) * t1; 309 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 310 311 t0 = t3; 312 t0.scale(s0); 313 t1 = t3 * Eigen::Scaling(s0); 314 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 315 t0.prescale(s0); 316 t1 = Eigen::Scaling(s0) * t1; 317 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 318 319 t0.setIdentity(); 320 t0.prerotate(q1).prescale(v0).pretranslate(v0); 321 // translation * aligned scaling and transformation * mat 322 t1 = (Translation3(v0) * AlignedScaling3(v0)) * Transform3(q1); 323 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 324 // scaling * mat and translation * mat 325 t1 = Translation3(v0) * (AlignedScaling3(v0) * Transform3(q1)); 326 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 327 328 t0.setIdentity(); 329 t0.scale(v0).translate(v0).rotate(q1); 330 // translation * mat and aligned scaling * transformation 331 t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); 332 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 333 // transformation * aligned scaling 334 t0.scale(v0); 335 t1 *= AlignedScaling3(v0); 336 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 337 t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); 338 t1 = t1 * v0.asDiagonal(); 339 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 340 // transformation * translation 341 t0.translate(v0); 342 t1 = t1 * Translation3(v0); 343 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 344 // translation * transformation 345 t0.pretranslate(v0); 346 t1 = Translation3(v0) * t1; 347 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 348 349 // transform * quaternion 350 t0.rotate(q1); 351 t1 = t1 * q1; 352 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 353 354 // translation * quaternion 355 t0.translate(v1).rotate(q1); 356 t1 = t1 * (Translation3(v1) * q1); 357 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 358 359 // aligned scaling * quaternion 360 t0.scale(v1).rotate(q1); 361 t1 = t1 * (AlignedScaling3(v1) * q1); 362 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 363 364 // quaternion * transform 365 t0.prerotate(q1); 366 t1 = q1 * t1; 367 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 368 369 // quaternion * translation 370 t0.rotate(q1).translate(v1); 371 t1 = t1 * (q1 * Translation3(v1)); 372 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 373 374 // quaternion * aligned scaling 375 t0.rotate(q1).scale(v1); 376 t1 = t1 * (q1 * AlignedScaling3(v1)); 377 VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); 378 379 // test transform inversion 380 t0.setIdentity(); 381 t0.translate(v0); 382 do { 383 t0.linear().setRandom(); 384 } while(t0.linear().jacobiSvd().singularValues()(2)<test_precision<Scalar>()); 385 Matrix4 t044 = Matrix4::Zero(); 386 t044(3,3) = 1; 387 t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); 388 VERIFY_IS_APPROX(t0.inverse(Affine).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4)); 389 t0.setIdentity(); 390 t0.translate(v0).rotate(q1); 391 t044 = Matrix4::Zero(); 392 t044(3,3) = 1; 393 t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); 394 VERIFY_IS_APPROX(t0.inverse(Isometry).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4)); 395 396 Matrix3 mat_rotation, mat_scaling; 397 t0.setIdentity(); 398 t0.translate(v0).rotate(q1).scale(v1); 399 t0.computeRotationScaling(&mat_rotation, &mat_scaling); 400 VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling); 401 VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); 402 VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); 403 t0.computeScalingRotation(&mat_scaling, &mat_rotation); 404 VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation); 405 VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); 406 VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); 407 408 // test casting 409 Transform<float,3,Mode> t1f = t1.template cast<float>(); 410 VERIFY_IS_APPROX(t1f.template cast<Scalar>(),t1); 411 Transform<double,3,Mode> t1d = t1.template cast<double>(); 412 VERIFY_IS_APPROX(t1d.template cast<Scalar>(),t1); 413 414 Translation3 tr1(v0); 415 Translation<float,3> tr1f = tr1.template cast<float>(); 416 VERIFY_IS_APPROX(tr1f.template cast<Scalar>(),tr1); 417 Translation<double,3> tr1d = tr1.template cast<double>(); 418 VERIFY_IS_APPROX(tr1d.template cast<Scalar>(),tr1); 419 420 AngleAxis<float> aa1f = aa1.template cast<float>(); 421 VERIFY_IS_APPROX(aa1f.template cast<Scalar>(),aa1); 422 AngleAxis<double> aa1d = aa1.template cast<double>(); 423 VERIFY_IS_APPROX(aa1d.template cast<Scalar>(),aa1); 424 425 Rotation2D<Scalar> r2d1(internal::random<Scalar>()); 426 Rotation2D<float> r2d1f = r2d1.template cast<float>(); 427 VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1); 428 Rotation2D<double> r2d1d = r2d1.template cast<double>(); 429 VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1); 430 431 for(int k=0; k<100; ++k) 432 { 433 Scalar angle = internal::random<Scalar>(-100,100); 434 Rotation2D<Scalar> rot2(angle); 435 VERIFY( rot2.smallestPositiveAngle() >= 0 ); 436 VERIFY( rot2.smallestPositiveAngle() <= Scalar(2)*Scalar(EIGEN_PI) ); 437 VERIFY_IS_APPROX( angleToVec(rot2.smallestPositiveAngle()), angleToVec(rot2.angle()) ); 438 439 VERIFY( rot2.smallestAngle() >= -Scalar(EIGEN_PI) ); 440 VERIFY( rot2.smallestAngle() <= Scalar(EIGEN_PI) ); 441 VERIFY_IS_APPROX( angleToVec(rot2.smallestAngle()), angleToVec(rot2.angle()) ); 442 443 Matrix<Scalar,2,2> rot2_as_mat(rot2); 444 Rotation2D<Scalar> rot3(rot2_as_mat); 445 VERIFY_IS_APPROX( angleToVec(rot2.smallestAngle()), angleToVec(rot3.angle()) ); 446 } 447 448 s0 = internal::random<Scalar>(-100,100); 449 s1 = internal::random<Scalar>(-100,100); 450 Rotation2D<Scalar> R0(s0), R1(s1); 451 452 t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0)); 453 t21 = Translation2(v20) * R0 * Eigen::Scaling(s0); 454 VERIFY_IS_APPROX(t20,t21); 455 456 t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0)); 457 t21 = Translation2(v20) * Eigen::Scaling(s0); 458 VERIFY_IS_APPROX(t20,t21); 459 460 VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle()); 461 VERIFY_IS_APPROX( angleToVec(R1.smallestPositiveAngle()), angleToVec((R0.slerp(1, R1)).smallestPositiveAngle()) ); 462 VERIFY_IS_APPROX(R0.smallestPositiveAngle(), (R0.slerp(0.5, R0)).smallestPositiveAngle()); 463 464 if(std::cos(s0)>0) 465 VERIFY_IS_MUCH_SMALLER_THAN((R0.slerp(0.5, R0.inverse())).smallestAngle(), Scalar(1)); 466 else 467 VERIFY_IS_APPROX(Scalar(EIGEN_PI), (R0.slerp(0.5, R0.inverse())).smallestPositiveAngle()); 468 469 // Check path length 470 Scalar l = 0; 471 int path_steps = 100; 472 for(int k=0; k<path_steps; ++k) 473 { 474 Scalar a1 = R0.slerp(Scalar(k)/Scalar(path_steps), R1).angle(); 475 Scalar a2 = R0.slerp(Scalar(k+1)/Scalar(path_steps), R1).angle(); 476 l += std::abs(a2-a1); 477 } 478 VERIFY(l<=Scalar(EIGEN_PI)*(Scalar(1)+NumTraits<Scalar>::epsilon()*Scalar(path_steps/2))); 479 480 // check basic features 481 { 482 Rotation2D<Scalar> r1; // default ctor 483 r1 = Rotation2D<Scalar>(s0); // copy assignment 484 VERIFY_IS_APPROX(r1.angle(),s0); 485 Rotation2D<Scalar> r2(r1); // copy ctor 486 VERIFY_IS_APPROX(r2.angle(),s0); 487 } 488 489 { 490 Transform3 t32(Matrix4::Random()), t33, t34; 491 t34 = t33 = t32; 492 t32.scale(v0); 493 t33*=AlignedScaling3(v0); 494 VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); 495 t33 = t34 * AlignedScaling3(v0); 496 VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); 497 } 498 499 } 500 501 template<typename A1, typename A2, typename P, typename Q, typename V, typename H> 502 void transform_associativity_left(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) 503 { 504 VERIFY_IS_APPROX( q*(a1*v), (q*a1)*v ); 505 VERIFY_IS_APPROX( q*(a2*v), (q*a2)*v ); 506 VERIFY_IS_APPROX( q*(p*h).hnormalized(), ((q*p)*h).hnormalized() ); 507 } 508 509 template<typename A1, typename A2, typename P, typename Q, typename V, typename H> 510 void transform_associativity2(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) 511 { 512 VERIFY_IS_APPROX( a1*(q*v), (a1*q)*v ); 513 VERIFY_IS_APPROX( a2*(q*v), (a2*q)*v ); 514 VERIFY_IS_APPROX( p *(q*v).homogeneous(), (p *q)*v.homogeneous() ); 515 516 transform_associativity_left(a1, a2,p, q, v, h); 517 } 518 519 template<typename Scalar, int Dim, int Options,typename RotationType> 520 void transform_associativity(const RotationType& R) 521 { 522 typedef Matrix<Scalar,Dim,1> VectorType; 523 typedef Matrix<Scalar,Dim+1,1> HVectorType; 524 typedef Matrix<Scalar,Dim,Dim> LinearType; 525 typedef Matrix<Scalar,Dim+1,Dim+1> MatrixType; 526 typedef Transform<Scalar,Dim,AffineCompact,Options> AffineCompactType; 527 typedef Transform<Scalar,Dim,Affine,Options> AffineType; 528 typedef Transform<Scalar,Dim,Projective,Options> ProjectiveType; 529 typedef DiagonalMatrix<Scalar,Dim> ScalingType; 530 typedef Translation<Scalar,Dim> TranslationType; 531 532 AffineCompactType A1c; A1c.matrix().setRandom(); 533 AffineCompactType A2c; A2c.matrix().setRandom(); 534 AffineType A1(A1c); 535 AffineType A2(A2c); 536 ProjectiveType P1; P1.matrix().setRandom(); 537 VectorType v1 = VectorType::Random(); 538 VectorType v2 = VectorType::Random(); 539 HVectorType h1 = HVectorType::Random(); 540 Scalar s1 = internal::random<Scalar>(); 541 LinearType L = LinearType::Random(); 542 MatrixType M = MatrixType::Random(); 543 544 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2, v2, h1) ); 545 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2c, v2, h1) ); 546 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, v1.asDiagonal(), v2, h1) ); 547 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, ScalingType(v1), v2, h1) ); 548 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(v1), v2, h1) ); 549 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(s1), v2, h1) ); 550 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, TranslationType(v1), v2, h1) ); 551 CALL_SUBTEST( transform_associativity_left(A1c, A1, P1, L, v2, h1) ); 552 CALL_SUBTEST( transform_associativity2(A1c, A1, P1, R, v2, h1) ); 553 554 VERIFY_IS_APPROX( A1*(M*h1), (A1*M)*h1 ); 555 VERIFY_IS_APPROX( A1c*(M*h1), (A1c*M)*h1 ); 556 VERIFY_IS_APPROX( P1*(M*h1), (P1*M)*h1 ); 557 558 VERIFY_IS_APPROX( M*(A1*h1), (M*A1)*h1 ); 559 VERIFY_IS_APPROX( M*(A1c*h1), (M*A1c)*h1 ); 560 VERIFY_IS_APPROX( M*(P1*h1), ((M*P1)*h1) ); 561 } 562 563 template<typename Scalar> void transform_alignment() 564 { 565 typedef Transform<Scalar,3,Projective,AutoAlign> Projective3a; 566 typedef Transform<Scalar,3,Projective,DontAlign> Projective3u; 567 568 EIGEN_ALIGN_MAX Scalar array1[16]; 569 EIGEN_ALIGN_MAX Scalar array2[16]; 570 EIGEN_ALIGN_MAX Scalar array3[16+1]; 571 Scalar* array3u = array3+1; 572 573 Projective3a *p1 = ::new(reinterpret_cast<void*>(array1)) Projective3a; 574 Projective3u *p2 = ::new(reinterpret_cast<void*>(array2)) Projective3u; 575 Projective3u *p3 = ::new(reinterpret_cast<void*>(array3u)) Projective3u; 576 577 p1->matrix().setRandom(); 578 *p2 = *p1; 579 *p3 = *p1; 580 581 VERIFY_IS_APPROX(p1->matrix(), p2->matrix()); 582 VERIFY_IS_APPROX(p1->matrix(), p3->matrix()); 583 584 VERIFY_IS_APPROX( (*p1) * (*p1), (*p2)*(*p3)); 585 586 #if defined(EIGEN_VECTORIZE) && EIGEN_MAX_STATIC_ALIGN_BYTES>0 587 if(internal::packet_traits<Scalar>::Vectorizable) 588 VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Projective3a)); 589 #endif 590 } 591 592 template<typename Scalar, int Dim, int Options> void transform_products() 593 { 594 typedef Matrix<Scalar,Dim+1,Dim+1> Mat; 595 typedef Transform<Scalar,Dim,Projective,Options> Proj; 596 typedef Transform<Scalar,Dim,Affine,Options> Aff; 597 typedef Transform<Scalar,Dim,AffineCompact,Options> AffC; 598 599 Proj p; p.matrix().setRandom(); 600 Aff a; a.linear().setRandom(); a.translation().setRandom(); 601 AffC ac = a; 602 603 Mat p_m(p.matrix()), a_m(a.matrix()); 604 605 VERIFY_IS_APPROX((p*p).matrix(), p_m*p_m); 606 VERIFY_IS_APPROX((a*a).matrix(), a_m*a_m); 607 VERIFY_IS_APPROX((p*a).matrix(), p_m*a_m); 608 VERIFY_IS_APPROX((a*p).matrix(), a_m*p_m); 609 VERIFY_IS_APPROX((ac*a).matrix(), a_m*a_m); 610 VERIFY_IS_APPROX((a*ac).matrix(), a_m*a_m); 611 VERIFY_IS_APPROX((p*ac).matrix(), p_m*a_m); 612 VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m); 613 } 614 615 void test_geo_transformations() 616 { 617 for(int i = 0; i < g_repeat; i++) { 618 CALL_SUBTEST_1(( transformations<double,Affine,AutoAlign>() )); 619 CALL_SUBTEST_1(( non_projective_only<double,Affine,AutoAlign>() )); 620 621 CALL_SUBTEST_2(( transformations<float,AffineCompact,AutoAlign>() )); 622 CALL_SUBTEST_2(( non_projective_only<float,AffineCompact,AutoAlign>() )); 623 CALL_SUBTEST_2(( transform_alignment<float>() )); 624 625 CALL_SUBTEST_3(( transformations<double,Projective,AutoAlign>() )); 626 CALL_SUBTEST_3(( transformations<double,Projective,DontAlign>() )); 627 CALL_SUBTEST_3(( transform_alignment<double>() )); 628 629 CALL_SUBTEST_4(( transformations<float,Affine,RowMajor|AutoAlign>() )); 630 CALL_SUBTEST_4(( non_projective_only<float,Affine,RowMajor>() )); 631 632 CALL_SUBTEST_5(( transformations<double,AffineCompact,RowMajor|AutoAlign>() )); 633 CALL_SUBTEST_5(( non_projective_only<double,AffineCompact,RowMajor>() )); 634 635 CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|AutoAlign>() )); 636 CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|DontAlign>() )); 637 638 639 CALL_SUBTEST_7(( transform_products<double,3,RowMajor|AutoAlign>() )); 640 CALL_SUBTEST_7(( transform_products<float,2,AutoAlign>() )); 641 642 CALL_SUBTEST_8(( transform_associativity<double,2,ColMajor>(Rotation2D<double>(internal::random<double>()*double(EIGEN_PI))) )); 643 CALL_SUBTEST_8(( transform_associativity<double,3,ColMajor>(Quaterniond::UnitRandom()) )); 644 } 645 } 646
