FieldKinematicTransform.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.CalculusFieldElement;
  20. import org.hipparchus.Field;
  21. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  22. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  23. import org.hipparchus.util.MathArrays;
  24. import org.orekit.time.AbsoluteDate;
  25. import org.orekit.time.FieldAbsoluteDate;
  26. import org.orekit.utils.FieldPVCoordinates;
  27. import org.orekit.utils.PVCoordinates;
  28. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  29. import org.orekit.utils.TimeStampedPVCoordinates;

  31. import java.util.Arrays;

  33. /**
  34.  * A transform that only includes translation and rotation as well as their respective rates.
  35.  * It is kinematic in the sense that it cannot transform an acceleration vector.
  36.  *
  37.  * @author Romain Serra
  38.  * @see FieldStaticTransform
  39.  * @see FieldTransform
  40.  * @see KinematicTransform
  41.  * @since 12.1
  42.  */
  43. public interface FieldKinematicTransform<T extends CalculusFieldElement<T>> extends FieldStaticTransform<T> {

  45.     /**
  46.      * Get the identity kinematic transform.
  47.      *
  48.      * @param <T> type of the elements
  49.      * @param field field used by default
  50.      * @return identity transform.
  51.      */
  52.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> getIdentity(final Field<T> field) {
  53.         return FieldTransform.getIdentity(field);
  54.     }

  56.     /** Compute a composite velocity.
  57.      * @param first first applied transform
  58.      * @param second second applied transform
  59.      * @param <T> the type of the field elements
  60.      * @return velocity part of the composite transform
  61.      */
  62.     static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeVelocity(final FieldKinematicTransform<T> first,
  63.                                                                                   final FieldKinematicTransform<T> second) {

  65.         final FieldVector3D<T> v1 = first.getVelocity();
  66.         final FieldRotation<T> r1 = first.getRotation();
  67.         final FieldVector3D<T> o1 = first.getRotationRate();
  68.         final FieldVector3D<T> p2 = second.getTranslation();
  69.         final FieldVector3D<T> v2 = second.getVelocity();

  71.         final FieldVector3D<T> crossP = FieldVector3D.crossProduct(o1, p2);

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

  76.     /** Compute a composite rotation rate.
  77.      * @param <T> type of the elements
  78.      * @param first first applied transform
  79.      * @param second second applied transform
  80.      * @return rotation rate part of the composite transform
  81.      */
  82.     static <T extends CalculusFieldElement<T>> FieldVector3D<T> compositeRotationRate(final FieldKinematicTransform<T> first,
  83.                                                                                       final FieldKinematicTransform<T> second) {

  85.         final FieldVector3D<T> o1 = first.getRotationRate();
  86.         final FieldRotation<T> r2 = second.getRotation();
  87.         final FieldVector3D<T> o2 = second.getRotationRate();

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

  92.     /** Transform {@link PVCoordinates}, without the acceleration vector.
  93.      * @param pv the position-velocity couple to transform.
  94.      * @return transformed position-velocity
  95.      */
  96.     default FieldPVCoordinates<T> transformOnlyPV(final FieldPVCoordinates<T> pv) {
  97.         final FieldVector3D<T> transformedP = transformPosition(pv.getPosition());
  98.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(getRotationRate(), transformedP);
  99.         final FieldVector3D<T> transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
  100.         return new FieldPVCoordinates<>(transformedP, transformedV);
  101.     }

  103.     /** Transform {@link TimeStampedPVCoordinates}, without the acceleration vector.
  104.      * <p>
  105.      * In order to allow the user more flexibility, this method does <em>not</em> check for
  106.      * consistency between the transform {@link #getDate() date} and the time-stamped
  107.      * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
  108.      * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
  109.      * the input argument, regardless of the instance {@link #getDate() date}.
  110.      * </p>
  111.      * @param pv the position-velocity couple to transform.
  112.      * @return transformed position-velocity
  113.      */
  114.     default TimeStampedFieldPVCoordinates<T> transformOnlyPV(final TimeStampedFieldPVCoordinates<T> pv) {
  115.         final FieldVector3D<T> transformedP = transformPosition(pv.getPosition());
  116.         final FieldVector3D<T> crossP       = FieldVector3D.crossProduct(getRotationRate(), transformedP);
  117.         final FieldVector3D<T> transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
  118.         return new TimeStampedFieldPVCoordinates<>(pv.getDate(), transformedP, transformedV,
  119.                 FieldVector3D.getZero(pv.getDate().getField()));
  120.     }

  122.     /** Compute the Jacobian of the {@link #transformOnlyPV(FieldPVCoordinates)} (FieldPVCoordinates)}
  123.      * method of the transform.
  124.      * <p>
  125.      * Element {@code jacobian[i][j]} is the derivative of Cartesian coordinate i
  126.      * of the transformed {@link FieldPVCoordinates} with respect to Cartesian coordinate j
  127.      * of the input {@link FieldPVCoordinates} in method {@link #transformOnlyPV(FieldPVCoordinates)}.
  128.      * </p>
  129.      * <p>
  130.      * This definition implies that if we define position-velocity coordinates
  131.      * <pre>
  132.      * PV₁ = transform.transformPVCoordinates(PV₀), then
  133.      * </pre>
  134.      * <p> their differentials dPV₁ and dPV₀ will obey the following relation
  135.      * where J is the matrix computed by this method:
  136.      * <pre>
  137.      * dPV₁ = J &times; dPV₀
  138.      * </pre>
  139.      *
  140.      * @return Jacobian matrix
  141.      */
  142.     default T[][] getPVJacobian() {
  143.         final Field<T> field = getFieldDate().getField();
  144.         final T zero = field.getZero();
  145.         final T[][] jacobian = MathArrays.buildArray(field, 6, 6);

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

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

  155.         // dP1/dV0
  156.         Arrays.fill(jacobian[0], 3, 6, zero);
  157.         Arrays.fill(jacobian[1], 3, 6, zero);
  158.         Arrays.fill(jacobian[2], 3, 6, zero);

  160.         // dV1/dP0
  161.         final FieldVector3D<T> o = getRotationRate();
  162.         final T ox = o.getX();
  163.         final T oy = o.getY();
  164.         final T oz = o.getZ();
  165.         for (int i = 0; i < 3; ++i) {
  166.             jacobian[3][i] = oz.multiply(mData[1][i]).subtract(oy.multiply(mData[2][i]));
  167.             jacobian[4][i] = ox.multiply(mData[2][i]).subtract(oz.multiply(mData[0][i]));
  168.             jacobian[5][i] = oy.multiply(mData[0][i]).subtract(ox.multiply(mData[1][i]));
  169.         }

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

  176.         return jacobian;
  177.     }

  179.     /** Get the first time derivative of the translation.
  180.      * @return first time derivative of the translation
  181.      * @see #getTranslation()
  182.      */
  183.     FieldVector3D<T> getVelocity();

  185.     /** Get the first time derivative of the rotation.
  186.      * <p>The norm represents the angular rate.</p>
  187.      * @return First time derivative of the rotation
  188.      * @see #getRotation()
  189.      */
  190.     FieldVector3D<T> getRotationRate();

  192.     /**
  193.      * Get the inverse transform of the instance.
  194.      *
  195.      * @return inverse transform of the instance
  196.      */
  197.     FieldKinematicTransform<T> getInverse();

  199.     /**
  200.      * Build a transform by combining two existing ones.
  201.      * <p>
  202.      * Note that the dates of the two existing transformed are <em>ignored</em>,
  203.      * and the combined transform date is set to the date supplied in this
  204.      * constructor without any attempt to shift the raw transforms. This is a
  205.      * design choice allowing user full control of the combination.
  206.      * </p>
  207.      *
  208.      * @param <T> type of the elements
  209.      * @param date   date of the transform
  210.      * @param first  first transform applied
  211.      * @param second second transform applied
  212.      * @return the newly created kinematic transform that has the same effect as
  213.      * applying {@code first}, then {@code second}.
  214.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  215.      */
  216.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> compose(final FieldAbsoluteDate<T> date,
  217.                                                                                   final FieldKinematicTransform<T> first,
  218.                                                                                   final FieldKinematicTransform<T> second) {
  219.         final FieldVector3D<T> composedTranslation = FieldStaticTransform.compositeTranslation(first, second);
  220.         final FieldVector3D<T> composedTranslationRate = FieldKinematicTransform.compositeVelocity(first, second);
  221.         return of(date, new FieldPVCoordinates<>(composedTranslation, composedTranslationRate),
  222.                 FieldStaticTransform.compositeRotation(first, second),
  223.                 FieldKinematicTransform.compositeRotationRate(first, second));
  224.     }

  226.     /**
  227.      * Create a new kinematic transform from a rotation and zero, constant translation.
  228.      *
  229.      * @param <T> type of the elements
  230.      * @param date     of translation.
  231.      * @param rotation to apply after the translation. That is after translating
  232.      *                 applying this rotation produces positions expressed in
  233.      *                 the new frame.
  234.      * @param rotationRate rate of rotation
  235.      * @return the newly created kinematic transform.
  236.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  237.      */
  238.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
  239.                                                                              final FieldRotation<T> rotation,
  240.                                                                              final FieldVector3D<T> rotationRate) {
  241.         return of(date, FieldPVCoordinates.getZero(date.getField()), rotation, rotationRate);
  242.     }

  244.     /**
  245.      * Create a new kinematic transform from a translation and its rate.
  246.      *
  247.      * @param <T> type of the elements
  248.      * @param date        of translation.
  249.      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
  250.      *                    opposite of the coordinates of the new origin in the
  251.      *                    old frame.
  252.      * @return the newly created kinematic transform.
  253.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  254.      */
  255.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
  256.                                                                              final FieldPVCoordinates<T> pvCoordinates) {
  257.         final Field<T> field = date.getField();
  258.         return of(date, pvCoordinates, FieldRotation.getIdentity(field), FieldVector3D.getZero(field));
  259.     }

  261.     /**
  262.      * Create a new kinematic transform from a non-Field version.
  263.      *
  264.      * @param <T> type of the elements
  265.      * @param field field.
  266.      * @param kinematicTransform non-Field kinematic transform
  267.      * @return the newly created kinematic transform.
  268.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  269.      */
  270.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final Field<T> field,
  271.                                                                              final KinematicTransform kinematicTransform) {
  272.         final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, kinematicTransform.getDate());
  273.         final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(field,
  274.             new PVCoordinates(kinematicTransform.getTranslation(), kinematicTransform.getVelocity()));
  275.         final FieldRotation<T> rotation = new FieldRotation<>(field, kinematicTransform.getRotation());
  276.         final FieldVector3D<T> rotationRate = new FieldVector3D<>(field, kinematicTransform.getRotationRate());
  277.         return of(date, pvCoordinates, rotation, rotationRate);
  278.     }

  280.     /**
  281.      * Create a new kinematic transform from a translation and rotation.
  282.      *
  283.      * @param <T> type of the elements
  284.      * @param date        of translation.
  285.      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
  286.      *                    opposite of the coordinates of the new origin in the
  287.      *                    old frame.
  288.      * @param rotation    to apply after the translation. That is after
  289.      *                    translating applying this rotation produces positions
  290.      *                    expressed in the new frame.
  291.      * @param rotationRate rate of rotation
  292.      * @return the newly created kinematic transform.
  293.      * @see #compose(FieldAbsoluteDate, FieldKinematicTransform, FieldKinematicTransform)
  294.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  295.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  296.      */
  297.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
  298.                                                                           final FieldPVCoordinates<T> pvCoordinates,
  299.                                                                           final FieldRotation<T> rotation,
  300.                                                                           final FieldVector3D<T> rotationRate) {
  301.         return new FieldKinematicTransform<T>() {

  303.             @Override
  304.             public FieldKinematicTransform<T> getInverse() {
  305.                 final FieldRotation<T> r = getRotation();
  306.                 final FieldVector3D<T> rp = r.applyTo(getTranslation());
  307.                 final FieldVector3D<T> pInv = rp.negate();
  308.                 final FieldVector3D<T> crossP      = FieldVector3D.crossProduct(getRotationRate(), rp);
  309.                 final FieldVector3D<T> vInv        = crossP.subtract(getRotation().applyTo(getVelocity()));
  310.                 final FieldRotation<T> rInv = r.revert();
  311.                 return FieldKinematicTransform.of(date, new FieldPVCoordinates<>(pInv, vInv),
  312.                         rInv, rInv.applyTo(getRotationRate()).negate());
  313.             }

  315.             @Override
  316.             public AbsoluteDate getDate() {
  317.                 return date.toAbsoluteDate();
  318.             }

  320.             @Override
  321.             public FieldAbsoluteDate<T> getFieldDate() {
  322.                 return date;
  323.             }

  325.             @Override
  326.             public FieldVector3D<T> getTranslation() {
  327.                 return pvCoordinates.getPosition();
  328.             }

  330.             @Override
  331.             public FieldRotation<T> getRotation() {
  332.                 return rotation;
  333.             }

  335.             @Override
  336.             public FieldVector3D<T> getVelocity() {
  337.                 return pvCoordinates.getVelocity();
  338.             }

  340.             @Override
  341.             public FieldVector3D<T> getRotationRate() {
  342.                 return rotationRate;
  343.             }
  344.         };
  345.     }

  347. }
Created with JaCoCo 0.8.12.202403310830