Brian Silverman | 72890c2 | 2015-09-19 14:37:37 -0400 | [diff] [blame^] | 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 | } |