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 * https://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 /*
19 * This is not the original file distributed by the Apache Software Foundation
20 * It has been modified by the Hipparchus project
21 */
22
23 package org.hipparchus.geometry.euclidean.threed;
24
25 import org.hipparchus.CalculusFieldElement;
26 import org.hipparchus.exception.MathIllegalStateException;
27 import org.hipparchus.geometry.LocalizedGeometryFormats;
28 import org.hipparchus.util.FastMath;
29 import org.hipparchus.util.MathArrays;
30 import org.hipparchus.util.MathUtils;
31
32 /** Enumerate representing a rotation order specification for Cardan or Euler angles.
33 * <p>
34 * Since Hipparchus 1.7 this class is an enumerate class.
35 * </p>
36 */
37 public enum RotationOrder {
38
39 /** Set of Cardan angles.
40 * this ordered set of rotations is around X, then around Y, then
41 * around Z
42 */
43 XYZ(RotationStage.X, RotationStage.Y, RotationStage.Z),
44
45 /** Set of Cardan angles.
46 * this ordered set of rotations is around X, then around Z, then
47 * around Y
48 */
49 XZY(RotationStage.X, RotationStage.Z, RotationStage.Y),
50
51 /** Set of Cardan angles.
52 * this ordered set of rotations is around Y, then around X, then
53 * around Z
54 */
55 YXZ(RotationStage.Y, RotationStage.X, RotationStage.Z),
56
57 /** Set of Cardan angles.
58 * this ordered set of rotations is around Y, then around Z, then
59 * around X
60 */
61 YZX(RotationStage.Y, RotationStage.Z, RotationStage.X),
62
63 /** Set of Cardan angles.
64 * this ordered set of rotations is around Z, then around X, then
65 * around Y
66 */
67 ZXY(RotationStage.Z, RotationStage.X, RotationStage.Y),
68
69 /** Set of Cardan angles.
70 * this ordered set of rotations is around Z, then around Y, then
71 * around X
72 */
73 ZYX(RotationStage.Z, RotationStage.Y, RotationStage.X),
74
75 /** Set of Euler angles.
76 * this ordered set of rotations is around X, then around Y, then
77 * around X
78 */
79 XYX(RotationStage.X, RotationStage.Y, RotationStage.X),
80
81 /** Set of Euler angles.
82 * this ordered set of rotations is around X, then around Z, then
83 * around X
84 */
85 XZX(RotationStage.X, RotationStage.Z, RotationStage.X),
86
87 /** Set of Euler angles.
88 * this ordered set of rotations is around Y, then around X, then
89 * around Y
90 */
91 YXY(RotationStage.Y, RotationStage.X, RotationStage.Y),
92
93 /** Set of Euler angles.
94 * this ordered set of rotations is around Y, then around Z, then
95 * around Y
96 */
97 YZY(RotationStage.Y, RotationStage.Z, RotationStage.Y),
98
99 /** Set of Euler angles.
100 * this ordered set of rotations is around Z, then around X, then
101 * around Z
102 */
103 ZXZ(RotationStage.Z, RotationStage.X, RotationStage.Z),
104
105 /** Set of Euler angles.
106 * this ordered set of rotations is around Z, then around Y, then
107 * around Z
108 */
109 ZYZ(RotationStage.Z, RotationStage.Y, RotationStage.Z);
110
111 /** Switch to safe computation of asin/acos.
112 * @since 3.1
113 */
114 private static final double SAFE_SWITCH = 0.999;
115
116 /** Name of the rotations order. */
117 private final String name;
118
119 /** First stage. */
120 private final RotationStage stage1;
121
122 /** Second stage. */
123 private final RotationStage stage2;
124
125 /** Third stage. */
126 private final RotationStage stage3;
127
128 /** Missing stage (for Euler rotations). */
129 private final RotationStage missing;
130
131 /** Sign for direct order (i.e. X before Y, Y before Z, Z before X). */
132 private final double sign;
133
134 /** Enum constructor.
135 * @param stage1 first stage
136 * @param stage2 second stage
137 * @param stage3 third stage
138 */
139 RotationOrder(final RotationStage stage1, final RotationStage stage2, final RotationStage stage3) {
140 this.name = stage1.name() + stage2.name() + stage3.name();
141 this.stage1 = stage1;
142 this.stage2 = stage2;
143 this.stage3 = stage3;
144
145 if (stage1 == stage3) {
146 // Euler rotations
147 if (stage1 != RotationStage.X && stage2 != RotationStage.X) {
148 missing = RotationStage.X;
149 } else if (stage1 != RotationStage.Y && stage2 != RotationStage.Y) {
150 missing = RotationStage.Y;
151 } else {
152 missing = RotationStage.Z;
153 }
154 } else {
155 // Cardan rotations
156 missing = null;
157 }
158
159 // check if the first two rotations are in direct or indirect order
160 final Vector3D a1 = stage1.getAxis();
161 final Vector3D a2 = stage2.getAxis();
162 final Vector3D a3 = missing == null ? stage3.getAxis() : missing.getAxis();
163 sign = FastMath.copySign(1.0, Vector3D.dotProduct(a3, Vector3D.crossProduct(a1, a2)));
164
165 }
166
167 /** Get a string representation of the instance.
168 * @return a string representation of the instance (in fact, its name)
169 */
170 @Override
171 public String toString() {
172 return name;
173 }
174
175 /** Get the axis of the first rotation.
176 * @return axis of the first rotation
177 */
178 public Vector3D getA1() {
179 return stage1.getAxis();
180 }
181
182 /** Get the axis of the second rotation.
183 * @return axis of the second rotation
184 */
185 public Vector3D getA2() {
186 return stage2.getAxis();
187 }
188
189 /** Get the axis of the third rotation.
190 * @return axis of the third rotation
191 */
192 public Vector3D getA3() {
193 return stage3.getAxis();
194 }
195
196 /** Get the rotation order corresponding to a string representation.
197 * @param value name
198 * @return a rotation order object
199 * @since 1.7
200 */
201 public static RotationOrder getRotationOrder(final String value) {
202 try {
203 return RotationOrder.valueOf(value);
204 } catch (IllegalArgumentException iae) {
205 // Invalid value. An exception is thrown
206 throw new MathIllegalStateException(iae,
207 LocalizedGeometryFormats.INVALID_ROTATION_ORDER_NAME,
208 value);
209 }
210 }
211
212 /** Get the Cardan or Euler angles corresponding to the instance.
213 * <p>
214 * The algorithm used here works even when the rotation is exactly at the
215 * the singularity of the rotation order and convention. In this case, one of
216 * the angles in the singular pair is arbitrarily set to exactly 0 and the
217 * second angle is computed. The angle set to 0 in the singular case is the
218 * angle of the first rotation in the case of Cardan orders, and it is the angle
219 * of the last rotation in the case of Euler orders. This implies that extracting
220 * the angles of a rotation never fails (it used to trigger an exception in singular
221 * cases up to Hipparchus 3.0).
222 * </p>
223 * @param rotation rotation from which angles should be extracted
224 * @param convention convention to use for the semantics of the angle
225 * @return an array of three angles, in the order specified by the set
226 * @since 3.1
227 */
228 public double[] getAngles(final Rotation rotation, final RotationConvention convention) {
229
230 // Cardan/Euler angles rotations matrices have the following form, taking
231 // RotationOrder.XYZ (φ about X, θ about Y, ψ about Z) and RotationConvention.FRAME_TRANSFORM
232 // as an example:
233 // [ cos θ cos ψ cos φ sin ψ + sin φ sin θ cos ψ sin φ sin ψ - cos φ sin θ cos ψ ]
234 // [ -cos θ sin ψ cos φ cos ψ - sin φ sin θ sin ψ cos ψ sin φ + cos φ sin θ sin ψ ]
235 // [ sin θ - sin φ cos θ cos φ cos θ ]
236
237 // One can see that there is a "simple" column (the first column in the example above) and a "simple"
238 // row (the last row in the example above). In fact, the simple column always corresponds to the axis
239 // of the first rotation in RotationConvention.FRAME_TRANSFORM convention (i.e. X axis here, hence
240 // first column) and to the axis of the third rotation in RotationConvention.VECTOR_OPERATOR
241 // convention. The "simple" row always corresponds to axis of the third rotation in
242 // RotationConvention.FRAME_TRANSFORM convention (i.e. Z axis here, hence last row) and to the axis
243 // of the first rotation in RotationConvention.VECTOR_OPERATOR convention.
244 final Vector3D vCol = getColumnVector(rotation, convention);
245 final Vector3D vRow = getRowVector(rotation, convention);
246
247 // in the example above, we see that the components of the simple row
248 // are [ sin θ, -sin φ cos θ, cos φ cos θ ], which means that if we select
249 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
250 // φ as atan2(-Yrow, +Zrow), i.e. the coordinates along the axes of the
251 // two final rotations in the Cardan sequence.
252
253 // in the example above, we see that the components of the simple column
254 // are [ cos θ cos ψ, -cos θ sin ψ, sin θ ], which means that if we select
255 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
256 // ψ as atan2(-Ycol, +Xcol), i.e. the coordinates along the axes of the
257 // two initial rotations in the Cardan sequence.
258
259 // if we are exactly on the singularity (i.e. θ = ±π/2), then the matrix
260 // degenerates to:
261 // [ 0 sin(ψ ± φ) ∓ cos(ψ ± φ) ]
262 // [ 0 cos(ψ ± φ) ± sin(ψ ± φ) ]
263 // [ ±1 0 0 ]
264 // at singularity, there is an infinite number of pairs (here ψ and φ) that represent
265 // the same rotation, so we have to make some assumptions, arbitrarily setting one angle
266 // of the pair and computing the other one from the non-singular middle column in the
267 // FRAME_TRANSFORM rotation convention or from the non-singular middle row in the
268 // VECTOR_OPERATOR rotation convention. In our XYZ rotation order and FRAME_TRANSFORM
269 // rotation convention example, we can extract a consistent set of angles set by:
270 // - setting θ = ±π/2 copying the sign from ±1 term,
271 // - arbitrarily set φ = 0
272 // - compute ψ = atan2(XmidCol, YmidCol)
273
274 // These results generalize to all sequences, with some adjustments for
275 // Euler sequence where we need to replace one axis by the missing axis
276 // and some sign adjustments depending on the rotation order being direct
277 // (i.e. X before Y, Y before Z, Z before X) or indirect
278
279 if (missing == null) {
280 // this is a Cardan angles order
281
282 if (convention == RotationConvention.FRAME_TRANSFORM) {
283 final double s = stage2.getComponent(vRow) * -sign;
284 final double c = stage3.getComponent(vRow);
285 if (s * s + c * c > 1.0e-30) {
286 // regular case
287 return new double[] {
288 FastMath.atan2(s, c),
289 safeAsin(stage1.getComponent(vRow), stage2.getComponent(vRow), stage3.getComponent(vRow)) * sign,
290 FastMath.atan2(stage2.getComponent(vCol) * -sign, stage1.getComponent(vCol))
291 };
292 } else {
293 // near singularity
294 final Vector3D midCol = rotation.applyTo(stage2.getAxis());
295 return new double[] {
296 0.0,
297 FastMath.copySign(MathUtils.SEMI_PI, stage1.getComponent(vRow) * sign),
298 FastMath.atan2(stage1.getComponent(midCol) * sign, stage2.getComponent(midCol))
299 };
300 }
301 } else {
302 final double s = stage2.getComponent(vCol) * -sign;
303 final double c = stage3.getComponent(vCol);
304 if (s * s + c * c > 1.0e-30) {
305 // regular case
306 return new double[] {
307 FastMath.atan2(s, c),
308 safeAsin(stage3.getComponent(vRow), stage1.getComponent(vRow), stage2.getComponent(vRow)) * sign,
309 FastMath.atan2(stage2.getComponent(vRow) * -sign, stage1.getComponent(vRow))
310 };
311 } else {
312 // near singularity
313 final Vector3D midRow = rotation.applyInverseTo(stage2.getAxis());
314 return new double[] {
315 0.0,
316 FastMath.copySign(MathUtils.SEMI_PI, stage3.getComponent(vRow) * sign),
317 FastMath.atan2(stage1.getComponent(midRow) * sign, stage2.getComponent(midRow))
318 };
319 }
320 }
321 } else {
322 // this is an Euler angles order
323 if (convention == RotationConvention.FRAME_TRANSFORM) {
324 final double s = stage2.getComponent(vRow);
325 final double c = missing.getComponent(vRow) * -sign;
326 if (s * s + c * c > 1.0e-30) {
327 // regular case
328 return new double[] {
329 FastMath.atan2(s, c),
330 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
331 FastMath.atan2(stage2.getComponent(vCol), missing.getComponent(vCol) * sign)
332 };
333 } else {
334 // near singularity
335 final Vector3D midCol = rotation.applyTo(stage2.getAxis());
336 return new double[] {
337 FastMath.atan2(missing.getComponent(midCol) * -sign, stage2.getComponent(midCol)) *
338 FastMath.copySign(1, stage1.getComponent(vRow)),
339 stage1.getComponent(vRow) > 0 ? 0 : FastMath.PI,
340 0.0
341 };
342 }
343 } else {
344 final double s = stage2.getComponent(vCol);
345 final double c = missing.getComponent(vCol) * -sign;
346 if (s * s + c * c > 1.0e-30) {
347 // regular case
348 return new double[] {
349 FastMath.atan2(s, c),
350 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
351 FastMath.atan2(stage2.getComponent(vRow), missing.getComponent(vRow) * sign)
352 };
353 } else {
354 // near singularity
355 final Vector3D midRow = rotation.applyInverseTo(stage2.getAxis());
356 return new double[] {
357 FastMath.atan2(missing.getComponent(midRow) * -sign, stage2.getComponent(midRow)) *
358 FastMath.copySign(1, stage1.getComponent(vRow)),
359 stage1.getComponent(vRow) > 0 ? 0 : FastMath.PI,
360 0.0
361 };
362 }
363 }
364 }
365 }
366
367 /** Get the Cardan or Euler angles corresponding to the instance.
368 * <p>
369 * The algorithm used here works even when the rotation is exactly at the
370 * the singularity of the rotation order and convention. In this case, one of
371 * the angles in the singular pair is arbitrarily set to exactly 0 and the
372 * second angle is computed. The angle set to 0 in the singular case is the
373 * angle of the first rotation in the case of Cardan orders, and it is the angle
374 * of the last rotation in the case of Euler orders. This implies that extracting
375 * the angles of a rotation never fails (it used to trigger an exception in singular
376 * cases up to Hipparchus 3.0).
377 * </p>
378 * @param <T> type of the field elements
379 * @param rotation rotation from which angles should be extracted
380 * @param convention convention to use for the semantics of the angle
381 * @return an array of three angles, in the order specified by the set
382 * @since 3.1
383 */
384 public <T extends CalculusFieldElement<T>> T[] getAngles(final FieldRotation<T> rotation, final RotationConvention convention) {
385
386 // Cardan/Euler angles rotations matrices have the following form, taking
387 // RotationOrder.XYZ (φ about X, θ about Y, ψ about Z) and RotationConvention.FRAME_TRANSFORM
388 // as an example:
389 // [ cos θ cos ψ cos φ sin ψ + sin φ sin θ cos ψ sin φ sin ψ - cos φ sin θ cos ψ ]
390 // [ -cos θ sin ψ cos φ cos ψ - sin φ sin θ sin ψ cos ψ sin φ + cos φ sin θ sin ψ ]
391 // [ sin θ - sin φ cos θ cos φ cos θ ]
392
393 // One can see that there is a "simple" column (the first column in the example above) and a "simple"
394 // row (the last row in the example above). In fact, the simple column always corresponds to the axis
395 // of the first rotation in RotationConvention.FRAME_TRANSFORM convention (i.e. X axis here, hence
396 // first column) and to the axis of the third rotation in RotationConvention.VECTOR_OPERATOR
397 // convention. The "simple" row always corresponds to axis of the third rotation in
398 // RotationConvention.FRAME_TRANSFORM convention (i.e. Z axis here, hence last row) and to the axis
399 // of the first rotation in RotationConvention.VECTOR_OPERATOR convention.
400 final FieldVector3D<T> vCol = getColumnVector(rotation, convention);
401 final FieldVector3D<T> vRow = getRowVector(rotation, convention);
402
403 // in the example above, we see that the components of the simple row
404 // are [ sin θ, -sin φ cos θ, cos φ cos θ ], which means that if we select
405 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
406 // φ as atan2(-Yrow, +Zrow), i.e. the coordinates along the axes of the
407 // two final rotations in the Cardan sequence.
408
409 // in the example above, we see that the components of the simple column
410 // are [ cos θ cos ψ, -cos θ sin ψ, sin θ ], which means that if we select
411 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
412 // ψ as atan2(-Ycol, +Xcol), i.e. the coordinates along the axes of the
413 // two initial rotations in the Cardan sequence.
414
415 // if we are exactly on the singularity (i.e. θ = ±π/2), then the matrix
416 // degenerates to:
417 // [ 0 sin(ψ ± φ) ∓ cos(ψ ± φ) ]
418 // [ 0 cos(ψ ± φ) ± sin(ψ ± φ) ]
419 // [ ±1 0 0 ]
420 // at singularity, there is an infinite number of pairs (here ψ and φ) that represent
421 // the same rotation, so we have to make some assumptions, arbitrarily setting one angle
422 // of the pair and computing the other one from the non-singular middle column in the
423 // FRAME_TRANSFORM rotation convention or from the non-singular middle row in the
424 // VECTOR_OPERATOR rotation convention. In our XYZ rotation order and FRAME_TRANSFORM
425 // rotation convention example, we can extract a consistent set of angles set by:
426 // - setting θ = ±π/2 copying the sign from ±1 term,
427 // - arbitrarily set φ = 0
428 // - compute ψ = atan2(XmidCol, YmidCol)
429
430 // These results generalize to all sequences, with some adjustments for
431 // Euler sequence where we need to replace one axis by the missing axis
432 // and some sign adjustments depending on the rotation order being direct
433 // (i.e. X before Y, Y before Z, Z before X) or indirect
434
435 if (missing == null) {
436 // this is a Cardan angles order
437 if (convention == RotationConvention.FRAME_TRANSFORM) {
438 final T s = stage2.getComponent(vRow).multiply(-sign);
439 final T c = stage3.getComponent(vRow);
440 if (s.square().add(c.square()).getReal() > 1.0e-30) {
441 // regular case
442 return buildArray(FastMath.atan2(s, c),
443 safeAsin(stage1.getComponent(vRow), stage2.getComponent(vRow), stage3.getComponent(vRow)).multiply(sign),
444 FastMath.atan2(stage2.getComponent(vCol).multiply(-sign), stage1.getComponent(vCol)));
445 } else {
446 // near singularity
447 final FieldVector3D<T> midCol = rotation.applyTo(stage2.getAxis());
448 return buildArray(s.getField().getZero(),
449 FastMath.copySign(s.getPi().multiply(0.5), stage1.getComponent(vRow).multiply(sign)),
450 FastMath.atan2(stage1.getComponent(midCol).multiply(sign), stage2.getComponent(midCol)));
451 }
452 } else {
453 final T s = stage2.getComponent(vCol).multiply(-sign);
454 final T c = stage3.getComponent(vCol);
455 if (s.square().add(c.square()).getReal() > 1.0e-30) {
456 // regular case
457 return buildArray(FastMath.atan2(stage2.getComponent(vCol).multiply(-sign), stage3.getComponent(vCol)),
458 safeAsin(stage3.getComponent(vRow), stage1.getComponent(vRow), stage2.getComponent(vRow)).multiply(sign),
459 FastMath.atan2(stage2.getComponent(vRow).multiply(-sign), stage1.getComponent(vRow)));
460 } else {
461 // near singularity
462 final FieldVector3D<T> midRow = rotation.applyInverseTo(stage2.getAxis());
463 return buildArray(s.getField().getZero(),
464 FastMath.copySign(s.getPi().multiply(0.5), stage3.getComponent(vRow).multiply(sign)),
465 FastMath.atan2(stage1.getComponent(midRow).multiply(sign), stage2.getComponent(midRow)));
466 }
467 }
468 } else {
469 // this is an Euler angles order
470 if (convention == RotationConvention.FRAME_TRANSFORM) {
471 final T s = stage2.getComponent(vRow);
472 final T c = missing.getComponent(vRow).multiply(-sign);
473 if (s.square().add(c.square()).getReal() > 1.0e-30) {
474 // regular case
475 return buildArray(FastMath.atan2(s, c),
476 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
477 FastMath.atan2(stage2.getComponent(vCol), missing.getComponent(vCol).multiply(sign)));
478 } else {
479 // near singularity
480 final FieldVector3D<T> midCol = rotation.applyTo(stage2.getAxis());
481 return buildArray(FastMath.atan2(missing.getComponent(midCol).multiply(-sign), stage2.getComponent(midCol)).
482 multiply(FastMath.copySign(1, stage1.getComponent(vRow).getReal())),
483 stage1.getComponent(vRow).getReal() > 0 ? s.getField().getZero() : s.getPi(),
484 s.getField().getZero());
485 }
486 } else {
487 final T s = stage2.getComponent(vCol);
488 final T c = missing.getComponent(vCol).multiply(-sign);
489 if (s.square().add(c.square()).getReal() > 1.0e-30) {
490 // regular case
491 return buildArray(FastMath.atan2(s, c),
492 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
493 FastMath.atan2(stage2.getComponent(vRow), missing.getComponent(vRow).multiply(sign)));
494 } else {
495 // near singularity
496 final FieldVector3D<T> midRow = rotation.applyInverseTo(stage2.getAxis());
497 return buildArray(FastMath.atan2(missing.getComponent(midRow).multiply(-sign), stage2.getComponent(midRow)).
498 multiply(FastMath.copySign(1, stage1.getComponent(vRow).getReal())),
499 stage1.getComponent(vRow).getReal() > 0 ? s.getField().getZero() : s.getPi(),
500 s.getField().getZero());
501 }
502 }
503 }
504 }
505
506 /** Get the simplest column vector from the rotation matrix.
507 * @param rotation rotation
508 * @param convention convention to use for the semantics of the angle
509 * @return column vector
510 * @since 3.1
511 */
512 private Vector3D getColumnVector(final Rotation rotation,
513 final RotationConvention convention) {
514 return rotation.applyTo(convention == RotationConvention.VECTOR_OPERATOR ?
515 stage3.getAxis() :
516 stage1.getAxis());
517 }
518
519 /** Get the simplest row vector from the rotation matrix.
520 * @param rotation rotation
521 * @param convention convention to use for the semantics of the angle
522 * @return row vector
523 * @since 3.1
524 */
525 private Vector3D getRowVector(final Rotation rotation,
526 final RotationConvention convention) {
527 return rotation.applyInverseTo(convention == RotationConvention.VECTOR_OPERATOR ?
528 stage1.getAxis() :
529 stage3.getAxis());
530 }
531
532 /** Get the simplest column vector from the rotation matrix.
533 * @param <T> type of the field elements
534 * @param rotation rotation
535 * @param convention convention to use for the semantics of the angle
536 * @return column vector
537 * @since 3.1
538 */
539 private <T extends CalculusFieldElement<T>> FieldVector3D<T> getColumnVector(final FieldRotation<T> rotation,
540 final RotationConvention convention) {
541 return rotation.applyTo(convention == RotationConvention.VECTOR_OPERATOR ?
542 stage3.getAxis() :
543 stage1.getAxis());
544 }
545
546 /** Get the simplest row vector from the rotation matrix.
547 * @param <T> type of the field elements
548 * @param rotation rotation
549 * @param convention convention to use for the semantics of the angle
550 * @return row vector
551 * @since 3.1
552 */
553 private <T extends CalculusFieldElement<T>> FieldVector3D<T> getRowVector(final FieldRotation<T> rotation,
554 final RotationConvention convention) {
555 return rotation.applyInverseTo(convention == RotationConvention.VECTOR_OPERATOR ?
556 stage1.getAxis() :
557 stage3.getAxis());
558 }
559
560 /** Safe computation of acos(some vector coordinate) working around singularities.
561 * @param cos cosine coordinate
562 * @param sin1 one of the sine coordinates
563 * @param sin2 other sine coordinate
564 * @return acos of the coordinate
565 * @since 3.1
566 */
567 private static double safeAcos(final double cos, final double sin1, final double sin2) {
568 if (cos < -SAFE_SWITCH) {
569 return FastMath.PI - FastMath.asin(FastMath.sqrt(sin1 * sin1 + sin2 * sin2));
570 } else if (cos > SAFE_SWITCH) {
571 return FastMath.asin(FastMath.sqrt(sin1 * sin1 + sin2 * sin2));
572 } else {
573 return FastMath.acos(cos);
574 }
575 }
576
577 /** Safe computation of asin(some vector coordinate) working around singularities.
578 * @param sin sine coordinate
579 * @param cos1 one of the cosine coordinates
580 * @param cos2 other cosine coordinate
581 * @return asin of the coordinate
582 * @since 3.1
583 */
584 private static double safeAsin(final double sin, final double cos1, final double cos2) {
585 if (sin < -SAFE_SWITCH) {
586 return -FastMath.acos(FastMath.sqrt(cos1 * cos1 + cos2 * cos2));
587 } else if (sin > SAFE_SWITCH) {
588 return FastMath.acos(FastMath.sqrt(cos1 * cos1 + cos2 * cos2));
589 } else {
590 return FastMath.asin(sin);
591 }
592 }
593
594 /** Safe computation of acos(some vector coordinate) working around singularities.
595 * @param <T> type of the field elements
596 * @param cos cosine coordinate
597 * @param sin1 one of the sine coordinates
598 * @param sin2 other sine coordinate
599 * @return acos of the coordinate
600 * @since 3.1
601 */
602 private static <T extends CalculusFieldElement<T>> T safeAcos(final T cos,
603 final T sin1,
604 final T sin2) {
605 if (cos.getReal() < -SAFE_SWITCH) {
606 return FastMath.asin(FastMath.sqrt(sin1.square().add(sin2.square()))).subtract(sin1.getPi()).negate();
607 } else if (cos.getReal() > SAFE_SWITCH) {
608 return FastMath.asin(FastMath.sqrt(sin1.square().add(sin2.square())));
609 } else {
610 return FastMath.acos(cos);
611 }
612 }
613
614 /** Safe computation of asin(some vector coordinate) working around singularities.
615 * @param <T> type of the field elements
616 * @param sin sine coordinate
617 * @param cos1 one of the cosine coordinates
618 * @param cos2 other cosine coordinate
619 * @return asin of the coordinate
620 * @since 3.1
621 */
622 private static <T extends CalculusFieldElement<T>> T safeAsin(final T sin,
623 final T cos1,
624 final T cos2) {
625 if (sin.getReal() < -SAFE_SWITCH) {
626 return FastMath.acos(FastMath.sqrt(cos1.square().add(cos2.square()))).negate();
627 } else if (sin.getReal() > SAFE_SWITCH) {
628 return FastMath.acos(FastMath.sqrt(cos1.square().add(cos2.square())));
629 } else {
630 return FastMath.asin(sin);
631 }
632 }
633
634 /** Create a dimension 3 array.
635 * @param <T> type of the field elements
636 * @param a0 first array element
637 * @param a1 second array element
638 * @param a2 third array element
639 * @return new array
640 * @since 3.1
641 */
642 private static <T extends CalculusFieldElement<T>> T[] buildArray(final T a0, final T a1, final T a2) {
643 final T[] array = MathArrays.buildArray(a0.getField(), 3);
644 array[0] = a0;
645 array[1] = a1;
646 array[2] = a2;
647 return array;
648 }
649
650 }