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 × 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;
}
};
}
}