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;
18  
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;
24  
25  /**
26   * A transform that only includes translation and rotation as well as their respective rates.
27   * It is kinematic in the sense that it cannot transform an acceleration vector.
28   *
29   * @author Romain Serra
30   * @see StaticTransform
31   * @see Transform
32   * @since 12.1
33   */
34  public interface KinematicTransform extends StaticTransform {
35  
36      /**
37       * Get the identity kinematic transform.
38       *
39       * @return identity transform.
40       */
41      static KinematicTransform getIdentity() {
42          return Transform.IDENTITY;
43      }
44  
45      /** Compute a composite velocity.
46       * @param first first applied transform
47       * @param second second applied transform
48       * @return velocity part of the composite transform
49       */
50      static Vector3D compositeVelocity(final KinematicTransform first, final KinematicTransform second) {
51  
52          final Vector3D v1 = first.getVelocity();
53          final Rotation r1 = first.getRotation();
54          final Vector3D o1 = first.getRotationRate();
55          final Vector3D p2 = second.getTranslation();
56          final Vector3D v2 = second.getVelocity();
57  
58          final Vector3D crossP = Vector3D.crossProduct(o1, p2);
59  
60          return v1.add(r1.applyInverseTo(v2.add(crossP)));
61      }
62  
63      /** Compute a composite rotation rate.
64       * @param first first applied transform
65       * @param second second applied transform
66       * @return rotation rate part of the composite transform
67       */
68      static Vector3D compositeRotationRate(final KinematicTransform first, final KinematicTransform second) {
69  
70          final Vector3D o1 = first.getRotationRate();
71          final Rotation r2 = second.getRotation();
72          final Vector3D o2 = second.getRotationRate();
73  
74          return o2.add(r2.applyTo(o1));
75      }
76  
77      /** Transform {@link PVCoordinates}, without the acceleration vector.
78       * @param pv the position-velocity couple to transform.
79       * @return transformed position-velocity
80       */
81      default PVCoordinates transformOnlyPV(final PVCoordinates pv) {
82          final Vector3D transformedP = transformPosition(pv.getPosition());
83          final Vector3D crossP       = Vector3D.crossProduct(getRotationRate(), transformedP);
84          final Vector3D transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
85          return new PVCoordinates(transformedP, transformedV);
86      }
87  
88      /** Transform {@link TimeStampedPVCoordinates}, without the acceleration vector.
89       * <p>
90       * In order to allow the user more flexibility, this method does <em>not</em> check for
91       * consistency between the transform {@link #getDate() date} and the time-stamped
92       * position-velocity {@link TimeStampedPVCoordinates#getDate() date}. The returned
93       * value will always have the same {@link TimeStampedPVCoordinates#getDate() date} as
94       * the input argument, regardless of the instance {@link #getDate() date}.
95       * </p>
96       * @param pv the position-velocity couple to transform.
97       * @return transformed position-velocity
98       */
99      default TimeStampedPVCoordinates transformOnlyPV(final TimeStampedPVCoordinates pv) {
100         final Vector3D transformedP = transformPosition(pv.getPosition());
101         final Vector3D crossP       = Vector3D.crossProduct(getRotationRate(), transformedP);
102         final Vector3D transformedV = getRotation().applyTo(pv.getVelocity().add(getVelocity())).subtract(crossP);
103         return new TimeStampedPVCoordinates(pv.getDate(), transformedP, transformedV);
104     }
105 
106     /** Get the first time derivative of the translation.
107      * @return first time derivative of the translation
108      * @see #getTranslation()
109      */
110     Vector3D getVelocity();
111 
112     /** Get the first time derivative of the rotation.
113      * <p>The norm represents the angular rate.</p>
114      * @return First time derivative of the rotation
115      * @see #getRotation()
116      */
117     Vector3D getRotationRate();
118 
119     /**
120      * Get the inverse transform of the instance.
121      *
122      * @return inverse transform of the instance
123      */
124     KinematicTransform getInverse();
125 
126     /**
127      * Build a transform by combining two existing ones.
128      * <p>
129      * Note that the dates of the two existing transformed are <em>ignored</em>,
130      * and the combined transform date is set to the date supplied in this
131      * constructor without any attempt to shift the raw transforms. This is a
132      * design choice allowing user full control of the combination.
133      * </p>
134      *
135      * @param date   date of the transform
136      * @param first  first transform applied
137      * @param second second transform applied
138      * @return the newly created kinematic transform that has the same effect as
139      * applying {@code first}, then {@code second}.
140      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
141      */
142     static KinematicTransform compose(final AbsoluteDate date,
143                                       final KinematicTransform first,
144                                       final KinematicTransform second) {
145         final Vector3D composedTranslation = StaticTransform.compositeTranslation(first, second);
146         final Vector3D composedTranslationRate = KinematicTransform.compositeVelocity(first, second);
147         return of(date, new PVCoordinates(composedTranslation, composedTranslationRate),
148                 StaticTransform.compositeRotation(first, second),
149                 KinematicTransform.compositeRotationRate(first, second));
150     }
151 
152     /**
153      * Create a new kinematic transform from a rotation and zero, constant translation.
154      *
155      * @param date     of translation.
156      * @param rotation to apply after the translation. That is after translating
157      *                 applying this rotation produces positions expressed in
158      *                 the new frame.
159      * @param rotationRate rate of rotation
160      * @return the newly created kinematic transform.
161      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
162      */
163     static KinematicTransform of(final AbsoluteDate date,
164                                  final Rotation rotation,
165                                  final Vector3D rotationRate) {
166         return of(date, PVCoordinates.ZERO, rotation, rotationRate);
167     }
168 
169     /**
170      * Create a new kinematic transform from a translation and its rate.
171      *
172      * @param date        of translation.
173      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
174      *                    opposite of the coordinates of the new origin in the
175      *                    old frame.
176      * @return the newly created kinematic transform.
177      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
178      */
179     static KinematicTransform of(final AbsoluteDate date,
180                                  final PVCoordinates pvCoordinates) {
181         return of(date, pvCoordinates, Rotation.IDENTITY, Vector3D.ZERO);
182     }
183 
184     /**
185      * Create a new kinematic transform from a translation and rotation.
186      *
187      * @param date        of translation.
188      * @param pvCoordinates translation (with rate) to apply, expressed in the old frame. That is, the
189      *                    opposite of the coordinates of the new origin in the
190      *                    old frame.
191      * @param rotation    to apply after the translation. That is after
192      *                    translating applying this rotation produces positions
193      *                    expressed in the new frame.
194      * @param rotationRate rate of rotation
195      * @return the newly created kinematic transform.
196      * @see #compose(AbsoluteDate, KinematicTransform, KinematicTransform)
197      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
198      * @see #of(AbsoluteDate, PVCoordinates, Rotation, Vector3D)
199      */
200     static KinematicTransform of(final AbsoluteDate date, final PVCoordinates pvCoordinates,
201                                  final Rotation rotation, final Vector3D rotationRate) {
202         return new KinematicTransform() {
203 
204             @Override
205             public KinematicTransform getInverse() {
206                 final Rotation r = getRotation();
207                 final Vector3D rp = r.applyTo(getTranslation());
208                 final Vector3D pInv = rp.negate();
209                 final Vector3D crossP      = Vector3D.crossProduct(getRotationRate(), rp);
210                 final Vector3D vInv        = crossP.subtract(getRotation().applyTo(getVelocity()));
211                 final Rotation rInv = r.revert();
212                 return KinematicTransform.of(getDate(), new PVCoordinates(pInv, vInv),
213                         rInv, rInv.applyTo(getRotationRate()).negate());
214             }
215 
216             @Override
217             public AbsoluteDate getDate() {
218                 return date;
219             }
220 
221             @Override
222             public Vector3D getTranslation() {
223                 return pvCoordinates.getPosition();
224             }
225 
226             @Override
227             public Rotation getRotation() {
228                 return rotation;
229             }
230 
231             @Override
232             public Vector3D getVelocity() {
233                 return pvCoordinates.getVelocity();
234             }
235 
236             @Override
237             public Vector3D getRotationRate() {
238                 return rotationRate;
239             }
240         };
241     }
242 
243 }