1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22 package org.hipparchus.complex;
23
24 import java.util.Random;
25
26 import org.hipparchus.exception.MathIllegalArgumentException;
27 import org.hipparchus.util.FastMath;
28 import org.hipparchus.util.MathArrays;
29 import org.junit.Assert;
30 import org.junit.Test;
31
32 public class QuaternionTest {
33
34 private static final double EPS = Math.ulp(1d);
35
36 private static final double COMPARISON_EPS = 1e-14;
37
38 @Test
39 public final void testAccessors1() {
40 final double q0 = 2;
41 final double q1 = 5.4;
42 final double q2 = 17;
43 final double q3 = 0.0005;
44 final Quaternion q = new Quaternion(q0, q1, q2, q3);
45
46 Assert.assertEquals(q0, q.getQ0(), 0);
47 Assert.assertEquals(q1, q.getQ1(), 0);
48 Assert.assertEquals(q2, q.getQ2(), 0);
49 Assert.assertEquals(q3, q.getQ3(), 0);
50 }
51
52 @Test
53 public final void testAccessors2() {
54 final double q0 = 2;
55 final double q1 = 5.4;
56 final double q2 = 17;
57 final double q3 = 0.0005;
58 final Quaternion q = new Quaternion(q0, q1, q2, q3);
59
60 final double sP = q.getScalarPart();
61 final double[] vP = q.getVectorPart();
62
63 Assert.assertEquals(q0, sP, 0);
64 Assert.assertEquals(q1, vP[0], 0);
65 Assert.assertEquals(q2, vP[1], 0);
66 Assert.assertEquals(q3, vP[2], 0);
67 }
68
69 @Test
70 public final void testAccessors3() {
71 final double q0 = 2;
72 final double q1 = 5.4;
73 final double q2 = 17;
74 final double q3 = 0.0005;
75 final Quaternion q = new Quaternion(q0, new double[] { q1, q2, q3 });
76
77 final double sP = q.getScalarPart();
78 final double[] vP = q.getVectorPart();
79
80 Assert.assertEquals(q0, sP, 0);
81 Assert.assertEquals(q1, vP[0], 0);
82 Assert.assertEquals(q2, vP[1], 0);
83 Assert.assertEquals(q3, vP[2], 0);
84 }
85
86 @Test(expected=MathIllegalArgumentException.class)
87 public void testWrongDimension() {
88 new Quaternion(new double[] { 1, 2 });
89 }
90
91 @Test
92 public final void testConjugate() {
93 final double q0 = 2;
94 final double q1 = 5.4;
95 final double q2 = 17;
96 final double q3 = 0.0005;
97 final Quaternion q = new Quaternion(q0, q1, q2, q3);
98
99 final Quaternion qConjugate = q.getConjugate();
100
101 Assert.assertEquals(q0, qConjugate.getQ0(), 0);
102 Assert.assertEquals(-q1, qConjugate.getQ1(), 0);
103 Assert.assertEquals(-q2, qConjugate.getQ2(), 0);
104 Assert.assertEquals(-q3, qConjugate.getQ3(), 0);
105 }
106
107 @Test
108 public final void testProductQuaternionQuaternion() {
109
110
111
112 final Quaternion qA = new Quaternion(1, 0.5, -3, 4);
113 final Quaternion qB = new Quaternion(6, 2, 1, -9);
114 final Quaternion qResult = Quaternion.multiply(qA, qB);
115
116 Assert.assertEquals(44, qResult.getQ0(), EPS);
117 Assert.assertEquals(28, qResult.getQ1(), EPS);
118 Assert.assertEquals(-4.5, qResult.getQ2(), EPS);
119 Assert.assertEquals(21.5, qResult.getQ3(), EPS);
120
121
122
123
124 final Quaternion conjugateOfProduct = qB.getConjugate().multiply(qA.getConjugate());
125 final Quaternion productOfConjugate = (qA.multiply(qB)).getConjugate();
126
127 Assert.assertEquals(conjugateOfProduct.getQ0(), productOfConjugate.getQ0(), EPS);
128 Assert.assertEquals(conjugateOfProduct.getQ1(), productOfConjugate.getQ1(), EPS);
129 Assert.assertEquals(conjugateOfProduct.getQ2(), productOfConjugate.getQ2(), EPS);
130 Assert.assertEquals(conjugateOfProduct.getQ3(), productOfConjugate.getQ3(), EPS);
131 }
132
133 @Test
134 public final void testProductQuaternionVector() {
135
136
137
138 final Quaternion quaternion = new Quaternion(4, 7, -1, 2);
139 final double[] vector = {2.0, 1.0, 3.0};
140 final Quaternion qResultQxV = Quaternion.multiply(quaternion, new Quaternion(vector));
141
142 Assert.assertEquals(-19, qResultQxV.getQ0(), EPS);
143 Assert.assertEquals(3, qResultQxV.getQ1(), EPS);
144 Assert.assertEquals(-13, qResultQxV.getQ2(), EPS);
145 Assert.assertEquals(21, qResultQxV.getQ3(), EPS);
146
147
148
149
150 final double[] vectorQ = quaternion.getVectorPart();
151
152 final double scalarPartRefQxV = -MathArrays.linearCombination(vectorQ, vector);
153 Assert.assertEquals(scalarPartRefQxV, qResultQxV.getScalarPart(), EPS);
154
155
156
157 final Quaternion qResultVxQ = Quaternion.multiply(new Quaternion(vector), quaternion);
158
159 Assert.assertEquals(-19, qResultVxQ.getQ0(), EPS);
160 Assert.assertEquals(13, qResultVxQ.getQ1(), EPS);
161 Assert.assertEquals(21, qResultVxQ.getQ2(), EPS);
162 Assert.assertEquals(3, qResultVxQ.getQ3(), EPS);
163
164
165
166
167 final double scalarPartRefVxQ = -MathArrays.linearCombination(vectorQ, vector);
168 Assert.assertEquals(scalarPartRefVxQ, qResultVxQ.getScalarPart(), EPS);
169
170 }
171
172 @Test
173 public final void testDotProductQuaternionQuaternion() {
174
175 final double expected = -6.;
176
177 final Quaternion q1 = new Quaternion(1, 2, 2, 1);
178 final Quaternion q2 = new Quaternion(3, -2, -1, -3);
179
180 final double actual1 = Quaternion.dotProduct(q1, q2);
181 final double actual2 = q1.dotProduct(q2);
182
183 Assert.assertEquals(expected, actual1, EPS);
184 Assert.assertEquals(expected, actual2, EPS);
185 }
186
187 @Test
188 public final void testScalarMultiplyDouble() {
189
190 final double w = 1.6;
191 final double x = -4.8;
192 final double y = 11.20;
193 final double z = 2.56;
194
195 final Quaternion q1 = new Quaternion(0.5, -1.5, 3.5, 0.8);
196 final double a = 3.2;
197
198 final Quaternion q = q1.multiply(a);
199
200 Assert.assertEquals(w, q.getQ0(), COMPARISON_EPS);
201 Assert.assertEquals(x, q.getQ1(), COMPARISON_EPS);
202 Assert.assertEquals(y, q.getQ2(), COMPARISON_EPS);
203 Assert.assertEquals(z, q.getQ3(), COMPARISON_EPS);
204 }
205
206 @Test
207 public final void testAddQuaternionQuaternion() {
208
209 final double w = 4;
210 final double x = -1;
211 final double y = 2;
212 final double z = -4;
213
214 final Quaternion q1 = new Quaternion(1., 2., -2., -1.);
215 final Quaternion q2 = new Quaternion(3., -3., 4., -3.);
216
217 final Quaternion qa = Quaternion.add(q1, q2);
218 final Quaternion qb = q1.add(q2);
219
220 Assert.assertEquals(w, qa.getQ0(), EPS);
221 Assert.assertEquals(x, qa.getQ1(), EPS);
222 Assert.assertEquals(y, qa.getQ2(), EPS);
223 Assert.assertEquals(z, qa.getQ3(), EPS);
224
225 Assert.assertEquals(w, qb.getQ0(), EPS);
226 Assert.assertEquals(x, qb.getQ1(), EPS);
227 Assert.assertEquals(y, qb.getQ2(), EPS);
228 Assert.assertEquals(z, qb.getQ3(), EPS);
229 }
230
231 @Test
232 public final void testSubtractQuaternionQuaternion() {
233
234 final double w = -2.;
235 final double x = 5.;
236 final double y = -6.;
237 final double z = 2.;
238
239 final Quaternion q1 = new Quaternion(1., 2., -2., -1.);
240 final Quaternion q2 = new Quaternion(3., -3., 4., -3.);
241
242 final Quaternion qa = Quaternion.subtract(q1, q2);
243 final Quaternion qb = q1.subtract(q2);
244
245 Assert.assertEquals(w, qa.getQ0(), EPS);
246 Assert.assertEquals(x, qa.getQ1(), EPS);
247 Assert.assertEquals(y, qa.getQ2(), EPS);
248 Assert.assertEquals(z, qa.getQ3(), EPS);
249
250 Assert.assertEquals(w, qb.getQ0(), EPS);
251 Assert.assertEquals(x, qb.getQ1(), EPS);
252 Assert.assertEquals(y, qb.getQ2(), EPS);
253 Assert.assertEquals(z, qb.getQ3(), EPS);
254 }
255
256 @Test
257 public final void testNorm() {
258
259 final double q0 = 2;
260 final double q1 = 1;
261 final double q2 = -4;
262 final double q3 = 3;
263 final Quaternion q = new Quaternion(q0, q1, q2, q3);
264
265 final double norm = q.getNorm();
266
267 Assert.assertEquals(FastMath.sqrt(30), norm, 0);
268
269 final double normSquareRef = Quaternion.multiply(q, q.getConjugate()).getScalarPart();
270 Assert.assertEquals(FastMath.sqrt(normSquareRef), norm, 0);
271 }
272
273 @Test
274 public final void testNormalize() {
275
276 final Quaternion q = new Quaternion(2, 1, -4, -2);
277
278 final Quaternion versor = q.normalize();
279
280 Assert.assertEquals(2.0 / 5.0, versor.getQ0(), 0);
281 Assert.assertEquals(1.0 / 5.0, versor.getQ1(), 0);
282 Assert.assertEquals(-4.0 / 5.0, versor.getQ2(), 0);
283 Assert.assertEquals(-2.0 / 5.0, versor.getQ3(), 0);
284
285 Assert.assertEquals(1, versor.getNorm(), 0);
286 }
287
288 @Test(expected=MathIllegalArgumentException.class)
289 public final void testNormalizeFail() {
290 final Quaternion zeroQ = new Quaternion(0, 0, 0, 0);
291 zeroQ.normalize();
292 }
293
294 @Test
295 public final void testObjectEquals() {
296 final double one = 1;
297 final Quaternion q1 = new Quaternion(one, one, one, one);
298 Assert.assertTrue(q1.equals(q1));
299
300 final Quaternion q2 = new Quaternion(one, one, one, one);
301 Assert.assertTrue(q2.equals(q1));
302
303 final Quaternion q3 = new Quaternion(one, FastMath.nextUp(one), one, one);
304 Assert.assertFalse(q3.equals(q1));
305 }
306
307 @Test
308 public final void testQuaternionEquals() {
309 final double inc = 1e-5;
310 final Quaternion q1 = new Quaternion(2, 1, -4, -2);
311 final Quaternion q2 = new Quaternion(q1.getQ0() + inc, q1.getQ1(), q1.getQ2(), q1.getQ3());
312 final Quaternion q3 = new Quaternion(q1.getQ0(), q1.getQ1() + inc, q1.getQ2(), q1.getQ3());
313 final Quaternion q4 = new Quaternion(q1.getQ0(), q1.getQ1(), q1.getQ2() + inc, q1.getQ3());
314 final Quaternion q5 = new Quaternion(q1.getQ0(), q1.getQ1(), q1.getQ2(), q1.getQ3() + inc);
315
316 Assert.assertFalse(q1.equals(q2, 0.9 * inc));
317 Assert.assertFalse(q1.equals(q3, 0.9 * inc));
318 Assert.assertFalse(q1.equals(q4, 0.9 * inc));
319 Assert.assertFalse(q1.equals(q5, 0.9 * inc));
320
321 Assert.assertTrue(q1.equals(q2, 1.1 * inc));
322 Assert.assertTrue(q1.equals(q3, 1.1 * inc));
323 Assert.assertTrue(q1.equals(q4, 1.1 * inc));
324 Assert.assertTrue(q1.equals(q5, 1.1 * inc));
325 }
326
327 @Test
328 public final void testQuaternionEquals2() {
329 final Quaternion q1 = new Quaternion(1, 4, 2, 3);
330 final double gap = 1e-5;
331 final Quaternion q2 = new Quaternion(1 + gap, 4 + gap, 2 + gap, 3 + gap);
332
333 Assert.assertTrue(q1.equals(q2, 10 * gap));
334 Assert.assertFalse(q1.equals(q2, gap));
335 Assert.assertFalse(q1.equals(q2, gap / 10));
336 }
337
338 @Test
339 public final void testIsUnitQuaternion() {
340 final Random r = new Random(48);
341 final int numberOfTrials = 1000;
342 for (int i = 0; i < numberOfTrials; i++) {
343 final Quaternion q1 = new Quaternion(r.nextDouble(), r.nextDouble(), r.nextDouble(), r.nextDouble());
344 final Quaternion q2 = q1.normalize();
345 Assert.assertTrue(q2.isUnitQuaternion(COMPARISON_EPS));
346 }
347
348 final Quaternion q = new Quaternion(1, 1, 1, 1);
349 Assert.assertFalse(q.isUnitQuaternion(COMPARISON_EPS));
350 }
351
352 @Test
353 public final void testIsPureQuaternion() {
354 final Quaternion q1 = new Quaternion(0, 5, 4, 8);
355 Assert.assertTrue(q1.isPureQuaternion(EPS));
356
357 final Quaternion q2 = new Quaternion(0 - EPS, 5, 4, 8);
358 Assert.assertTrue(q2.isPureQuaternion(EPS));
359
360 final Quaternion q3 = new Quaternion(0 - 1.1 * EPS, 5, 4, 8);
361 Assert.assertFalse(q3.isPureQuaternion(EPS));
362
363 final Random r = new Random(48);
364 final double[] v = {r.nextDouble(), r.nextDouble(), r.nextDouble()};
365 final Quaternion q4 = new Quaternion(v);
366 Assert.assertTrue(q4.isPureQuaternion(0));
367
368 final Quaternion q5 = new Quaternion(0, v);
369 Assert.assertTrue(q5.isPureQuaternion(0));
370 }
371
372 @Test
373 public final void testGetInverse() {
374 final Quaternion q = new Quaternion(1.5, 4, 2, -2.5);
375
376 final Quaternion inverseQ = q.getInverse();
377 Assert.assertEquals(1.5 / 28.5, inverseQ.getQ0(), 0);
378 Assert.assertEquals(-4.0 / 28.5, inverseQ.getQ1(), 0);
379 Assert.assertEquals(-2.0 / 28.5, inverseQ.getQ2(), 0);
380 Assert.assertEquals(2.5 / 28.5, inverseQ.getQ3(), 0);
381
382 final Quaternion product = Quaternion.multiply(inverseQ, q);
383 Assert.assertEquals(1, product.getQ0(), EPS);
384 Assert.assertEquals(0, product.getQ1(), EPS);
385 Assert.assertEquals(0, product.getQ2(), EPS);
386 Assert.assertEquals(0, product.getQ3(), EPS);
387
388 final Quaternion qNul = new Quaternion(0, 0, 0, 0);
389 try {
390 final Quaternion inverseQNul = qNul.getInverse();
391 Assert.fail("expecting MathIllegalArgumentException but got : " + inverseQNul);
392 } catch (MathIllegalArgumentException ex) {
393
394 }
395 }
396
397 @Test
398 public final void testToString() {
399 final Quaternion q = new Quaternion(1, 2, 3, 4);
400 Assert.assertTrue(q.toString().equals("[1.0 2.0 3.0 4.0]"));
401 }
402 }