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 /**
135 * /** Private constructor. This is a utility class that cannot be
136 * instantiated by the user, so its only constructor is private.
137 *
138 * @param stage1 first stage
139 * @param stage2 second stage
140 * @param stage3 third stage
141 */
142 RotationOrder(final RotationStage stage1, final RotationStage stage2, final RotationStage stage3) {
143 this.name = stage1.name() + stage2.name() + stage3.name();
144 this.stage1 = stage1;
145 this.stage2 = stage2;
146 this.stage3 = stage3;
147
148 if (stage1 == stage3) {
149 // Euler rotations
150 if (stage1 != RotationStage.X && stage2 != RotationStage.X) {
151 missing = RotationStage.X;
152 } else if (stage1 != RotationStage.Y && stage2 != RotationStage.Y) {
153 missing = RotationStage.Y;
154 } else {
155 missing = RotationStage.Z;
156 }
157 } else {
158 // Cardan rotations
159 missing = null;
160 }
161
162 // check if the first two rotations are in direct or indirect order
163 final Vector3D a1 = stage1.getAxis();
164 final Vector3D a2 = stage2.getAxis();
165 final Vector3D a3 = missing == null ? stage3.getAxis() : missing.getAxis();
166 sign = FastMath.copySign(1.0, Vector3D.dotProduct(a3, Vector3D.crossProduct(a1, a2)));
167
168 }
169
170 /** Get a string representation of the instance.
171 * @return a string representation of the instance (in fact, its name)
172 */
173 @Override
174 public String toString() {
175 return name;
176 }
177
178 /** Get the axis of the first rotation.
179 * @return axis of the first rotation
180 */
181 public Vector3D getA1() {
182 return stage1.getAxis();
183 }
184
185 /** Get the axis of the second rotation.
186 * @return axis of the second rotation
187 */
188 public Vector3D getA2() {
189 return stage2.getAxis();
190 }
191
192 /** Get the axis of the third rotation.
193 * @return axis of the third rotation
194 */
195 public Vector3D getA3() {
196 return stage3.getAxis();
197 }
198
199 /** Get the rotation order corresponding to a string representation.
200 * @param value name
201 * @return a rotation order object
202 * @since 1.7
203 */
204 public static RotationOrder getRotationOrder(final String value) {
205 try {
206 return RotationOrder.valueOf(value);
207 } catch (IllegalArgumentException iae) {
208 // Invalid value. An exception is thrown
209 throw new MathIllegalStateException(iae,
210 LocalizedGeometryFormats.INVALID_ROTATION_ORDER_NAME,
211 value);
212 }
213 }
214
215 /** Get the Cardan or Euler angles corresponding to the instance.
216 * @param rotation rotation from which angles should be extracted
217 * @param convention convention to use for the semantics of the angle
218 * @return an array of three angles, in the order specified by the set
219 * @since 3.1
220 */
221 public double[] getAngles(final Rotation rotation, final RotationConvention convention) {
222
223 // Cardan/Euler angles rotations matrices have the following form, taking
224 // RotationOrder.XYZ (φ about X, θ about Y, ψ about Z) and RotationConvention.FRAME_TRANSFORM
225 // as an example:
226 // [ cos θ cos ψ cos φ sin ψ + sin φ sin θ cos ψ sin φ sin ψ - cos φ sin θ cos ψ ]
227 // [ -cos θ sin ψ cos φ cos ψ - sin φ sin θ sin ψ cos ψ sin φ + cos φ sin θ sin ψ ]
228 // [ sin θ - sin φ cos θ cos φ cos θ ]
229
230 // One can see that there is a "simple" column (the first column in the example above) and a "simple"
231 // row (the last row in the example above). In fact, the simple column always corresponds to the axis
232 // of the first rotation in RotationConvention.FRAME_TRANSFORM convention (i.e. X axis here, hence
233 // first column) and to the axis of the third rotation in RotationConvention.VECTOR_OPERATOR
234 // convention. The "simple" row always corresponds to axis of the third rotation in
235 // RotationConvention.FRAME_TRANSFORM convention (i.e. Z axis here, hence last row) and to the axis
236 // of the first rotation in RotationConvention.VECTOR_OPERATOR convention.
237 final Vector3D vCol = getColumnVector(rotation, convention);
238 final Vector3D vRow = getRowVector(rotation, convention);
239
240 // in the example above, we see that the components of the simple row
241 // are [ sin θ, -sin φ cos θ, cos φ cos θ ], which means that if we select
242 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
243 // φ as atan2(-Yrow, +Zrow), i.e. the coordinates along the axes of the
244 // two final rotations in the Cardan sequence.
245
246 // in the example above, we see that the components of the simple column
247 // are [ cos θ cos ψ, -cos θ sin ψ, sin θ ], which means that if we select
248 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
249 // ψ as atan2(-Ycol, +Xcol), i.e. the coordinates along the axes of the
250 // two initial rotations in the Cardan sequence.
251
252 // if we are exactly on the singularity (i.e. θ = ±π/2), then the matrix
253 // degenerates to:
254 // [ 0 sin(ψ ± φ) ∓ cos(ψ ± φ) ]
255 // [ 0 cos(ψ ± φ) ± sin(ψ ± φ) ]
256 // [ ±1 0 0 ]
257 // so we can set θ=±π/2 copying the sign from ±1 term,
258 // arbitrarily set φ=0
259 // and extract ψ as atan2(XmidCol, YmidCol)
260
261 // These results generalize to all sequences, with some adjustments for
262 // Euler sequence where we need to replace one axis by the missing axis
263 // and some sign adjustments depending on the rotation order being direct
264 // (i.e. X before Y, Y before Z, Z before X) or indirect
265
266 if (missing == null) {
267 // this is a Cardan angles order
268
269 if (convention == RotationConvention.FRAME_TRANSFORM) {
270 final double s = stage2.getComponent(vRow) * -sign;
271 final double c = stage3.getComponent(vRow);
272 if (s * s + c * c > 1.0e-30) {
273 // regular case
274 return new double[] {
275 FastMath.atan2(s, c),
276 safeAsin(stage1.getComponent(vRow), stage2.getComponent(vRow), stage3.getComponent(vRow)) * sign,
277 FastMath.atan2(stage2.getComponent(vCol) * -sign, stage1.getComponent(vCol))
278 };
279 } else {
280 // near singularity
281 final Vector3D midCol = rotation.applyTo(stage2.getAxis());
282 return new double[] {
283 0.0,
284 FastMath.copySign(MathUtils.SEMI_PI, stage1.getComponent(vRow) * sign),
285 FastMath.atan2(stage1.getComponent(midCol) * sign, stage2.getComponent(midCol))
286 };
287 }
288 } else {
289 final double s = stage2.getComponent(vCol) * -sign;
290 final double c = stage3.getComponent(vCol);
291 if (s * s + c * c > 1.0e-30) {
292 // regular case
293 return new double[] {
294 FastMath.atan2(s, c),
295 safeAsin(stage3.getComponent(vRow), stage1.getComponent(vRow), stage2.getComponent(vRow)) * sign,
296 FastMath.atan2(stage2.getComponent(vRow) * -sign, stage1.getComponent(vRow))
297 };
298 } else {
299 // near singularity
300 final Vector3D midRow = rotation.applyInverseTo(stage2.getAxis());
301 return new double[] {
302 0.0,
303 FastMath.copySign(MathUtils.SEMI_PI, stage3.getComponent(vRow) * sign),
304 FastMath.atan2(stage1.getComponent(midRow) * sign, stage2.getComponent(midRow))
305 };
306 }
307 }
308 } else {
309 // this is an Euler angles order
310 if (convention == RotationConvention.FRAME_TRANSFORM) {
311 final double s = stage2.getComponent(vRow);
312 final double c = missing.getComponent(vRow) * -sign;
313 if (s * s + c * c > 1.0e-30) {
314 // regular case
315 return new double[] {
316 FastMath.atan2(s, c),
317 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
318 FastMath.atan2(stage2.getComponent(vCol), missing.getComponent(vCol) * sign)
319 };
320 } else {
321 // near singularity
322 final Vector3D midCol = rotation.applyTo(stage2.getAxis());
323 return new double[] {
324 FastMath.atan2(missing.getComponent(midCol) * -sign, stage2.getComponent(midCol)) *
325 FastMath.copySign(1, stage1.getComponent(vRow)),
326 stage1.getComponent(vRow) > 0 ? 0 : FastMath.PI,
327 0.0
328 };
329 }
330 } else {
331 final double s = stage2.getComponent(vCol);
332 final double c = missing.getComponent(vCol) * -sign;
333 if (s * s + c * c > 1.0e-30) {
334 // regular case
335 return new double[] {
336 FastMath.atan2(s, c),
337 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
338 FastMath.atan2(stage2.getComponent(vRow), missing.getComponent(vRow) * sign)
339 };
340 } else {
341 // near singularity
342 final Vector3D midRow = rotation.applyInverseTo(stage2.getAxis());
343 return new double[] {
344 FastMath.atan2(missing.getComponent(midRow) * -sign, stage2.getComponent(midRow)) *
345 FastMath.copySign(1, stage1.getComponent(vRow)),
346 stage1.getComponent(vRow) > 0 ? 0 : FastMath.PI,
347 0.0
348 };
349 }
350 }
351 }
352 }
353
354 /** Get the Cardan or Euler angles corresponding to the instance.
355 * @param <T> type of the field elements
356 * @param rotation rotation from which angles should be extracted
357 * @param convention convention to use for the semantics of the angle
358 * @return an array of three angles, in the order specified by the set
359 * @since 3.1
360 */
361 public <T extends CalculusFieldElement<T>> T[] getAngles(final FieldRotation<T> rotation, final RotationConvention convention) {
362
363 // Cardan/Euler angles rotations matrices have the following form, taking
364 // RotationOrder.XYZ (φ about X, θ about Y, ψ about Z) and RotationConvention.FRAME_TRANSFORM
365 // as an example:
366 // [ cos θ cos ψ cos φ sin ψ + sin φ sin θ cos ψ sin φ sin ψ - cos φ sin θ cos ψ ]
367 // [ -cos θ sin ψ cos φ cos ψ - sin φ sin θ sin ψ cos ψ sin φ + cos φ sin θ sin ψ ]
368 // [ sin θ - sin φ cos θ cos φ cos θ ]
369
370 // One can see that there is a "simple" column (the first column in the example above) and a "simple"
371 // row (the last row in the example above). In fact, the simple column always corresponds to the axis
372 // of the first rotation in RotationConvention.FRAME_TRANSFORM convention (i.e. X axis here, hence
373 // first column) and to the axis of the third rotation in RotationConvention.VECTOR_OPERATOR
374 // convention. The "simple" row always corresponds to axis of the third rotation in
375 // RotationConvention.FRAME_TRANSFORM convention (i.e. Z axis here, hence last row) and to the axis
376 // of the first rotation in RotationConvention.VECTOR_OPERATOR convention.
377 final FieldVector3D<T> vCol = getColumnVector(rotation, convention);
378 final FieldVector3D<T> vRow = getRowVector(rotation, convention);
379
380 // in the example above, we see that the components of the simple row
381 // are [ sin θ, -sin φ cos θ, cos φ cos θ ], which means that if we select
382 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
383 // φ as atan2(-Yrow, +Zrow), i.e. the coordinates along the axes of the
384 // two final rotations in the Cardan sequence.
385
386 // in the example above, we see that the components of the simple column
387 // are [ cos θ cos ψ, -cos θ sin ψ, sin θ ], which means that if we select
388 // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
389 // ψ as atan2(-Ycol, +Xcol), i.e. the coordinates along the axes of the
390 // two initial rotations in the Cardan sequence.
391
392 // if we are exactly on the singularity (i.e. θ = ±π/2), then the matrix
393 // degenerates to:
394 // [ 0 sin(ψ ± φ) ∓ cos(ψ ± φ) ]
395 // [ 0 cos(ψ ± φ) ± sin(ψ ± φ) ]
396 // [ ±1 0 0 ]
397 // so we can set θ=±π/2 copying the sign from ±1 term,
398 // arbitrarily set φ=0
399 // and extract ψ as atan2(XmidCol, YmidCol)
400
401 // These results generalize to all sequences, with some adjustments for
402 // Euler sequence where we need to replace one axis by the missing axis
403 // and some sign adjustments depending on the rotation order being direct
404 // (i.e. X before Y, Y before Z, Z before X) or indirect
405
406 if (missing == null) {
407 // this is a Cardan angles order
408 if (convention == RotationConvention.FRAME_TRANSFORM) {
409 final T s = stage2.getComponent(vRow).multiply(-sign);
410 final T c = stage3.getComponent(vRow);
411 if (s.square().add(c.square()).getReal() > 1.0e-30) {
412 // regular case
413 return buildArray(FastMath.atan2(s, c),
414 safeAsin(stage1.getComponent(vRow), stage2.getComponent(vRow), stage3.getComponent(vRow)).multiply(sign),
415 FastMath.atan2(stage2.getComponent(vCol).multiply(-sign), stage1.getComponent(vCol)));
416 } else {
417 // near singularity
418 final FieldVector3D<T> midCol = rotation.applyTo(stage2.getAxis());
419 return buildArray(s.getField().getZero(),
420 FastMath.copySign(s.getPi().multiply(0.5), stage1.getComponent(vRow).multiply(sign)),
421 FastMath.atan2(stage1.getComponent(midCol).multiply(sign), stage2.getComponent(midCol)));
422 }
423 } else {
424 final T s = stage2.getComponent(vCol).multiply(-sign);
425 final T c = stage3.getComponent(vCol);
426 if (s.square().add(c.square()).getReal() > 1.0e-30) {
427 // regular case
428 return buildArray(FastMath.atan2(stage2.getComponent(vCol).multiply(-sign), stage3.getComponent(vCol)),
429 safeAsin(stage3.getComponent(vRow), stage1.getComponent(vRow), stage2.getComponent(vRow)).multiply(sign),
430 FastMath.atan2(stage2.getComponent(vRow).multiply(-sign), stage1.getComponent(vRow)));
431 } else {
432 // near singularity
433 final FieldVector3D<T> midRow = rotation.applyInverseTo(stage2.getAxis());
434 return buildArray(s.getField().getZero(),
435 FastMath.copySign(s.getPi().multiply(0.5), stage3.getComponent(vRow).multiply(sign)),
436 FastMath.atan2(stage1.getComponent(midRow).multiply(sign), stage2.getComponent(midRow)));
437 }
438 }
439 } else {
440 // this is an Euler angles order
441 if (convention == RotationConvention.FRAME_TRANSFORM) {
442 final T s = stage2.getComponent(vRow);
443 final T c = missing.getComponent(vRow).multiply(-sign);
444 if (s.square().add(c.square()).getReal() > 1.0e-30) {
445 // regular case
446 return buildArray(FastMath.atan2(s, c),
447 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
448 FastMath.atan2(stage2.getComponent(vCol), missing.getComponent(vCol).multiply(sign)));
449 } else {
450 // near singularity
451 final FieldVector3D<T> midCol = rotation.applyTo(stage2.getAxis());
452 return buildArray(FastMath.atan2(missing.getComponent(midCol).multiply(-sign), stage2.getComponent(midCol)).
453 multiply(FastMath.copySign(1, stage1.getComponent(vRow).getReal())),
454 stage1.getComponent(vRow).getReal() > 0 ? s.getField().getZero() : s.getPi(),
455 s.getField().getZero());
456 }
457 } else {
458 final T s = stage2.getComponent(vCol);
459 final T c = missing.getComponent(vCol).multiply(-sign);
460 if (s.square().add(c.square()).getReal() > 1.0e-30) {
461 // regular case
462 return buildArray(FastMath.atan2(s, c),
463 safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
464 FastMath.atan2(stage2.getComponent(vRow), missing.getComponent(vRow).multiply(sign)));
465 } else {
466 // near singularity
467 final FieldVector3D<T> midRow = rotation.applyInverseTo(stage2.getAxis());
468 return buildArray(FastMath.atan2(missing.getComponent(midRow).multiply(-sign), stage2.getComponent(midRow)).
469 multiply(FastMath.copySign(1, stage1.getComponent(vRow).getReal())),
470 stage1.getComponent(vRow).getReal() > 0 ? s.getField().getZero() : s.getPi(),
471 s.getField().getZero());
472 }
473 }
474 }
475 }
476
477 /** Get the simplest column vector from the rotation matrix.
478 * @param rotation rotation
479 * @param convention convention to use for the semantics of the angle
480 * @return column vector
481 * @since 3.1
482 */
483 private Vector3D getColumnVector(final Rotation rotation,
484 final RotationConvention convention) {
485 return rotation.applyTo(convention == RotationConvention.VECTOR_OPERATOR ?
486 stage3.getAxis() :
487 stage1.getAxis());
488 }
489
490 /** Get the simplest row vector from the rotation matrix.
491 * @param rotation rotation
492 * @param convention convention to use for the semantics of the angle
493 * @return row vector
494 * @since 3.1
495 */
496 private Vector3D getRowVector(final Rotation rotation,
497 final RotationConvention convention) {
498 return rotation.applyInverseTo(convention == RotationConvention.VECTOR_OPERATOR ?
499 stage1.getAxis() :
500 stage3.getAxis());
501 }
502
503 /** Get the simplest column vector from the rotation matrix.
504 * @param <T> type of the field elements
505 * @param rotation rotation
506 * @param convention convention to use for the semantics of the angle
507 * @return column vector
508 * @since 3.1
509 */
510 private <T extends CalculusFieldElement<T>> FieldVector3D<T> getColumnVector(final FieldRotation<T> rotation,
511 final RotationConvention convention) {
512 return rotation.applyTo(convention == RotationConvention.VECTOR_OPERATOR ?
513 stage3.getAxis() :
514 stage1.getAxis());
515 }
516
517 /** Get the simplest row vector from the rotation matrix.
518 * @param <T> type of the field elements
519 * @param rotation rotation
520 * @param convention convention to use for the semantics of the angle
521 * @return row vector
522 * @since 3.1
523 */
524 private <T extends CalculusFieldElement<T>> FieldVector3D<T> getRowVector(final FieldRotation<T> rotation,
525 final RotationConvention convention) {
526 return rotation.applyInverseTo(convention == RotationConvention.VECTOR_OPERATOR ?
527 stage1.getAxis() :
528 stage3.getAxis());
529 }
530
531 /** Safe computation of acos(some vector coordinate) working around singularities.
532 * @param cos cosine coordinate
533 * @param sin1 one of the sine coordinates
534 * @param sin2 other sine coordinate
535 * @return acos of the coordinate
536 * @since 3.1
537 */
538 private static double safeAcos(final double cos, final double sin1, final double sin2) {
539 if (cos < -SAFE_SWITCH) {
540 return FastMath.PI - FastMath.asin(FastMath.sqrt(sin1 * sin1 + sin2 * sin2));
541 } else if (cos > SAFE_SWITCH) {
542 return FastMath.asin(FastMath.sqrt(sin1 * sin1 + sin2 * sin2));
543 } else {
544 return FastMath.acos(cos);
545 }
546 }
547
548 /** Safe computation of asin(some vector coordinate) working around singularities.
549 * @param sin sine coordinate
550 * @param cos1 one of the cosine coordinates
551 * @param cos2 other cosine coordinate
552 * @return asin of the coordinate
553 * @since 3.1
554 */
555 private static double safeAsin(final double sin, final double cos1, final double cos2) {
556 if (sin < -SAFE_SWITCH) {
557 return -FastMath.acos(FastMath.sqrt(cos1 * cos1 + cos2 * cos2));
558 } else if (sin > SAFE_SWITCH) {
559 return FastMath.acos(FastMath.sqrt(cos1 * cos1 + cos2 * cos2));
560 } else {
561 return FastMath.asin(sin);
562 }
563 }
564
565 /** Safe computation of acos(some vector coordinate) working around singularities.
566 * @param <T> type of the field elements
567 * @param cos cosine coordinate
568 * @param sin1 one of the sine coordinates
569 * @param sin2 other sine coordinate
570 * @return acos of the coordinate
571 * @since 3.1
572 */
573 private static <T extends CalculusFieldElement<T>> T safeAcos(final T cos,
574 final T sin1,
575 final T sin2) {
576 if (cos.getReal() < -SAFE_SWITCH) {
577 return FastMath.asin(FastMath.sqrt(sin1.square().add(sin2.square()))).subtract(sin1.getPi()).negate();
578 } else if (cos.getReal() > SAFE_SWITCH) {
579 return FastMath.asin(FastMath.sqrt(sin1.square().add(sin2.square())));
580 } else {
581 return FastMath.acos(cos);
582 }
583 }
584
585 /** Safe computation of asin(some vector coordinate) working around singularities.
586 * @param <T> type of the field elements
587 * @param sin sine coordinate
588 * @param cos1 one of the cosine coordinates
589 * @param cos2 other cosine coordinate
590 * @return asin of the coordinate
591 * @since 3.1
592 */
593 private static <T extends CalculusFieldElement<T>> T safeAsin(final T sin,
594 final T cos1,
595 final T cos2) {
596 if (sin.getReal() < -SAFE_SWITCH) {
597 return FastMath.acos(FastMath.sqrt(cos1.square().add(cos2.square()))).negate();
598 } else if (sin.getReal() > SAFE_SWITCH) {
599 return FastMath.acos(FastMath.sqrt(cos1.square().add(cos2.square())));
600 } else {
601 return FastMath.asin(sin);
602 }
603 }
604
605 /** Create a dimension 3 array.
606 * @param <T> type of the field elements
607 * @param a0 first array element
608 * @param a1 second array element
609 * @param a2 third array element
610 * @return new array
611 * @since 3.1
612 */
613 private static <T extends CalculusFieldElement<T>> T[] buildArray(final T a0, final T a1, final T a2) {
614 final T[] array = MathArrays.buildArray(a0.getField(), 3);
615 array[0] = a0;
616 array[1] = a1;
617 array[2] = a2;
618 return array;
619 }
620
621 }