PrecessionFinder.java

/* Copyright 2023 Luc Maisonobe
 * Licensed to CS GROUP (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS 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
 *
 *   http://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.
 */
package org.orekit.files.ccsds.ndm.adm;

import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.SinCos;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;

/** Utility to extract precession from the motion of a spin axis.
 * <p>
 * The precession is used in {@link AttitudeType#SPIN_NUTATION} and
 * {@link AttitudeType#SPIN_NUTATION_MOMENTUM} attitude types. CCSDS
 * calls it nutation, but it is really precession.
 * </p>
 * @author Luc Maisonobe
 * @since 12.0
 */
class PrecessionFinder {

    /** Precession axis (axis of the cone described by spin). */
    private final Vector3D axis;

    /** Precession angle (fixed cone angle between precession axis and spin axis). */
    private final double precessionAngle;

    /** Angular velocity around the precession axis (rad/s). */
    private final double angularVelocity;

    /** Build from spin axis motion.
     * <p>
     * Note that the derivatives up to second order are really needed
     * in order to retrieve the precession motion.
     * </p>
     * @param spin spin axis, including value, first and second derivative
     */
    PrecessionFinder(final FieldVector3D<UnivariateDerivative2> spin) {

        // using a suitable coordinates frame, the spin axis can be considered to describe
        // a cone of half aperture angle 0 ≤ η ≤ π around k axis, at angular rate ω ≥ 0
        // with an initial position in the (+i,±k) half-plane. In this frame, the normalized
        // direction of spin s = spin/||spin|| and its first and second time derivatives
        // have coordinates:
        //           s(t):        (sin η cos ω(t-t₀), sin η sin ω(t-t₀), cos η)
        //           ds/dt(t):    (-ω sin η sin ω(t-t₀), ω sin η cos ω(t-t₀), 0)
        //           d²s/dt²(t):  (-ω² sin η cos ω(t-t₀), -ω² sin η sin ω(t-t₀), 0)
        // at initial time t = t₀ this leads to:
        //           s₀ = s(t₀):       (sin η, 0, cos η)
        //           s₁ = ds/dt(t₀):   (0, ω sin η, 0)
        //           s₂ = d²s/dt²(t₀): (-ω² sin η, 0, 0)
        // however, only the spin vector itself is provided, we don't initially know the unit
        // vectors (i, j, k) of this "suitable coordinates frame". We can however easily
        // build another frame (u, v, w) from the normalized spin vector s as follows:
        //           u = s₀, v = s₁/||s₁||, w = u ⨯ v
        // the coordinates of vectors u, v and w in the "suitable coordinates frame" are:
        //           u: ( sin η,   0,  cos η)
        //           v: (     0,   1,      0)
        //           w: (-cos η,   0,  sin η)
        // we can then deduce the following relations, which can be computed regardless
        // of frames used to express the various vectors:
        //           s₁ · v = ω  sin η  = ||s₁||
        //           s₂ · w = ω² sin η cos η
        // these relations can be solved for η and ω (we know that 0 ≤ η ≤ π and ω ≥ 0):
        //           η = atan2(||s₁||², s₂ · w)
        //           ω = ||s₁|| / sin  η
        // then the k axis, which is the precession axis, can be deduced as:
        //           k = cos η u + sin η w

        final UnivariateDerivative2 nS = spin.getNorm();
        if (nS.getValue() == 0) {
            // special case, no motion at all, set up arbitrary values
            axis            = Vector3D.PLUS_K;
            precessionAngle = 0.0;
            angularVelocity = 0.0;
        } else {

            // build the derivatives vectors
            final FieldVector3D<UnivariateDerivative2> s = spin.scalarMultiply(nS.reciprocal());
            final Vector3D s0 = new Vector3D(s.getX().getValue(),
                                             s.getY().getValue(),
                                             s.getZ().getValue());
            final Vector3D s1 = new Vector3D(s.getX().getFirstDerivative(),
                                             s.getY().getFirstDerivative(),
                                             s.getZ().getFirstDerivative());
            final Vector3D s2 = new Vector3D(s.getX().getSecondDerivative(),
                                             s.getY().getSecondDerivative(),
                                             s.getZ().getSecondDerivative());

            final double nV2 = s1.getNormSq();
            if (nV2 == 0.0) {
                // special case: we have a fixed spin vector
                axis            = s0;
                precessionAngle = 0.0;
                angularVelocity = 0.0;
            } else {

                // check second derivatives were provided ; we do it on spin rather than s2
                // and use test against exact 0.0 because the normalization process changes
                // the derivatives and what we really want to check are missing derivatives
                if (new Vector3D(spin.getX().getSecondDerivative(),
                                 spin.getY().getSecondDerivative(),
                                 spin.getZ().getSecondDerivative()).getNormSq() == 0) {
                    throw new OrekitException(OrekitMessages.CANNOT_ESTIMATE_PRECESSION_WITHOUT_PROPER_DERIVATIVES);
                }

                // build the unit vectors
                final double   nV = FastMath.sqrt(nV2);
                final Vector3D v  = s1.scalarMultiply(1.0 / nV);
                final Vector3D w  = Vector3D.crossProduct(s0, v);

                // compute precession model
                precessionAngle  = FastMath.atan2(nV2, Vector3D.dotProduct(s2, w));
                final SinCos sc  = FastMath.sinCos(precessionAngle);
                angularVelocity  = nV / sc.sin();
                axis             = new Vector3D(sc.cos(), s0, sc.sin(), w);

            }
        }

    }

    /** Get the precession axis.
     * @return precession axis
     */
    public Vector3D getAxis() {
        return axis;
    }

    /** Get the precession angle.
     * @return fixed cone angle between precession axis an spin axis (rad)
     */
    public double getPrecessionAngle() {
        return precessionAngle;
    }

    /** Get the angular velocity around precession axis.
     * @return angular velocity around precession axis (rad/s)
     */
    public double getAngularVelocity() {
        return angularVelocity;
    }

}