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 Scalar, int Mode, int Options> void non_projective_only()
|
---|
16 | {
|
---|
17 | /* this test covers the following files:
|
---|
18 | Cross.h Quaternion.h, Transform.cpp
|
---|
19 | */
|
---|
20 | typedef Matrix<Scalar,3,1> Vector3;
|
---|
21 | typedef Quaternion<Scalar> Quaternionx;
|
---|
22 | typedef AngleAxis<Scalar> AngleAxisx;
|
---|
23 | typedef Transform<Scalar,3,Mode,Options> Transform3;
|
---|
24 | typedef DiagonalMatrix<Scalar,3> AlignedScaling3;
|
---|
25 | typedef Translation<Scalar,3> Translation3;
|
---|
26 |
|
---|
27 | Vector3 v0 = Vector3::Random(),
|
---|
28 | v1 = Vector3::Random();
|
---|
29 |
|
---|
30 | Transform3 t0, t1, t2;
|
---|
31 |
|
---|
32 | Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
|
---|
33 |
|
---|
34 | Quaternionx q1, q2;
|
---|
35 |
|
---|
36 | q1 = AngleAxisx(a, v0.normalized());
|
---|
37 |
|
---|
38 | t0 = Transform3::Identity();
|
---|
39 | VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
|
---|
40 |
|
---|
41 | t0.linear() = q1.toRotationMatrix();
|
---|
42 |
|
---|
43 | v0 << 50, 2, 1;
|
---|
44 | t0.scale(v0);
|
---|
45 |
|
---|
46 | VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template head<3>().norm(), v0.x());
|
---|
47 |
|
---|
48 | t0.setIdentity();
|
---|
49 | t1.setIdentity();
|
---|
50 | v1 << 1, 2, 3;
|
---|
51 | t0.linear() = q1.toRotationMatrix();
|
---|
52 | t0.pretranslate(v0);
|
---|
53 | t0.scale(v1);
|
---|
54 | t1.linear() = q1.conjugate().toRotationMatrix();
|
---|
55 | t1.prescale(v1.cwiseInverse());
|
---|
56 | t1.translate(-v0);
|
---|
57 |
|
---|
58 | VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>()));
|
---|
59 |
|
---|
60 | t1.fromPositionOrientationScale(v0, q1, v1);
|
---|
61 | VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
|
---|
62 | VERIFY_IS_APPROX(t1*v1, t0*v1);
|
---|
63 |
|
---|
64 | // translation * vector
|
---|
65 | t0.setIdentity();
|
---|
66 | t0.translate(v0);
|
---|
67 | VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1);
|
---|
68 |
|
---|
69 | // AlignedScaling * vector
|
---|
70 | t0.setIdentity();
|
---|
71 | t0.scale(v0);
|
---|
72 | VERIFY_IS_APPROX((t0 * v1).template head<3>(), AlignedScaling3(v0) * v1);
|
---|
73 | }
|
---|
74 |
|
---|
75 | template<typename Scalar, int Mode, int Options> void transformations()
|
---|
76 | {
|
---|
77 | /* this test covers the following files:
|
---|
78 | Cross.h Quaternion.h, Transform.cpp
|
---|
79 | */
|
---|
80 | using std::cos;
|
---|
81 | using std::abs;
|
---|
82 | typedef Matrix<Scalar,3,3> Matrix3;
|
---|
83 | typedef Matrix<Scalar,4,4> Matrix4;
|
---|
84 | typedef Matrix<Scalar,2,1> Vector2;
|
---|
85 | typedef Matrix<Scalar,3,1> Vector3;
|
---|
86 | typedef Matrix<Scalar,4,1> Vector4;
|
---|
87 | typedef Quaternion<Scalar> Quaternionx;
|
---|
88 | typedef AngleAxis<Scalar> AngleAxisx;
|
---|
89 | typedef Transform<Scalar,2,Mode,Options> Transform2;
|
---|
90 | typedef Transform<Scalar,3,Mode,Options> Transform3;
|
---|
91 | typedef typename Transform3::MatrixType MatrixType;
|
---|
92 | typedef DiagonalMatrix<Scalar,3> AlignedScaling3;
|
---|
93 | typedef Translation<Scalar,2> Translation2;
|
---|
94 | typedef Translation<Scalar,3> Translation3;
|
---|
95 |
|
---|
96 | Vector3 v0 = Vector3::Random(),
|
---|
97 | v1 = Vector3::Random();
|
---|
98 | Matrix3 matrot1, m;
|
---|
99 |
|
---|
100 | Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
|
---|
101 | Scalar s0 = internal::random<Scalar>(),
|
---|
102 | s1 = internal::random<Scalar>();
|
---|
103 |
|
---|
104 | while(v0.norm() < test_precision<Scalar>()) v0 = Vector3::Random();
|
---|
105 | while(v1.norm() < test_precision<Scalar>()) v1 = Vector3::Random();
|
---|
106 |
|
---|
107 |
|
---|
108 | VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0);
|
---|
109 | VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0);
|
---|
110 | if(abs(cos(a)) > test_precision<Scalar>())
|
---|
111 | {
|
---|
112 | VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0));
|
---|
113 | }
|
---|
114 | m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint();
|
---|
115 | VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized()));
|
---|
116 | VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m);
|
---|
117 |
|
---|
118 | Quaternionx q1, q2;
|
---|
119 | q1 = AngleAxisx(a, v0.normalized());
|
---|
120 | q2 = AngleAxisx(a, v1.normalized());
|
---|
121 |
|
---|
122 | // rotation matrix conversion
|
---|
123 | matrot1 = AngleAxisx(Scalar(0.1), Vector3::UnitX())
|
---|
124 | * AngleAxisx(Scalar(0.2), Vector3::UnitY())
|
---|
125 | * AngleAxisx(Scalar(0.3), Vector3::UnitZ());
|
---|
126 | VERIFY_IS_APPROX(matrot1 * v1,
|
---|
127 | AngleAxisx(Scalar(0.1), Vector3(1,0,0)).toRotationMatrix()
|
---|
128 | * (AngleAxisx(Scalar(0.2), Vector3(0,1,0)).toRotationMatrix()
|
---|
129 | * (AngleAxisx(Scalar(0.3), Vector3(0,0,1)).toRotationMatrix() * v1)));
|
---|
130 |
|
---|
131 | // angle-axis conversion
|
---|
132 | AngleAxisx aa = AngleAxisx(q1);
|
---|
133 | VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
|
---|
134 |
|
---|
135 | if(abs(aa.angle()) > NumTraits<Scalar>::dummy_precision())
|
---|
136 | {
|
---|
137 | VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
|
---|
138 | }
|
---|
139 |
|
---|
140 | aa.fromRotationMatrix(aa.toRotationMatrix());
|
---|
141 | VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
|
---|
142 | if(abs(aa.angle()) > NumTraits<Scalar>::dummy_precision())
|
---|
143 | {
|
---|
144 | VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
|
---|
145 | }
|
---|
146 |
|
---|
147 | // AngleAxis
|
---|
148 | VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(),
|
---|
149 | Quaternionx(AngleAxisx(a,v1.normalized())).toRotationMatrix());
|
---|
150 |
|
---|
151 | AngleAxisx aa1;
|
---|
152 | m = q1.toRotationMatrix();
|
---|
153 | aa1 = m;
|
---|
154 | VERIFY_IS_APPROX(AngleAxisx(m).toRotationMatrix(),
|
---|
155 | Quaternionx(m).toRotationMatrix());
|
---|
156 |
|
---|
157 | // Transform
|
---|
158 | // TODO complete the tests !
|
---|
159 | a = 0;
|
---|
160 | while (abs(a)<Scalar(0.1))
|
---|
161 | a = internal::random<Scalar>(-Scalar(0.4)*Scalar(M_PI), Scalar(0.4)*Scalar(M_PI));
|
---|
162 | q1 = AngleAxisx(a, v0.normalized());
|
---|
163 | Transform3 t0, t1, t2;
|
---|
164 |
|
---|
165 | // first test setIdentity() and Identity()
|
---|
166 | t0.setIdentity();
|
---|
167 | VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
|
---|
168 | t0.matrix().setZero();
|
---|
169 | t0 = Transform3::Identity();
|
---|
170 | VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity());
|
---|
171 |
|
---|
172 | t0.setIdentity();
|
---|
173 | t1.setIdentity();
|
---|
174 | v1 << 1, 2, 3;
|
---|
175 | t0.linear() = q1.toRotationMatrix();
|
---|
176 | t0.pretranslate(v0);
|
---|
177 | t0.scale(v1);
|
---|
178 | t1.linear() = q1.conjugate().toRotationMatrix();
|
---|
179 | t1.prescale(v1.cwiseInverse());
|
---|
180 | t1.translate(-v0);
|
---|
181 |
|
---|
182 | VERIFY((t0 * t1).matrix().isIdentity(test_precision<Scalar>()));
|
---|
183 |
|
---|
184 | t1.fromPositionOrientationScale(v0, q1, v1);
|
---|
185 | VERIFY_IS_APPROX(t1.matrix(), t0.matrix());
|
---|
186 |
|
---|
187 | t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix());
|
---|
188 | t1.setIdentity(); t1.scale(v0).rotate(q1);
|
---|
189 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
190 |
|
---|
191 | t0.setIdentity(); t0.scale(v0).rotate(AngleAxisx(q1));
|
---|
192 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
193 |
|
---|
194 | VERIFY_IS_APPROX(t0.scale(a).matrix(), t1.scale(Vector3::Constant(a)).matrix());
|
---|
195 | VERIFY_IS_APPROX(t0.prescale(a).matrix(), t1.prescale(Vector3::Constant(a)).matrix());
|
---|
196 |
|
---|
197 | // More transform constructors, operator=, operator*=
|
---|
198 |
|
---|
199 | Matrix3 mat3 = Matrix3::Random();
|
---|
200 | Matrix4 mat4;
|
---|
201 | mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose();
|
---|
202 | Transform3 tmat3(mat3), tmat4(mat4);
|
---|
203 | if(Mode!=int(AffineCompact))
|
---|
204 | tmat4.matrix()(3,3) = Scalar(1);
|
---|
205 | VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix());
|
---|
206 |
|
---|
207 | Scalar a3 = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
|
---|
208 | Vector3 v3 = Vector3::Random().normalized();
|
---|
209 | AngleAxisx aa3(a3, v3);
|
---|
210 | Transform3 t3(aa3);
|
---|
211 | Transform3 t4;
|
---|
212 | t4 = aa3;
|
---|
213 | VERIFY_IS_APPROX(t3.matrix(), t4.matrix());
|
---|
214 | t4.rotate(AngleAxisx(-a3,v3));
|
---|
215 | VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity());
|
---|
216 | t4 *= aa3;
|
---|
217 | VERIFY_IS_APPROX(t3.matrix(), t4.matrix());
|
---|
218 |
|
---|
219 | v3 = Vector3::Random();
|
---|
220 | Translation3 tv3(v3);
|
---|
221 | Transform3 t5(tv3);
|
---|
222 | t4 = tv3;
|
---|
223 | VERIFY_IS_APPROX(t5.matrix(), t4.matrix());
|
---|
224 | t4.translate(-v3);
|
---|
225 | VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity());
|
---|
226 | t4 *= tv3;
|
---|
227 | VERIFY_IS_APPROX(t5.matrix(), t4.matrix());
|
---|
228 |
|
---|
229 | AlignedScaling3 sv3(v3);
|
---|
230 | Transform3 t6(sv3);
|
---|
231 | t4 = sv3;
|
---|
232 | VERIFY_IS_APPROX(t6.matrix(), t4.matrix());
|
---|
233 | t4.scale(v3.cwiseInverse());
|
---|
234 | VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity());
|
---|
235 | t4 *= sv3;
|
---|
236 | VERIFY_IS_APPROX(t6.matrix(), t4.matrix());
|
---|
237 |
|
---|
238 | // matrix * transform
|
---|
239 | VERIFY_IS_APPROX((t3.matrix()*t4).matrix(), (t3*t4).matrix());
|
---|
240 |
|
---|
241 | // chained Transform product
|
---|
242 | VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix());
|
---|
243 |
|
---|
244 | // check that Transform product doesn't have aliasing problems
|
---|
245 | t5 = t4;
|
---|
246 | t5 = t5*t5;
|
---|
247 | VERIFY_IS_APPROX(t5, t4*t4);
|
---|
248 |
|
---|
249 | // 2D transformation
|
---|
250 | Transform2 t20, t21;
|
---|
251 | Vector2 v20 = Vector2::Random();
|
---|
252 | Vector2 v21 = Vector2::Random();
|
---|
253 | for (int k=0; k<2; ++k)
|
---|
254 | if (abs(v21[k])<Scalar(1e-3)) v21[k] = Scalar(1e-3);
|
---|
255 | t21.setIdentity();
|
---|
256 | t21.linear() = Rotation2D<Scalar>(a).toRotationMatrix();
|
---|
257 | VERIFY_IS_APPROX(t20.fromPositionOrientationScale(v20,a,v21).matrix(),
|
---|
258 | t21.pretranslate(v20).scale(v21).matrix());
|
---|
259 |
|
---|
260 | t21.setIdentity();
|
---|
261 | t21.linear() = Rotation2D<Scalar>(-a).toRotationMatrix();
|
---|
262 | VERIFY( (t20.fromPositionOrientationScale(v20,a,v21)
|
---|
263 | * (t21.prescale(v21.cwiseInverse()).translate(-v20))).matrix().isIdentity(test_precision<Scalar>()) );
|
---|
264 |
|
---|
265 | // Transform - new API
|
---|
266 | // 3D
|
---|
267 | t0.setIdentity();
|
---|
268 | t0.rotate(q1).scale(v0).translate(v0);
|
---|
269 | // mat * aligned scaling and mat * translation
|
---|
270 | t1 = (Matrix3(q1) * AlignedScaling3(v0)) * Translation3(v0);
|
---|
271 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
272 | t1 = (Matrix3(q1) * Eigen::Scaling(v0)) * Translation3(v0);
|
---|
273 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
274 | t1 = (q1 * Eigen::Scaling(v0)) * Translation3(v0);
|
---|
275 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
276 | // mat * transformation and aligned scaling * translation
|
---|
277 | t1 = Matrix3(q1) * (AlignedScaling3(v0) * Translation3(v0));
|
---|
278 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
279 |
|
---|
280 |
|
---|
281 | t0.setIdentity();
|
---|
282 | t0.scale(s0).translate(v0);
|
---|
283 | t1 = Eigen::Scaling(s0) * Translation3(v0);
|
---|
284 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
285 | t0.prescale(s0);
|
---|
286 | t1 = Eigen::Scaling(s0) * t1;
|
---|
287 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
288 |
|
---|
289 | t0 = t3;
|
---|
290 | t0.scale(s0);
|
---|
291 | t1 = t3 * Eigen::Scaling(s0,s0,s0);
|
---|
292 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
293 | t0.prescale(s0);
|
---|
294 | t1 = Eigen::Scaling(s0,s0,s0) * t1;
|
---|
295 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
296 |
|
---|
297 | t0 = t3;
|
---|
298 | t0.scale(s0);
|
---|
299 | t1 = t3 * Eigen::Scaling(s0);
|
---|
300 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
301 | t0.prescale(s0);
|
---|
302 | t1 = Eigen::Scaling(s0) * t1;
|
---|
303 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
304 |
|
---|
305 | t0.setIdentity();
|
---|
306 | t0.prerotate(q1).prescale(v0).pretranslate(v0);
|
---|
307 | // translation * aligned scaling and transformation * mat
|
---|
308 | t1 = (Translation3(v0) * AlignedScaling3(v0)) * Transform3(q1);
|
---|
309 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
310 | // scaling * mat and translation * mat
|
---|
311 | t1 = Translation3(v0) * (AlignedScaling3(v0) * Transform3(q1));
|
---|
312 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
313 |
|
---|
314 | t0.setIdentity();
|
---|
315 | t0.scale(v0).translate(v0).rotate(q1);
|
---|
316 | // translation * mat and aligned scaling * transformation
|
---|
317 | t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1));
|
---|
318 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
319 | // transformation * aligned scaling
|
---|
320 | t0.scale(v0);
|
---|
321 | t1 *= AlignedScaling3(v0);
|
---|
322 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
323 | // transformation * translation
|
---|
324 | t0.translate(v0);
|
---|
325 | t1 = t1 * Translation3(v0);
|
---|
326 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
327 | // translation * transformation
|
---|
328 | t0.pretranslate(v0);
|
---|
329 | t1 = Translation3(v0) * t1;
|
---|
330 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
331 |
|
---|
332 | // transform * quaternion
|
---|
333 | t0.rotate(q1);
|
---|
334 | t1 = t1 * q1;
|
---|
335 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
336 |
|
---|
337 | // translation * quaternion
|
---|
338 | t0.translate(v1).rotate(q1);
|
---|
339 | t1 = t1 * (Translation3(v1) * q1);
|
---|
340 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
341 |
|
---|
342 | // aligned scaling * quaternion
|
---|
343 | t0.scale(v1).rotate(q1);
|
---|
344 | t1 = t1 * (AlignedScaling3(v1) * q1);
|
---|
345 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
346 |
|
---|
347 | // quaternion * transform
|
---|
348 | t0.prerotate(q1);
|
---|
349 | t1 = q1 * t1;
|
---|
350 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
351 |
|
---|
352 | // quaternion * translation
|
---|
353 | t0.rotate(q1).translate(v1);
|
---|
354 | t1 = t1 * (q1 * Translation3(v1));
|
---|
355 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
356 |
|
---|
357 | // quaternion * aligned scaling
|
---|
358 | t0.rotate(q1).scale(v1);
|
---|
359 | t1 = t1 * (q1 * AlignedScaling3(v1));
|
---|
360 | VERIFY_IS_APPROX(t0.matrix(), t1.matrix());
|
---|
361 |
|
---|
362 | // test transform inversion
|
---|
363 | t0.setIdentity();
|
---|
364 | t0.translate(v0);
|
---|
365 | do {
|
---|
366 | t0.linear().setRandom();
|
---|
367 | } while(t0.linear().jacobiSvd().singularValues()(2)<test_precision<Scalar>());
|
---|
368 | Matrix4 t044 = Matrix4::Zero();
|
---|
369 | t044(3,3) = 1;
|
---|
370 | t044.block(0,0,t0.matrix().rows(),4) = t0.matrix();
|
---|
371 | VERIFY_IS_APPROX(t0.inverse(Affine).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4));
|
---|
372 | t0.setIdentity();
|
---|
373 | t0.translate(v0).rotate(q1);
|
---|
374 | t044 = Matrix4::Zero();
|
---|
375 | t044(3,3) = 1;
|
---|
376 | t044.block(0,0,t0.matrix().rows(),4) = t0.matrix();
|
---|
377 | VERIFY_IS_APPROX(t0.inverse(Isometry).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4));
|
---|
378 |
|
---|
379 | Matrix3 mat_rotation, mat_scaling;
|
---|
380 | t0.setIdentity();
|
---|
381 | t0.translate(v0).rotate(q1).scale(v1);
|
---|
382 | t0.computeRotationScaling(&mat_rotation, &mat_scaling);
|
---|
383 | VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling);
|
---|
384 | VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
|
---|
385 | VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));
|
---|
386 | t0.computeScalingRotation(&mat_scaling, &mat_rotation);
|
---|
387 | VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation);
|
---|
388 | VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
|
---|
389 | VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));
|
---|
390 |
|
---|
391 | // test casting
|
---|
392 | Transform<float,3,Mode> t1f = t1.template cast<float>();
|
---|
393 | VERIFY_IS_APPROX(t1f.template cast<Scalar>(),t1);
|
---|
394 | Transform<double,3,Mode> t1d = t1.template cast<double>();
|
---|
395 | VERIFY_IS_APPROX(t1d.template cast<Scalar>(),t1);
|
---|
396 |
|
---|
397 | Translation3 tr1(v0);
|
---|
398 | Translation<float,3> tr1f = tr1.template cast<float>();
|
---|
399 | VERIFY_IS_APPROX(tr1f.template cast<Scalar>(),tr1);
|
---|
400 | Translation<double,3> tr1d = tr1.template cast<double>();
|
---|
401 | VERIFY_IS_APPROX(tr1d.template cast<Scalar>(),tr1);
|
---|
402 |
|
---|
403 | AngleAxis<float> aa1f = aa1.template cast<float>();
|
---|
404 | VERIFY_IS_APPROX(aa1f.template cast<Scalar>(),aa1);
|
---|
405 | AngleAxis<double> aa1d = aa1.template cast<double>();
|
---|
406 | VERIFY_IS_APPROX(aa1d.template cast<Scalar>(),aa1);
|
---|
407 |
|
---|
408 | Rotation2D<Scalar> r2d1(internal::random<Scalar>());
|
---|
409 | Rotation2D<float> r2d1f = r2d1.template cast<float>();
|
---|
410 | VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1);
|
---|
411 | Rotation2D<double> r2d1d = r2d1.template cast<double>();
|
---|
412 | VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);
|
---|
413 |
|
---|
414 | t20 = Translation2(v20) * (Rotation2D<Scalar>(s0) * Eigen::Scaling(s0));
|
---|
415 | t21 = Translation2(v20) * Rotation2D<Scalar>(s0) * Eigen::Scaling(s0);
|
---|
416 | VERIFY_IS_APPROX(t20,t21);
|
---|
417 |
|
---|
418 | Rotation2D<Scalar> R0(s0), R1(s1);
|
---|
419 | t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0));
|
---|
420 | t21 = Translation2(v20) * R0 * Eigen::Scaling(s0);
|
---|
421 | VERIFY_IS_APPROX(t20,t21);
|
---|
422 |
|
---|
423 | t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0));
|
---|
424 | t21 = Translation2(v20) * Eigen::Scaling(s0);
|
---|
425 | VERIFY_IS_APPROX(t20,t21);
|
---|
426 |
|
---|
427 | VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle());
|
---|
428 | VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle());
|
---|
429 | VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle());
|
---|
430 | VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle());
|
---|
431 |
|
---|
432 | // check basic features
|
---|
433 | {
|
---|
434 | Rotation2D<Scalar> r1; // default ctor
|
---|
435 | r1 = Rotation2D<Scalar>(s0); // copy assignment
|
---|
436 | VERIFY_IS_APPROX(r1.angle(),s0);
|
---|
437 | Rotation2D<Scalar> r2(r1); // copy ctor
|
---|
438 | VERIFY_IS_APPROX(r2.angle(),s0);
|
---|
439 | }
|
---|
440 | }
|
---|
441 |
|
---|
442 | template<typename Scalar> void transform_alignment()
|
---|
443 | {
|
---|
444 | typedef Transform<Scalar,3,Projective,AutoAlign> Projective3a;
|
---|
445 | typedef Transform<Scalar,3,Projective,DontAlign> Projective3u;
|
---|
446 |
|
---|
447 | EIGEN_ALIGN16 Scalar array1[16];
|
---|
448 | EIGEN_ALIGN16 Scalar array2[16];
|
---|
449 | EIGEN_ALIGN16 Scalar array3[16+1];
|
---|
450 | Scalar* array3u = array3+1;
|
---|
451 |
|
---|
452 | Projective3a *p1 = ::new(reinterpret_cast<void*>(array1)) Projective3a;
|
---|
453 | Projective3u *p2 = ::new(reinterpret_cast<void*>(array2)) Projective3u;
|
---|
454 | Projective3u *p3 = ::new(reinterpret_cast<void*>(array3u)) Projective3u;
|
---|
455 |
|
---|
456 | p1->matrix().setRandom();
|
---|
457 | *p2 = *p1;
|
---|
458 | *p3 = *p1;
|
---|
459 |
|
---|
460 | VERIFY_IS_APPROX(p1->matrix(), p2->matrix());
|
---|
461 | VERIFY_IS_APPROX(p1->matrix(), p3->matrix());
|
---|
462 |
|
---|
463 | VERIFY_IS_APPROX( (*p1) * (*p1), (*p2)*(*p3));
|
---|
464 |
|
---|
465 | #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY
|
---|
466 | if(internal::packet_traits<Scalar>::Vectorizable)
|
---|
467 | VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Projective3a));
|
---|
468 | #endif
|
---|
469 | }
|
---|
470 |
|
---|
471 | template<typename Scalar, int Dim, int Options> void transform_products()
|
---|
472 | {
|
---|
473 | typedef Matrix<Scalar,Dim+1,Dim+1> Mat;
|
---|
474 | typedef Transform<Scalar,Dim,Projective,Options> Proj;
|
---|
475 | typedef Transform<Scalar,Dim,Affine,Options> Aff;
|
---|
476 | typedef Transform<Scalar,Dim,AffineCompact,Options> AffC;
|
---|
477 |
|
---|
478 | Proj p; p.matrix().setRandom();
|
---|
479 | Aff a; a.linear().setRandom(); a.translation().setRandom();
|
---|
480 | AffC ac = a;
|
---|
481 |
|
---|
482 | Mat p_m(p.matrix()), a_m(a.matrix());
|
---|
483 |
|
---|
484 | VERIFY_IS_APPROX((p*p).matrix(), p_m*p_m);
|
---|
485 | VERIFY_IS_APPROX((a*a).matrix(), a_m*a_m);
|
---|
486 | VERIFY_IS_APPROX((p*a).matrix(), p_m*a_m);
|
---|
487 | VERIFY_IS_APPROX((a*p).matrix(), a_m*p_m);
|
---|
488 | VERIFY_IS_APPROX((ac*a).matrix(), a_m*a_m);
|
---|
489 | VERIFY_IS_APPROX((a*ac).matrix(), a_m*a_m);
|
---|
490 | VERIFY_IS_APPROX((p*ac).matrix(), p_m*a_m);
|
---|
491 | VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m);
|
---|
492 | }
|
---|
493 |
|
---|
494 | void test_geo_transformations()
|
---|
495 | {
|
---|
496 | for(int i = 0; i < g_repeat; i++) {
|
---|
497 | CALL_SUBTEST_1(( transformations<double,Affine,AutoAlign>() ));
|
---|
498 | CALL_SUBTEST_1(( non_projective_only<double,Affine,AutoAlign>() ));
|
---|
499 |
|
---|
500 | CALL_SUBTEST_2(( transformations<float,AffineCompact,AutoAlign>() ));
|
---|
501 | CALL_SUBTEST_2(( non_projective_only<float,AffineCompact,AutoAlign>() ));
|
---|
502 | CALL_SUBTEST_2(( transform_alignment<float>() ));
|
---|
503 |
|
---|
504 | CALL_SUBTEST_3(( transformations<double,Projective,AutoAlign>() ));
|
---|
505 | CALL_SUBTEST_3(( transformations<double,Projective,DontAlign>() ));
|
---|
506 | CALL_SUBTEST_3(( transform_alignment<double>() ));
|
---|
507 |
|
---|
508 | CALL_SUBTEST_4(( transformations<float,Affine,RowMajor|AutoAlign>() ));
|
---|
509 | CALL_SUBTEST_4(( non_projective_only<float,Affine,RowMajor>() ));
|
---|
510 |
|
---|
511 | CALL_SUBTEST_5(( transformations<double,AffineCompact,RowMajor|AutoAlign>() ));
|
---|
512 | CALL_SUBTEST_5(( non_projective_only<double,AffineCompact,RowMajor>() ));
|
---|
513 |
|
---|
514 | CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|AutoAlign>() ));
|
---|
515 | CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|DontAlign>() ));
|
---|
516 |
|
---|
517 |
|
---|
518 | CALL_SUBTEST_7(( transform_products<double,3,RowMajor|AutoAlign>() ));
|
---|
519 | CALL_SUBTEST_7(( transform_products<float,2,AutoAlign>() ));
|
---|
520 | }
|
---|
521 | }
|
---|