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