Examples with FieldRotation org.hipparchus.geometry.euclidean.threed.FieldRotation used on opensource projects

Example 6 with FieldRotation

use of org.hipparchus.geometry.euclidean.threed.FieldRotation in project Orekit by CS-SI.

the class AngularCoordinates method toDerivativeStructureRotation.

/**
 * Transform the instance to a {@link FieldRotation}<{@link DerivativeStructure}>.
 * <p>
 * The {@link DerivativeStructure} coordinates correspond to time-derivatives up
 * to the user-specified order.
 * </p>
 * @param order derivation order for the vector components
 * @return rotation with time-derivatives embedded within the coordinates
 * @exception OrekitException if the user specified order is too large
 */
public FieldRotation<DerivativeStructure> toDerivativeStructureRotation(final int order) throws OrekitException {
    // quaternion components
    final double q0 = rotation.getQ0();
    final double q1 = rotation.getQ1();
    final double q2 = rotation.getQ2();
    final double q3 = rotation.getQ3();
    // first time-derivatives of the quaternion
    final double oX = rotationRate.getX();
    final double oY = rotationRate.getY();
    final double oZ = rotationRate.getZ();
    final double q0Dot = 0.5 * MathArrays.linearCombination(-q1, oX, -q2, oY, -q3, oZ);
    final double q1Dot = 0.5 * MathArrays.linearCombination(q0, oX, -q3, oY, q2, oZ);
    final double q2Dot = 0.5 * MathArrays.linearCombination(q3, oX, q0, oY, -q1, oZ);
    final double q3Dot = 0.5 * MathArrays.linearCombination(-q2, oX, q1, oY, q0, oZ);
    // second time-derivatives of the quaternion
    final double oXDot = rotationAcceleration.getX();
    final double oYDot = rotationAcceleration.getY();
    final double oZDot = rotationAcceleration.getZ();
    final double q0DotDot = -0.5 * MathArrays.linearCombination(new double[] { q1, q2, q3, q1Dot, q2Dot, q3Dot }, new double[] { oXDot, oYDot, oZDot, oX, oY, oZ });
    final double q1DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q2, -q3, q0Dot, q2Dot, -q3Dot }, new double[] { oXDot, oZDot, oYDot, oX, oZ, oY });
    final double q2DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q3, -q1, q0Dot, q3Dot, -q1Dot }, new double[] { oYDot, oXDot, oZDot, oY, oX, oZ });
    final double q3DotDot = 0.5 * MathArrays.linearCombination(new double[] { q0, q1, -q2, q0Dot, q1Dot, -q2Dot }, new double[] { oZDot, oYDot, oXDot, oZ, oY, oX });
    final DSFactory factory;
    final DerivativeStructure q0DS;
    final DerivativeStructure q1DS;
    final DerivativeStructure q2DS;
    final DerivativeStructure q3DS;
    switch(order) {
        case 0:
            factory = new DSFactory(1, order);
            q0DS = factory.build(q0);
            q1DS = factory.build(q1);
            q2DS = factory.build(q2);
            q3DS = factory.build(q3);
            break;
        case 1:
            factory = new DSFactory(1, order);
            q0DS = factory.build(q0, q0Dot);
            q1DS = factory.build(q1, q1Dot);
            q2DS = factory.build(q2, q2Dot);
            q3DS = factory.build(q3, q3Dot);
            break;
        case 2:
            factory = new DSFactory(1, order);
            q0DS = factory.build(q0, q0Dot, q0DotDot);
            q1DS = factory.build(q1, q1Dot, q1DotDot);
            q2DS = factory.build(q2, q2Dot, q2DotDot);
            q3DS = factory.build(q3, q3Dot, q3DotDot);
            break;
        default:
            throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, order);
    }
    return new FieldRotation<>(q0DS, q1DS, q2DS, q3DS, false);
}