KinematicTransform.java

  1. /* Copyright 2022-2025 Romain Serra
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.frames;

  19. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  20. import org.hipparchus.geometry.euclidean.threed.Rotation;
  21. import org.orekit.time.AbsoluteDate;
  22. import org.orekit.utils.PVCoordinates;
  23. import org.orekit.utils.TimeStampedPVCoordinates;

  25. import java.util.Arrays;

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

  38.     /**
  39.      * Get the identity kinematic transform.
  40.      *
  41.      * @return identity transform.
  42.      */
  43.     static KinematicTransform getIdentity() {
  44.         return Transform.IDENTITY;
  45.     }

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

  54.         final Vector3D v1 = first.getVelocity();
  55.         final Rotation r1 = first.getRotation();
  56.         final Vector3D o1 = first.getRotationRate();
  57.         final Vector3D p2 = second.getTranslation();
  58.         final Vector3D v2 = second.getVelocity();

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

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

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

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

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

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

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

  108.     /** Compute the Jacobian of the {@link #transformOnlyPV(PVCoordinates)} (PVCoordinates)}
  109.      * method of the transform.
  110.      * <p>
  111.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  112.      * of the transformed {@link PVCoordinates} with respect to Cartesian coordinate j
  113.      * of the input {@link PVCoordinates} in method {@link #transformOnlyPV(PVCoordinates)}.
  114.      * </p>
  115.      * <p>
  116.      * This definition implies that if we define position-velocity coordinates
  117.      * <pre>
  118.      * PV₁ = transform.transformPVCoordinates(PV₀), then
  119.      * </pre>
  120.      * <p> their differentials dPV₁ and dPV₀ will obey the following relation
  121.      * where J is the matrix computed by this method:
  122.      * <pre>
  123.      * dPV₁ = J &times; dPV₀
  124.      * </pre>
  125.      *
  126.      * @return Jacobian matrix
  127.      */
  128.     default double[][] getPVJacobian() {
  129.         final double[][] jacobian = new double[6][6];

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

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

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

  144.         // dV1/dP0
  145.         final Vector3D o = getRotationRate();
  146.         final double ox = o.getX();
  147.         final double oy = o.getY();
  148.         final double oz = o.getZ();
  149.         for (int i = 0; i < 3; ++i) {
  150.             jacobian[3][i] = -(oy * mData[2][i] - oz * mData[1][i]);
  151.             jacobian[4][i] = -(oz * mData[0][i] - ox * mData[2][i]);
  152.             jacobian[5][i] = -(ox * mData[1][i] - oy * mData[0][i]);
  153.         }

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

  160.         return jacobian;
  161.     }

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

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

  176.     /**
  177.      * Get the inverse transform of the instance.
  178.      *
  179.      * @return inverse transform of the instance
  180.      */
  181.     KinematicTransform getInverse();

  183.     /**
  184.      * Build a transform by combining two existing ones.
  185.      * <p>
  186.      * Note that the dates of the two existing transformed are <em>ignored</em>,
  187.      * and the combined transform date is set to the date supplied in this
  188.      * constructor without any attempt to shift the raw transforms. This is a
  189.      * design choice allowing user full control of the combination.
  190.      * </p>
  191.      *
  192.      * @param date   date of the transform
  193.      * @param first  first transform applied
  194.      * @param second second transform applied
  195.      * @return the newly created kinematic transform that has the same effect as
  196.      * applying {@code first}, then {@code second}.
  197.      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
  198.      */
  199.     static KinematicTransform compose(final AbsoluteDate date,
  200.                                       final KinematicTransform first,
  201.                                       final KinematicTransform second) {
  202.         final Vector3D composedTranslation = StaticTransform.compositeTranslation(first, second);
  203.         final Vector3D composedTranslationRate = KinematicTransform.compositeVelocity(first, second);
  204.         return of(date, new PVCoordinates(composedTranslation, composedTranslationRate),
  205.                 StaticTransform.compositeRotation(first, second),
  206.                 KinematicTransform.compositeRotationRate(first, second));
  207.     }

  209.     /**
  210.      * Create a new kinematic transform from a rotation and zero, constant translation.
  211.      *
  212.      * @param date     of translation.
  213.      * @param rotation to apply after the translation. That is after translating
  214.      *                 applying this rotation produces positions expressed in
  215.      *                 the new frame.
  216.      * @param rotationRate rate of rotation
  217.      * @return the newly created kinematic transform.
  218.      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
  219.      */
  220.     static KinematicTransform of(final AbsoluteDate date,
  221.                                  final Rotation rotation,
  222.                                  final Vector3D rotationRate) {
  223.         return of(date, PVCoordinates.ZERO, rotation, rotationRate);
  224.     }

  226.     /**
  227.      * Create a new kinematic transform from a translation and its rate.
  228.      *
  229.      * @param date        of translation.
  230.      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
  231.      *                    opposite of the coordinates of the new origin in the
  232.      *                    old frame.
  233.      * @return the newly created kinematic transform.
  234.      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
  235.      */
  236.     static KinematicTransform of(final AbsoluteDate date,
  237.                                  final PVCoordinates pvCoordinates) {
  238.         return of(date, pvCoordinates, Rotation.IDENTITY, Vector3D.ZERO);
  239.     }

  241.     /**
  242.      * Create a new kinematic transform from a translation and rotation.
  243.      *
  244.      * @param date        of translation.
  245.      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
  246.      *                    opposite of the coordinates of the new origin in the
  247.      *                    old frame.
  248.      * @param rotation    to apply after the translation. That is after
  249.      *                    translating applying this rotation produces positions
  250.      *                    expressed in the new frame.
  251.      * @param rotationRate rate of rotation
  252.      * @return the newly created kinematic transform.
  253.      * @see #compose(AbsoluteDate, KinematicTransform, KinematicTransform)
  254.      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
  255.      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
  256.      */
  257.     static KinematicTransform of(final AbsoluteDate date, final PVCoordinates pvCoordinates,
  258.                                  final Rotation rotation, final Vector3D rotationRate) {
  259.         return new KinematicTransform() {

  261.             @Override
  262.             public KinematicTransform getInverse() {
  263.                 final Rotation r = getRotation();
  264.                 final Vector3D rp = r.applyTo(getTranslation());
  265.                 final Vector3D pInv = rp.negate();
  266.                 final Vector3D crossP      = Vector3D.crossProduct(getRotationRate(), rp);
  267.                 final Vector3D vInv        = crossP.subtract(getRotation().applyTo(getVelocity()));
  268.                 final Rotation rInv = r.revert();
  269.                 return KinematicTransform.of(getDate(), new PVCoordinates(pInv, vInv),
  270.                         rInv, rInv.applyTo(getRotationRate()).negate());
  271.             }

  273.             @Override
  274.             public AbsoluteDate getDate() {
  275.                 return date;
  276.             }

  278.             @Override
  279.             public Vector3D getTranslation() {
  280.                 return pvCoordinates.getPosition();
  281.             }

  283.             @Override
  284.             public Rotation getRotation() {
  285.                 return rotation;
  286.             }

  288.             @Override
  289.             public Vector3D getVelocity() {
  290.                 return pvCoordinates.getVelocity();
  291.             }

  293.             @Override
  294.             public Vector3D getRotationRate() {
  295.                 return rotationRate;
  296.             }
  297.         };
  298.     }

  300. }
Created with JaCoCo 0.8.12.202403310830