• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 template<typename T>
angleToVec(T a)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
dont_over_optimize(T & x)24 void dont_over_optimize(T& x) { volatile typename T::Scalar tmp = x(0); x(0) = tmp; }
25 
non_projective_only()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 
transformations()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>
transform_associativity_left(const A1 & a1,const A2 & a2,const P & p,const Q & q,const V & v,const H & 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>
transform_associativity2(const A1 & a1,const A2 & a2,const P & p,const Q & q,const V & v,const H & 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>
transform_associativity(const RotationType & R)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 
transform_alignment()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 
transform_products()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 
test_geo_transformations()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