Laas2015.java

/* Copyright 2002-2024 CS GROUP
 * 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.
 */

/*
 * MIT License
 *
 * Copyright (c) 2019 Romain Serra
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
package org.orekit.ssa.collision.shorttermencounter.probability.twod;

import org.hipparchus.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.linear.FieldVector;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.stat.StatUtils;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathArrays;
import org.hipparchus.util.MathUtils;
import org.orekit.ssa.metrics.FieldProbabilityOfCollision;
import org.orekit.ssa.metrics.ProbabilityOfCollision;

/**
 * Compute the probability of collision using the method described in : "SERRA, Romain, ARZELIER, Denis, JOLDES, Mioara, et
 * al. Fast and accurate computation of orbital collision probability for short-term encounters. Journal of Guidance,
 * Control, and Dynamics, 2016, vol. 39, no 5, p. 1009-1021.".
 * <p>
 * It is one of the recommended methods to use.
 * <p>
 * It assumes :
 *     <ul>
 *         <li>Short encounter leading to a linear relative motion.</li>
 *         <li>Spherical collision object.</li>
 *         <li>Uncorrelated positional covariance.</li>
 *         <li>Gaussian distribution of the position uncertainties.</li>
 *         <li>Deterministic velocity i.e. no velocity uncertainties.</li>
 *     </ul>
 * <p>
 * The following constants are defined when using the empty constructor :
 * <ul>
 *     <li>A default absolute accuracy of 1e-30.</li>
 *     <li>A maximum number of computed terms of 37000.</li>
 * </ul>
 * <p>
 * This implementation has been translated from python from the provided source code of Romain SERRA on the
 * <a href="https://github.com/Serrof/SST/blob/master/collision/short_term_poc.py">following github account</a>
 *
 * @author Vincent Cucchietti
 * @author Romain Serra
 * @since 12.0
 */
public class Laas2015 extends AbstractShortTermEncounter2DPOCMethod {

    /** Default scaling threshold to use when sum becomes large. */
    public static final double DEFAULT_SCALING_THRESHOLD = 1e10;

    /**
     * Defines the absolute accuracy of this method. For example, given an absolute accuracy of 1e-10, the probability of
     * collision will be exact until its 1e-10 digit.
     */
    private final double absoluteAccuracy;

    /** Defines the max number of terms that the method will use, thus reducing the computation time in particular cases. */
    private final int maxNumberOfTerms;

    /**
     * Default constructor.
     * <p>
     * It uses a default absolute accuracy of 1e-30 and a maximum number of terms of 37000 which is the max number of terms
     * computed based on Romain SERRA's observation (p.56 of "Romain Serra. Opérations de proximité en orbite : * évaluation
     * du risque de collision et calcul de manoeuvres optimales pour l’évitement et le rendez-vous. Automatique / *
     * Robotique. INSA de Toulouse, 2015. Français. NNT : 2015ISAT0035. tel-01261497") about Alfano test case 5 where he
     * explains that 37000 terms were enough to meet the required precision of 5 significant digits.
     */
    public Laas2015() {
        this(1.E-30, 37000);
    }

    /** Simple constructor.
     * @param absoluteAccuracy absolute accuracy of the result
     * @param maxNumberOfTerms max number of terms to compute
     */
    public Laas2015(final double absoluteAccuracy, final int maxNumberOfTerms) {
        super(ShortTermEncounter2DPOCMethodType.LAAS_2015.name());
        this.absoluteAccuracy = absoluteAccuracy;
        this.maxNumberOfTerms = maxNumberOfTerms;
    }

    /** {@inheritDoc} */
    public final ProbabilityOfCollision compute(final double xm, final double ym,
                                                final double sigmaX,
                                                final double sigmaY,
                                                final double radius) {

        // CHECKSTYLE: stop Indentation check
        // Initializing recurrent terms
        final double xmSquared     = xm * xm;
        final double ymSquared     = ym * ym;
        final double sigmaXSq      = sigmaX * sigmaX;
        final double sigmaYSq      = sigmaY * sigmaY;
        final double radiusSquared = radius * radius;

        final double p        = 1. / (2. * (sigmaX * sigmaX));
        final double phiY     = 1. - (sigmaX / sigmaY * sigmaX / sigmaY);
        final double omegaX   = (xm / (2. * sigmaXSq)) * (xm / (2. * sigmaXSq));
        final double omegaY   = (ym / (2. * sigmaYSq)) * (ym / (2. * sigmaYSq));
        final double bigOmega = phiY * 0.5 + (omegaX + omegaY) / p;

        final double alpha0 = 0.5 * FastMath.exp(-(xmSquared / sigmaXSq + ymSquared / sigmaYSq) * 0.5) / sigmaX / sigmaY;

        // Lower boundary
        final double l0 = alpha0 * (1. - FastMath.exp(-p * radiusSquared)) / p;

        // Upper boundary
        final double u0Temp = alpha0 * (FastMath.exp(p * bigOmega * radiusSquared) -
                FastMath.exp(-p * radiusSquared)) / (p * (1. + bigOmega));
        final double u0 = u0Temp > 1 ? 1 : u0Temp;

        // If the boundaries are close enough to the actual value according to defined relative accuracy
        if (u0 - l0 <= absoluteAccuracy) {
            return new ProbabilityOfCollision(StatUtils.mean(u0, l0), u0, l0, getName(),
                                              isAMaximumProbabilityOfCollisionMethod());
        }
        // Otherwise
        else {
            final int n1 = (int) (2. * FastMath.ceil(FastMath.E * p * radiusSquared * (1. + bigOmega)));
            final double n2_inter =
                    alpha0 * FastMath.exp(p * radiusSquared * bigOmega) /
                            (absoluteAccuracy * p * FastMath.sqrt(MathUtils.TWO_PI * n1) *
                                    (1. + bigOmega));
            final int n2 = (int) FastMath.ceil(FastMath.log(2, n2_inter));

            // Number of terms to get the relative accuracy desired
            int nMax = FastMath.max(n1, n2) - 1;
            nMax = FastMath.min(nMax, maxNumberOfTerms);

            // Initializing terms used in the equations
            final double pSquared                  = p * p;
            final double pCubed                    = pSquared * p;
            final double pTimesRadiusSquared       = p * radiusSquared;
            final double radiusFourth              = radiusSquared * radiusSquared;
            final double radiusSixth               = radiusFourth * radiusSquared;
            final double phiYSquared               = phiY * phiY;
            final double pPhiY                     = p * phiY;
            final double omegaSum                  = omegaX + omegaY;
            final double pSqTimesHalfPhiYSqPlusOne = pSquared * MathArrays.linearCombination(0.5, phiYSquared, 1., 1.);

            final double recurrentTerm0 = MathArrays.linearCombination(0.5, phiY, 1., 1.);
            final double recurrentTerm1 = MathArrays.linearCombination(p, recurrentTerm0, 1., omegaSum);
            final double recurrentTerm2 = MathArrays.linearCombination(1., pSqTimesHalfPhiYSqPlusOne, 2., pPhiY * omegaY);
            final double recurrentTerm3 = recurrentTerm1 * recurrentTerm1;

            final double auxiliaryTerm0 = radiusSixth * pCubed * phiYSquared * omegaX;
            final double auxiliaryTerm1 = radiusFourth * pSquared * phiY;
            final double auxiliaryTerm2 = 2. * omegaX * recurrentTerm0;

            final double auxiliaryTerm3 = phiY * MathArrays.linearCombination(2., omegaX, 1.5, p) + omegaSum;
            final double auxiliaryTerm4 = pPhiY * recurrentTerm0 * 2.;
            final double auxiliaryTerm5 = p * (2. * phiY + 1.);

            double kPlus2 = 2.;
            double kPlus3 = 3.;
            double kPlus4 = 4.;
            double kPlus5 = 5.;
            double halfY  = 2.5;

            // Initialize recurrence
            double c0 = alpha0 * radiusSquared;
            double c1 = c0 * radiusSquared * 0.5 * recurrentTerm1;
            double c2 = c0 * (radiusFourth / 12.) * (recurrentTerm3 + recurrentTerm2);
            double c3 = c0 * (radiusSixth / 144.) * (recurrentTerm1 * (recurrentTerm3 + 3. * recurrentTerm2) +
                    2. * (pCubed * (1. + phiYSquared * phiY * 0.5) + 3. * pSquared * phiYSquared * omegaY));
            final double[] initialCoefficients = new double[] { c0, c1, c2, c3 };

            double sum              = 0.;
            double rescalingCounter = 0.;
            for (int i = 0; i < FastMath.min(nMax, 4); i++) {
                sum += initialCoefficients[i];

                // Rescale quantities if necessary
                if (sum > DEFAULT_SCALING_THRESHOLD) {
                    rescalingCounter += FastMath.log10(DEFAULT_SCALING_THRESHOLD);
                    c0 /= DEFAULT_SCALING_THRESHOLD;
                    c1 /= DEFAULT_SCALING_THRESHOLD;
                    c2 /= DEFAULT_SCALING_THRESHOLD;
                    c3 /= DEFAULT_SCALING_THRESHOLD;
                    sum /= DEFAULT_SCALING_THRESHOLD;
                }

            }

            // Iterate
            double temp;
            for (int k = 0; k < nMax - 4; k++) {

                // Rescale quantities if necessary
                if (sum > DEFAULT_SCALING_THRESHOLD) {
                    rescalingCounter += FastMath.log10(DEFAULT_SCALING_THRESHOLD);
                    c0 /= DEFAULT_SCALING_THRESHOLD;
                    c1 /= DEFAULT_SCALING_THRESHOLD;
                    c2 /= DEFAULT_SCALING_THRESHOLD;
                    c3 /= DEFAULT_SCALING_THRESHOLD;
                    sum /= DEFAULT_SCALING_THRESHOLD;
                }

                // Recurrence relation
                final double denominator = kPlus4 * kPlus3;

                temp = c3 * MathArrays.linearCombination(1, recurrentTerm1, kPlus3, auxiliaryTerm5);
                temp -= c2 * pTimesRadiusSquared * MathArrays.linearCombination(halfY, auxiliaryTerm4, 1, auxiliaryTerm3) /
                        kPlus4;
                temp += c1 * auxiliaryTerm1 * MathArrays.linearCombination(halfY, pPhiY, 1., auxiliaryTerm2) / denominator;
                temp -= c0 * auxiliaryTerm0 / (denominator * kPlus2);
                temp *= radiusSquared / (kPlus4 * kPlus5);

                c0 = c1;
                c1 = c2;
                c2 = c3;
                c3 = temp;

                // Update intermediate variables
                kPlus2 = kPlus3;
                kPlus3 = kPlus4;
                kPlus4 = kPlus5;
                kPlus5 = kPlus5 + 1.;
                halfY += 1.;

                // Update sum
                sum += c3;

            }
            // CHECKSTYLE: resume Indentation check
            final double value = sum *
                    FastMath.exp(MathArrays.linearCombination(FastMath.log(10.), rescalingCounter, -p, radiusSquared));

            return new ProbabilityOfCollision(value, l0, u0, getName(), isAMaximumProbabilityOfCollisionMethod());
        }
    }

    /** {@inheritDoc} */
    public final <T extends CalculusFieldElement<T>> FieldProbabilityOfCollision<T> compute(final T xm, final T ym,
                                                                                            final T sigmaX,
                                                                                            final T sigmaY,
                                                                                            final T radius) {

        // CHECKSTYLE: stop Indentation check
        // Initializing recurrent terms
        final Field<T> field = xm.getField();
        final T        zero  = field.getZero();
        final T        one   = field.getOne();

        final T xmSquared     = xm.square();
        final T ymSquared     = ym.square();
        final T sigmaXSquared = sigmaX.square();
        final T sigmaYSquared = sigmaY.square();
        final T twoSigmaXY    = sigmaX.multiply(sigmaY).multiply(2);
        final T radiusSquared = radius.square();
        final T radiusFourth  = radiusSquared.square();
        final T radiusSixth   = radiusFourth.multiply(radiusSquared);

        final T p                   = sigmaX.square().reciprocal().multiply(0.5);
        final T pTimesRadiusSquared = p.multiply(radiusSquared);
        final T phiY                = sigmaXSquared.divide(sigmaYSquared).negate().add(1.);
        final T omegaX              = xm.divide(sigmaXSquared.multiply(2.)).pow(2.);
        final T omegaY              = ym.divide(sigmaYSquared.multiply(2.)).pow(2.);
        final T omegaSum            = omegaX.add(omegaY);
        final T bigOmega            = phiY.multiply(0.5).add(omegaX.add(omegaY).divide(p));

        final T minusP                    = p.negate();
        final T pSquared                  = p.square();
        final T pCubed                    = p.multiply(pSquared);
        final T pPhiY                     = p.multiply(phiY);
        final T phiYSquared               = phiY.square();
        final T pRadiusSquaredBigOmega    = p.multiply(radiusSquared).multiply(bigOmega);
        final T bigOmegaPlusOne           = bigOmega.add(1.);
        final T pSqTimesHalfPhiYSqPlusOne = pSquared.multiply(phiYSquared.multiply(0.5).add(1.));

        final T alpha0 = xmSquared.divide(sigmaXSquared).add(ymSquared.divide(sigmaYSquared)).multiply(-0.5).exp()
                                  .divide(twoSigmaXY);

        // Lower boundary
        final T l0 = alpha0.multiply(radiusSquared.multiply(minusP).exp().negate().add(1.)).divide(p);

        // Upper boundary
        final T u0Temp = alpha0.multiply(pRadiusSquaredBigOmega.exp().subtract(radiusSquared.multiply(minusP).exp()))
                               .divide(p.multiply(bigOmegaPlusOne));
        final T u0 = u0Temp.getReal() > 1 ? one : u0Temp;

        // If the boundaries are close enough to the actual value according to defined relative accuracy
        if (u0.getReal() - l0.getReal() <= absoluteAccuracy) {
            return new FieldProbabilityOfCollision<>(u0.add(l0).multiply(0.5), u0, l0, getName(),
                                                     isAMaximumProbabilityOfCollisionMethod());
        }
        // Otherwise
        else {
            final int n1 = (int) (2. *
                    FastMath.ceil(FastMath.E * p.multiply(radiusSquared).multiply(bigOmegaPlusOne).getReal()));
            final double n2_inter =
                    alpha0.getReal() * FastMath.exp(pRadiusSquaredBigOmega.getReal()) /
                            (absoluteAccuracy * p.getReal() * FastMath.sqrt(MathUtils.TWO_PI * n1) *
                                    (1. + bigOmega.getReal()));
            final int n2 = (int) FastMath.ceil(FastMath.log(2., n2_inter));

            // Number of terms to get the relative accuracy desired
            int nMax = FastMath.max(n1, n2) - 1;
            nMax = FastMath.min(nMax, maxNumberOfTerms);

            // Recurrent term in the equations
            final T recurrentTerm0 = phiY.multiply(0.5).add(1);
            final T recurrentTerm1 = p.multiply(recurrentTerm0).add(omegaSum);
            final T recurrentTerm2 = pSqTimesHalfPhiYSqPlusOne.add(pPhiY.multiply(omegaY).multiply(2.));
            final T recurrentTerm3 = recurrentTerm1.multiply(recurrentTerm1);

            final T auxiliaryTerm0 = radiusSixth.multiply(pCubed).multiply(phiYSquared).multiply(omegaX);
            final T auxiliaryTerm1 = radiusFourth.multiply(pSquared).multiply(phiY);
            final T auxiliaryTerm2 = omegaX.multiply(recurrentTerm0).multiply(2.);
            final T auxiliaryTerm3 = phiY.multiply(omegaX.multiply(2.).add(p.multiply(1.5))).add(omegaSum);
            final T auxiliaryTerm4 = pPhiY.multiply(recurrentTerm0).multiply(2.);
            final T auxiliaryTerm5 = p.multiply(phiY.multiply(2.).add(1.));

            T kPlus2 = one.newInstance(2.);
            T kPlus3 = one.newInstance(3.);
            T kPlus4 = one.newInstance(4.);
            T kPlus5 = one.newInstance(5.);
            T halfY  = one.newInstance(2.5);

            // Initialize recurrence
            T c0 = alpha0.multiply(radiusSquared);
            T c1 = c0.multiply(radiusSquared).multiply(0.5).multiply(recurrentTerm1);
            T c2 = c0.multiply(radiusFourth.divide(12.)).multiply(recurrentTerm3.add(recurrentTerm2));
            T c3 = c0.multiply(radiusSixth.divide(144.)).multiply(
                    recurrentTerm1.multiply(recurrentTerm2.multiply(3.).add(recurrentTerm3))
                                  .add(pCubed.multiply(2.).multiply(phiYSquared.multiply(phiY).multiply(0.5).add(1.)))
                                  .add(pSquared.multiply(phiYSquared).multiply(omegaY).multiply(6.)));
            final FieldVector<T> initialCoefficients = MatrixUtils.createFieldVector(field, 4);
            initialCoefficients.setEntry(0, c0);
            initialCoefficients.setEntry(1, c1);
            initialCoefficients.setEntry(2, c2);
            initialCoefficients.setEntry(3, c3);

            T sum              = zero;
            T rescalingCounter = zero;
            for (int i = 0; i < FastMath.min(nMax, 4); i++) {
                sum = sum.add(initialCoefficients.getEntry(i));

                // Rescale quantities if necessary
                if (sum.getReal() > DEFAULT_SCALING_THRESHOLD) {
                    rescalingCounter = rescalingCounter.add(FastMath.log10(DEFAULT_SCALING_THRESHOLD));
                    c0               = c0.divide(DEFAULT_SCALING_THRESHOLD);
                    c1               = c1.divide(DEFAULT_SCALING_THRESHOLD);
                    c2               = c2.divide(DEFAULT_SCALING_THRESHOLD);
                    c3               = c3.divide(DEFAULT_SCALING_THRESHOLD);
                    sum              = sum.divide(DEFAULT_SCALING_THRESHOLD);
                }

            }

            // Iterate
            T temp;
            for (int k = 0; k < nMax - 4; k++) {

                // Rescale quantities if necessary
                if (sum.getReal() > DEFAULT_SCALING_THRESHOLD) {
                    rescalingCounter = rescalingCounter.add(FastMath.log10(DEFAULT_SCALING_THRESHOLD));
                    c0               = c0.divide(DEFAULT_SCALING_THRESHOLD);
                    c1               = c1.divide(DEFAULT_SCALING_THRESHOLD);
                    c2               = c2.divide(DEFAULT_SCALING_THRESHOLD);
                    c3               = c3.divide(DEFAULT_SCALING_THRESHOLD);
                    sum              = sum.divide(DEFAULT_SCALING_THRESHOLD);
                }

                // Recurrence relation
                final T denominator = kPlus4.multiply(kPlus3);

                temp = c3.multiply(recurrentTerm1.add(auxiliaryTerm5.multiply(kPlus3)));
                temp = temp.subtract(
                        c2.multiply(pTimesRadiusSquared).multiply(auxiliaryTerm4.multiply(halfY).add(auxiliaryTerm3))
                          .divide(kPlus4));
                temp = temp.add(
                        c1.multiply(auxiliaryTerm1).multiply(pPhiY.multiply(halfY).add(auxiliaryTerm2)).divide(denominator));
                temp = temp.subtract(c0.multiply(auxiliaryTerm0).divide(denominator.multiply(kPlus2)));
                temp = temp.multiply(radiusSquared.divide(kPlus4.multiply(kPlus5)));

                c0 = c1;
                c1 = c2;
                c2 = c3;
                c3 = temp;

                // Update intermediate variables
                kPlus2 = kPlus3;
                kPlus3 = kPlus4;
                kPlus4 = kPlus5;
                kPlus5 = kPlus5.add(1.);
                halfY  = halfY.add(1.);

                // Update sum
                sum = sum.add(c3);

            }
            final T value =
                    sum.multiply((rescalingCounter.multiply(FastMath.log(10.)).subtract(pTimesRadiusSquared)).exp());

            return new FieldProbabilityOfCollision<>(value, l0, u0, getName(), isAMaximumProbabilityOfCollisionMethod());
            // CHECKSTYLE: resume Indentation check
        }
    }

    /** {@inheritDoc} */
    @Override
    public ShortTermEncounter2DPOCMethodType getType() {
        return ShortTermEncounter2DPOCMethodType.LAAS_2015;
    }

}