FieldKinematicTransform.java

  1. /* Copyright 2022-2024 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.orekit.time.AbsoluteDate;
  24. import org.orekit.time.FieldAbsoluteDate;
  25. import org.orekit.utils.FieldPVCoordinates;
  26. import org.orekit.utils.PVCoordinates;
  27. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  28. import org.orekit.utils.TimeStampedPVCoordinates;

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

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

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

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

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

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

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

  82.         final FieldVector3D<T> o1 = first.getRotationRate();
  83.         final FieldRotation<T> r2 = second.getRotation();
  84.         final FieldVector3D<T> o2 = second.getRotationRate();

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

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

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

  119.     /** Get the first time derivative of the translation.
  120.      * @return first time derivative of the translation
  121.      * @see #getTranslation()
  122.      */
  123.     FieldVector3D<T> getVelocity();

  125.     /** Get the first time derivative of the rotation.
  126.      * <p>The norm represents the angular rate.</p>
  127.      * @return First time derivative of the rotation
  128.      * @see #getRotation()
  129.      */
  130.     FieldVector3D<T> getRotationRate();

  132.     /**
  133.      * Get the inverse transform of the instance.
  134.      *
  135.      * @return inverse transform of the instance
  136.      */
  137.     FieldKinematicTransform<T> getInverse();

  139.     /**
  140.      * Build a transform by combining two existing ones.
  141.      * <p>
  142.      * Note that the dates of the two existing transformed are <em>ignored</em>,
  143.      * and the combined transform date is set to the date supplied in this
  144.      * constructor without any attempt to shift the raw transforms. This is a
  145.      * design choice allowing user full control of the combination.
  146.      * </p>
  147.      *
  148.      * @param <T> type of the elements
  149.      * @param date   date of the transform
  150.      * @param first  first transform applied
  151.      * @param second second transform applied
  152.      * @return the newly created kinematic transform that has the same effect as
  153.      * applying {@code first}, then {@code second}.
  154.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  155.      */
  156.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> compose(final FieldAbsoluteDate<T> date,
  157.                                                                                   final FieldKinematicTransform<T> first,
  158.                                                                                   final FieldKinematicTransform<T> second) {
  159.         final FieldVector3D<T> composedTranslation = FieldStaticTransform.compositeTranslation(first, second);
  160.         final FieldVector3D<T> composedTranslationRate = FieldKinematicTransform.compositeVelocity(first, second);
  161.         return of(date, new FieldPVCoordinates<>(composedTranslation, composedTranslationRate),
  162.                 FieldStaticTransform.compositeRotation(first, second),
  163.                 FieldKinematicTransform.compositeRotationRate(first, second));
  164.     }

  166.     /**
  167.      * Create a new kinematic transform from a rotation and zero, constant translation.
  168.      *
  169.      * @param <T> type of the elements
  170.      * @param date     of translation.
  171.      * @param rotation to apply after the translation. That is after translating
  172.      *                 applying this rotation produces positions expressed in
  173.      *                 the new frame.
  174.      * @param rotationRate rate of rotation
  175.      * @return the newly created kinematic transform.
  176.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  177.      */
  178.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
  179.                                                                              final FieldRotation<T> rotation,
  180.                                                                              final FieldVector3D<T> rotationRate) {
  181.         return of(date, FieldPVCoordinates.getZero(date.getField()), rotation, rotationRate);
  182.     }

  184.     /**
  185.      * Create a new kinematic transform from a translation and its rate.
  186.      *
  187.      * @param <T> type of the elements
  188.      * @param date        of translation.
  189.      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
  190.      *                    opposite of the coordinates of the new origin in the
  191.      *                    old frame.
  192.      * @return the newly created kinematic transform.
  193.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  194.      */
  195.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
  196.                                                                              final FieldPVCoordinates<T> pvCoordinates) {
  197.         final Field<T> field = date.getField();
  198.         return of(date, pvCoordinates, FieldRotation.getIdentity(field), FieldVector3D.getZero(field));
  199.     }

  201.     /**
  202.      * Create a new kinematic transform from a non-Field version.
  203.      *
  204.      * @param <T> type of the elements
  205.      * @param field field.
  206.      * @param kinematicTransform non-Field kinematic transform
  207.      * @return the newly created kinematic transform.
  208.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  209.      */
  210.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final Field<T> field,
  211.                                                                              final KinematicTransform kinematicTransform) {
  212.         final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, kinematicTransform.getDate());
  213.         final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(field,
  214.             new PVCoordinates(kinematicTransform.getTranslation(), kinematicTransform.getVelocity()));
  215.         final FieldRotation<T> rotation = new FieldRotation<>(field, kinematicTransform.getRotation());
  216.         final FieldVector3D<T> rotationRate = new FieldVector3D<>(field, kinematicTransform.getRotationRate());
  217.         return of(date, pvCoordinates, rotation, rotationRate);
  218.     }

  220.     /**
  221.      * Create a new kinematic transform from a translation and rotation.
  222.      *
  223.      * @param <T> type of the elements
  224.      * @param date        of translation.
  225.      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
  226.      *                    opposite of the coordinates of the new origin in the
  227.      *                    old frame.
  228.      * @param rotation    to apply after the translation. That is after
  229.      *                    translating applying this rotation produces positions
  230.      *                    expressed in the new frame.
  231.      * @param rotationRate rate of rotation
  232.      * @return the newly created kinematic transform.
  233.      * @see #compose(FieldAbsoluteDate, FieldKinematicTransform, FieldKinematicTransform)
  234.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  235.      * @see #of(FieldAbsoluteDate, FieldPVCoordinates, FieldRotation, FieldVector3D)
  236.      */
  237.     static <T extends CalculusFieldElement<T>> FieldKinematicTransform<T> of(final FieldAbsoluteDate<T> date,
  238.                                                                           final FieldPVCoordinates<T> pvCoordinates,
  239.                                                                           final FieldRotation<T> rotation,
  240.                                                                           final FieldVector3D<T> rotationRate) {
  241.         return new FieldKinematicTransform<T>() {

  243.             @Override
  244.             public FieldKinematicTransform<T> getInverse() {
  245.                 final FieldRotation<T> r = getRotation();
  246.                 final FieldVector3D<T> rp = r.applyTo(getTranslation());
  247.                 final FieldVector3D<T> pInv = rp.negate();
  248.                 final FieldVector3D<T> crossP      = FieldVector3D.crossProduct(getRotationRate(), rp);
  249.                 final FieldVector3D<T> vInv        = crossP.subtract(getRotation().applyTo(getVelocity()));
  250.                 final FieldRotation<T> rInv = r.revert();
  251.                 return FieldKinematicTransform.of(date, new FieldPVCoordinates<>(pInv, vInv),
  252.                         rInv, rInv.applyTo(getRotationRate()).negate());
  253.             }

  255.             @Override
  256.             public AbsoluteDate getDate() {
  257.                 return date.toAbsoluteDate();
  258.             }

  260.             @Override
  261.             public FieldAbsoluteDate<T> getFieldDate() {
  262.                 return date;
  263.             }

  265.             @Override
  266.             public FieldVector3D<T> getTranslation() {
  267.                 return pvCoordinates.getPosition();
  268.             }

  270.             @Override
  271.             public FieldRotation<T> getRotation() {
  272.                 return rotation;
  273.             }

  275.             @Override
  276.             public FieldVector3D<T> getVelocity() {
  277.                 return pvCoordinates.getVelocity();
  278.             }

  280.             @Override
  281.             public FieldVector3D<T> getRotationRate() {
  282.                 return rotationRate;
  283.             }
  284.         };
  285.     }

  287. }
Created with JaCoCo 0.8.12.202403310830