NumericalTimeDerivativesEquations.java

/* Copyright 2022-2025 Romain Serra
 * 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.propagation.numerical;

import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.errors.OrekitMessages;
import org.orekit.forces.ForceModel;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.SpacecraftState;

import java.util.Arrays;
import java.util.List;

/**
 * First-order differential model representing the equations of motion.
 *
 * @author Luc Maisonobe
 * @author Romain Serra
 * @since 13.1
 * @see NumericalPropagator
 */
class NumericalTimeDerivativesEquations implements TimeDerivativesEquations {

    /** Derivatives array. */
    private final double[] yDot;

    /** Orbit type of coordinates. */
    private final OrbitType orbitType;

    /** Position angle type of coordinates. */
    private final PositionAngleType positionAngleType;

    /** Forces list. */
    private final List<ForceModel> forceModels;

    /** Jacobian of the orbital parameters with respect to the Cartesian parameters. */
    private double[][] coordinatesJacobian;

    /** Current state. */
    private SpacecraftState currentState;

    /**
     * Constructor.
     * @param orbitType orbit type used in equations
     * @param positionAngleType angle type used in equations if applicable
     * @param forceModels forces
     */
    NumericalTimeDerivativesEquations(final OrbitType orbitType, final PositionAngleType positionAngleType,
                                      final List<ForceModel> forceModels) {
        this.orbitType = orbitType;
        this.positionAngleType = positionAngleType;
        this.forceModels = forceModels;
        this.yDot     = new double[7];
        this.coordinatesJacobian = new double[6][6];
        // default value for Jacobian is identity
        for (int i = 0; i < coordinatesJacobian.length; ++i) {
            Arrays.fill(coordinatesJacobian[i], 0.0);
            coordinatesJacobian[i][i] = 1.0;
        }
    }

    /**
     * Getter for the force models.
     * @return forces
     */
    List<ForceModel> getForceModels() {
        return forceModels;
    }

    /**
     * Setter for the coordinates' Jacobian matrix.
     * @param coordinatesJacobian matrix
     */
    void setCoordinatesJacobian(final double[][] coordinatesJacobian) {
        this.coordinatesJacobian = coordinatesJacobian;
    }

    /**
     * Setter for the current state.
     * @param currentState state
     */
    void setCurrentState(final SpacecraftState currentState) {
        this.currentState = currentState;
    }

    /**
     * Compute first-order, time derivatives a.k.a. rates.
     * @param state state where to evaluate derivatives
     * @return derivatives
     */
    double[] computeTimeDerivatives(final SpacecraftState state) {
        currentState = state;
        Arrays.fill(yDot, 0.0);

        // compute the contributions of all perturbing forces,
        // using the Kepler contribution at the end since
        // NewtonianAttraction is always the last instance in the list
        for (final ForceModel forceModel : forceModels) {
            forceModel.addContribution(state, this);
        }

        if (orbitType == null) {
            // position derivative is velocity, and was not added above in the force models
            // (it is added when orbit type is non-null because NewtonianAttraction considers it)
            final Vector3D velocity = state.getPVCoordinates().getVelocity();
            yDot[0] += velocity.getX();
            yDot[1] += velocity.getY();
            yDot[2] += velocity.getZ();
        }

        return yDot.clone();
    }

    /** {@inheritDoc} */
    @Override
    public void addKeplerContribution(final double mu) {
        if (orbitType == null) {
            // if mu is neither 0 nor NaN, we want to include Newtonian acceleration
            if (mu > 0) {
                // velocity derivative is Newtonian acceleration
                final Vector3D position = currentState.getPosition();
                final double r2         = position.getNorm2Sq();
                final double coeff      = -mu / (r2 * FastMath.sqrt(r2));
                yDot[3] += coeff * position.getX();
                yDot[4] += coeff * position.getY();
                yDot[5] += coeff * position.getZ();
            }

        } else {
            // propagation uses regular orbits
            orbitType.convertType(currentState.getOrbit()).addKeplerContribution(positionAngleType, mu, yDot);
        }
    }

    /** {@inheritDoc} */
    @Override
    public void addNonKeplerianAcceleration(final Vector3D gamma) {
        for (int i = 0; i < 6; ++i) {
            final double[] jRow = coordinatesJacobian[i];
            yDot[i] += jRow[3] * gamma.getX() + jRow[4] * gamma.getY() + jRow[5] * gamma.getZ();
        }
    }

    /** {@inheritDoc} */
    @Override
    public void addMassDerivative(final double q) {
        if (q > 0) {
            throw new OrekitIllegalArgumentException(OrekitMessages.POSITIVE_FLOW_RATE, q);
        }
        yDot[6] += q;
    }
}