← Back to branch summary

~ubuntu-branches/ubuntu/oneiric/commons-math/oneiric

« back to all changes in this revision

Viewing changes to src/test/java/org/apache/commons/math/geometry/RotationTest.java

  • Committer: Bazaar Package Importer
  • Author(s): Damien Raude-Morvan
  • Date: 2009-08-22 01:13:25 UTC
  • mfrom: (1.1.1 upstream)
  • Revision ID: james.westby@ubuntu.com-20090822011325-hi4peq1ua5weguwn
Tags: 2.0-1
* New upstream release.
* Set Maintainer field to Debian Java Team
* Add myself as Uploaders
* Switch to Quilt patch system:
  - Refresh all patchs
  - Remove B-D on dpatch, Add B-D on quilt
  - Include patchsys-quilt.mk in debian/rules
* Bump Standards-Version to 3.8.3:
  - Add a README.source to describe patch system
* Maven POMs:
  - Add a Build-Depends-Indep dependency on maven-repo-helper
  - Use mh_installpom and mh_installjar to install the POM and the jar to the
    Maven repository
* Use default-jdk/jre:
  - Depends on java5-runtime-headless
  - Build-Depends on default-jdk
  - Use /usr/lib/jvm/default-java as JAVA_HOME
* Move api documentation to /usr/share/doc/libcommons-math-java/api
* Build-Depends on junit4 instead of junit

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/test/java/org/apache/commons/math/geometry/RotationTest.java
 
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.math.geometry;
 
19
 
 
20
import org.apache.commons.math.geometry.CardanEulerSingularityException;
 
21
import org.apache.commons.math.geometry.NotARotationMatrixException;
 
22
import org.apache.commons.math.geometry.Rotation;
 
23
import org.apache.commons.math.geometry.RotationOrder;
 
24
import org.apache.commons.math.geometry.Vector3D;
 
25
import org.apache.commons.math.util.MathUtils;
 
26
 
 
27
import junit.framework.*;
 
28
 
 
29
public class RotationTest
 
30
  extends TestCase {
 
31
 
 
32
  public RotationTest(String name) {
 
33
    super(name);
 
34
  }
 
35
 
 
36
  public void testIdentity() {
 
37
 
 
38
    Rotation r = Rotation.IDENTITY;
 
39
    checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
 
40
    checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
 
41
    checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
 
42
    checkAngle(r.getAngle(), 0);
 
43
 
 
44
    r = new Rotation(-1, 0, 0, 0, false);
 
45
    checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
 
46
    checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
 
47
    checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
 
48
    checkAngle(r.getAngle(), 0);
 
49
 
 
50
    r = new Rotation(42, 0, 0, 0, true);
 
51
    checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
 
52
    checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
 
53
    checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
 
54
    checkAngle(r.getAngle(), 0);
 
55
 
 
56
  }
 
57
 
 
58
  public void testAxisAngle() {
 
59
 
 
60
    Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * Math.PI / 3);
 
61
    checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
 
62
    checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K);
 
63
    checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I);
 
64
    double s = 1 / Math.sqrt(3);
 
65
    checkVector(r.getAxis(), new Vector3D(s, s, s));
 
66
    checkAngle(r.getAngle(), 2 * Math.PI / 3);
 
67
 
 
68
    try {
 
69
      new Rotation(new Vector3D(0, 0, 0), 2 * Math.PI / 3);
 
70
      fail("an exception should have been thrown");
 
71
    } catch (ArithmeticException e) {
 
72
    } catch (Exception e) {
 
73
      fail("unexpected exception");
 
74
    }
 
75
 
 
76
    r = new Rotation(Vector3D.PLUS_K, 1.5 * Math.PI);
 
77
    checkVector(r.getAxis(), new Vector3D(0, 0, -1));
 
78
    checkAngle(r.getAngle(), 0.5 * Math.PI);
 
79
 
 
80
    r = new Rotation(Vector3D.PLUS_J, Math.PI);
 
81
    checkVector(r.getAxis(), Vector3D.PLUS_J);
 
82
    checkAngle(r.getAngle(), Math.PI);
 
83
 
 
84
    checkVector(Rotation.IDENTITY.getAxis(), Vector3D.PLUS_I);
 
85
 
 
86
  }
 
87
 
 
88
  public void testRevert() {
 
89
    Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
 
90
    Rotation reverted = r.revert();
 
91
    checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
 
92
    checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
 
93
    assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
 
94
    assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12);
 
95
  }
 
96
 
 
97
  public void testVectorOnePair() {
 
98
 
 
99
    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
}