SpacecraftToObservedBody.java
/* Copyright 2013-2025 CS GROUP
* 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.rugged.utils;
import java.io.Serializable;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.FastMath;
import org.orekit.frames.FactoryManagedFrame;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.Predefined;
import org.orekit.frames.Transform;
import org.orekit.rugged.errors.DumpManager;
import org.orekit.rugged.errors.RuggedException;
import org.orekit.rugged.errors.RuggedMessages;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeInterpolator;
import org.orekit.time.TimeOffset;
import org.orekit.utils.AngularCoordinates;
import org.orekit.utils.AngularDerivativesFilter;
import org.orekit.utils.CartesianDerivativesFilter;
import org.orekit.utils.ImmutableTimeStampedCache;
import org.orekit.utils.PVCoordinates;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedAngularCoordinatesHermiteInterpolator;
import org.orekit.utils.TimeStampedCache;
import org.orekit.utils.TimeStampedPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
/** Provider for observation transforms.
* @author Luc Maisonobe
* @author Guylaine Prat
*/
public class SpacecraftToObservedBody implements Serializable {
/** Serializable UID. */
private static final long serialVersionUID = 20250427L;
/** Inertial frame. */
private final Frame inertialFrame;
/** Body frame. */
private final Frame bodyFrame;
/** Start of search time span. */
private final AbsoluteDate minDate;
/** End of search time span. */
private final AbsoluteDate maxDate;
/** Step to use for inertial frame to body frame transforms cache computations. */
private final double tStep;
/** Tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting. */
private final double overshootTolerance;
/** Transforms sample from observed body frame to inertial frame. */
private final List<Transform> bodyToInertial;
/** Transforms sample from inertial frame to observed body frame. */
private final List<Transform> inertialToBody;
/** Transforms sample from spacecraft frame to inertial frame. */
private final List<Transform> scToInertial;
/** Simple constructor.
* @param inertialFrame inertial frame
* @param bodyFrame observed body frame
* @param minDate start of search time span
* @param maxDate end of search time span
* @param tStep step to use for inertial frame to body frame transforms cache computations
* @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
* slightly the position, velocity and quaternions ephemerides
* @param positionsVelocities satellite position and velocity
* @param pvInterpolationNumber number of points to use for position/velocity interpolation
* @param pvFilter filter for derivatives from the sample to use in position/velocity interpolation
* @param quaternions satellite quaternions
* @param aInterpolationNumber number of points to use for attitude interpolation
* @param aFilter filter for derivatives from the sample to use in attitude interpolation
*/
public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
final double overshootTolerance,
final List<TimeStampedPVCoordinates> positionsVelocities, final int pvInterpolationNumber,
final CartesianDerivativesFilter pvFilter,
final List<TimeStampedAngularCoordinates> quaternions, final int aInterpolationNumber,
final AngularDerivativesFilter aFilter) {
this.inertialFrame = inertialFrame;
this.bodyFrame = bodyFrame;
this.minDate = minDate;
this.maxDate = maxDate;
this.overshootTolerance = overshootTolerance;
// safety checks
final AbsoluteDate minPVDate = positionsVelocities.get(0).getDate();
final AbsoluteDate maxPVDate = positionsVelocities.get(positionsVelocities.size() - 1).getDate();
if (minPVDate.durationFrom(minDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minPVDate, maxPVDate);
}
if (maxDate.durationFrom(maxPVDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minPVDate, maxPVDate);
}
final AbsoluteDate minQDate = quaternions.get(0).getDate();
final AbsoluteDate maxQDate = quaternions.get(quaternions.size() - 1).getDate();
if (minQDate.durationFrom(minDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minQDate, maxQDate);
}
if (maxDate.durationFrom(maxQDate) > overshootTolerance) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minQDate, maxQDate);
}
// set up the cache for position-velocities
final TimeStampedCache<TimeStampedPVCoordinates> pvCache =
new ImmutableTimeStampedCache<>(pvInterpolationNumber, positionsVelocities);
// set up the TimeStampedPVCoordinates interpolator
final TimeInterpolator<TimeStampedPVCoordinates> pvInterpolator =
new TimeStampedPVCoordinatesHermiteInterpolator(pvInterpolationNumber, pvFilter);
// set up the cache for attitudes
final TimeStampedCache<TimeStampedAngularCoordinates> aCache =
new ImmutableTimeStampedCache<>(aInterpolationNumber, quaternions);
// set up the TimeStampedAngularCoordinates Hermite interpolator
final TimeInterpolator<TimeStampedAngularCoordinates> angularInterpolator =
new TimeStampedAngularCoordinatesHermiteInterpolator(aInterpolationNumber, aFilter);
final int n = (int) FastMath.ceil(maxDate.durationFrom(minDate) / tStep);
this.tStep = tStep;
this.bodyToInertial = new ArrayList<>(n);
this.inertialToBody = new ArrayList<>(n);
this.scToInertial = new ArrayList<>(n);
for (AbsoluteDate date = minDate; bodyToInertial.size() < n; date = date.shiftedBy(tStep)) {
// interpolate position-velocity, allowing slight extrapolation near the boundaries
final AbsoluteDate pvInterpolationDate;
if (date.compareTo(pvCache.getEarliest().getDate()) < 0) {
pvInterpolationDate = pvCache.getEarliest().getDate();
} else if (date.compareTo(pvCache.getLatest().getDate()) > 0) {
pvInterpolationDate = pvCache.getLatest().getDate();
} else {
pvInterpolationDate = date;
}
final TimeStampedPVCoordinates interpolatedPV =
pvInterpolator.interpolate(pvInterpolationDate,
pvCache.getNeighbors(pvInterpolationDate));
final TimeStampedPVCoordinates pv = interpolatedPV.shiftedBy(date.durationFrom(pvInterpolationDate));
// interpolate attitude, allowing slight extrapolation near the boundaries
final AbsoluteDate aInterpolationDate;
if (date.compareTo(aCache.getEarliest().getDate()) < 0) {
aInterpolationDate = aCache.getEarliest().getDate();
} else if (date.compareTo(aCache.getLatest().getDate()) > 0) {
aInterpolationDate = aCache.getLatest().getDate();
} else {
aInterpolationDate = date;
}
final TimeStampedAngularCoordinates interpolatedQuaternion =
angularInterpolator.interpolate(aInterpolationDate,
aCache.getNeighbors(aInterpolationDate).collect(Collectors.toList()));
final TimeStampedAngularCoordinates quaternion = interpolatedQuaternion.shiftedBy(date.durationFrom(aInterpolationDate));
// store transform from spacecraft frame to inertial frame
scToInertial.add(new Transform(date,
new Transform(date, quaternion.revert()),
new Transform(date, pv)));
// store transform from body frame to inertial frame
final Transform b2i = bodyFrame.getTransformTo(inertialFrame, date);
bodyToInertial.add(b2i);
inertialToBody.add(b2i.getInverse());
}
}
/** Simple constructor.
* @param inertialFrame inertial frame
* @param bodyFrame observed body frame
* @param minDate start of search time span
* @param maxDate end of search time span
* @param tStep step to use for inertial frame to body frame transforms cache computations
* @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
* slightly the position, velocity and quaternions ephemerides
* @param bodyToInertial transforms sample from observed body frame to inertial frame
* @param scToInertial transforms sample from spacecraft frame to inertial frame
*/
public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
final double overshootTolerance,
final List<Transform> bodyToInertial, final List<Transform> scToInertial) {
this.inertialFrame = inertialFrame;
this.bodyFrame = bodyFrame;
this.minDate = minDate;
this.maxDate = maxDate;
this.tStep = tStep;
this.overshootTolerance = overshootTolerance;
this.bodyToInertial = bodyToInertial;
this.scToInertial = scToInertial;
this.inertialToBody = new ArrayList<>(bodyToInertial.size());
for (final Transform b2i : bodyToInertial) {
inertialToBody.add(b2i.getInverse());
}
}
/** Get the inertial frame.
* @return inertial frame
*/
public Frame getInertialFrame() {
return inertialFrame;
}
/** Get the body frame.
* @return body frame
*/
public Frame getBodyFrame() {
return bodyFrame;
}
/** Get the start of search time span.
* @return start of search time span
*/
public AbsoluteDate getMinDate() {
return minDate;
}
/** Get the end of search time span.
* @return end of search time span
*/
public AbsoluteDate getMaxDate() {
return maxDate;
}
/** Get the step to use for inertial frame to body frame transforms cache computations.
* @return step to use for inertial frame to body frame transforms cache computations
*/
public double getTStep() {
return tStep;
}
/** Get the tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting.
* @return tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting
*/
public double getOvershootTolerance() {
return overshootTolerance;
}
/** Get transform from spacecraft to inertial frame.
* @param date date of the transform
* @return transform from spacecraft to inertial frame
*/
public Transform getScToInertial(final AbsoluteDate date) {
return interpolate(date, scToInertial);
}
/** Get transform from inertial frame to observed body frame.
* @param date date of the transform
* @return transform from inertial frame to observed body frame
*/
public Transform getInertialToBody(final AbsoluteDate date) {
return interpolate(date, inertialToBody);
}
/** Get transform from observed body frame to inertial frame.
* @param date date of the transform
* @return transform from observed body frame to inertial frame
*/
public Transform getBodyToInertial(final AbsoluteDate date) {
return interpolate(date, bodyToInertial);
}
/** Interpolate transform.
* @param date date of the transform
* @param list transforms list to interpolate from
* @return interpolated transform
*/
private Transform interpolate(final AbsoluteDate date, final List<Transform> list) {
// check date range
if (!isInRange(date)) {
throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, date, minDate, maxDate);
}
final double s = date.durationFrom(list.get(0).getDate()) / tStep;
final int index = FastMath.max(0, FastMath.min(list.size() - 1, (int) FastMath.rint(s)));
DumpManager.dumpTransform(this, index, bodyToInertial.get(index), scToInertial.get(index));
final Transform close = list.get(index);
return close.shiftedBy(date.durationFrom(close.getDate()));
}
/** Check if a date is in the supported range.
* @param date date to check
* @return true if date is in the supported range
*/
public boolean isInRange(final AbsoluteDate date) {
return minDate.durationFrom(date) <= overshootTolerance &&
date.durationFrom(maxDate) <= overshootTolerance;
}
/** Replace the instance with a data transfer object for serialization.
* @return data transfer object that will be serialized
*/
private Object writeReplace() {
return new DataTransferObject(((FactoryManagedFrame) inertialFrame).getFactoryKey(),
((FactoryManagedFrame) bodyFrame).getFactoryKey(),
minDate, maxDate, tStep, overshootTolerance,
extractTimeOffsets(bodyToInertial),
extractCoordinates(bodyToInertial),
extractTimeOffsets(scToInertial),
extractCoordinates(scToInertial));
}
/** Extract time offsets from a transforms list.
* @param transforms transforms to convert
* @return time offsets
* @since 4.0
*/
private long[] extractTimeOffsets(final List<Transform> transforms) {
final long[] offsets = new long[2 * transforms.size()];
for (int i = 0; i < transforms.size(); ++i) {
final Transform ti = transforms.get(i);
offsets[2 * i] = ti.getDate().getSeconds();
offsets[2 * i + 1] = ti.getDate().getAttoSeconds();
}
return offsets;
}
/** Extract coordinates from a transforms list.
* @param transforms transforms to convert
* @return time offsets
* @since 4.0
*/
private double[] extractCoordinates(final List<Transform> transforms) {
final double[] coordinates = new double[19 * transforms.size()];
for (int i = 0; i < transforms.size(); ++i) {
final PVCoordinates pv = transforms.get(i).getCartesian();
final AngularCoordinates ag = transforms.get(i).getAngular();
coordinates[19 * i] = pv.getPosition().getX();
coordinates[19 * i + 1] = pv.getPosition().getY();
coordinates[19 * i + 2] = pv.getPosition().getZ();
coordinates[19 * i + 3] = pv.getVelocity().getX();
coordinates[19 * i + 4] = pv.getVelocity().getY();
coordinates[19 * i + 5] = pv.getVelocity().getZ();
coordinates[19 * i + 6] = pv.getAcceleration().getX();
coordinates[19 * i + 7] = pv.getAcceleration().getY();
coordinates[19 * i + 8] = pv.getAcceleration().getZ();
coordinates[19 * i + 9] = ag.getRotation().getQ0();
coordinates[19 * i + 10] = ag.getRotation().getQ1();
coordinates[19 * i + 11] = ag.getRotation().getQ2();
coordinates[19 * i + 12] = ag.getRotation().getQ3();
coordinates[19 * i + 13] = ag.getRotationRate().getX();
coordinates[19 * i + 14] = ag.getRotationRate().getY();
coordinates[19 * i + 15] = ag.getRotationRate().getZ();
coordinates[19 * i + 16] = ag.getRotationAcceleration().getX();
coordinates[19 * i + 17] = ag.getRotationAcceleration().getY();
coordinates[19 * i + 18] = ag.getRotationAcceleration().getZ();
}
return coordinates;
}
/** Internal class used only for serialization.
* @since 4.0
*/
private static class DataTransferObject implements Serializable {
/** Serializable UID. */
private static final long serialVersionUID = 20250427L;
/** Inertial frame. */
private final Predefined inertialFrame;
/** Body frame. */
private final Predefined bodyFrame;
/** Start of search time span. */
private final AbsoluteDate minDate;
/** End of search time span. */
private final AbsoluteDate maxDate;
/** Step to use for inertial frame to body frame transforms cache computations. */
private final double tStep;
/** Tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting. */
private final double overshootTolerance;
/** Transforms sample from observed body frame to inertial frame. */
private final long[] bodyToInertialTimeOffset;
/** Transforms sample from observed body frame to inertial frame. */
private final double[] bodyToInertialCoordinates;
/** Transforms sample from spacecraft frame to inertial frame. */
private final long[] scToInertialTimOffset;
/** Transforms sample from spacecraft frame to inertial frame. */
private final double[] scToInertialCoordinates;
/** Simple constructor.
* @param inertialFrame inertial frame
* @param bodyFrame observed body frame
* @param minDate start of search time span
* @param maxDate end of search time span
* @param tStep step to use for inertial frame to body frame transforms cache computations
* @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
* slightly the position, velocity and quaternions ephemerides
* @param bodyToInertialTimeOffset time offsets of transforms sample from observed body frame to inertial frame
* @param bodyToInertialCoordinates coordinates of transforms sample from observed body frame to inertial frame
* @param scToInertialTimOffset time offsets transforms sample from spacecraft frame to inertial frame
* @param scToInertialCoordinates coordinates transforms sample from spacecraft frame to inertial frame
*/
DataTransferObject(final Predefined inertialFrame, final Predefined bodyFrame,
final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
final double overshootTolerance,
final long[] bodyToInertialTimeOffset, final double[] bodyToInertialCoordinates,
final long[] scToInertialTimOffset, final double[] scToInertialCoordinates) {
this.inertialFrame = inertialFrame;
this.bodyFrame = bodyFrame;
this.minDate = minDate;
this.maxDate = maxDate;
this.tStep = tStep;
this.overshootTolerance = overshootTolerance;
this.bodyToInertialTimeOffset = bodyToInertialTimeOffset;
this.bodyToInertialCoordinates = bodyToInertialCoordinates;
this.scToInertialTimOffset = scToInertialTimOffset;
this.scToInertialCoordinates = scToInertialCoordinates;
}
/** Create a transform.
* @param i index of the transfor
* @param timeOffsets time offsets array
* @param coordinates coordinates array
* @return transform
*/
private Transform createTransform(final int i,
final long[] timeOffsets,
final double[] coordinates) {
final AbsoluteDate date = new AbsoluteDate(new TimeOffset(timeOffsets[2 * i],
timeOffsets[2 * i + 1]));
final PVCoordinates pv = new PVCoordinates(new Vector3D(coordinates[19 * i],
coordinates[19 * i + 1],
coordinates[19 * i + 2]),
new Vector3D(coordinates[19 * i + 3],
coordinates[19 * i + 4],
coordinates[19 * i + 5]),
new Vector3D(coordinates[19 * i + 6],
coordinates[19 * i + 7],
coordinates[19 * i + 8]));
final AngularCoordinates ag = new AngularCoordinates(new Rotation(coordinates[19 * i + 9],
coordinates[19 * i + 10],
coordinates[19 * i + 11],
coordinates[19 * i + 12],
false),
new Vector3D(coordinates[19 * i + 13],
coordinates[19 * i + 14],
coordinates[19 * i + 15]),
new Vector3D(coordinates[19 * i + 16],
coordinates[19 * i + 17],
coordinates[19 * i + 18]));
return new Transform(date, pv, ag);
}
/** Create all transforms.
* @param timeOffsets time offsets array
* @param coordinates coordinates array
* @return all transforms
*/
private List<Transform> createAllTransforms(final long[] timeOffsets,
final double[] coordinates) {
final List<Transform> transforms = new ArrayList<>(timeOffsets.length / 2);
for (int i = 0; i < timeOffsets.length / 2; ++i) {
transforms.add(createTransform(i, timeOffsets, coordinates));
}
return transforms;
}
/** Replace the deserialized data transfer object with a
* {@link SpacecraftToObservedBody}.
* @return replacement {@link SpacecraftToObservedBody}
*/
private Object readResolve() {
return new SpacecraftToObservedBody(FramesFactory.getFrame(inertialFrame),
FramesFactory.getFrame(bodyFrame),
minDate, maxDate, tStep, overshootTolerance,
createAllTransforms(bodyToInertialTimeOffset,
bodyToInertialCoordinates),
createAllTransforms(scToInertialTimOffset,
scToInertialCoordinates));
}
}
}