KinematicTransform.java

/* Copyright 2022-2024 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.frames;

import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;

import java.util.Arrays;

/**
 * A transform that only includes translation and rotation as well as their respective rates.
 * It is kinematic in the sense that it cannot transform an acceleration vector.
 *
 * @author Romain Serra
 * @see StaticTransform
 * @see Transform
 * @since 12.1
 */
public interface KinematicTransform extends StaticTransform {

    /**
     * Get the identity kinematic transform.
     *
     * @return identity transform.
     */
    static KinematicTransform getIdentity() {
        return Transform.IDENTITY;
    }

    /** Compute a composite velocity.
     * @param first first applied transform
     * @param second second applied transform
     * @return velocity part of the composite transform
     */
    static Vector3D compositeVelocity(final KinematicTransform first, final KinematicTransform second) {

        final Vector3D v1 = first.getVelocity();
        final Rotation r1 = first.getRotation();
        final Vector3D o1 = first.getRotationRate();
        final Vector3D p2 = second.getTranslation();
        final Vector3D v2 = second.getVelocity();

        final Vector3D crossP = Vector3D.crossProduct(o1, p2);

        return v1.add(r1.applyInverseTo(v2.add(crossP)));
    }

    /** Compute a composite rotation rate.
     * @param first first applied transform
     * @param second second applied transform
     * @return rotation rate part of the composite transform
     */
    static Vector3D compositeRotationRate(final KinematicTransform first, final KinematicTransform second) {

        final Vector3D o1 = first.getRotationRate();
        final Rotation r2 = second.getRotation();
        final Vector3D o2 = second.getRotationRate();

        return o2.add(r2.applyTo(o1));
    }

    /** Transform {@link PVCoordinates}, without the acceleration vector.
     * @param pv the position-velocity couple to transform.
     * @return transformed position-velocity
     */
    default PVCoordinates transformOnlyPV(final PVCoordinates pv) {
        final Vector3D transformedP = transformPosition(pv.getPosition());
        final Vector3D crossP       = Vector3D.crossProduct(getRotationRate(), transformedP);
        final Vector3D transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
        return new PVCoordinates(transformedP, transformedV);
    }

    /** Transform {@link TimeStampedPVCoordinates}, without the acceleration vector.
     * <p>
     * In order to allow the user more flexibility, this method does <em>not</em> check for
     * consistency between the transform {@link #getDate() date} and the time-stamped
     * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
     * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
     * the input argument, regardless of the instance {@link #getDate() date}.
     * </p>
     * @param pv the position-velocity couple to transform.
     * @return transformed position-velocity
     */
    default TimeStampedPVCoordinates transformOnlyPV(final TimeStampedPVCoordinates pv) {
        final Vector3D transformedP = transformPosition(pv.getPosition());
        final Vector3D crossP       = Vector3D.crossProduct(getRotationRate(), transformedP);
        final Vector3D transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
        return new TimeStampedPVCoordinates(pv.getDate(), transformedP, transformedV);
    }

    /** Compute the Jacobian of the {@link #transformOnlyPV(PVCoordinates)} (PVCoordinates)}
     * method of the transform.
     * <p>
     * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
     * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j
     * of the input {@link PVCoordinates} in method {@link #transformOnlyPV(PVCoordinates)}.
     * </p>
     * <p>
     * This definition implies that if we define position-velocity coordinates
     * <pre>
     * PV₁ = transform.transformPVCoordinates(PV₀), then
     * </pre>
     * <p> their differentials dPV₁ and dPV₀ will obey the following relation
     * where J is the matrix computed by this method:
     * <pre>
     * dPV₁ = J &times; dPV₀
     * </pre>
     *
     * @return Jacobian matrix
     */
    default double[][] getPVJacobian() {
        final double[][] jacobian = new double[6][6];

        // elementary matrix for rotation
        final double[][] mData = getRotation().getMatrix();

        // dP1/dP0
        System.arraycopy(mData[0], 0, jacobian[0], 0, 3);
        System.arraycopy(mData[1], 0, jacobian[1], 0, 3);
        System.arraycopy(mData[2], 0, jacobian[2], 0, 3);

        // dP1/dV0
        Arrays.fill(jacobian[0], 3, 6, 0.0);
        Arrays.fill(jacobian[1], 3, 6, 0.0);
        Arrays.fill(jacobian[2], 3, 6, 0.0);

        // dV1/dP0
        final Vector3D o = getRotationRate();
        final double ox = o.getX();
        final double oy = o.getY();
        final double oz = o.getZ();
        for (int i = 0; i < 3; ++i) {
            jacobian[3][i] = -(oy * mData[2][i] - oz * mData[1][i]);
            jacobian[4][i] = -(oz * mData[0][i] - ox * mData[2][i]);
            jacobian[5][i] = -(ox * mData[1][i] - oy * mData[0][i]);
        }

        // dV1/dV0
        System.arraycopy(mData[0], 0, jacobian[3], 3, 3);
        System.arraycopy(mData[1], 0, jacobian[4], 3, 3);
        System.arraycopy(mData[2], 0, jacobian[5], 3, 3);

        return jacobian;
    }

    /** Get the first time derivative of the translation.
     * @return first time derivative of the translation
     * @see #getTranslation()
     */
    Vector3D getVelocity();

    /** Get the first time derivative of the rotation.
     * <p>The norm represents the angular rate.</p>
     * @return First time derivative of the rotation
     * @see #getRotation()
     */
    Vector3D getRotationRate();

    /**
     * Get the inverse transform of the instance.
     *
     * @return inverse transform of the instance
     */
    KinematicTransform getInverse();

    /**
     * Build a transform by combining two existing ones.
     * <p>
     * Note that the dates of the two existing transformed are <em>ignored</em>,
     * and the combined transform date is set to the date supplied in this
     * constructor without any attempt to shift the raw transforms. This is a
     * design choice allowing user full control of the combination.
     * </p>
     *
     * @param date   date of the transform
     * @param first  first transform applied
     * @param second second transform applied
     * @return the newly created kinematic transform that has the same effect as
     * applying {@code first}, then {@code second}.
     * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
     */
    static KinematicTransform compose(final AbsoluteDate date,
                                      final KinematicTransform first,
                                      final KinematicTransform second) {
        final Vector3D composedTranslation = StaticTransform.compositeTranslation(first, second);
        final Vector3D composedTranslationRate = KinematicTransform.compositeVelocity(first, second);
        return of(date, new PVCoordinates(composedTranslation, composedTranslationRate),
                StaticTransform.compositeRotation(first, second),
                KinematicTransform.compositeRotationRate(first, second));
    }

    /**
     * Create a new kinematic transform from a rotation and zero, constant translation.
     *
     * @param date     of translation.
     * @param rotation to apply after the translation. That is after translating
     *                 applying this rotation produces positions expressed in
     *                 the new frame.
     * @param rotationRate rate of rotation
     * @return the newly created kinematic transform.
     * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
     */
    static KinematicTransform of(final AbsoluteDate date,
                                 final Rotation rotation,
                                 final Vector3D rotationRate) {
        return of(date, PVCoordinates.ZERO, rotation, rotationRate);
    }

    /**
     * Create a new kinematic transform from a translation and its rate.
     *
     * @param date        of translation.
     * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
     *                    opposite of the coordinates of the new origin in the
     *                    old frame.
     * @return the newly created kinematic transform.
     * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
     */
    static KinematicTransform of(final AbsoluteDate date,
                                 final PVCoordinates pvCoordinates) {
        return of(date, pvCoordinates, Rotation.IDENTITY, Vector3D.ZERO);
    }

    /**
     * Create a new kinematic transform from a translation and rotation.
     *
     * @param date        of translation.
     * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
     *                    opposite of the coordinates of the new origin in the
     *                    old frame.
     * @param rotation    to apply after the translation. That is after
     *                    translating applying this rotation produces positions
     *                    expressed in the new frame.
     * @param rotationRate rate of rotation
     * @return the newly created kinematic transform.
     * @see #compose(AbsoluteDate, KinematicTransform, KinematicTransform)
     * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
     * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
     */
    static KinematicTransform of(final AbsoluteDate date, final PVCoordinates pvCoordinates,
                                 final Rotation rotation, final Vector3D rotationRate) {
        return new KinematicTransform() {

            @Override
            public KinematicTransform getInverse() {
                final Rotation r = getRotation();
                final Vector3D rp = r.applyTo(getTranslation());
                final Vector3D pInv = rp.negate();
                final Vector3D crossP      = Vector3D.crossProduct(getRotationRate(), rp);
                final Vector3D vInv        = crossP.subtract(getRotation().applyTo(getVelocity()));
                final Rotation rInv = r.revert();
                return KinematicTransform.of(getDate(), new PVCoordinates(pInv, vInv),
                        rInv, rInv.applyTo(getRotationRate()).negate());
            }

            @Override
            public AbsoluteDate getDate() {
                return date;
            }

            @Override
            public Vector3D getTranslation() {
                return pvCoordinates.getPosition();
            }

            @Override
            public Rotation getRotation() {
                return rotation;
            }

            @Override
            public Vector3D getVelocity() {
                return pvCoordinates.getVelocity();
            }

            @Override
            public Vector3D getRotationRate() {
                return rotationRate;
            }
        };
    }

}