RotationOrder.java

/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *      https://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * This is not the original file distributed by the Apache Software Foundation
 * It has been modified by the Hipparchus project
 */

package org.hipparchus.geometry.euclidean.threed;

import org.hipparchus.CalculusFieldElement;
import org.hipparchus.exception.MathIllegalStateException;
import org.hipparchus.geometry.LocalizedGeometryFormats;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathArrays;
import org.hipparchus.util.MathUtils;

/** Enumerate representing a rotation order specification for Cardan or Euler angles.
 * <p>
 * Since Hipparchus 1.7 this class is an enumerate class.
 * </p>
 */
public enum RotationOrder {

    /** Set of Cardan angles.
     * this ordered set of rotations is around X, then around Y, then
     * around Z
     */
     XYZ(RotationStage.X, RotationStage.Y, RotationStage.Z),

    /** Set of Cardan angles.
     * this ordered set of rotations is around X, then around Z, then
     * around Y
     */
     XZY(RotationStage.X, RotationStage.Z, RotationStage.Y),

    /** Set of Cardan angles.
     * this ordered set of rotations is around Y, then around X, then
     * around Z
     */
     YXZ(RotationStage.Y, RotationStage.X, RotationStage.Z),

    /** Set of Cardan angles.
     * this ordered set of rotations is around Y, then around Z, then
     * around X
     */
     YZX(RotationStage.Y, RotationStage.Z, RotationStage.X),

    /** Set of Cardan angles.
     * this ordered set of rotations is around Z, then around X, then
     * around Y
     */
     ZXY(RotationStage.Z, RotationStage.X, RotationStage.Y),

    /** Set of Cardan angles.
     * this ordered set of rotations is around Z, then around Y, then
     * around X
     */
     ZYX(RotationStage.Z, RotationStage.Y, RotationStage.X),

    /** Set of Euler angles.
     * this ordered set of rotations is around X, then around Y, then
     * around X
     */
     XYX(RotationStage.X, RotationStage.Y, RotationStage.X),

    /** Set of Euler angles.
     * this ordered set of rotations is around X, then around Z, then
     * around X
     */
     XZX(RotationStage.X, RotationStage.Z, RotationStage.X),

    /** Set of Euler angles.
     * this ordered set of rotations is around Y, then around X, then
     * around Y
     */
     YXY(RotationStage.Y, RotationStage.X, RotationStage.Y),

    /** Set of Euler angles.
     * this ordered set of rotations is around Y, then around Z, then
     * around Y
     */
     YZY(RotationStage.Y, RotationStage.Z, RotationStage.Y),

    /** Set of Euler angles.
     * this ordered set of rotations is around Z, then around X, then
     * around Z
     */
     ZXZ(RotationStage.Z, RotationStage.X, RotationStage.Z),

    /** Set of Euler angles.
     * this ordered set of rotations is around Z, then around Y, then
     * around Z
     */
     ZYZ(RotationStage.Z, RotationStage.Y, RotationStage.Z);

    /** Switch to safe computation of asin/acos.
     * @since 3.1
     */
    private static final double SAFE_SWITCH = 0.999;

    /** Name of the rotations order. */
    private final String name;

    /** First stage. */
    private final RotationStage stage1;

    /** Second stage. */
    private final RotationStage stage2;

    /** Third stage. */
    private final RotationStage stage3;

    /** Missing stage (for Euler rotations). */
    private final RotationStage missing;

    /** Sign for direct order (i.e. X before Y, Y before Z, Z before X). */
    private final double sign;

    /** Enum constructor.
     * @param stage1 first stage
     * @param stage2 second stage
     * @param stage3 third stage
     */
    RotationOrder(final RotationStage stage1, final RotationStage stage2, final RotationStage stage3) {
        this.name   = stage1.name() + stage2.name() + stage3.name();
        this.stage1 = stage1;
        this.stage2 = stage2;
        this.stage3 = stage3;

        if (stage1 == stage3) {
            // Euler rotations
            if (stage1 != RotationStage.X && stage2 != RotationStage.X) {
                missing = RotationStage.X;
            } else if (stage1 != RotationStage.Y && stage2 != RotationStage.Y) {
                missing = RotationStage.Y;
            } else {
                missing = RotationStage.Z;
            }
        } else {
            // Cardan rotations
            missing = null;
        }

        // check if the first two rotations are in direct or indirect order
        final Vector3D a1 = stage1.getAxis();
        final Vector3D a2 = stage2.getAxis();
        final Vector3D a3 = missing == null ? stage3.getAxis() : missing.getAxis();
        sign = FastMath.copySign(1.0, Vector3D.dotProduct(a3, Vector3D.crossProduct(a1, a2)));

    }

    /** Get a string representation of the instance.
     * @return a string representation of the instance (in fact, its name)
     */
    @Override
    public String toString() {
        return name;
    }

    /** Get the axis of the first rotation.
     * @return axis of the first rotation
     */
    public Vector3D getA1() {
        return stage1.getAxis();
    }

    /** Get the axis of the second rotation.
     * @return axis of the second rotation
     */
    public Vector3D getA2() {
        return stage2.getAxis();
    }

    /** Get the axis of the third rotation.
     * @return axis of the third rotation
     */
    public Vector3D getA3() {
        return stage3.getAxis();
    }

    /** Get the rotation order corresponding to a string representation.
     * @param value name
     * @return a rotation order object
     * @since 1.7
     */
    public static RotationOrder getRotationOrder(final String value) {
        try {
            return RotationOrder.valueOf(value);
        } catch (IllegalArgumentException iae) {
            // Invalid value. An exception is thrown
            throw new MathIllegalStateException(iae,
                                                LocalizedGeometryFormats.INVALID_ROTATION_ORDER_NAME,
                                                value);
        }
    }

    /** Get the Cardan or Euler angles corresponding to the instance.
     * <p>
     * The algorithm used here works even when the rotation is exactly at the
     * the singularity of the rotation order and convention. In this case, one of
     * the angles in the singular pair is arbitrarily set to exactly 0 and the
     * second angle is computed. The angle set to 0 in the singular case is the
     * angle of the first rotation in the case of Cardan orders, and it is the angle
     * of the last rotation in the case of Euler orders. This implies that extracting
     * the angles of a rotation never fails (it used to trigger an exception in singular
     * cases up to Hipparchus 3.0).
     * </p>
     * @param rotation rotation from which angles should be extracted
     * @param convention convention to use for the semantics of the angle
     * @return an array of three angles, in the order specified by the set
     * @since 3.1
     */
    public double[] getAngles(final Rotation rotation, final RotationConvention convention) {

        // Cardan/Euler angles rotations matrices have the following form, taking
        // RotationOrder.XYZ (φ about X, θ about Y, ψ about Z) and RotationConvention.FRAME_TRANSFORM
        // as an example:
        // [  cos θ cos ψ     cos φ sin ψ + sin φ sin θ cos ψ     sin φ sin ψ - cos φ sin θ cos ψ ]
        // [ -cos θ sin ψ     cos φ cos ψ - sin φ sin θ sin ψ     cos ψ sin φ + cos φ sin θ sin ψ ]
        // [  sin θ                       - sin φ cos θ                         cos φ cos θ       ]

        // One can see that there is a "simple" column (the first column in the example above) and a "simple"
        // row (the last row in the example above). In fact, the simple column always corresponds to the axis
        // of the first rotation in RotationConvention.FRAME_TRANSFORM convention (i.e. X axis here, hence
        // first column) and to the axis of the third rotation in RotationConvention.VECTOR_OPERATOR
        // convention. The "simple" row always corresponds to axis of the third rotation in
        // RotationConvention.FRAME_TRANSFORM convention (i.e. Z axis here, hence last row) and to the axis
        // of the first rotation in RotationConvention.VECTOR_OPERATOR convention.
        final Vector3D vCol = getColumnVector(rotation, convention);
        final Vector3D vRow = getRowVector(rotation, convention);

        // in the example above, we see that the components of the simple row
        // are [ sin θ, -sin φ cos θ, cos φ cos θ ], which means that if we select
        // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
        // φ as atan2(-Yrow, +Zrow), i.e. the coordinates along the axes of the
        // two final rotations in the Cardan sequence.

        // in the example above, we see that the components of the simple column
        // are [ cos θ cos ψ, -cos θ sin ψ, sin θ ], which means that if we select
        // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
        // ψ as atan2(-Ycol, +Xcol), i.e. the coordinates along the axes of the
        // two initial rotations in the Cardan sequence.

        // if we are exactly on the singularity (i.e. θ = ±π/2), then the matrix
        // degenerates to:
        // [  0    sin(ψ ± φ)    ∓ cos(ψ ± φ) ]
        // [  0    cos(ψ ± φ)    ± sin(ψ ± φ) ]
        // [ ±1       0              0        ]
        // at singularity, there is an infinite number of pairs (here ψ and φ) that represent
        // the same rotation, so we have to make some assumptions, arbitrarily setting one angle
        // of the pair and computing the other one from the non-singular middle column in the
        // FRAME_TRANSFORM rotation convention or from the non-singular middle row in the
        // VECTOR_OPERATOR rotation convention. In our XYZ rotation order and FRAME_TRANSFORM
        // rotation convention example, we can extract a consistent set of angles set by:
        //   - setting θ = ±π/2 copying the sign from ±1 term,
        //   - arbitrarily set φ = 0
        //   - compute ψ = atan2(XmidCol, YmidCol)

        // These results generalize to all sequences, with some adjustments for
        // Euler sequence where we need to replace one axis by the missing axis
        // and some sign adjustments depending on the rotation order being direct
        // (i.e. X before Y, Y before Z, Z before X) or indirect

        if (missing == null) {
            // this is a Cardan angles order

            if (convention == RotationConvention.FRAME_TRANSFORM) {
                final double s = stage2.getComponent(vRow) * -sign;
                final double c = stage3.getComponent(vRow);
                if (s * s + c * c > 1.0e-30) {
                    // regular case
                    return new double[] {
                        FastMath.atan2(s, c),
                        safeAsin(stage1.getComponent(vRow), stage2.getComponent(vRow), stage3.getComponent(vRow)) * sign,
                        FastMath.atan2(stage2.getComponent(vCol) * -sign, stage1.getComponent(vCol))
                    };
                } else {
                    // near singularity
                    final Vector3D midCol = rotation.applyTo(stage2.getAxis());
                    return new double[] {
                        0.0,
                        FastMath.copySign(MathUtils.SEMI_PI, stage1.getComponent(vRow) * sign),
                        FastMath.atan2(stage1.getComponent(midCol) * sign, stage2.getComponent(midCol))
                    };
                }
            } else {
                final double s = stage2.getComponent(vCol) * -sign;
                final double c = stage3.getComponent(vCol);
                if (s * s + c * c > 1.0e-30) {
                    // regular case
                    return new double[] {
                        FastMath.atan2(s, c),
                        safeAsin(stage3.getComponent(vRow), stage1.getComponent(vRow), stage2.getComponent(vRow)) * sign,
                        FastMath.atan2(stage2.getComponent(vRow) * -sign, stage1.getComponent(vRow))
                    };
                } else {
                    // near singularity
                    final Vector3D midRow = rotation.applyInverseTo(stage2.getAxis());
                    return new double[] {
                        0.0,
                        FastMath.copySign(MathUtils.SEMI_PI, stage3.getComponent(vRow) * sign),
                        FastMath.atan2(stage1.getComponent(midRow) * sign, stage2.getComponent(midRow))
                    };
                }
            }
        } else {
            // this is an Euler angles order
            if (convention == RotationConvention.FRAME_TRANSFORM) {
                final double s = stage2.getComponent(vRow);
                final double c = missing.getComponent(vRow) * -sign;
                if (s * s + c * c > 1.0e-30) {
                    // regular case
                    return new double[] {
                        FastMath.atan2(s, c),
                        safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
                        FastMath.atan2(stage2.getComponent(vCol), missing.getComponent(vCol) * sign)
                    };
                } else {
                    // near singularity
                    final Vector3D midCol = rotation.applyTo(stage2.getAxis());
                    return new double[] {
                        FastMath.atan2(missing.getComponent(midCol) * -sign, stage2.getComponent(midCol)) *
                        FastMath.copySign(1, stage1.getComponent(vRow)),
                        stage1.getComponent(vRow) > 0 ? 0 : FastMath.PI,
                        0.0
                    };
                }
            } else {
                final double s = stage2.getComponent(vCol);
                final double c = missing.getComponent(vCol) * -sign;
                if (s * s + c * c > 1.0e-30) {
                    // regular case
                    return new double[] {
                        FastMath.atan2(s, c),
                        safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
                        FastMath.atan2(stage2.getComponent(vRow), missing.getComponent(vRow) * sign)
                    };
                } else {
                    // near singularity
                    final Vector3D midRow = rotation.applyInverseTo(stage2.getAxis());
                    return new double[] {
                        FastMath.atan2(missing.getComponent(midRow) * -sign, stage2.getComponent(midRow)) *
                        FastMath.copySign(1, stage1.getComponent(vRow)),
                        stage1.getComponent(vRow) > 0 ? 0 : FastMath.PI,
                        0.0
                    };
                }
            }
        }
    }

    /** Get the Cardan or Euler angles corresponding to the instance.
     * <p>
     * The algorithm used here works even when the rotation is exactly at the
     * the singularity of the rotation order and convention. In this case, one of
     * the angles in the singular pair is arbitrarily set to exactly 0 and the
     * second angle is computed. The angle set to 0 in the singular case is the
     * angle of the first rotation in the case of Cardan orders, and it is the angle
     * of the last rotation in the case of Euler orders. This implies that extracting
     * the angles of a rotation never fails (it used to trigger an exception in singular
     * cases up to Hipparchus 3.0).
     * </p>
     * @param <T> type of the field elements
     * @param rotation rotation from which angles should be extracted
     * @param convention convention to use for the semantics of the angle
     * @return an array of three angles, in the order specified by the set
     * @since 3.1
     */
    public <T extends CalculusFieldElement<T>> T[] getAngles(final FieldRotation<T> rotation, final RotationConvention convention) {

        // Cardan/Euler angles rotations matrices have the following form, taking
        // RotationOrder.XYZ (φ about X, θ about Y, ψ about Z) and RotationConvention.FRAME_TRANSFORM
        // as an example:
        // [  cos θ cos ψ     cos φ sin ψ + sin φ sin θ cos ψ     sin φ sin ψ - cos φ sin θ cos ψ ]
        // [ -cos θ sin ψ     cos φ cos ψ - sin φ sin θ sin ψ     cos ψ sin φ + cos φ sin θ sin ψ ]
        // [  sin θ                       - sin φ cos θ                         cos φ cos θ       ]

        // One can see that there is a "simple" column (the first column in the example above) and a "simple"
        // row (the last row in the example above). In fact, the simple column always corresponds to the axis
        // of the first rotation in RotationConvention.FRAME_TRANSFORM convention (i.e. X axis here, hence
        // first column) and to the axis of the third rotation in RotationConvention.VECTOR_OPERATOR
        // convention. The "simple" row always corresponds to axis of the third rotation in
        // RotationConvention.FRAME_TRANSFORM convention (i.e. Z axis here, hence last row) and to the axis
        // of the first rotation in RotationConvention.VECTOR_OPERATOR convention.
        final FieldVector3D<T> vCol = getColumnVector(rotation, convention);
        final FieldVector3D<T> vRow = getRowVector(rotation, convention);

        // in the example above, we see that the components of the simple row
        // are [ sin θ, -sin φ cos θ, cos φ cos θ ], which means that if we select
        // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
        // φ as atan2(-Yrow, +Zrow), i.e. the coordinates along the axes of the
        // two final rotations in the Cardan sequence.

        // in the example above, we see that the components of the simple column
        // are [ cos θ cos ψ, -cos θ sin ψ, sin θ ], which means that if we select
        // θ strictly between -π/2 and +π/2 (i.e. cos θ > 0), then we can extract
        // ψ as atan2(-Ycol, +Xcol), i.e. the coordinates along the axes of the
        // two initial rotations in the Cardan sequence.

        // if we are exactly on the singularity (i.e. θ = ±π/2), then the matrix
        // degenerates to:
        // [  0    sin(ψ ± φ)    ∓ cos(ψ ± φ) ]
        // [  0    cos(ψ ± φ)    ± sin(ψ ± φ) ]
        // [ ±1       0              0        ]
        // at singularity, there is an infinite number of pairs (here ψ and φ) that represent
        // the same rotation, so we have to make some assumptions, arbitrarily setting one angle
        // of the pair and computing the other one from the non-singular middle column in the
        // FRAME_TRANSFORM rotation convention or from the non-singular middle row in the
        // VECTOR_OPERATOR rotation convention. In our XYZ rotation order and FRAME_TRANSFORM
        // rotation convention example, we can extract a consistent set of angles set by:
        //   - setting θ = ±π/2 copying the sign from ±1 term,
        //   - arbitrarily set φ = 0
        //   - compute ψ = atan2(XmidCol, YmidCol)

        // These results generalize to all sequences, with some adjustments for
        // Euler sequence where we need to replace one axis by the missing axis
        // and some sign adjustments depending on the rotation order being direct
        // (i.e. X before Y, Y before Z, Z before X) or indirect

        if (missing == null) {
            // this is a Cardan angles order
            if (convention == RotationConvention.FRAME_TRANSFORM) {
                final T s = stage2.getComponent(vRow).multiply(-sign);
                final T c = stage3.getComponent(vRow);
                if (s.square().add(c.square()).getReal() > 1.0e-30) {
                    // regular case
                    return buildArray(FastMath.atan2(s, c),
                                      safeAsin(stage1.getComponent(vRow), stage2.getComponent(vRow), stage3.getComponent(vRow)).multiply(sign),
                                      FastMath.atan2(stage2.getComponent(vCol).multiply(-sign), stage1.getComponent(vCol)));
                } else {
                    // near singularity
                    final FieldVector3D<T> midCol = rotation.applyTo(stage2.getAxis());
                    return buildArray(s.getField().getZero(),
                                      FastMath.copySign(s.getPi().multiply(0.5), stage1.getComponent(vRow).multiply(sign)),
                                      FastMath.atan2(stage1.getComponent(midCol).multiply(sign), stage2.getComponent(midCol)));
                }
            } else {
                final T s = stage2.getComponent(vCol).multiply(-sign);
                final T c = stage3.getComponent(vCol);
                if (s.square().add(c.square()).getReal() > 1.0e-30) {
                    // regular case
                    return buildArray(FastMath.atan2(stage2.getComponent(vCol).multiply(-sign), stage3.getComponent(vCol)),
                                      safeAsin(stage3.getComponent(vRow), stage1.getComponent(vRow), stage2.getComponent(vRow)).multiply(sign),
                                      FastMath.atan2(stage2.getComponent(vRow).multiply(-sign), stage1.getComponent(vRow)));
                } else {
                    // near singularity
                    final FieldVector3D<T> midRow = rotation.applyInverseTo(stage2.getAxis());
                    return buildArray(s.getField().getZero(),
                                      FastMath.copySign(s.getPi().multiply(0.5), stage3.getComponent(vRow).multiply(sign)),
                                      FastMath.atan2(stage1.getComponent(midRow).multiply(sign), stage2.getComponent(midRow)));
                }
            }
        } else {
            // this is an Euler angles order
            if (convention == RotationConvention.FRAME_TRANSFORM) {
                final T s = stage2.getComponent(vRow);
                final T c = missing.getComponent(vRow).multiply(-sign);
                if (s.square().add(c.square()).getReal() > 1.0e-30) {
                    // regular case
                    return buildArray(FastMath.atan2(s, c),
                                      safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
                                      FastMath.atan2(stage2.getComponent(vCol), missing.getComponent(vCol).multiply(sign)));
                } else {
                    // near singularity
                    final FieldVector3D<T> midCol = rotation.applyTo(stage2.getAxis());
                    return buildArray(FastMath.atan2(missing.getComponent(midCol).multiply(-sign), stage2.getComponent(midCol)).
                                      multiply(FastMath.copySign(1, stage1.getComponent(vRow).getReal())),
                                      stage1.getComponent(vRow).getReal() > 0 ? s.getField().getZero() : s.getPi(),
                                      s.getField().getZero());
                }
            } else {
                final T s = stage2.getComponent(vCol);
                final T c = missing.getComponent(vCol).multiply(-sign);
                if (s.square().add(c.square()).getReal() > 1.0e-30) {
                    // regular case
                    return buildArray(FastMath.atan2(s, c),
                                      safeAcos(stage1.getComponent(vRow), stage2.getComponent(vRow), missing.getComponent(vRow)),
                                      FastMath.atan2(stage2.getComponent(vRow), missing.getComponent(vRow).multiply(sign)));
                } else {
                    // near singularity
                    final FieldVector3D<T> midRow = rotation.applyInverseTo(stage2.getAxis());
                    return buildArray(FastMath.atan2(missing.getComponent(midRow).multiply(-sign), stage2.getComponent(midRow)).
                                      multiply(FastMath.copySign(1, stage1.getComponent(vRow).getReal())),
                                      stage1.getComponent(vRow).getReal() > 0 ? s.getField().getZero() : s.getPi(),
                                      s.getField().getZero());
                }
            }
        }
    }

    /** Get the simplest column vector from the rotation matrix.
     * @param rotation rotation
     * @param convention convention to use for the semantics of the angle
     * @return column vector
     * @since 3.1
     */
    private Vector3D getColumnVector(final Rotation rotation,
                                     final RotationConvention convention) {
        return rotation.applyTo(convention == RotationConvention.VECTOR_OPERATOR ?
                                stage3.getAxis() :
                                stage1.getAxis());
    }

    /** Get the simplest row vector from the rotation matrix.
     * @param rotation rotation
     * @param convention convention to use for the semantics of the angle
     * @return row vector
     * @since 3.1
     */
    private Vector3D getRowVector(final Rotation rotation,
                                  final RotationConvention convention) {
        return rotation.applyInverseTo(convention == RotationConvention.VECTOR_OPERATOR ?
                                       stage1.getAxis() :
                                       stage3.getAxis());
    }

    /** Get the simplest column vector from the rotation matrix.
     * @param <T> type of the field elements
     * @param rotation rotation
     * @param convention convention to use for the semantics of the angle
     * @return column vector
     * @since 3.1
     */
    private <T extends CalculusFieldElement<T>> FieldVector3D<T> getColumnVector(final FieldRotation<T> rotation,
                                                                                 final RotationConvention convention) {
        return rotation.applyTo(convention == RotationConvention.VECTOR_OPERATOR ?
                                stage3.getAxis() :
                                stage1.getAxis());
    }

    /** Get the simplest row vector from the rotation matrix.
     * @param <T> type of the field elements
     * @param rotation rotation
     * @param convention convention to use for the semantics of the angle
     * @return row vector
     * @since 3.1
     */
    private <T extends CalculusFieldElement<T>> FieldVector3D<T> getRowVector(final FieldRotation<T> rotation,
                                                                              final RotationConvention convention) {
        return rotation.applyInverseTo(convention == RotationConvention.VECTOR_OPERATOR ?
                                       stage1.getAxis() :
                                       stage3.getAxis());
    }

    /** Safe computation of acos(some vector coordinate) working around singularities.
     * @param cos  cosine coordinate
     * @param sin1 one of the sine coordinates
     * @param sin2 other sine coordinate
     * @return acos of the coordinate
     * @since 3.1
     */
    private static double safeAcos(final double cos, final double sin1, final double sin2) {
        if (cos < -SAFE_SWITCH) {
            return FastMath.PI - FastMath.asin(FastMath.sqrt(sin1 * sin1 + sin2 * sin2));
        } else if (cos > SAFE_SWITCH) {
            return FastMath.asin(FastMath.sqrt(sin1 * sin1 + sin2 * sin2));
        } else {
            return FastMath.acos(cos);
        }
    }

    /** Safe computation of asin(some vector coordinate) working around singularities.
     * @param sin sine coordinate
     * @param cos1 one of the cosine coordinates
     * @param cos2 other cosine coordinate
     * @return asin of the coordinate
     * @since 3.1
     */
    private static double safeAsin(final double sin, final double cos1, final double cos2) {
        if (sin < -SAFE_SWITCH) {
            return -FastMath.acos(FastMath.sqrt(cos1 * cos1 + cos2 * cos2));
        } else if (sin > SAFE_SWITCH) {
            return FastMath.acos(FastMath.sqrt(cos1 * cos1 + cos2 * cos2));
        } else {
            return FastMath.asin(sin);
        }
    }

    /** Safe computation of acos(some vector coordinate) working around singularities.
     * @param <T> type of the field elements
     * @param cos  cosine coordinate
     * @param sin1 one of the sine coordinates
     * @param sin2 other sine coordinate
     * @return acos of the coordinate
     * @since 3.1
     */
    private static <T extends CalculusFieldElement<T>> T safeAcos(final T cos,
                                                                  final T sin1,
                                                                  final T sin2) {
        if (cos.getReal() < -SAFE_SWITCH) {
            return FastMath.asin(FastMath.sqrt(sin1.square().add(sin2.square()))).subtract(sin1.getPi()).negate();
        } else if (cos.getReal() > SAFE_SWITCH) {
            return FastMath.asin(FastMath.sqrt(sin1.square().add(sin2.square())));
        } else {
            return FastMath.acos(cos);
        }
    }

    /** Safe computation of asin(some vector coordinate) working around singularities.
     * @param <T> type of the field elements
     * @param sin sine coordinate
     * @param cos1 one of the cosine coordinates
     * @param cos2 other cosine coordinate
     * @return asin of the coordinate
     * @since 3.1
     */
    private static <T extends CalculusFieldElement<T>> T safeAsin(final T sin,
                                                                  final T cos1,
                                                                  final T cos2) {
        if (sin.getReal() < -SAFE_SWITCH) {
            return FastMath.acos(FastMath.sqrt(cos1.square().add(cos2.square()))).negate();
        } else if (sin.getReal() > SAFE_SWITCH) {
            return FastMath.acos(FastMath.sqrt(cos1.square().add(cos2.square())));
        } else {
            return FastMath.asin(sin);
        }
    }

    /** Create a dimension 3 array.
     * @param <T> type of the field elements
     * @param a0 first array element
     * @param a1 second array element
     * @param a2 third array element
     * @return new array
     * @since 3.1
     */
    private static <T extends CalculusFieldElement<T>> T[] buildArray(final T a0, final T a1, final T a2) {
        final T[] array = MathArrays.buildArray(a0.getField(), 3);
        array[0] = a0;
        array[1] = a1;
        array[2] = a2;
        return array;
    }

}