001 /* 002 * Licensed to the Apache Software Foundation (ASF) under one or more 003 * contributor license agreements. See the NOTICE file distributed with 004 * this work for additional information regarding copyright ownership. 005 * The ASF licenses this file to You under the Apache License, Version 2.0 006 * (the "License"); you may not use this file except in compliance with 007 * the License. You may obtain a copy of the License at 008 * 009 * http://www.apache.org/licenses/LICENSE-2.0 010 * 011 * Unless required by applicable law or agreed to in writing, software 012 * distributed under the License is distributed on an "AS IS" BASIS, 013 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 014 * See the License for the specific language governing permissions and 015 * limitations under the License. 016 */ 017 018 package org.apache.commons.math.geometry; 019 020 import org.apache.commons.math.geometry.CardanEulerSingularityException; 021 import org.apache.commons.math.geometry.NotARotationMatrixException; 022 import org.apache.commons.math.geometry.Rotation; 023 import org.apache.commons.math.geometry.RotationOrder; 024 import org.apache.commons.math.geometry.Vector3D; 025 import org.apache.commons.math.util.MathUtils; 026 027 import junit.framework.*; 028 029 public class RotationTest 030 extends TestCase { 031 032 public RotationTest(String name) { 033 super(name); 034 } 035 036 public void testIdentity() { 037 038 Rotation r = Rotation.IDENTITY; 039 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I); 040 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J); 041 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K); 042 checkAngle(r.getAngle(), 0); 043 044 r = new Rotation(-1, 0, 0, 0, false); 045 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I); 046 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J); 047 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K); 048 checkAngle(r.getAngle(), 0); 049 050 r = new Rotation(42, 0, 0, 0, true); 051 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I); 052 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J); 053 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K); 054 checkAngle(r.getAngle(), 0); 055 056 } 057 058 public void testAxisAngle() { 059 060 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * Math.PI / 3); 061 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J); 062 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K); 063 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I); 064 double s = 1 / Math.sqrt(3); 065 checkVector(r.getAxis(), new Vector3D(s, s, s)); 066 checkAngle(r.getAngle(), 2 * Math.PI / 3); 067 068 try { 069 new Rotation(new Vector3D(0, 0, 0), 2 * Math.PI / 3); 070 fail("an exception should have been thrown"); 071 } catch (ArithmeticException e) { 072 } catch (Exception e) { 073 fail("unexpected exception"); 074 } 075 076 r = new Rotation(Vector3D.PLUS_K, 1.5 * Math.PI); 077 checkVector(r.getAxis(), new Vector3D(0, 0, -1)); 078 checkAngle(r.getAngle(), 0.5 * Math.PI); 079 080 r = new Rotation(Vector3D.PLUS_J, Math.PI); 081 checkVector(r.getAxis(), Vector3D.PLUS_J); 082 checkAngle(r.getAngle(), Math.PI); 083 084 checkVector(Rotation.IDENTITY.getAxis(), Vector3D.PLUS_I); 085 086 } 087 088 public void testRevert() { 089 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true); 090 Rotation reverted = r.revert(); 091 checkRotation(r.applyTo(reverted), 1, 0, 0, 0); 092 checkRotation(reverted.applyTo(r), 1, 0, 0, 0); 093 assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12); 094 assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12); 095 } 096 097 public void testVectorOnePair() { 098 099 Vector3D u = new Vector3D(3, 2, 1); 100 Vector3D v = new Vector3D(-4, 2, 2); 101 Rotation r = new Rotation(u, v); 102 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm())); 103 104 checkAngle(new Rotation(u, u.negate()).getAngle(), Math.PI); 105 106 try { 107 new Rotation(u, Vector3D.ZERO); 108 fail("an exception should have been thrown"); 109 } catch (IllegalArgumentException e) { 110 // expected behavior 111 } catch (Exception e) { 112 fail("unexpected exception"); 113 } 114 115 } 116 117 public void testVectorTwoPairs() { 118 119 Vector3D u1 = new Vector3D(3, 0, 0); 120 Vector3D u2 = new Vector3D(0, 5, 0); 121 Vector3D v1 = new Vector3D(0, 0, 2); 122 Vector3D v2 = new Vector3D(-2, 0, 2); 123 Rotation r = new Rotation(u1, u2, v1, v2); 124 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K); 125 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.MINUS_I); 126 127 r = new Rotation(u1, u2, u1.negate(), u2.negate()); 128 Vector3D axis = r.getAxis(); 129 if (Vector3D.dotProduct(axis, Vector3D.PLUS_K) > 0) { 130 checkVector(axis, Vector3D.PLUS_K); 131 } else { 132 checkVector(axis, Vector3D.MINUS_K); 133 } 134 checkAngle(r.getAngle(), Math.PI); 135 136 double sqrt = Math.sqrt(2) / 2; 137 r = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J, 138 new Vector3D(0.5, 0.5, sqrt), 139 new Vector3D(0.5, 0.5, -sqrt)); 140 checkRotation(r, sqrt, 0.5, 0.5, 0); 141 142 r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2)); 143 checkRotation(r, sqrt, -sqrt, 0, 0); 144 145 checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0); 146 147 try { 148 new Rotation(u1, u2, Vector3D.ZERO, v2); 149 fail("an exception should have been thrown"); 150 } catch (IllegalArgumentException e) { 151 // expected behavior 152 } catch (Exception e) { 153 fail("unexpected exception"); 154 } 155 156 } 157 158 public void testMatrix() 159 throws NotARotationMatrixException { 160 161 try { 162 new Rotation(new double[][] { 163 { 0.0, 1.0, 0.0 }, 164 { 1.0, 0.0, 0.0 } 165 }, 1.0e-7); 166 } catch (NotARotationMatrixException nrme) { 167 // expected behavior 168 } catch (Exception e) { 169 fail("wrong exception caught: " + e.getMessage()); 170 } 171 172 try { 173 new Rotation(new double[][] { 174 { 0.445888, 0.797184, -0.407040 }, 175 { 0.821760, -0.184320, 0.539200 }, 176 { -0.354816, 0.574912, 0.737280 } 177 }, 1.0e-7); 178 } catch (NotARotationMatrixException nrme) { 179 // expected behavior 180 } catch (Exception e) { 181 fail("wrong exception caught: " + e.getMessage()); 182 } 183 184 try { 185 new Rotation(new double[][] { 186 { 0.4, 0.8, -0.4 }, 187 { -0.4, 0.6, 0.7 }, 188 { 0.8, -0.2, 0.5 } 189 }, 1.0e-15); 190 } catch (NotARotationMatrixException nrme) { 191 // expected behavior 192 } catch (Exception e) { 193 fail("wrong exception caught: " + e.getMessage()); 194 } 195 196 checkRotation(new Rotation(new double[][] { 197 { 0.445888, 0.797184, -0.407040 }, 198 { -0.354816, 0.574912, 0.737280 }, 199 { 0.821760, -0.184320, 0.539200 } 200 }, 1.0e-10), 201 0.8, 0.288, 0.384, 0.36); 202 203 checkRotation(new Rotation(new double[][] { 204 { 0.539200, 0.737280, 0.407040 }, 205 { 0.184320, -0.574912, 0.797184 }, 206 { 0.821760, -0.354816, -0.445888 } 207 }, 1.0e-10), 208 0.36, 0.8, 0.288, 0.384); 209 210 checkRotation(new Rotation(new double[][] { 211 { -0.445888, 0.797184, -0.407040 }, 212 { 0.354816, 0.574912, 0.737280 }, 213 { 0.821760, 0.184320, -0.539200 } 214 }, 1.0e-10), 215 0.384, 0.36, 0.8, 0.288); 216 217 checkRotation(new Rotation(new double[][] { 218 { -0.539200, 0.737280, 0.407040 }, 219 { -0.184320, -0.574912, 0.797184 }, 220 { 0.821760, 0.354816, 0.445888 } 221 }, 1.0e-10), 222 0.288, 0.384, 0.36, 0.8); 223 224 double[][] m1 = { { 0.0, 1.0, 0.0 }, 225 { 0.0, 0.0, 1.0 }, 226 { 1.0, 0.0, 0.0 } }; 227 Rotation r = new Rotation(m1, 1.0e-7); 228 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K); 229 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I); 230 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J); 231 232 double[][] m2 = { { 0.83203, -0.55012, -0.07139 }, 233 { 0.48293, 0.78164, -0.39474 }, 234 { 0.27296, 0.29396, 0.91602 } }; 235 r = new Rotation(m2, 1.0e-12); 236 237 double[][] m3 = r.getMatrix(); 238 double d00 = m2[0][0] - m3[0][0]; 239 double d01 = m2[0][1] - m3[0][1]; 240 double d02 = m2[0][2] - m3[0][2]; 241 double d10 = m2[1][0] - m3[1][0]; 242 double d11 = m2[1][1] - m3[1][1]; 243 double d12 = m2[1][2] - m3[1][2]; 244 double d20 = m2[2][0] - m3[2][0]; 245 double d21 = m2[2][1] - m3[2][1]; 246 double d22 = m2[2][2] - m3[2][2]; 247 248 assertTrue(Math.abs(d00) < 6.0e-6); 249 assertTrue(Math.abs(d01) < 6.0e-6); 250 assertTrue(Math.abs(d02) < 6.0e-6); 251 assertTrue(Math.abs(d10) < 6.0e-6); 252 assertTrue(Math.abs(d11) < 6.0e-6); 253 assertTrue(Math.abs(d12) < 6.0e-6); 254 assertTrue(Math.abs(d20) < 6.0e-6); 255 assertTrue(Math.abs(d21) < 6.0e-6); 256 assertTrue(Math.abs(d22) < 6.0e-6); 257 258 assertTrue(Math.abs(d00) > 4.0e-7); 259 assertTrue(Math.abs(d01) > 4.0e-7); 260 assertTrue(Math.abs(d02) > 4.0e-7); 261 assertTrue(Math.abs(d10) > 4.0e-7); 262 assertTrue(Math.abs(d11) > 4.0e-7); 263 assertTrue(Math.abs(d12) > 4.0e-7); 264 assertTrue(Math.abs(d20) > 4.0e-7); 265 assertTrue(Math.abs(d21) > 4.0e-7); 266 assertTrue(Math.abs(d22) > 4.0e-7); 267 268 for (int i = 0; i < 3; ++i) { 269 for (int j = 0; j < 3; ++j) { 270 double m3tm3 = m3[i][0] * m3[j][0] 271 + m3[i][1] * m3[j][1] 272 + m3[i][2] * m3[j][2]; 273 if (i == j) { 274 assertTrue(Math.abs(m3tm3 - 1.0) < 1.0e-10); 275 } else { 276 assertTrue(Math.abs(m3tm3) < 1.0e-10); 277 } 278 } 279 } 280 281 checkVector(r.applyTo(Vector3D.PLUS_I), 282 new Vector3D(m3[0][0], m3[1][0], m3[2][0])); 283 checkVector(r.applyTo(Vector3D.PLUS_J), 284 new Vector3D(m3[0][1], m3[1][1], m3[2][1])); 285 checkVector(r.applyTo(Vector3D.PLUS_K), 286 new Vector3D(m3[0][2], m3[1][2], m3[2][2])); 287 288 double[][] m4 = { { 1.0, 0.0, 0.0 }, 289 { 0.0, -1.0, 0.0 }, 290 { 0.0, 0.0, -1.0 } }; 291 r = new Rotation(m4, 1.0e-7); 292 checkAngle(r.getAngle(), Math.PI); 293 294 try { 295 double[][] m5 = { { 0.0, 0.0, 1.0 }, 296 { 0.0, 1.0, 0.0 }, 297 { 1.0, 0.0, 0.0 } }; 298 r = new Rotation(m5, 1.0e-7); 299 fail("got " + r + ", should have caught an exception"); 300 } catch (NotARotationMatrixException e) { 301 // expected 302 } catch (Exception e) { 303 fail("wrong exception caught"); 304 } 305 306 } 307 308 public void testAngles() 309 throws CardanEulerSingularityException { 310 311 RotationOrder[] CardanOrders = { 312 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, 313 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX 314 }; 315 316 for (int i = 0; i < CardanOrders.length; ++i) { 317 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { 318 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) { 319 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { 320 Rotation r = new Rotation(CardanOrders[i], alpha1, alpha2, alpha3); 321 double[] angles = r.getAngles(CardanOrders[i]); 322 checkAngle(angles[0], alpha1); 323 checkAngle(angles[1], alpha2); 324 checkAngle(angles[2], alpha3); 325 } 326 } 327 } 328 } 329 330 RotationOrder[] EulerOrders = { 331 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, 332 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ 333 }; 334 335 for (int i = 0; i < EulerOrders.length; ++i) { 336 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { 337 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) { 338 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { 339 Rotation r = new Rotation(EulerOrders[i], 340 alpha1, alpha2, alpha3); 341 double[] angles = r.getAngles(EulerOrders[i]); 342 checkAngle(angles[0], alpha1); 343 checkAngle(angles[1], alpha2); 344 checkAngle(angles[2], alpha3); 345 } 346 } 347 } 348 } 349 350 } 351 352 public void testSingularities() { 353 354 RotationOrder[] CardanOrders = { 355 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, 356 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX 357 }; 358 359 double[] singularCardanAngle = { Math.PI / 2, -Math.PI / 2 }; 360 for (int i = 0; i < CardanOrders.length; ++i) { 361 for (int j = 0; j < singularCardanAngle.length; ++j) { 362 Rotation r = new Rotation(CardanOrders[i], 0.1, singularCardanAngle[j], 0.3); 363 try { 364 r.getAngles(CardanOrders[i]); 365 fail("an exception should have been caught"); 366 } catch (CardanEulerSingularityException cese) { 367 // expected behavior 368 } catch (Exception e) { 369 fail("wrong exception caught: " + e.getMessage()); 370 } 371 } 372 } 373 374 RotationOrder[] EulerOrders = { 375 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, 376 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ 377 }; 378 379 double[] singularEulerAngle = { 0, Math.PI }; 380 for (int i = 0; i < EulerOrders.length; ++i) { 381 for (int j = 0; j < singularEulerAngle.length; ++j) { 382 Rotation r = new Rotation(EulerOrders[i], 0.1, singularEulerAngle[j], 0.3); 383 try { 384 r.getAngles(EulerOrders[i]); 385 fail("an exception should have been caught"); 386 } catch (CardanEulerSingularityException cese) { 387 // expected behavior 388 } catch (Exception e) { 389 fail("wrong exception caught: " + e.getMessage()); 390 } 391 } 392 } 393 394 395 } 396 397 public void testQuaternion() { 398 399 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7); 400 double n = 23.5; 401 Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(), 402 n * r1.getQ2(), n * r1.getQ3(), 403 true); 404 for (double x = -0.9; x < 0.9; x += 0.2) { 405 for (double y = -0.9; y < 0.9; y += 0.2) { 406 for (double z = -0.9; z < 0.9; z += 0.2) { 407 Vector3D u = new Vector3D(x, y, z); 408 checkVector(r2.applyTo(u), r1.applyTo(u)); 409 } 410 } 411 } 412 413 r1 = new Rotation( 0.288, 0.384, 0.36, 0.8, false); 414 checkRotation(r1, -r1.getQ0(), -r1.getQ1(), -r1.getQ2(), -r1.getQ3()); 415 416 } 417 418 public void testCompose() { 419 420 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7); 421 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3); 422 Rotation r3 = r2.applyTo(r1); 423 424 for (double x = -0.9; x < 0.9; x += 0.2) { 425 for (double y = -0.9; y < 0.9; y += 0.2) { 426 for (double z = -0.9; z < 0.9; z += 0.2) { 427 Vector3D u = new Vector3D(x, y, z); 428 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u)); 429 } 430 } 431 } 432 433 } 434 435 public void testComposeInverse() { 436 437 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7); 438 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3); 439 Rotation r3 = r2.applyInverseTo(r1); 440 441 for (double x = -0.9; x < 0.9; x += 0.2) { 442 for (double y = -0.9; y < 0.9; y += 0.2) { 443 for (double z = -0.9; z < 0.9; z += 0.2) { 444 Vector3D u = new Vector3D(x, y, z); 445 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u)); 446 } 447 } 448 } 449 450 } 451 452 public void testApplyInverseTo() { 453 454 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7); 455 for (double lambda = 0; lambda < 6.2; lambda += 0.2) { 456 for (double phi = -1.55; phi < 1.55; phi += 0.2) { 457 Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi), 458 Math.sin(lambda) * Math.cos(phi), 459 Math.sin(phi)); 460 r.applyInverseTo(r.applyTo(u)); 461 checkVector(u, r.applyInverseTo(r.applyTo(u))); 462 checkVector(u, r.applyTo(r.applyInverseTo(u))); 463 } 464 } 465 466 r = Rotation.IDENTITY; 467 for (double lambda = 0; lambda < 6.2; lambda += 0.2) { 468 for (double phi = -1.55; phi < 1.55; phi += 0.2) { 469 Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi), 470 Math.sin(lambda) * Math.cos(phi), 471 Math.sin(phi)); 472 checkVector(u, r.applyInverseTo(r.applyTo(u))); 473 checkVector(u, r.applyTo(r.applyInverseTo(u))); 474 } 475 } 476 477 r = new Rotation(Vector3D.PLUS_K, Math.PI); 478 for (double lambda = 0; lambda < 6.2; lambda += 0.2) { 479 for (double phi = -1.55; phi < 1.55; phi += 0.2) { 480 Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi), 481 Math.sin(lambda) * Math.cos(phi), 482 Math.sin(phi)); 483 checkVector(u, r.applyInverseTo(r.applyTo(u))); 484 checkVector(u, r.applyTo(r.applyInverseTo(u))); 485 } 486 } 487 488 } 489 490 private void checkVector(Vector3D v1, Vector3D v2) { 491 assertTrue(v1.subtract(v2).getNorm() < 1.0e-10); 492 } 493 494 private void checkAngle(double a1, double a2) { 495 assertEquals(a1, MathUtils.normalizeAngle(a2, a1), 1.0e-10); 496 } 497 498 private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) { 499 assertEquals(0, Rotation.distance(r, new Rotation(q0, q1, q2, q3, false)), 1.0e-12); 500 } 501 502 public static Test suite() { 503 return new TestSuite(RotationTest.class); 504 } 505 506 }