1 /* 2 * Licensed to the Apache Software Foundation (ASF) under one or more 3 * contributor license agreements. See the NOTICE file distributed with 4 * this work for additional information regarding copyright ownership. 5 * The ASF licenses this file to You under the Apache License, Version 2.0 6 * (the "License"); you may not use this file except in compliance with 7 * the License. You may obtain a copy of the License at 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 */ 17 18 package org.apache.commons.math3.geometry.euclidean.threed; 19 20 import org.apache.commons.math3.exception.MathArithmeticException; 21 import org.apache.commons.math3.exception.MathIllegalArgumentException; 22 import org.apache.commons.math3.util.FastMath; 23 import org.apache.commons.math3.util.MathUtils; 24 import org.junit.Assert; 25 import org.junit.Test; 26 27 28 public class RotationTest { 29 30 @Test testIdentity()31 public void testIdentity() { 32 33 Rotation r = Rotation.IDENTITY; 34 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I); 35 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J); 36 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K); 37 checkAngle(r.getAngle(), 0); 38 39 r = new Rotation(-1, 0, 0, 0, false); 40 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I); 41 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J); 42 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K); 43 checkAngle(r.getAngle(), 0); 44 45 r = new Rotation(42, 0, 0, 0, true); 46 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I); 47 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J); 48 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K); 49 checkAngle(r.getAngle(), 0); 50 51 } 52 53 @Test 54 @Deprecated testAxisAngleDeprecated()55 public void testAxisAngleDeprecated() throws MathIllegalArgumentException { 56 57 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3); 58 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J); 59 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K); 60 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I); 61 double s = 1 / FastMath.sqrt(3); 62 checkVector(r.getAxis(), new Vector3D(s, s, s)); 63 checkAngle(r.getAngle(), 2 * FastMath.PI / 3); 64 65 try { 66 new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3); 67 Assert.fail("an exception should have been thrown"); 68 } catch (MathIllegalArgumentException e) { 69 } 70 71 r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI); 72 checkVector(r.getAxis(), new Vector3D(0, 0, -1)); 73 checkAngle(r.getAngle(), 0.5 * FastMath.PI); 74 75 r = new Rotation(Vector3D.PLUS_J, FastMath.PI); 76 checkVector(r.getAxis(), Vector3D.PLUS_J); 77 checkAngle(r.getAngle(), FastMath.PI); 78 79 checkVector(Rotation.IDENTITY.getAxis(), Vector3D.PLUS_I); 80 81 } 82 83 @Test testAxisAngleVectorOperator()84 public void testAxisAngleVectorOperator() throws MathIllegalArgumentException { 85 86 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR); 87 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J); 88 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K); 89 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I); 90 double s = 1 / FastMath.sqrt(3); 91 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D( s, s, s)); 92 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(-s, -s, -s)); 93 checkAngle(r.getAngle(), 2 * FastMath.PI / 3); 94 95 try { 96 new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR); 97 Assert.fail("an exception should have been thrown"); 98 } catch (MathIllegalArgumentException e) { 99 } 100 101 r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.VECTOR_OPERATOR); 102 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, -1)); 103 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, +1)); 104 checkAngle(r.getAngle(), 0.5 * FastMath.PI); 105 106 r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.VECTOR_OPERATOR); 107 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_J); 108 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_J); 109 checkAngle(r.getAngle(), FastMath.PI); 110 111 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I); 112 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I); 113 114 } 115 116 @Test testAxisAngleFrameTransform()117 public void testAxisAngleFrameTransform() throws MathIllegalArgumentException { 118 119 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM); 120 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K); 121 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I); 122 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J); 123 double s = 1 / FastMath.sqrt(3); 124 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D( s, s, s)); 125 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(-s, -s, -s)); 126 checkAngle(r.getAngle(), 2 * FastMath.PI / 3); 127 128 try { 129 new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM); 130 Assert.fail("an exception should have been thrown"); 131 } catch (MathIllegalArgumentException e) { 132 } 133 134 r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.FRAME_TRANSFORM); 135 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, -1)); 136 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, +1)); 137 checkAngle(r.getAngle(), 0.5 * FastMath.PI); 138 139 r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.FRAME_TRANSFORM); 140 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.PLUS_J); 141 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.MINUS_J); 142 checkAngle(r.getAngle(), FastMath.PI); 143 144 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I); 145 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I); 146 147 } 148 149 @Test testRevertDeprecated()150 public void testRevertDeprecated() { 151 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true); 152 Rotation reverted = r.revert(); 153 checkRotation(r.applyTo(reverted), 1, 0, 0, 0); 154 checkRotation(reverted.applyTo(r), 1, 0, 0, 0); 155 Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12); 156 Assert.assertEquals(-1, 157 Vector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR), 158 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)), 159 1.0e-12); 160 } 161 162 @Test testRevertVectorOperator()163 public void testRevertVectorOperator() { 164 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true); 165 Rotation reverted = r.revert(); 166 checkRotation(r.compose(reverted, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0); 167 checkRotation(reverted.compose(r, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0); 168 Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12); 169 Assert.assertEquals(-1, 170 Vector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR), 171 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)), 172 1.0e-12); 173 } 174 175 @Test testRevertFrameTransform()176 public void testRevertFrameTransform() { 177 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true); 178 Rotation reverted = r.revert(); 179 checkRotation(r.compose(reverted, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0); 180 checkRotation(reverted.compose(r, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0); 181 Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12); 182 Assert.assertEquals(-1, 183 Vector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM), 184 reverted.getAxis(RotationConvention.FRAME_TRANSFORM)), 185 1.0e-12); 186 } 187 188 @Test testVectorOnePair()189 public void testVectorOnePair() throws MathArithmeticException { 190 191 Vector3D u = new Vector3D(3, 2, 1); 192 Vector3D v = new Vector3D(-4, 2, 2); 193 Rotation r = new Rotation(u, v); 194 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm())); 195 196 checkAngle(new Rotation(u, u.negate()).getAngle(), FastMath.PI); 197 198 try { 199 new Rotation(u, Vector3D.ZERO); 200 Assert.fail("an exception should have been thrown"); 201 } catch (MathArithmeticException e) { 202 // expected behavior 203 } 204 205 } 206 207 @Test testVectorTwoPairs()208 public void testVectorTwoPairs() throws MathArithmeticException { 209 210 Vector3D u1 = new Vector3D(3, 0, 0); 211 Vector3D u2 = new Vector3D(0, 5, 0); 212 Vector3D v1 = new Vector3D(0, 0, 2); 213 Vector3D v2 = new Vector3D(-2, 0, 2); 214 Rotation r = new Rotation(u1, u2, v1, v2); 215 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K); 216 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.MINUS_I); 217 218 r = new Rotation(u1, u2, u1.negate(), u2.negate()); 219 Vector3D axis = r.getAxis(RotationConvention.VECTOR_OPERATOR); 220 if (Vector3D.dotProduct(axis, Vector3D.PLUS_K) > 0) { 221 checkVector(axis, Vector3D.PLUS_K); 222 } else { 223 checkVector(axis, Vector3D.MINUS_K); 224 } 225 checkAngle(r.getAngle(), FastMath.PI); 226 227 double sqrt = FastMath.sqrt(2) / 2; 228 r = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J, 229 new Vector3D(0.5, 0.5, sqrt), 230 new Vector3D(0.5, 0.5, -sqrt)); 231 checkRotation(r, sqrt, 0.5, 0.5, 0); 232 233 r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2)); 234 checkRotation(r, sqrt, -sqrt, 0, 0); 235 236 checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0); 237 238 try { 239 new Rotation(u1, u2, Vector3D.ZERO, v2); 240 Assert.fail("an exception should have been thrown"); 241 } catch (MathArithmeticException e) { 242 // expected behavior 243 } 244 245 } 246 247 @Test testMatrix()248 public void testMatrix() 249 throws NotARotationMatrixException { 250 251 try { 252 new Rotation(new double[][] { 253 { 0.0, 1.0, 0.0 }, 254 { 1.0, 0.0, 0.0 } 255 }, 1.0e-7); 256 Assert.fail("Expecting NotARotationMatrixException"); 257 } catch (NotARotationMatrixException nrme) { 258 // expected behavior 259 } 260 261 try { 262 new Rotation(new double[][] { 263 { 0.445888, 0.797184, -0.407040 }, 264 { 0.821760, -0.184320, 0.539200 }, 265 { -0.354816, 0.574912, 0.737280 } 266 }, 1.0e-7); 267 Assert.fail("Expecting NotARotationMatrixException"); 268 } catch (NotARotationMatrixException nrme) { 269 // expected behavior 270 } 271 272 try { 273 new Rotation(new double[][] { 274 { 0.4, 0.8, -0.4 }, 275 { -0.4, 0.6, 0.7 }, 276 { 0.8, -0.2, 0.5 } 277 }, 1.0e-15); 278 Assert.fail("Expecting NotARotationMatrixException"); 279 } catch (NotARotationMatrixException nrme) { 280 // expected behavior 281 } 282 283 checkRotation(new Rotation(new double[][] { 284 { 0.445888, 0.797184, -0.407040 }, 285 { -0.354816, 0.574912, 0.737280 }, 286 { 0.821760, -0.184320, 0.539200 } 287 }, 1.0e-10), 288 0.8, 0.288, 0.384, 0.36); 289 290 checkRotation(new Rotation(new double[][] { 291 { 0.539200, 0.737280, 0.407040 }, 292 { 0.184320, -0.574912, 0.797184 }, 293 { 0.821760, -0.354816, -0.445888 } 294 }, 1.0e-10), 295 0.36, 0.8, 0.288, 0.384); 296 297 checkRotation(new Rotation(new double[][] { 298 { -0.445888, 0.797184, -0.407040 }, 299 { 0.354816, 0.574912, 0.737280 }, 300 { 0.821760, 0.184320, -0.539200 } 301 }, 1.0e-10), 302 0.384, 0.36, 0.8, 0.288); 303 304 checkRotation(new Rotation(new double[][] { 305 { -0.539200, 0.737280, 0.407040 }, 306 { -0.184320, -0.574912, 0.797184 }, 307 { 0.821760, 0.354816, 0.445888 } 308 }, 1.0e-10), 309 0.288, 0.384, 0.36, 0.8); 310 311 double[][] m1 = { { 0.0, 1.0, 0.0 }, 312 { 0.0, 0.0, 1.0 }, 313 { 1.0, 0.0, 0.0 } }; 314 Rotation r = new Rotation(m1, 1.0e-7); 315 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K); 316 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I); 317 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J); 318 319 double[][] m2 = { { 0.83203, -0.55012, -0.07139 }, 320 { 0.48293, 0.78164, -0.39474 }, 321 { 0.27296, 0.29396, 0.91602 } }; 322 r = new Rotation(m2, 1.0e-12); 323 324 double[][] m3 = r.getMatrix(); 325 double d00 = m2[0][0] - m3[0][0]; 326 double d01 = m2[0][1] - m3[0][1]; 327 double d02 = m2[0][2] - m3[0][2]; 328 double d10 = m2[1][0] - m3[1][0]; 329 double d11 = m2[1][1] - m3[1][1]; 330 double d12 = m2[1][2] - m3[1][2]; 331 double d20 = m2[2][0] - m3[2][0]; 332 double d21 = m2[2][1] - m3[2][1]; 333 double d22 = m2[2][2] - m3[2][2]; 334 335 Assert.assertTrue(FastMath.abs(d00) < 6.0e-6); 336 Assert.assertTrue(FastMath.abs(d01) < 6.0e-6); 337 Assert.assertTrue(FastMath.abs(d02) < 6.0e-6); 338 Assert.assertTrue(FastMath.abs(d10) < 6.0e-6); 339 Assert.assertTrue(FastMath.abs(d11) < 6.0e-6); 340 Assert.assertTrue(FastMath.abs(d12) < 6.0e-6); 341 Assert.assertTrue(FastMath.abs(d20) < 6.0e-6); 342 Assert.assertTrue(FastMath.abs(d21) < 6.0e-6); 343 Assert.assertTrue(FastMath.abs(d22) < 6.0e-6); 344 345 Assert.assertTrue(FastMath.abs(d00) > 4.0e-7); 346 Assert.assertTrue(FastMath.abs(d01) > 4.0e-7); 347 Assert.assertTrue(FastMath.abs(d02) > 4.0e-7); 348 Assert.assertTrue(FastMath.abs(d10) > 4.0e-7); 349 Assert.assertTrue(FastMath.abs(d11) > 4.0e-7); 350 Assert.assertTrue(FastMath.abs(d12) > 4.0e-7); 351 Assert.assertTrue(FastMath.abs(d20) > 4.0e-7); 352 Assert.assertTrue(FastMath.abs(d21) > 4.0e-7); 353 Assert.assertTrue(FastMath.abs(d22) > 4.0e-7); 354 355 for (int i = 0; i < 3; ++i) { 356 for (int j = 0; j < 3; ++j) { 357 double m3tm3 = m3[i][0] * m3[j][0] 358 + m3[i][1] * m3[j][1] 359 + m3[i][2] * m3[j][2]; 360 if (i == j) { 361 Assert.assertTrue(FastMath.abs(m3tm3 - 1.0) < 1.0e-10); 362 } else { 363 Assert.assertTrue(FastMath.abs(m3tm3) < 1.0e-10); 364 } 365 } 366 } 367 368 checkVector(r.applyTo(Vector3D.PLUS_I), 369 new Vector3D(m3[0][0], m3[1][0], m3[2][0])); 370 checkVector(r.applyTo(Vector3D.PLUS_J), 371 new Vector3D(m3[0][1], m3[1][1], m3[2][1])); 372 checkVector(r.applyTo(Vector3D.PLUS_K), 373 new Vector3D(m3[0][2], m3[1][2], m3[2][2])); 374 375 double[][] m4 = { { 1.0, 0.0, 0.0 }, 376 { 0.0, -1.0, 0.0 }, 377 { 0.0, 0.0, -1.0 } }; 378 r = new Rotation(m4, 1.0e-7); 379 checkAngle(r.getAngle(), FastMath.PI); 380 381 try { 382 double[][] m5 = { { 0.0, 0.0, 1.0 }, 383 { 0.0, 1.0, 0.0 }, 384 { 1.0, 0.0, 0.0 } }; 385 r = new Rotation(m5, 1.0e-7); 386 Assert.fail("got " + r + ", should have caught an exception"); 387 } catch (NotARotationMatrixException e) { 388 // expected 389 } 390 391 } 392 393 @Test 394 @Deprecated 395 public void testAnglesDeprecated() 396 throws CardanEulerSingularityException { 397 398 RotationOrder[] CardanOrders = { 399 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, 400 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX 401 }; 402 403 for (int i = 0; i < CardanOrders.length; ++i) { 404 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { 405 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) { 406 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { 407 Rotation r = new Rotation(CardanOrders[i], alpha1, alpha2, alpha3); 408 double[] angles = r.getAngles(CardanOrders[i]); 409 checkAngle(angles[0], alpha1); 410 checkAngle(angles[1], alpha2); 411 checkAngle(angles[2], alpha3); 412 } 413 } 414 } 415 } 416 417 RotationOrder[] EulerOrders = { 418 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, 419 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ 420 }; 421 422 for (int i = 0; i < EulerOrders.length; ++i) { 423 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { 424 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) { 425 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { 426 Rotation r = new Rotation(EulerOrders[i], 427 alpha1, alpha2, alpha3); 428 double[] angles = r.getAngles(EulerOrders[i]); 429 checkAngle(angles[0], alpha1); 430 checkAngle(angles[1], alpha2); 431 checkAngle(angles[2], alpha3); 432 } 433 } 434 } 435 } 436 437 } 438 439 @Test 440 public void testAngles() 441 throws CardanEulerSingularityException { 442 443 for (RotationConvention convention : RotationConvention.values()) { 444 RotationOrder[] CardanOrders = { 445 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, 446 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX 447 }; 448 449 for (int i = 0; i < CardanOrders.length; ++i) { 450 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { 451 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) { 452 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { 453 Rotation r = new Rotation(CardanOrders[i], convention, alpha1, alpha2, alpha3); 454 double[] angles = r.getAngles(CardanOrders[i], convention); 455 checkAngle(angles[0], alpha1); 456 checkAngle(angles[1], alpha2); 457 checkAngle(angles[2], alpha3); 458 } 459 } 460 } 461 } 462 463 RotationOrder[] EulerOrders = { 464 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, 465 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ 466 }; 467 468 for (int i = 0; i < EulerOrders.length; ++i) { 469 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { 470 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) { 471 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { 472 Rotation r = new Rotation(EulerOrders[i], convention, 473 alpha1, alpha2, alpha3); 474 double[] angles = r.getAngles(EulerOrders[i], convention); 475 checkAngle(angles[0], alpha1); 476 checkAngle(angles[1], alpha2); 477 checkAngle(angles[2], alpha3); 478 } 479 } 480 } 481 } 482 } 483 484 } 485 486 @Test 487 public void testSingularities() { 488 489 for (RotationConvention convention : RotationConvention.values()) { 490 RotationOrder[] CardanOrders = { 491 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, 492 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX 493 }; 494 495 double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 }; 496 for (int i = 0; i < CardanOrders.length; ++i) { 497 for (int j = 0; j < singularCardanAngle.length; ++j) { 498 Rotation r = new Rotation(CardanOrders[i], convention, 0.1, singularCardanAngle[j], 0.3); 499 try { 500 r.getAngles(CardanOrders[i], convention); 501 Assert.fail("an exception should have been caught"); 502 } catch (CardanEulerSingularityException cese) { 503 // expected behavior 504 } 505 } 506 } 507 508 RotationOrder[] EulerOrders = { 509 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, 510 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ 511 }; 512 513 double[] singularEulerAngle = { 0, FastMath.PI }; 514 for (int i = 0; i < EulerOrders.length; ++i) { 515 for (int j = 0; j < singularEulerAngle.length; ++j) { 516 Rotation r = new Rotation(EulerOrders[i], convention, 0.1, singularEulerAngle[j], 0.3); 517 try { 518 r.getAngles(EulerOrders[i], convention); 519 Assert.fail("an exception should have been caught"); 520 } catch (CardanEulerSingularityException cese) { 521 // expected behavior 522 } 523 } 524 } 525 } 526 527 528 } 529 530 @Test 531 public void testQuaternion() throws MathIllegalArgumentException { 532 533 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR); 534 double n = 23.5; 535 Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(), 536 n * r1.getQ2(), n * r1.getQ3(), 537 true); 538 for (double x = -0.9; x < 0.9; x += 0.2) { 539 for (double y = -0.9; y < 0.9; y += 0.2) { 540 for (double z = -0.9; z < 0.9; z += 0.2) { 541 Vector3D u = new Vector3D(x, y, z); 542 checkVector(r2.applyTo(u), r1.applyTo(u)); 543 } 544 } 545 } 546 547 r1 = new Rotation( 0.288, 0.384, 0.36, 0.8, false); 548 checkRotation(r1, -r1.getQ0(), -r1.getQ1(), -r1.getQ2(), -r1.getQ3()); 549 550 } 551 552 @Test 553 public void testApplyTo() throws MathIllegalArgumentException { 554 555 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR); 556 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR); 557 Rotation r3 = r2.applyTo(r1); 558 559 for (double x = -0.9; x < 0.9; x += 0.2) { 560 for (double y = -0.9; y < 0.9; y += 0.2) { 561 for (double z = -0.9; z < 0.9; z += 0.2) { 562 Vector3D u = new Vector3D(x, y, z); 563 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u)); 564 } 565 } 566 } 567 568 } 569 570 @Test 571 public void testComposeVectorOperator() throws MathIllegalArgumentException { 572 573 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR); 574 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR); 575 Rotation r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR); 576 577 for (double x = -0.9; x < 0.9; x += 0.2) { 578 for (double y = -0.9; y < 0.9; y += 0.2) { 579 for (double z = -0.9; z < 0.9; z += 0.2) { 580 Vector3D u = new Vector3D(x, y, z); 581 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u)); 582 } 583 } 584 } 585 586 } 587 588 @Test 589 public void testComposeFrameTransform() throws MathIllegalArgumentException { 590 591 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM); 592 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM); 593 Rotation r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM); 594 Rotation r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR); 595 Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15); 596 597 for (double x = -0.9; x < 0.9; x += 0.2) { 598 for (double y = -0.9; y < 0.9; y += 0.2) { 599 for (double z = -0.9; z < 0.9; z += 0.2) { 600 Vector3D u = new Vector3D(x, y, z); 601 checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u)); 602 } 603 } 604 } 605 606 } 607 608 @Test 609 public void testApplyInverseToRotation() throws MathIllegalArgumentException { 610 611 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR); 612 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR); 613 Rotation r3 = r2.applyInverseTo(r1); 614 615 for (double x = -0.9; x < 0.9; x += 0.2) { 616 for (double y = -0.9; y < 0.9; y += 0.2) { 617 for (double z = -0.9; z < 0.9; z += 0.2) { 618 Vector3D u = new Vector3D(x, y, z); 619 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u)); 620 } 621 } 622 } 623 624 } 625 626 @Test 627 public void testComposeInverseVectorOperator() throws MathIllegalArgumentException { 628 629 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR); 630 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR); 631 Rotation r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR); 632 633 for (double x = -0.9; x < 0.9; x += 0.2) { 634 for (double y = -0.9; y < 0.9; y += 0.2) { 635 for (double z = -0.9; z < 0.9; z += 0.2) { 636 Vector3D u = new Vector3D(x, y, z); 637 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u)); 638 } 639 } 640 } 641 642 } 643 644 @Test 645 public void testComposeInverseFrameTransform() throws MathIllegalArgumentException { 646 647 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM); 648 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM); 649 Rotation r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM); 650 Rotation r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR); 651 Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15); 652 653 for (double x = -0.9; x < 0.9; x += 0.2) { 654 for (double y = -0.9; y < 0.9; y += 0.2) { 655 for (double z = -0.9; z < 0.9; z += 0.2) { 656 Vector3D u = new Vector3D(x, y, z); 657 checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u)); 658 } 659 } 660 } 661 662 } 663 664 @Test 665 public void testArray() throws MathIllegalArgumentException { 666 667 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR); 668 669 for (double x = -0.9; x < 0.9; x += 0.2) { 670 for (double y = -0.9; y < 0.9; y += 0.2) { 671 for (double z = -0.9; z < 0.9; z += 0.2) { 672 Vector3D u = new Vector3D(x, y, z); 673 Vector3D v = r.applyTo(u); 674 double[] inOut = new double[] { x, y, z }; 675 r.applyTo(inOut, inOut); 676 Assert.assertEquals(v.getX(), inOut[0], 1.0e-10); 677 Assert.assertEquals(v.getY(), inOut[1], 1.0e-10); 678 Assert.assertEquals(v.getZ(), inOut[2], 1.0e-10); 679 r.applyInverseTo(inOut, inOut); 680 Assert.assertEquals(u.getX(), inOut[0], 1.0e-10); 681 Assert.assertEquals(u.getY(), inOut[1], 1.0e-10); 682 Assert.assertEquals(u.getZ(), inOut[2], 1.0e-10); 683 } 684 } 685 } 686 687 } 688 689 @Test 690 public void testApplyInverseTo() throws MathIllegalArgumentException { 691 692 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR); 693 for (double lambda = 0; lambda < 6.2; lambda += 0.2) { 694 for (double phi = -1.55; phi < 1.55; phi += 0.2) { 695 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi), 696 FastMath.sin(lambda) * FastMath.cos(phi), 697 FastMath.sin(phi)); 698 r.applyInverseTo(r.applyTo(u)); 699 checkVector(u, r.applyInverseTo(r.applyTo(u))); 700 checkVector(u, r.applyTo(r.applyInverseTo(u))); 701 } 702 } 703 704 r = Rotation.IDENTITY; 705 for (double lambda = 0; lambda < 6.2; lambda += 0.2) { 706 for (double phi = -1.55; phi < 1.55; phi += 0.2) { 707 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi), 708 FastMath.sin(lambda) * FastMath.cos(phi), 709 FastMath.sin(phi)); 710 checkVector(u, r.applyInverseTo(r.applyTo(u))); 711 checkVector(u, r.applyTo(r.applyInverseTo(u))); 712 } 713 } 714 715 r = new Rotation(Vector3D.PLUS_K, FastMath.PI, RotationConvention.VECTOR_OPERATOR); 716 for (double lambda = 0; lambda < 6.2; lambda += 0.2) { 717 for (double phi = -1.55; phi < 1.55; phi += 0.2) { 718 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi), 719 FastMath.sin(lambda) * FastMath.cos(phi), 720 FastMath.sin(phi)); 721 checkVector(u, r.applyInverseTo(r.applyTo(u))); 722 checkVector(u, r.applyTo(r.applyInverseTo(u))); 723 } 724 } 725 726 } 727 728 @Test 729 public void testIssue639() throws MathArithmeticException{ 730 Vector3D u1 = new Vector3D(-1321008684645961.0 / 268435456.0, 731 -5774608829631843.0 / 268435456.0, 732 -3822921525525679.0 / 4294967296.0); 733 Vector3D u2 =new Vector3D( -5712344449280879.0 / 2097152.0, 734 -2275058564560979.0 / 1048576.0, 735 4423475992255071.0 / 65536.0); 736 Rotation rot = new Rotation(u1, u2, Vector3D.PLUS_I,Vector3D.PLUS_K); 737 Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0(), 1.0e-15); 738 Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1(), 1.0e-15); 739 Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2(), 1.0e-15); 740 Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3(), 1.0e-15); 741 } 742 743 @Test 744 public void testIssue801() throws MathArithmeticException { 745 Vector3D u1 = new Vector3D(0.9999988431610581, -0.0015210774290851095, 0.0); 746 Vector3D u2 = new Vector3D(0.0, 0.0, 1.0); 747 748 Vector3D v1 = new Vector3D(0.9999999999999999, 0.0, 0.0); 749 Vector3D v2 = new Vector3D(0.0, 0.0, -1.0); 750 751 Rotation quat = new Rotation(u1, u2, v1, v2); 752 double q2 = quat.getQ0() * quat.getQ0() + 753 quat.getQ1() * quat.getQ1() + 754 quat.getQ2() * quat.getQ2() + 755 quat.getQ3() * quat.getQ3(); 756 Assert.assertEquals(1.0, q2, 1.0e-14); 757 Assert.assertEquals(0.0, Vector3D.angle(v1, quat.applyTo(u1)), 1.0e-14); 758 Assert.assertEquals(0.0, Vector3D.angle(v2, quat.applyTo(u2)), 1.0e-14); 759 760 } 761 762 @Test 763 public void testGithubPullRequest22A() { 764 final RotationOrder order = RotationOrder.ZYX; 765 final double xRotation = FastMath.toDegrees(30); 766 final double yRotation = FastMath.toDegrees(20); 767 final double zRotation = FastMath.toDegrees(10); 768 final Vector3D startingVector = Vector3D.PLUS_I; 769 Vector3D appliedIndividually = startingVector; 770 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually); 771 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually); 772 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually); 773 774 final Vector3D bad = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, yRotation, xRotation).applyTo(startingVector); 775 776 Assert.assertEquals(bad.getX(), appliedIndividually.getX(), 1e-12); 777 Assert.assertEquals(bad.getY(), appliedIndividually.getY(), 1e-12); 778 Assert.assertEquals(bad.getZ(), appliedIndividually.getZ(), 1e-12); 779 } 780 781 @Test 782 public void testGithubPullRequest22B() { 783 final RotationOrder order = RotationOrder.ZYX; 784 final double xRotation = FastMath.toDegrees(30); 785 final double yRotation = FastMath.toDegrees(20); 786 final double zRotation = FastMath.toDegrees(10); 787 final Vector3D startingVector = Vector3D.PLUS_I; 788 Vector3D appliedIndividually = startingVector; 789 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually); 790 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually); 791 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually); 792 793 final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM); 794 final Rotation r2 = new Rotation(order.getA2(), yRotation, RotationConvention.FRAME_TRANSFORM); 795 final Rotation r3 = new Rotation(order.getA3(), xRotation, RotationConvention.FRAME_TRANSFORM); 796 final Rotation composite = r1.compose(r2.compose(r3, 797 RotationConvention.FRAME_TRANSFORM), 798 RotationConvention.FRAME_TRANSFORM); 799 final Vector3D good = composite.applyTo(startingVector); 800 801 Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12); 802 Assert.assertEquals(good.getY(), appliedIndividually.getY(), 1e-12); 803 Assert.assertEquals(good.getZ(), appliedIndividually.getZ(), 1e-12); 804 } 805 806 private void checkVector(Vector3D v1, Vector3D v2) { 807 Assert.assertTrue(v1.subtract(v2).getNorm() < 1.0e-10); 808 } 809 810 private void checkAngle(double a1, double a2) { 811 Assert.assertEquals(a1, MathUtils.normalizeAngle(a2, a1), 1.0e-10); 812 } 813 814 private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) { 815 Assert.assertEquals(0, Rotation.distance(r, new Rotation(q0, q1, q2, q3, false)), 1.0e-12); 816 } 817 818 } 819