11 #include <Eigen/Geometry>
39 v1 = Vector3::Random();
41 Transform3 t0, t1, t2;
47 q1 = AngleAxisx(
a,
v0.normalized());
49 t0 = Transform3::Identity();
52 t0.linear() = q1.toRotationMatrix();
62 t0.linear() = q1.toRotationMatrix();
65 t1.linear() = q1.conjugate().toRotationMatrix();
66 t1.prescale(
v1.cwiseInverse());
69 VERIFY((t0 * t1).
matrix().isIdentity(test_precision<Scalar>()));
71 t1.fromPositionOrientationScale(
v0, q1,
v1);
108 v1 = Vector3::Random();
112 Scalar s0 = internal::random<Scalar>(), s1 = internal::random<Scalar>();
114 while(
v0.norm() < test_precision<Scalar>())
v0 = Vector3::Random();
115 while(
v1.norm() < test_precision<Scalar>())
v1 = Vector3::Random();
119 if(
abs(
cos(
a)) > test_precision<Scalar>())
123 m = AngleAxisx(
a,
v0.normalized()).toRotationMatrix().adjoint();
128 q1 = AngleAxisx(
a,
v0.normalized());
129 q2 = AngleAxisx(
a,
v1.normalized());
132 matrot1 = AngleAxisx(
Scalar(0.1), Vector3::UnitX())
133 * AngleAxisx(
Scalar(0.2), Vector3::UnitY())
134 * AngleAxisx(
Scalar(0.3), Vector3::UnitZ());
141 AngleAxisx aa = AngleAxisx(q1);
145 if( (
abs(aa.angle()) > test_precision<Scalar>()) && (
abs(aa.axis().dot(
v1.normalized()))<(
Scalar(1)-
Scalar(4)*test_precision<Scalar>())) )
150 aa.fromRotationMatrix(aa.toRotationMatrix());
153 if( (
abs(aa.angle()) > test_precision<Scalar>()) && (
abs(aa.axis().dot(
v1.normalized()))<(
Scalar(1)-
Scalar(4)*test_precision<Scalar>())) )
160 Quaternionx(AngleAxisx(
a,
v1.normalized())).toRotationMatrix());
163 m = q1.toRotationMatrix();
173 q1 = AngleAxisx(
a,
v0.normalized());
174 Transform3 t0, t1, t2;
179 t0.matrix().setZero();
180 t0 = Transform3::Identity();
186 t0.linear() = q1.toRotationMatrix();
189 t1.linear() = q1.conjugate().toRotationMatrix();
190 t1.prescale(
v1.cwiseInverse());
193 VERIFY((t0 * t1).
matrix().isIdentity(test_precision<Scalar>()));
195 t1.fromPositionOrientationScale(
v0, q1,
v1);
198 t0.setIdentity(); t0.scale(
v0).rotate(q1.toRotationMatrix());
199 t1.setIdentity(); t1.scale(
v0).rotate(q1);
202 t0.setIdentity(); t0.scale(
v0).rotate(AngleAxisx(q1));
206 VERIFY_IS_APPROX(t0.prescale(
a).matrix(), t1.prescale(Vector3::Constant(
a)).matrix());
210 Matrix3 mat3 = Matrix3::Random();
212 mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose();
213 Transform3 tmat3(mat3), tmat4(mat4);
215 tmat4.matrix()(3,3) =
Scalar(1);
219 Vector3 v3 = Vector3::Random().normalized();
220 AngleAxisx aa3(
a3,
v3);
225 t4.rotate(AngleAxisx(-
a3,
v3));
231 v3 = Vector3::Random();
234 Translation3 tv3(
v3);
238 t4.translate((-
v3).
eval());
243 AlignedScaling3 sv3(
v3);
247 t4.scale(
v3.cwiseInverse());
265 Vector2 v20 = Vector2::Random();
266 Vector2 v21 = Vector2::Random();
267 for (
int k=0; k<2; ++k)
272 t21.pretranslate(v20).scale(v21).matrix());
276 VERIFY( (t20.fromPositionOrientationScale(v20,
a,v21)
277 * (t21.prescale(v21.cwiseInverse()).translate(-v20))).matrix().isIdentity(test_precision<Scalar>()) );
282 t0.rotate(q1).scale(
v0).translate(
v0);
284 t1 = (Matrix3(q1) * AlignedScaling3(
v0)) * Translation3(
v0);
291 t1 = Matrix3(q1) * (AlignedScaling3(
v0) * Translation3(
v0));
296 t0.scale(s0).translate(
v0);
320 t0.prerotate(q1).prescale(
v0).pretranslate(
v0);
322 t1 = (Translation3(
v0) * AlignedScaling3(
v0)) * Transform3(q1);
325 t1 = Translation3(
v0) * (AlignedScaling3(
v0) * Transform3(q1));
329 t0.scale(
v0).translate(
v0).rotate(q1);
331 t1 = AlignedScaling3(
v0) * (Translation3(
v0) * Transform3(q1));
335 t1 *= AlignedScaling3(
v0);
337 t1 = AlignedScaling3(
v0) * (Translation3(
v0) * Transform3(q1));
338 t1 = t1 *
v0.asDiagonal();
342 t1 = t1 * Translation3(
v0);
346 t1 = Translation3(
v0) * t1;
355 t0.translate(
v1).rotate(q1);
356 t1 = t1 * (Translation3(
v1) * q1);
360 t0.scale(
v1).rotate(q1);
361 t1 = t1 * (AlignedScaling3(
v1) * q1);
370 t0.rotate(q1).translate(
v1);
371 t1 = t1 * (q1 * Translation3(
v1));
375 t0.rotate(q1).scale(
v1);
376 t1 = t1 * (q1 * AlignedScaling3(
v1));
383 t0.linear().setRandom();
384 }
while(t0.linear().jacobiSvd().singularValues()(2)<test_precision<Scalar>());
385 Matrix4 t044 = Matrix4::Zero();
387 t044.block(0,0,t0.matrix().rows(),4) = t0.matrix();
390 t0.translate(
v0).rotate(q1);
391 t044 = Matrix4::Zero();
393 t044.block(0,0,t0.matrix().rows(),4) = t0.matrix();
396 Matrix3 mat_rotation, mat_scaling;
398 t0.translate(
v0).rotate(q1).scale(
v1);
399 t0.computeRotationScaling(&mat_rotation, &mat_scaling);
403 t0.computeScalingRotation(&mat_scaling, &mat_rotation);
414 Translation3 tr1(
v0);
431 for(
int k=0; k<100; ++k)
433 Scalar angle = internal::random<Scalar>(-100,100);
448 s0 = internal::random<Scalar>(-100,100);
449 s1 = internal::random<Scalar>(-100,100);
471 int path_steps = 100;
472 for(
int k=0; k<path_steps; ++k)
490 Transform3 t32(Matrix4::Random()), t33, t34;
493 t33*=AlignedScaling3(
v0);
495 t33 = t34 * AlignedScaling3(
v0);
501 template<
typename A1,
typename A2,
typename P,
typename Q,
typename V,
typename H>
509 template<
typename A1,
typename A2,
typename P,
typename Q,
typename V,
typename H>
519 template<
typename Scalar,
int Dim,
int Options,
typename RotationType>
532 AffineCompactType A1c; A1c.matrix().setRandom();
533 AffineCompactType A2c; A2c.matrix().setRandom();
536 ProjectiveType
P1;
P1.matrix().setRandom();
539 HVectorType h1 = HVectorType::Random();
540 Scalar s1 = internal::random<Scalar>();
541 LinearType
L = LinearType::Random();
571 Scalar* array3u = array3+1;
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;
577 p1->matrix().setRandom();
594 Proj
p;
p.matrix().setRandom();
595 Aff
a;
a.linear().setRandom();
a.translation().setRandom();
598 Mat p_m(
p.matrix()), a_m(
a.matrix());
624 v1 = Vector3::Random();
626 Transform3 t0, t1, t2;
632 q1 = AngleAxisx(
a,
v0.normalized());
634 t0 = Transform3::Identity();
639 v1 = Vector3::Ones();
640 t0.linear() = q1.toRotationMatrix();
642 t1.linear() = q1.conjugate().toRotationMatrix();
645 VERIFY((t0 * t1).
matrix().isIdentity(test_precision<Scalar>()));
647 t1.fromPositionOrientationScale(
v0, q1,
v1);
658 t3.linear() = q1.toRotationMatrix();
659 t3.translation() =
v1;
660 Matrix4
m3 = t3.matrix();
661 VERIFY((
m3 *
m3.inverse()).isIdentity(test_precision<Scalar>()));
667 VERIFY(t3.rotation().data()==t3.linear().data());
678 Vector3 v0 = Vector3::Random().normalized(),
679 v1 = Vector3::Random().normalized(),
680 v3 = Vector3::Random().normalized();
683 Matrix3 rank2 = 50 *
v0 *
v0.adjoint() + 20 *
v1 *
v1.adjoint();
684 t0.linear() = rank2 + eps *
v3 *
v3.adjoint();
685 t1.linear() = rank2 - eps *
v3 *
v3.adjoint();
688 Matrix3 r0, s0,
r1, s1;
689 t0.computeRotationScaling(&r0, &s0);
690 t1.computeRotationScaling(&
r1, &s1);
694 VERIFY((s0 - s1).norm() <
c * eps);
701 CALL_SUBTEST_1(( non_projective_only<double,Affine,AutoAlign>() ));
702 CALL_SUBTEST_1(( transformations_computed_scaling_continuity<double,Affine,AutoAlign>() ));
704 CALL_SUBTEST_2(( transformations<float,AffineCompact,AutoAlign>() ));
705 CALL_SUBTEST_2(( non_projective_only<float,AffineCompact,AutoAlign>() ));
708 CALL_SUBTEST_3(( transformations<double,Projective,AutoAlign>() ));
709 CALL_SUBTEST_3(( transformations<double,Projective,DontAlign>() ));
712 CALL_SUBTEST_4(( transformations<float,Affine,RowMajor|AutoAlign>() ));
715 CALL_SUBTEST_5(( transformations<double,AffineCompact,RowMajor|AutoAlign>() ));
716 CALL_SUBTEST_5(( non_projective_only<double,AffineCompact,RowMajor>() ));
718 CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|AutoAlign>() ));
719 CALL_SUBTEST_6(( transformations<double,Projective,RowMajor|DontAlign>() ));
722 CALL_SUBTEST_7(( transform_products<double,3,RowMajor|AutoAlign>() ));
726 CALL_SUBTEST_8(( transform_associativity<double,3,ColMajor>(Quaterniond::UnitRandom()) ));
728 CALL_SUBTEST_9(( transformations_no_scale<double,Affine,AutoAlign>() ));
729 CALL_SUBTEST_9(( transformations_no_scale<double,Isometry,AutoAlign>() ));