FieldClohessyWiltshireMatrices.java

/* Copyright 2002-2026 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.
 */

package org.orekit.propagation.relative.clohessywiltshire;

import org.hipparchus.CalculusFieldElement;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.linear.FieldMatrix;
import org.hipparchus.linear.MatrixUtils;
import org.orekit.utils.FieldPVCoordinates;
import org.orekit.utils.TimeStampedFieldPVCoordinates;

/**
 * This class stores the 4 sub-matrices of the 6x6 state transition matrix of the Clohessy-Wiltshire equations.
 *
 * @param <T> Any scalar field.
 * @author Romain Cuvillon
 * @since 14.0
 */
public class FieldClohessyWiltshireMatrices<T extends CalculusFieldElement<T>> {

    /**
     * Time since epoch used in computing the matrices, in seconds.
     */
    private final T timeSinceEpoch;

    /**
     * Matrix that multiplies the position to compute the position.
     */
    private final FieldMatrix<T> phiRR;

    /**
     * Matrix that multiplies the velocity to compute the position.
     */
    private final FieldMatrix<T> phiRV;

    /**
     * Matrix that multiplies the position to compute the velocity.
     */
    private final FieldMatrix<T> phiVR;

    /**
     * Matrix that multiplies the velocity to compute the velocity.
     */
    private final FieldMatrix<T> phiVV;

    /**
     * Constructor of the 4 sub-matrices of the 6x6 state transition matrix of the Clohessy-Wiltshire equations.
     * <blockquote>
     * &delta;<b>r</b>(t) = &phi;<sub>rr</sub>(t) &delta;<b>r</b><sub>0</sub> + &phi;<sub>rv</sub>(t)
     * &delta;<b>v</b><sub>0</sub>
     * <br><br>
     * &delta;<b>v</b>(t) = &phi;<sub>vr</sub>(t) &delta;<b>r</b><sub>0</sub> + &phi;<sub>vv</sub>(t)
     * &delta;<b>v</b><sub>0</sub>
     * </blockquote>
     *
     * @param timeSinceEpoch duration in seconds since epoch
     * @param phiRR         position to position transition sub-matrix
     * @param phiRV         velocity to position transition sub-matrix
     * @param phiVR         position to velocity transition sub-matrix
     * @param phiVV         velocity to velocity transition sub-matrix
     */
    public FieldClohessyWiltshireMatrices(final T timeSinceEpoch, final FieldMatrix<T> phiRR,
                                          final FieldMatrix<T> phiRV, final FieldMatrix<T> phiVR,
                                          final FieldMatrix<T> phiVV) {
        this.timeSinceEpoch = timeSinceEpoch;
        this.phiRR          = phiRR;
        this.phiRV          = phiRV;
        this.phiVR          = phiVR;
        this.phiVV          = phiVV;
    }

    /**
     * Get the sub-matrix of the transition matrix &phi;<sub>rr</sub>(t).
     *
     * @return &phi;<sub>rr</sub>(t) sub-matrix
     */
    public FieldMatrix<T> getPhiRR() {
        return phiRR;
    }

    /**
     * Get the sub-matrix of the transition matrix &phi;<sub>rv</sub>(t).
     *
     * @return &phi;<sub>rv</sub>(t) sub-matrix
     */
    public FieldMatrix<T> getPhiRV() {
        return phiRV;
    }

    /**
     * Get the sub-matrix of the transition matrix &phi;<sub>vr</sub>(t).
     *
     * @return &phi;<sub>vr</sub>(t) sub-matrix
     */
    public FieldMatrix<T> getPhiVR() {
        return phiVR;
    }

    /**
     * Get the sub-matrix of the transition matrix &phi;<sub>vv</sub>(t).
     *
     * @return &phi;<sub>vv</sub>(t) sub-matrix
     */
    public FieldMatrix<T> getPhiVV() {
        return phiVV;
    }

    /**
     * Transforms the input initial PVT expressed in the target's QSW LOF to the PVT at the time encoded in the
     * Clohessy-Wiltshire matrices contained in the object.
     * <p>pos(t) = Φrr * pos(0) + Φrv * vel(0)</p>
     * <p>vel(t) = Φvr * pos(0) + Φvv * vel(0)</p>
     *
     * @param pvt input initial PVT expressed in the target's QSW LOF.
     * @return PVT at the time encoded in the Clohessy-Wiltshire matrices, expressed in the target's QSW LOF.
     */
    public TimeStampedFieldPVCoordinates<T> transform(final TimeStampedFieldPVCoordinates<T> pvt) {
        return new TimeStampedFieldPVCoordinates<>(pvt.getDate().shiftedBy(timeSinceEpoch),
                                                   transform(new FieldPVCoordinates<>(pvt.getPosition(),
                                                                                      pvt.getVelocity())));
    }

    /**
     * Transforms the input initial PV expressed in the target's QSW LOF to the PV at the time encoded in the
     * Clohessy-Wiltshire matrices contained in the object.
     * <p>pos(t) = Φrr * pos(0) + Φrv * vel(0)</p>
     * <p>vel(t) = Φvr * pos(0) + Φvv * vel(0)</p>
     *
     * @param pv input initial PV expressed in the target's QSW LOF.
     * @return PV at the time encoded in the Clohessy-Wiltshire matrices, expressed in the target's QSW LOF.
     */
    public FieldPVCoordinates<T> transform(final FieldPVCoordinates<T> pv) {
        // Convert Vector3D to RealMatrix
        final FieldMatrix<T> pos0 = MatrixUtils.createColumnFieldMatrix(pv.getPosition().toArray());
        final FieldMatrix<T> vel0 = MatrixUtils.createColumnFieldMatrix(pv.getVelocity().toArray());

        // Transform the input PV using the current matrices
        final FieldVector3D<T> pos1 =
                        new FieldVector3D<>(phiRR.multiply(pos0).add(phiRV.multiply(vel0)).getColumn(0));
        final FieldVector3D<T> vel1 =
                        new FieldVector3D<>(phiVR.multiply(pos0).add(phiVV.multiply(vel0)).getColumn(0));

        // Return transformed PV
        return new FieldPVCoordinates<>(pos1, vel1);
    }
}