diff --git a/src/main/java/org/orekit/propagation/FieldSpacecraftState.java b/src/main/java/org/orekit/propagation/FieldSpacecraftState.java index 27f78bef3080d492e4bd10765399fc308b73013c..0ebca64c44b673f3dd0b600ebfb10af14f5a4c4f 100644 --- a/src/main/java/org/orekit/propagation/FieldSpacecraftState.java +++ b/src/main/java/org/orekit/propagation/FieldSpacecraftState.java @@ -30,11 +30,14 @@ import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator; import org.hipparchus.exception.LocalizedCoreFormats; import org.hipparchus.exception.MathIllegalArgumentException; import org.hipparchus.exception.MathIllegalStateException; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.util.FastMath; import org.hipparchus.util.MathArrays; import org.orekit.attitudes.FieldAttitude; import org.orekit.attitudes.LofOffset; import org.orekit.errors.OrekitException; import org.orekit.errors.OrekitIllegalArgumentException; +import org.orekit.errors.OrekitIllegalStateException; import org.orekit.errors.OrekitMessages; import org.orekit.frames.FieldTransform; import org.orekit.frames.Frame; @@ -45,6 +48,8 @@ import org.orekit.time.FieldAbsoluteDate; import org.orekit.time.FieldTimeInterpolable; import org.orekit.time.FieldTimeShiftable; import org.orekit.time.FieldTimeStamped; +import org.orekit.utils.FieldAbsolutePVCoordinates; +import org.orekit.utils.FieldPVCoordinates; import org.orekit.utils.TimeStampedFieldPVCoordinates; @@ -72,6 +77,7 @@ import org.orekit.utils.TimeStampedFieldPVCoordinates; * @author Fabien Maussion * @author Véronique Pommier-Maurussane * @author Luc Maisonobe + * @author Vincent Mouraux */ public class FieldSpacecraftState > implements FieldTimeStamped, FieldTimeShiftable, T>, FieldTimeInterpolable, T> { @@ -88,6 +94,9 @@ public class FieldSpacecraftState > /** Orbital state. */ private final FieldOrbit orbit; + /** Trajectory state, when it is not an orbit. */ + private final FieldAbsolutePVCoordinates absPva; + /** FieldAttitude. */ private final FieldAttitude attitude; @@ -193,7 +202,7 @@ public class FieldSpacecraftState > this.orbit = orbit; this.attitude = attitude; this.mass = mass; - + this.absPva = null; if (additional == null) { this.additional = Collections.emptyMap(); @@ -206,51 +215,190 @@ public class FieldSpacecraftState > } } - /** Convert a {@link SpacecraftState}. + /** Convert a {@link FieldSpacecraftState}. * @param field field to which the elements belong * @param state state to convert */ public FieldSpacecraftState(final Field field, final SpacecraftState state) { - final double[] stateD = new double[6]; - final double[] stateDotD = state.getOrbit().hasDerivatives() ? new double[6] : null; - state.getOrbit().getType().mapOrbitToArray(state.getOrbit(), PositionAngle.TRUE, stateD, stateDotD); - final T[] stateF = MathArrays.buildArray(field, 6); - for (int i = 0; i < stateD.length; ++i) { - stateF[i] = field.getZero().add(stateD[i]); - } - final T[] stateDotF; - if (stateDotD == null) { - stateDotF = null; + if (state.isOrbitDefined()) { + final double[] stateD = new double[6]; + final double[] stateDotD = state.getOrbit().hasDerivatives() ? new double[6] : null; + state.getOrbit().getType().mapOrbitToArray(state.getOrbit(), PositionAngle.TRUE, stateD, stateDotD); + final T[] stateF = MathArrays.buildArray(field, 6); + for (int i = 0; i < stateD.length; ++i) { + stateF[i] = field.getZero().add(stateD[i]); + } + final T[] stateDotF; + if (stateDotD == null) { + stateDotF = null; + } else { + stateDotF = MathArrays.buildArray(field, 6); + for (int i = 0; i < stateDotD.length; ++i) { + stateDotF[i] = field.getZero().add(stateDotD[i]); + } + } + + final FieldAbsoluteDate dateF = new FieldAbsoluteDate<>(field, state.getDate()); + + this.orbit = state.getOrbit().getType().mapArrayToOrbit(stateF, stateDotF, + PositionAngle.TRUE, + dateF, field.getZero().add(state.getMu()), state.getFrame()); + this.attitude = new FieldAttitude<>(field, state.getAttitude()); + this.mass = field.getZero().add(state.getMass()); + this.absPva = null; + final Map additionalD = state.getAdditionalStates(); + if (additionalD.isEmpty()) { + this.additional = Collections.emptyMap(); + } else { + this.additional = new HashMap(additionalD.size()); + for (final Map.Entry entry : additionalD.entrySet()) { + final T[] array = MathArrays.buildArray(field, entry.getValue().length); + for (int k = 0; k < array.length; ++k) { + array[k] = field.getZero().add(entry.getValue()[k]); + } + this.additional.put(entry.getKey(), array); + } + } + } else { - stateDotF = MathArrays.buildArray(field, 6); - for (int i = 0; i < stateDotD.length; ++i) { - stateDotF[i] = field.getZero().add(stateDotD[i]); + final T zero = field.getZero(); + final T x = zero.add(state.getPVCoordinates().getPosition().getX()); + final T y = zero.add(state.getPVCoordinates().getPosition().getY()); + final T z = zero.add(state.getPVCoordinates().getPosition().getZ()); + final T vx = zero.add(state.getPVCoordinates().getVelocity().getX()); + final T vy = zero.add(state.getPVCoordinates().getVelocity().getY()); + final T vz = zero.add(state.getPVCoordinates().getVelocity().getZ()); + final T ax = zero.add(state.getPVCoordinates().getAcceleration().getX()); + final T ay = zero.add(state.getPVCoordinates().getAcceleration().getY()); + final T az = zero.add(state.getPVCoordinates().getAcceleration().getZ()); + final FieldPVCoordinates pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x, y, z), new FieldVector3D<>(vx, vy, vz), new FieldVector3D<>(ax, ay, az)); + final FieldAbsoluteDate dateF = new FieldAbsoluteDate<>(field, state.getDate()); + this.orbit = null; + this.attitude = new FieldAttitude<>(field, state.getAttitude()); + this.mass = field.getZero().add(state.getMass()); + this.absPva = new FieldAbsolutePVCoordinates<>(state.getFrame(), dateF, pva_f); + final Map additionalD = state.getAdditionalStates(); + if (additionalD.isEmpty()) { + this.additional = Collections.emptyMap(); + } else { + this.additional = new HashMap(additionalD.size()); + for (final Map.Entry entry : additionalD.entrySet()) { + final T[] array = MathArrays.buildArray(field, entry.getValue().length); + for (int k = 0; k < array.length; ++k) { + array[k] = field.getZero().add(entry.getValue()[k]); + } + this.additional.put(entry.getKey(), array); + } } } + } - final FieldAbsoluteDate dateF = new FieldAbsoluteDate<>(field, state.getDate()); + /** Build a spacecraft state from orbit only. + *

Attitude and mass are set to unspecified non-null arbitrary values.

+ * @param absPva position-velocity-acceleration + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva) { + this(absPva, + new LofOffset(absPva.getFrame(), LOFType.VVLH).getAttitude(absPva, absPva.getDate(), absPva.getFrame()), + absPva.getDate().getField().getZero().add(DEFAULT_MASS), null); + } - this.orbit = state.getOrbit().getType().mapArrayToOrbit(stateF, stateDotF, - PositionAngle.TRUE, - dateF, field.getZero().add(state.getMu()), state.getFrame()); - this.attitude = new FieldAttitude<>(field, state.getAttitude()); - this.mass = field.getZero().add(state.getMass()); + /** Build a spacecraft state from orbit and attitude provider. + *

Mass is set to an unspecified non-null arbitrary value.

+ * @param absPva position-velocity-acceleration + * @param attitude attitude + * @exception IllegalArgumentException if orbit and attitude dates + * or frames are not equal + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva, final FieldAttitude attitude) + throws IllegalArgumentException { + this(absPva, attitude, absPva.getDate().getField().getZero().add(DEFAULT_MASS), null); + } - final Map additionalD = state.getAdditionalStates(); - if (additionalD.isEmpty()) { + /** Create a new instance from orbit and mass. + *

Attitude law is set to an unspecified default attitude.

+ * @param absPva position-velocity-acceleration + * @param mass the mass (kg) + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva, final T mass) { + this(absPva, + new LofOffset(absPva.getFrame(), LOFType.VVLH).getAttitude(absPva, absPva.getDate(), absPva.getFrame()), + mass, null); + } + + /** Build a spacecraft state from orbit, attitude provider and mass. + * @param absPva position-velocity-acceleration + * @param attitude attitude + * @param mass the mass (kg) + * @exception IllegalArgumentException if orbit and attitude dates + * or frames are not equal + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva, final FieldAttitude attitude, final T mass) + throws IllegalArgumentException { + this(absPva, attitude, mass, null); + } + + /** Build a spacecraft state from orbit only. + *

Attitude and mass are set to unspecified non-null arbitrary values.

+ * @param absPva position-velocity-acceleration + * @param additional additional states + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva, final Map additional) { + this(absPva, + new LofOffset(absPva.getFrame(), LOFType.VVLH).getAttitude(absPva, absPva.getDate(), absPva.getFrame()), + absPva.getDate().getField().getZero().add(DEFAULT_MASS), additional); + } + + /** Build a spacecraft state from orbit and attitude provider. + *

Mass is set to an unspecified non-null arbitrary value.

+ * @param absPva position-velocity-acceleration + * @param attitude attitude + * @param additional additional states + * @exception IllegalArgumentException if orbit and attitude dates + * or frames are not equal + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva, final FieldAttitude attitude, final Map additional) + throws IllegalArgumentException { + this(absPva, attitude, absPva.getDate().getField().getZero().add(DEFAULT_MASS), additional); + } + + /** Create a new instance from orbit and mass. + *

Attitude law is set to an unspecified default attitude.

+ * @param absPva position-velocity-acceleration + * @param mass the mass (kg) + * @param additional additional states + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva, final T mass, final Map additional) { + this(absPva, + new LofOffset(absPva.getFrame(), LOFType.VVLH).getAttitude(absPva, absPva.getDate(), absPva.getFrame()), + mass, additional); + } + + /** Build a spacecraft state from orbit, attitude provider and mass. + * @param absPva position-velocity-acceleration + * @param attitude attitude + * @param mass the mass (kg) + * @param additional additional states (may be null if no additional states are available) + * @exception IllegalArgumentException if orbit and attitude dates + * or frames are not equal + */ + public FieldSpacecraftState(final FieldAbsolutePVCoordinates absPva, final FieldAttitude attitude, + final T mass, final Map additional) + throws IllegalArgumentException { + checkConsistency(absPva, attitude); + this.orbit = null; + this.absPva = absPva; + this.attitude = attitude; + this.mass = mass; + if (additional == null) { this.additional = Collections.emptyMap(); } else { - this.additional = new HashMap(additionalD.size()); - for (final Map.Entry entry : additionalD.entrySet()) { - final T[] array = MathArrays.buildArray(field, entry.getValue().length); - for (int k = 0; k < array.length; ++k) { - array[k] = field.getZero().add(entry.getValue()[k]); - } - this.additional.put(entry.getKey(), array); + this.additional = new HashMap(additional.size()); + for (final Map.Entry entry : additional.entrySet()) { + this.additional.put(entry.getKey(), entry.getValue().clone()); } } - } /** Add an additional state. @@ -276,7 +424,10 @@ public class FieldSpacecraftState > final Map newMap = new HashMap(additional.size() + 1); newMap.putAll(additional); newMap.put(name, value.clone()); - return new FieldSpacecraftState<>(orbit, attitude, mass, newMap); + if (absPva == null) + return new FieldSpacecraftState<>(orbit, attitude, mass, newMap); + else + return new FieldSpacecraftState<>(absPva, attitude, mass, newMap); } /** Check orbit and attitude dates are equal. @@ -301,6 +452,41 @@ public class FieldSpacecraftState > } } + /** Check if the state contains an orbit part. + *

+ * A state contains either an {@link FieldAbsolutePVCoordinates absolute + * position-velocity-acceleration} or an {@link FieldOrbit orbit}. + *

+ * @return true if state contains an orbit (in which case {@link #getOrbit()} + * will not throw an exception), or false if the state contains an + * absolut position-velocity-acceleration (in which case {@link #getAbsPVA()} + * will not throw an exception) + */ + public boolean isOrbitDefined() { + return orbit != null; + } + + /** + * Check FieldAbsolutePVCoordinates and attitude dates are equal. + * @param absPva position-velocity-acceleration + * @param attitude attitude + * @param the type of the field elements + * @exception IllegalArgumentException if orbit and attitude dates are not equal + */ + private static > void checkConsistency(final FieldAbsolutePVCoordinates absPva, final FieldAttitude attitude) + throws IllegalArgumentException { + if (FastMath.abs(absPva.getDate().durationFrom(attitude.getDate())).getReal() > + DATE_INCONSISTENCY_THRESHOLD) { + throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_AND_ATTITUDE_DATES_MISMATCH, + absPva.getDate(), attitude.getDate()); + } + if (absPva.getFrame() != attitude.getReferenceFrame()) { + throw new OrekitIllegalArgumentException(OrekitMessages.FRAMES_MISMATCH, + absPva.getFrame().getName(), + attitude.getReferenceFrame().getName()); + } + } + /** Get a time-shifted state. *

* The state can be slightly shifted to close dates. This shift is based on @@ -313,7 +499,7 @@ public class FieldSpacecraftState > *

* As a rough order of magnitude, the following table shows the extrapolation * errors obtained between this simple shift method and an {@link - * org.orekit.propagation.numerical.NumericalPropagator numerical + * org.orekit.propagation.numerical.FieldNumericalPropagator numerical * propagator} for a low Earth Sun Synchronous Orbit, with a 20x20 gravity field, * Sun and Moon third bodies attractions, drag and solar radiation pressure. * Beware that these results will be different for other orbits. @@ -333,8 +519,13 @@ public class FieldSpacecraftState > * except for the mass which stay unchanged */ public FieldSpacecraftState shiftedBy(final double dt) { - return new FieldSpacecraftState<>(orbit.shiftedBy(dt), attitude.shiftedBy(dt), - mass, additional); + if (absPva == null) { + return new FieldSpacecraftState<>(orbit.shiftedBy(dt), attitude.shiftedBy(dt), + mass, additional); + } else { + return new FieldSpacecraftState<>(absPva.shiftedBy(dt), attitude.shiftedBy(dt), + mass, additional); + } } /** Get a time-shifted state. @@ -349,7 +540,7 @@ public class FieldSpacecraftState > *

* As a rough order of magnitude, the following table shows the extrapolation * errors obtained between this simple shift method and an {@link - * org.orekit.propagation.numerical.NumericalPropagator numerical + * org.orekit.propagation.numerical.FieldNumericalPropagator numerical * propagator} for a low Earth Sun Synchronous Orbit, with a 20x20 gravity field, * Sun and Moon third bodies attractions, drag and solar radiation pressure. * Beware that these results will be different for other orbits. @@ -369,11 +560,16 @@ public class FieldSpacecraftState > * except for the mass which stay unchanged */ public FieldSpacecraftState shiftedBy(final T dt) { - return new FieldSpacecraftState<>(orbit.shiftedBy(dt), attitude.shiftedBy(dt), - mass, additional); + if (absPva == null) { + return new FieldSpacecraftState<>(orbit.shiftedBy(dt), attitude.shiftedBy(dt), + mass, additional); + } else { + return new FieldSpacecraftState<>(absPva.shiftedBy(dt), attitude.shiftedBy(dt), + mass, additional); + } } - /** Get an interpolated instance. + /** {@inheritDoc} *

* The additional states that are interpolated are the ones already present * in the instance. The sample instances must therefore have at least the same @@ -381,20 +577,33 @@ public class FieldSpacecraftState > * but the extra ones will be ignored. *

*

+ * The instance and all the sample instances must be based on similar + * trajectory data, i.e. they must either all be based on orbits or all be based + * on absolute position-velocity-acceleration. Any inconsistency will trigger + * an {@link OrekitIllegalStateException}. + *

+ *

* As this implementation of interpolation is polynomial, it should be used only * with small samples (about 10-20 points) in order to avoid Runge's phenomenon * and numerical problems (including NaN appearing). *

- * @param date interpolation date - * @param sample sample points on which interpolation should be done - * @return a new instance, interpolated at specified date + * @exception OrekitIllegalStateException if some instances are not based on + * similar trajectory data */ public FieldSpacecraftState interpolate(final FieldAbsoluteDate date, final Stream> sample) { // prepare interpolators - final List> orbits = new ArrayList<>(); + final List> orbits; + final List> absPvas; + if (isOrbitDefined()) { + orbits = new ArrayList>(); + absPvas = null; + } else { + orbits = null; + absPvas = new ArrayList>(); + } final List> attitudes = new ArrayList<>(); final FieldHermiteInterpolator massInterpolator = new FieldHermiteInterpolator<>(); final Map> additionalInterpolators = @@ -406,7 +615,11 @@ public class FieldSpacecraftState > // extract sample data sample.forEach(state -> { final T deltaT = state.getDate().durationFrom(date); - orbits.add(state.getOrbit()); + if (isOrbitDefined()) { + orbits.add(state.getOrbit()); + } else { + absPvas.add(state.getAbsPVA()); + } attitudes.add(state.getAttitude()); final T[] mm = MathArrays.buildArray(orbit.getA().getField(), 1); mm[0] = state.getMass(); @@ -418,7 +631,15 @@ public class FieldSpacecraftState > }); // perform interpolations - final FieldOrbit interpolatedOrbit = orbit.interpolate(date, orbits); + final FieldOrbit interpolatedOrbit; + final FieldAbsolutePVCoordinates interpolatedAbsPva; + if (isOrbitDefined()) { + interpolatedOrbit = orbit.interpolate(date, orbits); + interpolatedAbsPva = null; + } else { + interpolatedOrbit = null; + interpolatedAbsPva = absPva.interpolate(date, absPvas); + } final FieldAttitude interpolatedAttitude = attitude.interpolate(date, attitudes); final T interpolatedMass = massInterpolator.value(orbit.getA().getField().getZero())[0]; final Map interpolatedAdditional; @@ -432,15 +653,52 @@ public class FieldSpacecraftState > } // create the complete interpolated state - return new FieldSpacecraftState<>(interpolatedOrbit, interpolatedAttitude, - interpolatedMass, interpolatedAdditional); + if (isOrbitDefined()) { + return new FieldSpacecraftState<>(interpolatedOrbit, interpolatedAttitude, + interpolatedMass, interpolatedAdditional); + } else { + return new FieldSpacecraftState<>(interpolatedAbsPva, interpolatedAttitude, + interpolatedMass, interpolatedAdditional); + } + + } + /** Get the absolute position-velocity-acceleration. + *

+ * A state contains either an {@link FieldAbsolutePVCoordinates absolute + * position-velocity-acceleration} or an {@link FieldOrbit orbit}. Which + * one is present can be checked using {@link #isOrbitDefined()}. + *

+ * @return absolute position-velocity-acceleration + * @exception OrekitIllegalStateException if position-velocity-acceleration is null, + * which mean the state rather contains an {@link FieldOrbit} + * @see #isOrbitDefined() + * @see #getOrbit() + */ + public FieldAbsolutePVCoordinates getAbsPVA() throws OrekitIllegalStateException { + if (absPva == null) { + throw new OrekitIllegalStateException(OrekitMessages.UNDEFINED_ABSOLUTE_PVCOORDINATES); + } + return absPva; } - /** Gets the current orbit. + /** Get the current orbit. + *

+ * A state contains either an {@link FieldAbsolutePVCoordinates absolute + * position-velocity-acceleration} or an {@link FieldOrbit orbit}. Which + * one is present can be checked using {@link #isOrbitDefined()}. + *

* @return the orbit - */ - public FieldOrbit getOrbit() { + * @exception OrekitIllegalStateException if orbit is null, + * which means the state rather contains an {@link FieldAbsolutePVCoordinates absolute + * position-velocity-acceleration} + * @see #isOrbitDefined() + * @see #getAbsPVA() + */ + public FieldOrbit getOrbit() throws OrekitIllegalStateException { + if (orbit == null) { + throw new OrekitIllegalStateException(OrekitMessages.UNDEFINED_ORBIT); + } return orbit; } @@ -448,16 +706,17 @@ public class FieldSpacecraftState > * @return date */ public FieldAbsoluteDate getDate() { - return orbit.getDate(); + return (absPva == null) ? orbit.getDate() : absPva.getDate(); } - /** Get the inertial frame. - * @return the frame + /** Get the defining frame. + * @return the frame in which state is defined */ public Frame getFrame() { - return orbit.getFrame(); + return (absPva == null) ? orbit.getFrame() : absPva.getFrame(); } + /** Check if an additional state is available. * @param name name of the additional state * @return true if the additional state is available @@ -530,118 +789,141 @@ public class FieldSpacecraftState > return Collections.unmodifiableMap(additional); } - /** Compute the transform from orbite/attitude reference frame to spacecraft frame. + /** Compute the transform from state defining frame to spacecraft frame. *

The spacecraft frame origin is at the point defined by the orbit, * and its orientation is defined by the attitude.

* @return transform from specified frame to current spacecraft frame */ public FieldTransform toTransform() { - final FieldAbsoluteDate date = orbit.getDate(); - return new FieldTransform<>(date, - new FieldTransform<>(date, orbit.getPVCoordinates().negate()), - new FieldTransform<>(date, attitude.getOrientation())); + final TimeStampedFieldPVCoordinates pv = getPVCoordinates(); + return new FieldTransform<>(pv.getDate(), + new FieldTransform<>(pv.getDate(), pv.negate()), + new FieldTransform<>(pv.getDate(), attitude.getOrientation())); } /** Get the central attraction coefficient. - * @return mu central attraction coefficient (m^3/s^2) + * @return mu central attraction coefficient (m^3/s^2), or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather than an orbit */ public T getMu() { - return orbit.getMu(); + return (absPva == null) ? orbit.getMu() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the Keplerian period. *

The Keplerian period is computed directly from semi major axis * and central acceleration constant.

- * @return Keplerian period in seconds + * @return keplerian period in seconds, or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit */ public T getKeplerianPeriod() { - return orbit.getKeplerianPeriod(); + return (absPva == null) ? orbit.getKeplerianPeriod() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the Keplerian mean motion. *

The Keplerian mean motion is computed directly from semi major axis * and central acceleration constant.

- * @return Keplerian mean motion in radians per second + * @return keplerian mean motion in radians per second, or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit */ public T getKeplerianMeanMotion() { - return orbit.getKeplerianMeanMotion(); + return (absPva == null) ? orbit.getKeplerianMeanMotion() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the semi-major axis. - * @return semi-major axis (m) + * @return semi-major axis (m), or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit */ public T getA() { - return orbit.getA(); + return (absPva == null) ? orbit.getA() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the first component of the eccentricity vector (as per equinoctial parameters). - * @return e cos(ω + Ω), first component of eccentricity vector + * @return e cos(ω + Ω), first component of eccentricity vector, or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getE() */ public T getEquinoctialEx() { - return orbit.getEquinoctialEx(); + return (absPva == null) ? orbit.getEquinoctialEx() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the second component of the eccentricity vector (as per equinoctial parameters). - * @return e sin(ω + Ω), second component of the eccentricity vector + * @return e sin(ω + Ω), second component of the eccentricity vector, or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getE() */ public T getEquinoctialEy() { - return orbit.getEquinoctialEy(); + return (absPva == null) ? orbit.getEquinoctialEy() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the first component of the inclination vector (as per equinoctial parameters). - * @return tan(i/2) cos(Ω), first component of the inclination vector + * @return tan(i/2) cos(Ω), first component of the inclination vector, or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getI() */ public T getHx() { - return orbit.getHx(); + return (absPva == null) ? orbit.getHx() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the second component of the inclination vector (as per equinoctial parameters). - * @return tan(i/2) sin(Ω), second component of the inclination vector + * @return tan(i/2) sin(Ω), second component of the inclination vector, or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getI() */ public T getHy() { - return orbit.getHy(); + return (absPva == null) ? orbit.getHy() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the true latitude argument (as per equinoctial parameters). - * @return v + ω + Ω true latitude argument (rad) + * @return v + ω + Ω true longitude argument (rad), or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getLE() * @see #getLM() */ public T getLv() { - return orbit.getLv(); + return (absPva == null) ? orbit.getLv() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the eccentric latitude argument (as per equinoctial parameters). - * @return E + ω + Ω eccentric latitude argument (rad) + * @return E + ω + Ω eccentric longitude argument (rad), or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getLv() * @see #getLM() */ public T getLE() { - return orbit.getLE(); + return (absPva == null) ? orbit.getLE() : absPva.getDate().getField().getZero().add(Double.NaN); } - /** Get the mean latitude argument (as per equinoctial parameters). - * @return M + ω + Ω mean latitude argument (rad) + /** Get the mean longitude argument (as per equinoctial parameters). + * @return M + ω + Ω mean latitude argument (rad), or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getLv() * @see #getLE() */ public T getLM() { - return orbit.getLM(); + return (absPva == null) ? orbit.getLM() : absPva.getDate().getField().getZero().add(Double.NaN); } // Additional orbital elements /** Get the eccentricity. - * @return eccentricity + * @return eccentricity, or {code Double.NaN} if the + * state is contains an absolute position-velocity-acceleration rather + * than an orbit * @see #getEquinoctialEx() * @see #getEquinoctialEy() */ public T getE() { - return orbit.getE(); + return (absPva == null) ? orbit.getE() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the inclination. @@ -650,32 +932,36 @@ public class FieldSpacecraftState > * @see #getHy() */ public T getI() { - return orbit.getI(); + return (absPva == null) ? orbit.getI() : absPva.getDate().getField().getZero().add(Double.NaN); } /** Get the {@link TimeStampedFieldPVCoordinates} in orbit definition frame. + *

* Compute the position and velocity of the satellite. This method caches its * results, and recompute them only when the method is called with a new value * for mu. The result is provided as a reference to the internally cached * {@link TimeStampedFieldPVCoordinates}, so the caller is responsible to copy it in a separate * {@link TimeStampedFieldPVCoordinates} if it needs to keep the value for a while. + *

* @return pvCoordinates in orbit definition frame */ public TimeStampedFieldPVCoordinates getPVCoordinates() { - return orbit.getPVCoordinates(); + return (absPva == null) ? orbit.getPVCoordinates() : absPva.getPVCoordinates(); } /** Get the {@link TimeStampedFieldPVCoordinates} in given output frame. + *

* Compute the position and velocity of the satellite. This method caches its * results, and recompute them only when the method is called with a new value * for mu. The result is provided as a reference to the internally cached * {@link TimeStampedFieldPVCoordinates}, so the caller is responsible to copy it in a separate * {@link TimeStampedFieldPVCoordinates} if it needs to keep the value for a while. + *

* @param outputFrame frame in which coordinates should be defined * @return pvCoordinates in orbit definition frame */ - public TimeStampedFieldPVCoordinates getPVCoordinates(final Frame outputFrame) { - return orbit.getPVCoordinates(outputFrame); + public TimeStampedFieldPVCoordinatesgetPVCoordinates(final Frame outputFrame) { + return (absPva == null) ? orbit.getPVCoordinates(outputFrame) : absPva.getPVCoordinates(outputFrame); } /** Get the attitude. @@ -710,7 +996,14 @@ public class FieldSpacecraftState > map.put(entry.getKey(), array); } } - return new SpacecraftState(orbit.toOrbit(), attitude.toAttitude(), mass.getReal(), map); + if (isOrbitDefined()) { + return new SpacecraftState(orbit.toOrbit(), attitude.toAttitude(), + mass.getReal(), map); + } else { + return new SpacecraftState(absPva.toAbsolutePVCoordinates(), + attitude.toAttitude(), mass.getReal(), + map); + } } @Override diff --git a/src/main/java/org/orekit/propagation/integration/FieldAbstractIntegratedPropagator.java b/src/main/java/org/orekit/propagation/integration/FieldAbstractIntegratedPropagator.java index f9032ca6f42f7bb52ca1ce6661964fbedeff21dc..84d2b7962ecad2957308773cf22946f838ff5ca3 100644 --- a/src/main/java/org/orekit/propagation/integration/FieldAbstractIntegratedPropagator.java +++ b/src/main/java/org/orekit/propagation/integration/FieldAbstractIntegratedPropagator.java @@ -46,7 +46,6 @@ import org.orekit.errors.OrekitException; import org.orekit.errors.OrekitIllegalStateException; import org.orekit.errors.OrekitMessages; import org.orekit.frames.Frame; -import org.orekit.orbits.FieldOrbit; import org.orekit.orbits.OrbitType; import org.orekit.orbits.PositionAngle; import org.orekit.propagation.FieldAbstractPropagator; @@ -449,9 +448,9 @@ public abstract class FieldAbstractIntegratedPropagator initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit()); + //final FieldOrbit initialOrbit = stateMapper.getOrbitType().convertType(getInitialState().getOrbit()); if (Double.isNaN(getMu().getReal())) { - setMu(initialOrbit.getMu()); + setMu(getInitialState().getMu()); } if (getInitialState().getMass().getReal() <= 0.0) { throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE, diff --git a/src/main/java/org/orekit/propagation/numerical/FieldNumericalPropagator.java b/src/main/java/org/orekit/propagation/numerical/FieldNumericalPropagator.java index 6e892943469d50149ca9519de8fff7fd6658ec00..64ea0e8410d6868c023b876319f8f7bfe1c8860f 100644 --- a/src/main/java/org/orekit/propagation/numerical/FieldNumericalPropagator.java +++ b/src/main/java/org/orekit/propagation/numerical/FieldNumericalPropagator.java @@ -25,6 +25,7 @@ import org.hipparchus.Field; import org.hipparchus.RealFieldElement; import org.hipparchus.geometry.euclidean.threed.FieldVector3D; import org.hipparchus.ode.FieldODEIntegrator; +import org.hipparchus.util.FastMath; import org.hipparchus.util.MathArrays; import org.orekit.attitudes.AttitudeProvider; import org.orekit.attitudes.FieldAttitude; @@ -46,6 +47,7 @@ import org.orekit.propagation.integration.FieldAbstractIntegratedPropagator; import org.orekit.propagation.integration.FieldStateMapper; import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; +import org.orekit.utils.FieldAbsolutePVCoordinates; import org.orekit.utils.FieldPVCoordinates; import org.orekit.utils.ParameterDriver; import org.orekit.utils.ParameterObserver; @@ -152,6 +154,9 @@ public class FieldNumericalPropagator> extends Fie /** Field used by this class.*/ private final Field field; + /** boolean to ignore or not the creation of a NewtonianAttraction. */ + private boolean ignoreCentralAttraction = false; + /** Create a new instance of NumericalPropagator, based on orbit definition mu. * After creation, the instance is empty, i.e. the attitude provider is set to an * unspecified default law and there are no perturbing forces at all. @@ -176,12 +181,26 @@ public class FieldNumericalPropagator> extends Fie setPositionAngleType(PositionAngle.TRUE); } + public void setIgnoreCentralAttraction(final boolean ignoreCentralAttraction) { + this.ignoreCentralAttraction = ignoreCentralAttraction; + } + /** Set the central attraction coefficient μ. + *

+ * Setting the central attraction coefficient is + * equivalent to {@link #addForceModel(ForceModel) add} + * a {@link NewtonianAttraction} force model. + *

* @param mu central attraction coefficient (m³/s²) * @see #addForceModel(ForceModel) + * @see #getAllForceModels() */ public void setMu(final T mu) { - addForceModel(new NewtonianAttraction(mu.getReal())); + if (ignoreCentralAttraction) { + superSetMu(mu); + } else { + addForceModel(new NewtonianAttraction(mu.getReal())); + } } /** Set the central attraction coefficient μ only in upper class. @@ -364,15 +383,48 @@ public class FieldNumericalPropagator> extends Fie if (mass.getReal() <= 0.0) { throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE, mass); } - final FieldOrbit orbit = super.getOrbitType().mapArrayToOrbit(y, yDot, super.getPositionAngleType(), date, getMu(), getFrame()); - final FieldAttitude attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame()); - return new FieldSpacecraftState<>(orbit, attitude, mass); + + if (getOrbitType() == null) { + // propagation uses absolute position-velocity-acceleration + final FieldVector3D p = new FieldVector3D<>(y[0], y[1], y[2]); + final FieldVector3D v = new FieldVector3D<>(y[3], y[4], y[5]); + final FieldVector3D a; + final FieldAbsolutePVCoordinates absPva; + if (yDot == null) { + absPva = new FieldAbsolutePVCoordinates<>(getFrame(), new TimeStampedFieldPVCoordinates<>(date, p, v, FieldVector3D.getZero(date.getField()))); + } else { + a = new FieldVector3D<>(yDot[3], yDot[4], yDot[5]); + absPva = new FieldAbsolutePVCoordinates<>(getFrame(), new TimeStampedFieldPVCoordinates<>(date, p, v, a)); + } + + final FieldAttitude attitude = getAttitudeProvider().getAttitude(absPva, date, getFrame()); + return new FieldSpacecraftState<>(absPva, attitude, mass); + } else { + // propagation uses regular orbits + final FieldOrbit orbit = super.getOrbitType().mapArrayToOrbit(y, yDot, super.getPositionAngleType(), date, getMu(), getFrame()); + final FieldAttitude attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame()); + return new FieldSpacecraftState<>(orbit, attitude, mass); + } } /** {@inheritDoc} */ public void mapStateToArray(final FieldSpacecraftState state, final T[] y, final T[] yDot) { - super.getOrbitType().mapOrbitToArray(state.getOrbit(), super.getPositionAngleType(), y, yDot); - y[6] = state.getMass(); + if (getOrbitType() == null) { + // propagation uses absolute position-velocity-acceleration + final FieldVector3D p = state.getAbsPVA().getPosition(); + final FieldVector3D v = state.getAbsPVA().getVelocity(); + y[0] = p.getX(); + y[1] = p.getY(); + y[2] = p.getZ(); + y[3] = v.getX(); + y[4] = v.getY(); + y[5] = v.getZ(); + y[6] = state.getMass(); + } + else { + super.getOrbitType().mapOrbitToArray(state.getOrbit(), super.getPositionAngleType(), y, yDot); + y[6] = state.getMass(); + } } } @@ -388,8 +440,8 @@ public class FieldNumericalPropagator> extends Fie /** Derivatives array. */ private final T[] yDot; - /** Current orbit. */ - private FieldOrbit orbit; + /** Current state. */ + private FieldSpacecraftState currentState; /** Jacobian of the orbital parameters with respect to the Cartesian parameters. */ private T[][] jacobian; @@ -405,6 +457,15 @@ public class FieldNumericalPropagator> extends Fie forceModel.getFieldEventsDetectors(getField()).forEach(detector -> setUpEventDetector(integrator, detector)); } + if (getOrbitType() == null) { + // propagation uses absolute position-velocity-acceleration + // we can set Jacobian once and for all + for (int i = 0; i < jacobian.length; ++i) { + Arrays.fill(jacobian[i], getField().getZero()); + jacobian[i][i] = getField().getOne(); + } + } + } /** {@inheritDoc} */ @@ -421,9 +482,12 @@ public class FieldNumericalPropagator> extends Fie @Override public T[] computeDerivatives(final FieldSpacecraftState state) { final T zero = state.getA().getField().getZero(); - orbit = state.getOrbit(); + currentState = state; Arrays.fill(yDot, zero); - orbit.getJacobianWrtCartesian(getPositionAngleType(), jacobian); + if (getOrbitType() != null) { + // propagation uses regular orbits + currentState.getOrbit().getJacobianWrtCartesian(getPositionAngleType(), jacobian); + } // compute the contributions of all perturbing forces, // using the Kepler contribution at the end since @@ -432,6 +496,15 @@ public class FieldNumericalPropagator> extends Fie forceModel.addContribution(state, this); } + if (getOrbitType() == null) { + // position derivative is velocity, and was not added above in the force models + // (it is added when orbit type is non-null because NewtonianAttraction considers it) + final FieldVector3D velocity = currentState.getPVCoordinates().getVelocity(); + yDot[0] = yDot[0].add(velocity.getX()); + yDot[1] = yDot[1].add(velocity.getY()); + yDot[2] = yDot[2].add(velocity.getZ()); + } + return yDot.clone(); } @@ -439,7 +512,23 @@ public class FieldNumericalPropagator> extends Fie /** {@inheritDoc} */ @Override public void addKeplerContribution(final T mu) { - orbit.addKeplerContribution(getPositionAngleType(), mu, yDot); + if (getOrbitType() == null) { + + // if mu is neither 0 nor NaN, we want to include Newtonian acceleration + if (mu.getReal() > 0) { + // velocity derivative is Newtonian acceleration + final FieldVector3D position = currentState.getPVCoordinates().getPosition(); + final double r2 = position.getNormSq().getReal(); + final double coeff = -mu.getReal() / (r2 * FastMath.sqrt(r2)); + yDot[3] = yDot[3].add(coeff * position.getX().getReal()); + yDot[4] = yDot[4].add(coeff * position.getY().getReal()); + yDot[5] = yDot[5].add(coeff * position.getZ().getReal()); + } + + } else { + // propagation uses regular orbits + currentState.getOrbit().addKeplerContribution(getPositionAngleType(), mu, yDot); + } } /** {@inheritDoc} */ diff --git a/src/main/java/org/orekit/utils/AbsolutePVCoordinates.java b/src/main/java/org/orekit/utils/AbsolutePVCoordinates.java index bde78515572737bd877770d03dcfe8e74f745e73..48024b0e8e8b5cc94ff2684f01789e34c4aad378 100644 --- a/src/main/java/org/orekit/utils/AbsolutePVCoordinates.java +++ b/src/main/java/org/orekit/utils/AbsolutePVCoordinates.java @@ -34,6 +34,8 @@ import org.orekit.time.AbsoluteDate; import org.orekit.time.TimeInterpolable; import org.orekit.time.TimeStamped; +/** Position - Velocity - Acceleration linked to a date and a frame. + */ public class AbsolutePVCoordinates extends TimeStampedPVCoordinates implements TimeStamped, TimeInterpolable, Serializable, PVCoordinatesProvider { diff --git a/src/main/java/org/orekit/utils/FieldAbsolutePVCoordinates.java b/src/main/java/org/orekit/utils/FieldAbsolutePVCoordinates.java new file mode 100644 index 0000000000000000000000000000000000000000..8ee3b481c29ff82a77b3702b56e8dfab93f0f5ca --- /dev/null +++ b/src/main/java/org/orekit/utils/FieldAbsolutePVCoordinates.java @@ -0,0 +1,395 @@ +/* Copyright 2002-2019 CS Systèmes d'Information + * Licensed to CS Systèmes d'Information (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.utils; + +import java.io.Serializable; +import java.util.stream.Stream; + +import org.hipparchus.RealFieldElement; +import org.hipparchus.analysis.differentiation.DerivativeStructure; +import org.hipparchus.analysis.differentiation.FieldDerivativeStructure; +import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitIllegalArgumentException; +import org.orekit.errors.OrekitInternalError; +import org.orekit.errors.OrekitMessages; +import org.orekit.frames.FieldTransform; +import org.orekit.frames.Frame; +import org.orekit.time.FieldAbsoluteDate; +import org.orekit.time.FieldTimeInterpolable; +import org.orekit.time.FieldTimeStamped; + +/** Field implementation of AbsolutePVCoordinates. + * @link AbsolutePVCoordinates + * @author Vincent Mouraux + */ +public class FieldAbsolutePVCoordinates> extends TimeStampedFieldPVCoordinates + implements FieldTimeStamped, FieldTimeInterpolable, T>, + Serializable, FieldPVCoordinatesProvider { + + /** Serializable UID. */ + private static final long serialVersionUID = 20190604L; + + /** Frame in which are defined the coordinates. */ + private final Frame frame; + + /** Build from position, velocity, acceleration. + * @param frame the frame in which the coordinates are defined + * @param date coordinates date + * @param position the position vector (m) + * @param velocity the velocity vector (m/s) + * @param acceleration the acceleration vector (m/sÂý) + */ + public FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate date, + final FieldVector3D position, final FieldVector3D velocity, final FieldVector3D acceleration) { + super(date, position, velocity, acceleration); + this.frame = frame; + } + + /** Build from position and velocity. Acceleration is set to zero. + * @param frame the frame in which the coordinates are defined + * @param date coordinates date + * @param position the position vector (m) + * @param velocity the velocity vector (m/s) + */ + public FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate date, + final FieldVector3D position, + final FieldVector3D velocity) { + this(frame, date, position, velocity, FieldVector3D.getZero(date.getField())); + } + + /** Build from frame, date and FieldPVA coordinates. + * @param frame the frame in which the coordinates are defined + * @param date date of the coordinates + * @param pva TimeStampedPVCoordinates + */ + public FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate date, final FieldPVCoordinates pva) { + super(date, pva); + this.frame = frame; + } + + /** Build from frame and TimeStampedFieldPVCoordinates. + * @param frame the frame in which the coordinates are defined + * @param pva TimeStampedFieldPVCoordinates + */ + public FieldAbsolutePVCoordinates(final Frame frame, final TimeStampedFieldPVCoordinates pva) { + super(pva.getDate(), pva); + this.frame = frame; + } + + /** Multiplicative constructor + *

Build a FieldAbsolutePVCoordinates from another one and a scale factor.

+ *

The TimeStampedFieldPVCoordinates built will be a * AbsPva

+ * @param date date of the built coordinates + * @param a scale factor + * @param AbsPva base (unscaled) FieldAbsolutePVCoordinates + */ + public FieldAbsolutePVCoordinates(final FieldAbsoluteDate date, + final T a, final FieldAbsolutePVCoordinates AbsPva) { + super(date, a, AbsPva); + this.frame = AbsPva.frame; + } + + /** Subtractive constructor + *

Build a relative FieldAbsolutePVCoordinates from a start and an end position.

+ *

The FieldAbsolutePVCoordinates built will be end - start.

+ *

In case start and end use two different pseudo-inertial frames, + * the new FieldAbsolutePVCoordinates arbitrarily be defined in the start frame.

+ * @param date date of the built coordinates + * @param start Starting FieldAbsolutePVCoordinates + * @param end ending FieldAbsolutePVCoordinates + */ + public FieldAbsolutePVCoordinates(final FieldAbsoluteDate date, + final FieldAbsolutePVCoordinates start, final FieldAbsolutePVCoordinates end) { + super(date, start, end); + ensureIdenticalFrames(start, end); + this.frame = start.frame; + } + + /** Linear constructor + *

Build a FieldAbsolutePVCoordinates from two other ones and corresponding scale factors.

+ *

The FieldAbsolutePVCoordinates built will be a1 * u1 + a2 * u2

+ *

In case the FieldAbsolutePVCoordinates use different pseudo-inertial frames, + * the new FieldAbsolutePVCoordinates arbitrarily be defined in the first frame.

+ * @param date date of the built coordinates + * @param a1 first scale factor + * @param absPv1 first base (unscaled) FieldAbsolutePVCoordinates + * @param a2 second scale factor + * @param absPv2 second base (unscaled) FieldAbsolutePVCoordinates + */ + public FieldAbsolutePVCoordinates(final FieldAbsoluteDate date, + final T a1, final FieldAbsolutePVCoordinates absPv1, + final T a2, final FieldAbsolutePVCoordinates absPv2) { + super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates()); + ensureIdenticalFrames(absPv1, absPv2); + this.frame = absPv1.getFrame(); + } + + /** Linear constructor + *

Build a FieldAbsolutePVCoordinates from three other ones and corresponding scale factors.

+ *

The FieldAbsolutePVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3

+ *

In case the FieldAbsolutePVCoordinates use different pseudo-inertial frames, + * the new FieldAbsolutePVCoordinates arbitrarily be defined in the first frame.

+ * @param date date of the built coordinates + * @param a1 first scale factor + * @param absPv1 first base (unscaled) FieldAbsolutePVCoordinates + * @param a2 second scale factor + * @param absPv2 second base (unscaled) FieldAbsolutePVCoordinates + * @param a3 third scale factor + * @param absPv3 third base (unscaled) FieldAbsolutePVCoordinates + */ + public FieldAbsolutePVCoordinates(final FieldAbsoluteDate date, + final T a1, final FieldAbsolutePVCoordinates absPv1, + final T a2, final FieldAbsolutePVCoordinates absPv2, + final T a3, final FieldAbsolutePVCoordinates absPv3) { + super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates(), + a3, absPv3.getPVCoordinates()); + ensureIdenticalFrames(absPv1, absPv2); + ensureIdenticalFrames(absPv1, absPv3); + this.frame = absPv1.getFrame(); + } + + /** Linear constructor + *

Build a FieldAbsolutePVCoordinates from four other ones and corresponding scale factors.

+ *

The FieldAbsolutePVCoordinates built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4

+ *

In case the FieldAbsolutePVCoordinates use different pseudo-inertial frames, + * the new AbsolutePVCoordinates arbitrarily be defined in the first frame.

+ * @param date date of the built coordinates + * @param a1 first scale factor + * @param absPv1 first base (unscaled) FieldAbsolutePVCoordinates + * @param a2 second scale factor + * @param absPv2 second base (unscaled) FieldAbsolutePVCoordinates + * @param a3 third scale factor + * @param absPv3 third base (unscaled) FieldAbsolutePVCoordinates + * @param a4 fourth scale factor + * @param absPv4 fourth base (unscaled) FieldAbsolutePVCoordinates + */ + public FieldAbsolutePVCoordinates(final FieldAbsoluteDate date, + final T a1, final FieldAbsolutePVCoordinates absPv1, + final T a2, final FieldAbsolutePVCoordinates absPv2, + final T a3, final FieldAbsolutePVCoordinates absPv3, + final T a4, final FieldAbsolutePVCoordinates absPv4) { + super(date, a1, absPv1.getPVCoordinates(), a2, absPv2.getPVCoordinates(), + a3, absPv3.getPVCoordinates(), a4, absPv4.getPVCoordinates()); + ensureIdenticalFrames(absPv1, absPv2); + ensureIdenticalFrames(absPv1, absPv3); + ensureIdenticalFrames(absPv1, absPv4); + this.frame = absPv1.getFrame(); + } + + /** Builds a FieldAbsolutePVCoordinates triplet from a {@link FieldVector3D}<{@link DerivativeStructure}>. + *

+ * The vector components must have time as their only derivation parameter and + * have consistent derivation orders. + *

+ * @param frame the frame in which the parameters are defined + * @param date date of the built coordinates + * @param p vector with time-derivatives embedded within the coordinates + */ + public FieldAbsolutePVCoordinates(final Frame frame, final FieldAbsoluteDate date, + final FieldVector3D> p) { + super(date, p); + this.frame = frame; + } + + /** Ensure that the frames from two FieldAbsolutePVCoordinates are identical. + * @param absPv1 first FieldAbsolutePVCoordinates + * @param absPv2 first FieldAbsolutePVCoordinates + * @param the type of the field elements + * @throws OrekitIllegalArgumentException if frames are different + */ + private static > void ensureIdenticalFrames(final FieldAbsolutePVCoordinates absPv1, final FieldAbsolutePVCoordinates absPv2) + throws OrekitIllegalArgumentException { + if (!absPv1.frame.equals(absPv2.frame)) { + throw new OrekitIllegalArgumentException(OrekitMessages.INCOMPATIBLE_FRAMES, + absPv1.frame.getName(), absPv2.frame.getName()); + } + } + + /** Get a time-shifted state. + *

+ * The state can be slightly shifted to close dates. This shift is based on + * a simple Taylor expansion. It is not intended as a replacement for + * proper orbit propagation (it is not even Keplerian!) but should be sufficient + * for either small time shifts or coarse accuracy. + *

+ * @param dt time shift in seconds + * @return a new state, shifted with respect to the instance (which is immutable) + */ + public FieldAbsolutePVCoordinates shiftedBy(final T dt) { + final TimeStampedFieldPVCoordinates spv = super.shiftedBy(dt); + return new FieldAbsolutePVCoordinates<>(frame, spv); + } + + /** Get a time-shifted state. + *

+ * The state can be slightly shifted to close dates. This shift is based on + * a simple Taylor expansion. It is not intended as a replacement for + * proper orbit propagation (it is not even Keplerian!) but should be sufficient + * for either small time shifts or coarse accuracy. + *

+ * @param dt time shift in seconds + * @return a new state, shifted with respect to the instance (which is immutable) + */ + public FieldAbsolutePVCoordinates shiftedBy(final double dt) { + final TimeStampedFieldPVCoordinates spv = super.shiftedBy(dt); + return new FieldAbsolutePVCoordinates<>(frame, spv); + } + + /** Create a local provider using simply Taylor expansion through {@link #shiftedBy(double)}. + *

+ * The time evolution is based on a simple Taylor expansion. It is not intended as a + * replacement for proper orbit propagation (it is not even Keplerian!) but should be sufficient + * for either small time shifts or coarse accuracy. + *

+ * @return provider based on Taylor expansion, for small time shifts around instance date + */ + public FieldPVCoordinatesProvider toTaylorProvider() { + return new FieldPVCoordinatesProvider() { + /** {@inheritDoc} */ + public TimeStampedFieldPVCoordinates getPVCoordinates(final FieldAbsoluteDate d, final Frame f) { + final TimeStampedFieldPVCoordinates shifted = shiftedBy(d.durationFrom(getDate())); + final FieldTransform transform = frame.getTransformTo(f, d); + return transform.transformPVCoordinates(shifted); + } + }; + } + + /** Get the frame in which the coordinates are defined. + * @return frame in which the coordinates are defined + */ + public Frame getFrame() { + return frame; + } + + /** Get the TimeStampedFieldPVCoordinates. + * @return TimeStampedFieldPVCoordinates + */ + public TimeStampedFieldPVCoordinates getPVCoordinates() { + return this; + } + + /** Get the TimeStampedFieldPVCoordinates in a specified frame. + * @param outputFrame frame in which the position/velocity coordinates shall be computed + * @return TimeStampedFieldPVCoordinates + * @exception OrekitException if transformation between frames cannot be computed + * @see #getPVCoordinates() + */ + public TimeStampedFieldPVCoordinates getPVCoordinates(final Frame outputFrame) { + // If output frame requested is the same as definition frame, + // PV coordinates are returned directly + if (outputFrame == frame) { + return getPVCoordinates(); + } + + // Else, PV coordinates are transformed to output frame + final FieldTransform t = frame.getTransformTo(outputFrame, getDate()); + return t.transformPVCoordinates(getPVCoordinates()); + } + + @Override + public TimeStampedFieldPVCoordinates getPVCoordinates(final FieldAbsoluteDate otherDate, final Frame outputFrame) { + return shiftedBy(otherDate.durationFrom(getDate())).getPVCoordinates(outputFrame); + } + + @Override + public FieldAbsolutePVCoordinates interpolate(final FieldAbsoluteDate date, final Stream> sample) { + return interpolate(getFrame(), date, CartesianDerivativesFilter.USE_PVA, sample); + } + + /** Interpolate position-velocity. + *

+ * The interpolated instance is created by polynomial Hermite interpolation + * ensuring velocity remains the exact derivative of position. + *

+ *

+ * Note that even if first time derivatives (velocities) + * from sample can be ignored, the interpolated instance always includes + * interpolated derivatives. This feature can be used explicitly to + * compute these derivatives when it would be too complex to compute them + * from an analytical formula: just compute a few sample points from the + * explicit formula and set the derivatives to zero in these sample points, + * then use interpolation to add derivatives consistent with the positions. + *

+ * @param frame frame for the interpolted instance + * @param date interpolation date + * @param filter filter for derivatives from the sample to use in interpolation + * @param sample sample points on which interpolation should be done + * @param the type of the field elements + * @return a new position-velocity, interpolated at specified date + * @exception OrekitIllegalArgumentException if some elements in the sample do not + * have the same defining frame as other + */ + public static > FieldAbsolutePVCoordinates interpolate(final Frame frame, final FieldAbsoluteDate date, + final CartesianDerivativesFilter filter, + final Stream> sample) { + + + // set up an interpolator taking derivatives into account + final FieldHermiteInterpolator interpolator = new FieldHermiteInterpolator<>(); + + // add sample points + switch (filter) { + case USE_P : + // populate sample with position data, ignoring velocity + sample.forEach(pv -> { + final FieldVector3D position = pv.getPosition(); + interpolator.addSamplePoint(pv.getDate().durationFrom(date), + position.toArray()); + }); + break; + case USE_PV : + // populate sample with position and velocity data + sample.forEach(pv -> { + final FieldVector3D position = pv.getPosition(); + final FieldVector3D velocity = pv.getVelocity(); + interpolator.addSamplePoint(pv.getDate().durationFrom(date), + position.toArray(), velocity.toArray()); + }); + break; + case USE_PVA : + // populate sample with position, velocity and acceleration data + sample.forEach(pv -> { + final FieldVector3D position = pv.getPosition(); + final FieldVector3D velocity = pv.getVelocity(); + final FieldVector3D acceleration = pv.getAcceleration(); + interpolator.addSamplePoint(pv.getDate().durationFrom(date), + position.toArray(), velocity.toArray(), acceleration.toArray()); + }); + break; + default : + // this should never happen + throw new OrekitInternalError(null); + } + + // interpolate + final T[][] p = interpolator.derivatives(date.getField().getZero(), 2); + + // build a new interpolated instance + return new FieldAbsolutePVCoordinates<>(frame, date, new FieldVector3D<>(p[0]), new FieldVector3D<>(p[1]), new FieldVector3D<>(p[2])); + } + + /** + * Converts to an AbsolutePVCoordinates instance. + * @return AbsolutePVCoordinates with same properties + */ + public AbsolutePVCoordinates toAbsolutePVCoordinates() { + return new AbsolutePVCoordinates(frame, this.getDate() + .toAbsoluteDate(), this.getPVCoordinates().toPVCoordinates()); + } +} diff --git a/src/test/java/org/orekit/forces/inertia/InertialForcesTest.java b/src/test/java/org/orekit/forces/inertia/InertialForcesTest.java index 53ea4b8b7d7e244a24c8128baf3bcfa0d5967ae5..afc35f239c8bd8bb559b6bd056287a2f37029419 100644 --- a/src/test/java/org/orekit/forces/inertia/InertialForcesTest.java +++ b/src/test/java/org/orekit/forces/inertia/InertialForcesTest.java @@ -3,15 +3,26 @@ package org.orekit.forces.inertia; import static org.junit.Assert.assertFalse; import org.hipparchus.Field; +import org.hipparchus.analysis.differentiation.DSFactory; import org.hipparchus.analysis.differentiation.DerivativeStructure; import org.hipparchus.geometry.euclidean.threed.FieldRotation; import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator; +import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator; +import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator; +import org.hipparchus.ode.nonstiff.DormandPrince853Integrator; +import org.hipparchus.random.GaussianRandomGenerator; +import org.hipparchus.random.RandomGenerator; +import org.hipparchus.random.UncorrelatedRandomVectorGenerator; +import org.hipparchus.random.Well19937a; import org.hipparchus.util.FastMath; import org.junit.Assert; import org.junit.Before; import org.junit.Test; import org.orekit.Utils; import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitIllegalArgumentException; import org.orekit.errors.OrekitMessages; import org.orekit.forces.AbstractLegacyForceModelTest; import org.orekit.forces.ForceModel; @@ -21,8 +32,11 @@ import org.orekit.frames.FramesFactory; import org.orekit.orbits.KeplerianOrbit; import org.orekit.orbits.Orbit; import org.orekit.orbits.PositionAngle; +import org.orekit.propagation.FieldSpacecraftState; import org.orekit.propagation.Propagator; import org.orekit.propagation.SpacecraftState; +import org.orekit.propagation.numerical.FieldNumericalPropagator; +import org.orekit.propagation.numerical.NumericalPropagator; import org.orekit.time.AbsoluteDate; import org.orekit.time.DateComponents; import org.orekit.time.FieldAbsoluteDate; @@ -30,6 +44,10 @@ import org.orekit.time.TimeComponents; import org.orekit.time.TimeScalesFactory; import org.orekit.utils.AbsolutePVCoordinates; import org.orekit.utils.Constants; +import org.orekit.utils.FieldAbsolutePVCoordinates; +import org.orekit.utils.FieldPVCoordinates; +import org.orekit.utils.IERSConventions; +import org.orekit.utils.PVCoordinates; public class InertialForcesTest extends AbstractLegacyForceModelTest { @@ -89,6 +107,181 @@ public class InertialForcesTest extends AbstractLegacyForceModelTest { Propagator.DEFAULT_LAW, 1.0e-50, false); } + + @Test + public void RealFieldTest() { + DSFactory factory = new DSFactory(6, 5); + DerivativeStructure fpx = factory.variable(0, 0.8); + DerivativeStructure fpy = factory.variable(1, 0.2); + DerivativeStructure fpz = factory.variable(2, 0.0); + DerivativeStructure fvx = factory.variable(3, 0.0); + DerivativeStructure fvy = factory.variable(4, 0.0); + DerivativeStructure fvz = factory.variable(5, 0.1); + + final FieldPVCoordinates initialConditions = + new FieldPVCoordinates<>(new FieldVector3D<>(fpx, fpy, fpz), + new FieldVector3D<>(fvx, fvy, fvz)); + + final double minStep = 0.00001; + final double maxstep = 3600.0; + + Field field = fpx.getField(); + DerivativeStructure zero = field.getZero(); + FieldAbsoluteDate J2000 = new FieldAbsoluteDate<>(field); + + Frame EME = FramesFactory.getEME2000(); + + // PVCoordinates linked to a Frame and a Date + final FieldAbsolutePVCoordinates initialAbsPV = + new FieldAbsolutePVCoordinates<>(EME, J2000, initialConditions); + + + FieldSpacecraftState initialState = new FieldSpacecraftState<>(initialAbsPV); + + SpacecraftState iSR = initialState.toSpacecraftState(); + + final double positionTolerance = 0.01; + final double velocityTolerance = 0.01; + final double massTolerance = 1.0e-6; + final double[] vecAbsoluteTolerances = + { + positionTolerance, positionTolerance, positionTolerance, + velocityTolerance, velocityTolerance, velocityTolerance, + massTolerance + }; + final double[] vecRelativeTolerances = + new double[vecAbsoluteTolerances.length]; + + AdaptiveStepsizeFieldIntegrator integrator = + new DormandPrince853FieldIntegrator<>(field,minStep, maxstep, + vecAbsoluteTolerances, + vecRelativeTolerances); + integrator.setInitialStepSize(zero.add(60)); + AdaptiveStepsizeIntegrator RIntegrator = + new DormandPrince853Integrator(minStep, maxstep, + vecAbsoluteTolerances, + vecRelativeTolerances); + RIntegrator.setInitialStepSize(60); + + FieldNumericalPropagator FNP = new FieldNumericalPropagator<>(field, integrator); + FNP.setOrbitType(null); + FNP.setIgnoreCentralAttraction(true); + FNP.setInitialState(initialState); + + NumericalPropagator NP = new NumericalPropagator(RIntegrator); + NP.setOrbitType(null); + NP.setIgnoreCentralAttraction(true); + NP.setInitialState(iSR); + + final InertialForces forceModel = new InertialForces(EME); + + FNP.addForceModel(forceModel); + NP.addForceModel(forceModel); + + + FieldAbsoluteDate target = J2000.shiftedBy(1000.); + FieldSpacecraftState finalState_DS = FNP.propagate(target); + SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate()); + FieldPVCoordinates finPVC_DS = finalState_DS.getPVCoordinates(); + PVCoordinates finPVC_R = finalState_R.getPVCoordinates(); + + Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getX(), finPVC_R.getPosition().getX(), FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11); + Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getY(), finPVC_R.getPosition().getY(), FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11); + Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getZ(), finPVC_R.getPosition().getZ(), FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11); + + long number = 23091991; + RandomGenerator RG = new Well19937a(number); + GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG); + UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }, + new double[] {1e3, 0.01, 0.01, 0.01, 0.01, 0.01}, + NGG); + double px_R = fpx.getReal(); + double py_R = fpy.getReal(); + double pz_R = fpz.getReal(); + double vx_R = fvx.getReal(); + double vy_R = fvy.getReal(); + double vz_R = fvz.getReal(); + double maxP = 0; + double maxV = 0; + double maxA = 0; + for (int ii = 0; ii < 1; ii++){ + double[] rand_next = URVG.nextVector(); + double px_shift = px_R + rand_next[0]; + double py_shift = py_R + rand_next[1]; + double pz_shift = pz_R + rand_next[2]; + double vx_shift = vx_R + rand_next[3]; + double vy_shift = vy_R + rand_next[4]; + double vz_shift = vz_R + rand_next[5]; + + final PVCoordinates shiftedConditions = + new PVCoordinates(new Vector3D(px_shift, py_shift, pz_shift), + new Vector3D(vx_shift, vy_shift, vz_shift)); + // PVCoordinates linked to a Frame and a Date + final AbsolutePVCoordinates shiftedAbsPV = + new AbsolutePVCoordinates(EME, J2000.toAbsoluteDate(), shiftedConditions); + + SpacecraftState shift_iSR = new SpacecraftState(shiftedAbsPV); + + + + NumericalPropagator shift_NP = new NumericalPropagator(RIntegrator); + + shift_NP.setInitialState(shift_iSR); + + shift_NP.setOrbitType(null); + shift_NP.setIgnoreCentralAttraction(true); + shift_NP.addForceModel(forceModel); + + + SpacecraftState finalState_shift = shift_NP.propagate(target.toAbsoluteDate()); + + + PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates(); + + //position check + FieldVector3D pos_DS = finPVC_DS.getPosition(); + double x_DS = pos_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double y_DS = pos_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double z_DS = pos_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double x = finPVC_shift.getPosition().getX(); + double y = finPVC_shift.getPosition().getY(); + double z = finPVC_shift.getPosition().getZ(); + maxP = FastMath.max(maxP, FastMath.abs((x_DS - x) / (x - pos_DS.getX().getReal()))); + maxP = FastMath.max(maxP, FastMath.abs((y_DS - y) / (y - pos_DS.getY().getReal()))); + maxP = FastMath.max(maxP, FastMath.abs((z_DS - z) / (z - pos_DS.getZ().getReal()))); + + // velocity check + FieldVector3D vel_DS = finPVC_DS.getVelocity(); + double vx_DS = vel_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double vy_DS = vel_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double vz_DS = vel_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double vx = finPVC_shift.getVelocity().getX(); + double vy = finPVC_shift.getVelocity().getY(); + double vz = finPVC_shift.getVelocity().getZ(); + maxV = FastMath.max(maxV, FastMath.abs((vx_DS - vx) / vx)); + maxV = FastMath.max(maxV, FastMath.abs((vy_DS - vy) / vy)); + maxV = FastMath.max(maxV, FastMath.abs((vz_DS - vz) / vz)); + + // acceleration check + FieldVector3D acc_DS = finPVC_DS.getAcceleration(); + double ax_DS = acc_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double ay_DS = acc_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double az_DS = acc_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]); + double ax = finPVC_shift.getAcceleration().getX(); + double ay = finPVC_shift.getAcceleration().getY(); + double az = finPVC_shift.getAcceleration().getZ(); + if (ax != 0 || ay !=0 || az != 0) { + maxA = FastMath.max(maxA, FastMath.abs((ax_DS - ax) / ax)); + maxA = FastMath.max(maxA, FastMath.abs((ay_DS - ay) / ay)); + maxA = FastMath.max(maxA, FastMath.abs((az_DS - az) / az)); + } else { + maxA = 0; + } + } + Assert.assertEquals(0, maxP, 1.1e-14); + Assert.assertEquals(0, maxV, 1.0e-15); + Assert.assertEquals(0, maxA, 1.0e-15); + } @Test public void testNoParametersDrivers() { @@ -111,6 +304,20 @@ public class InertialForcesTest extends AbstractLegacyForceModelTest { } } + @Test + public void testNonInertialFrame() { + try { + // ECEF frame + final Frame ecef = FramesFactory.getITRF(IERSConventions.IERS_2010, true); + // Initialize inertial force with a non-inertial frame + final InertialForces force = new InertialForces(ecef); + force.getParametersDrivers(); + } catch (OrekitIllegalArgumentException oe) { + Assert.assertEquals(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME_NOT_SUITABLE_AS_REFERENCE_FOR_INERTIAL_FORCES, + oe.getSpecifier()); + } + } + @Before public void setUp() { Utils.setDataRoot("regular-data"); diff --git a/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java b/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java index a7da98f873ae0b1494815242f05330f1b6ae0f4c..0da8e1fb31e07ca66d5c03d9b858237084310764 100644 --- a/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java +++ b/src/test/java/org/orekit/propagation/FieldSpacecraftStateTest.java @@ -17,7 +17,10 @@ package org.orekit.propagation; +import java.text.ParseException; +import java.util.ArrayList; import java.util.HashMap; +import java.util.List; import java.util.Map; import org.hipparchus.Field; @@ -28,6 +31,7 @@ import org.hipparchus.exception.MathIllegalStateException; import org.hipparchus.geometry.euclidean.threed.FieldRotation; import org.hipparchus.geometry.euclidean.threed.FieldVector3D; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator; import org.hipparchus.util.Decimal64Field; import org.hipparchus.util.FastMath; import org.hipparchus.util.MathArrays; @@ -50,12 +54,14 @@ import org.orekit.orbits.Orbit; import org.orekit.orbits.PositionAngle; import org.orekit.propagation.analytical.FieldEcksteinHechlerPropagator; import org.orekit.propagation.analytical.FieldKeplerianPropagator; +import org.orekit.propagation.numerical.FieldNumericalPropagator; import org.orekit.time.AbsoluteDate; import org.orekit.time.DateComponents; import org.orekit.time.FieldAbsoluteDate; import org.orekit.time.TimeComponents; import org.orekit.time.TimeScalesFactory; import org.orekit.utils.Constants; +import org.orekit.utils.FieldAbsolutePVCoordinates; import org.orekit.utils.FieldPVCoordinates; import org.orekit.utils.IERSConventions; import org.orekit.utils.PVCoordinates; @@ -97,6 +103,31 @@ public class FieldSpacecraftStateTest { public void testAdditionalStates() { doTestAdditionalStates(Decimal64Field.getInstance()); } + + @Test + public void testInterpolation() throws ParseException { + doTestInterpolation(Decimal64Field.getInstance()); + } + + @Test + public void testFieldVSRealAbsPV() { + doTestFieldVsRealAbsPV(Decimal64Field.getInstance()); + } + + @Test + public void testDateConsistencyCloseAbsPV() { + doTestDateConsistencyCloseAbsPV(Decimal64Field.getInstance()); + } + + @Test(expected=IllegalArgumentException.class) + public void testFramesConsistencyAbsPV() { + doTestFramesConsistencyAbsPV(Decimal64Field.getInstance()); + } + + @Test + public void testAdditionalStatesAbsPV() { + doTestAdditionalStatesAbsPV(Decimal64Field.getInstance()); + } private > void doTestFieldVsReal(final Field field) { T zero = field.getZero(); @@ -481,6 +512,301 @@ public class FieldSpacecraftStateTest { Assert.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15); } + + private > void doTestInterpolation(Field field) + throws ParseException, OrekitException { + checkInterpolationError( 2, 106.46533, 0.40709287, 169847806.33e-9, 0.0, 450 * 450, field); + checkInterpolationError( 3, 0.00353, 0.00003250, 189886.01e-9, 0.0, 0.0, field); + checkInterpolationError( 4, 0.00002, 0.00000023, 232.25e-9, 0.0, 0.0, field); + } + + private > void checkInterpolationError(int n, double expectedErrorP, double expectedErrorV, + double expectedErrorA, double expectedErrorM, double expectedErrorQ, Field field) + { + + T zero = field.getZero(); + T mu = zero.add(3.9860047e14); + double ae = 6.378137e6; + double c20 = -1.08263e-3; + double c30 = 2.54e-6; + double c40 = 1.62e-6; + double c50 = 2.3e-7; + double c60 = -5.5e-7; + + + T mass = zero.add(2500); + T a = zero.add(7187990.1979844316); + T e = zero.add(0.5e-4); + T i = zero.add(1.7105407051081795); + T omega = zero.add(1.9674147913622104); + T OMEGA = zero.add(FastMath.toRadians(261)); + T lv = zero; + + FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01), + TimeComponents.H00, + TimeScalesFactory.getUTC()); + + FieldKeplerianOrbit orbit = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE, + FramesFactory.getEME2000(), date, zero.add(mu)); + + BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth); + + FieldEcksteinHechlerPropagator propagator = new FieldEcksteinHechlerPropagator<>(orbit, attitudeLaw, mass, + ae, mu, c20, c30, c40, c50, c60); + FieldAbsoluteDate centerDate = orbit.getDate().shiftedBy(100.0); + FieldSpacecraftState centerState = propagator.propagate(centerDate).addAdditionalState("quadratic", zero); + List> sample = new ArrayList>(); + for (int ii = 0; ii < n; ++ii) { + double dt = ii * 900.0 / (n - 1); + FieldSpacecraftState state = propagator.propagate(centerDate.shiftedBy(dt)); + state = state.addAdditionalState("quadratic", zero.add(dt * dt)); + sample.add(state); + } + double maxErrorP = 0; + double maxErrorV = 0; + double maxErrorA = 0; + double maxErrorM = 0; + double maxErrorQ = 0; + for (double dt = 0; dt < 900.0; dt += 5) { + FieldSpacecraftState interpolated = centerState.interpolate(centerDate.shiftedBy(dt), sample); + FieldSpacecraftState propagated = propagator.propagate(centerDate.shiftedBy(dt)); + FieldPVCoordinates dpv = new FieldPVCoordinates<>(propagated.getPVCoordinates(), interpolated.getPVCoordinates()); + maxErrorP = FastMath.max(maxErrorP, dpv.getPosition().getNorm().getReal()); + maxErrorV = FastMath.max(maxErrorV, dpv.getVelocity().getNorm().getReal()); + maxErrorA = FastMath.max(maxErrorA, FastMath.toDegrees(FieldRotation.distance(interpolated.getAttitude().getRotation(), + propagated.getAttitude().getRotation()).getReal())); + maxErrorM = FastMath.max(maxErrorM, FastMath.abs(interpolated.getMass().getReal() - propagated.getMass().getReal())); + maxErrorQ = FastMath.max(maxErrorQ, FastMath.abs(interpolated.getAdditionalState("quadratic")[0].getReal() - dt * dt)); + } + Assert.assertEquals(expectedErrorP, maxErrorP, 1.0e-3); + Assert.assertEquals(expectedErrorV, maxErrorV, 1.0e-6); + Assert.assertEquals(expectedErrorA, maxErrorA, 4.0e-10); + Assert.assertEquals(expectedErrorM, maxErrorM, 1.0e-15); + Assert.assertEquals(expectedErrorQ, maxErrorQ, 2.0e-10); + } + + private > void doTestFieldVsRealAbsPV(final Field field) { + T zero = field.getZero(); + + T x_f = zero.add(0.8); + T y_f = zero.add(0.2); + T z_f = zero; + T vx_f = zero; + T vy_f = zero; + T vz_f = zero.add(0.1); + + FieldAbsoluteDate t_f = new FieldAbsoluteDate<>(field); + + FieldPVCoordinates pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f)); + + FieldAbsolutePVCoordinates absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), t_f, pva_f); + + FieldSpacecraftState ScS_f = new FieldSpacecraftState<>(absPV_f); + SpacecraftState ScS_r = ScS_f.toSpacecraftState(); + + for (double dt = 0; dt < 500; dt+=100){ + SpacecraftState control_r = ScS_r.shiftedBy(dt); + FieldSpacecraftState control_f = ScS_f.shiftedBy(zero.add(dt)); + + + Assert.assertEquals(control_r.getMu(), control_f.getMu().getReal(), 1e-10); + Assert.assertEquals(control_r.getI(), control_f.getI().getReal(), 1e-10); + Assert.assertEquals(control_r.getHx(), control_f.getHx().getReal(), 1e-10); + Assert.assertEquals(control_r.getHy(), control_f.getHy().getReal(), 1e-10); + Assert.assertEquals(control_r.getLv(), control_f.getLv().getReal(), 1e-10); + Assert.assertEquals(control_r.getLE(), control_f.getLE().getReal(), 1e-10); + Assert.assertEquals(control_r.getLM(), control_f.getLM().getReal(), 1e-10); + Assert.assertEquals(control_r.getKeplerianMeanMotion(), control_f.getKeplerianMeanMotion().getReal(), 1e-10); + Assert.assertEquals(control_r.getKeplerianPeriod(), control_f.getKeplerianPeriod().getReal(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getPosition().getX(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getX(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getPosition().getY(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getY(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getPosition().getZ(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getZ(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getX(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getX(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getY(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getY(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getZ(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getZ(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getX(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getX(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getY(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getY(), 1e-10); + Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getZ(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getZ(), 1e-10); + Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ0(), control_f.getAttitude().getOrientation().getRotation().getQ0().getReal(), 1e-10); + Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ1(), control_f.getAttitude().getOrientation().getRotation().getQ1().getReal(), 1e-10); + Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ2(), control_f.getAttitude().getOrientation().getRotation().getQ2().getReal(), 1e-10); + Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ3(), control_f.getAttitude().getOrientation().getRotation().getQ3().getReal(), 1e-10); + + Assert.assertEquals(control_r.getAttitude().getSpin().getAlpha(), control_f.getAttitude().getSpin().getAlpha().getReal(), 1e-10); + Assert.assertEquals(control_r.getAttitude().getSpin().getDelta(), control_f.getAttitude().getSpin().getDelta().getReal(), 1e-10); + Assert.assertEquals(control_r.getAttitude().getSpin().getNorm(), control_f.getAttitude().getSpin().getNorm().getReal(), 1e-10); + + Assert.assertEquals(control_r.getAttitude().getReferenceFrame().isPseudoInertial(), control_f.getAttitude().getReferenceFrame().isPseudoInertial()); + Assert.assertEquals(control_r.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH), control_f.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH).getReal(), 1e-10); + + + } + } + + + /** + * Check orbit and attitude dates can be off by a few ulps. I see this when using + * FixedRate attitude provider. + */ + private > void doTestDateConsistencyCloseAbsPV(final Field field) { + + + //setup + T zero = field.getZero(); + T one = field.getOne(); + T x_f = zero.add(0.8); + T y_f = zero.add(0.2); + T z_f = zero; + T vx_f = zero; + T vy_f = zero; + T vz_f = zero.add(0.1); + FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01), + TimeComponents.H00, + TimeScalesFactory.getUTC()); + FieldPVCoordinates pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f)); + + FieldAbsolutePVCoordinates absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f); + + FieldAbsolutePVCoordinates AbsolutePVCoordinates10Shifts = absPV_f; + for (int ii = 0; ii < 10; ii++) { + AbsolutePVCoordinates10Shifts = AbsolutePVCoordinates10Shifts.shiftedBy(zero.add(0.1)); + } + final FieldAbsolutePVCoordinates AbsolutePVCoordinates1Shift = absPV_f.shiftedBy(one); + + BodyCenterPointing attitudeLaw = new BodyCenterPointing(absPV_f.getFrame(), earth); + + FieldAttitude shiftedAttitude = attitudeLaw + .getAttitude(AbsolutePVCoordinates1Shift, AbsolutePVCoordinates1Shift.getDate(), absPV_f.getFrame()); + + //verify dates are very close, but not equal + Assert.assertNotEquals(shiftedAttitude.getDate(), AbsolutePVCoordinates10Shifts.getDate()); + Assert.assertEquals( + shiftedAttitude.getDate().durationFrom(AbsolutePVCoordinates10Shifts.getDate()).getReal(), + 0, Precision.EPSILON); + + //action + verify no exception is thrown + new FieldSpacecraftState<>(AbsolutePVCoordinates10Shifts, shiftedAttitude); + } + + + // (expected=IllegalArgumentException.class) + private > void doTestFramesConsistencyAbsPV(final Field field) { + + T zero = field.getZero(); + + T x_f = zero.add(0.8); + T y_f = zero.add(0.2); + T z_f = zero; + T vx_f = zero; + T vy_f = zero; + T vz_f = zero.add(0.1); + + + FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01), + TimeComponents.H00, + TimeScalesFactory.getUTC()); + + FieldPVCoordinates pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f)); + + FieldAbsolutePVCoordinates absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f); + + new FieldSpacecraftState<>(absPV_f, + new FieldAttitude<>(absPV_f.getDate(), + FramesFactory.getGCRF(), + FieldRotation.getIdentity(field), + FieldVector3D.getZero(field), + FieldVector3D.getZero(field))); + } + + private > void doTestAdditionalStatesAbsPV(final Field field) { + + T zero = field.getZero(); + T x_f = zero.add(0.8); + T y_f = zero.add(0.2); + T z_f = zero; + T vx_f = zero; + T vy_f = zero; + T vz_f = zero.add(0.1); + + FieldAbsoluteDate date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01), + TimeComponents.H00, + TimeScalesFactory.getUTC()); + + FieldPVCoordinates pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f)); + + FieldAbsolutePVCoordinates absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f); + + FieldNumericalPropagator prop = new FieldNumericalPropagator<>(field, + new DormandPrince853FieldIntegrator<>(field, 0.1, 500, 0.001, 0.001)); + prop.setOrbitType(null); + + final FieldSpacecraftState initialState = new FieldSpacecraftState<>(absPV_f); + + prop.resetInitialState(initialState); + + final FieldSpacecraftState state = prop.propagate(absPV_f.getDate().shiftedBy(60)); + T[] add = MathArrays.buildArray(field, 2); + add[0] = zero.add(1.); + add[1] = zero.add(2.); + final FieldSpacecraftState extended = + state. + addAdditionalState("test-1", add). + addAdditionalState("test-2", zero.add(42.0)); + Assert.assertEquals(0, state.getAdditionalStates().size()); + Assert.assertFalse(state.hasAdditionalState("test-1")); + try { + state.getAdditionalState("test-1"); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE); + Assert.assertEquals(oe.getParts()[0], "test-1"); + } + try { + state.ensureCompatibleAdditionalStates(extended); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE); + Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-")); + } + try { + extended.ensureCompatibleAdditionalStates(state); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE); + Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-")); + } + try { + T[] kk = MathArrays.buildArray(field, 7); + extended.ensureCompatibleAdditionalStates(extended.addAdditionalState("test-2", kk)); + Assert.fail("an exception should have been thrown"); + } catch (MathIllegalStateException mise) { + Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier()); + Assert.assertEquals(7, ((Integer) mise.getParts()[0]).intValue()); + } + Assert.assertEquals(2, extended.getAdditionalStates().size()); + Assert.assertTrue(extended.hasAdditionalState("test-1")); + Assert.assertTrue(extended.hasAdditionalState("test-2")); + Assert.assertEquals( 1.0, extended.getAdditionalState("test-1")[0].getReal(), 1.0e-15); + Assert.assertEquals( 2.0, extended.getAdditionalState("test-1")[1].getReal(), 1.0e-15); + Assert.assertEquals(42.0, extended.getAdditionalState("test-2")[0].getReal(), 1.0e-15); + + // test various constructors + T[] dd = MathArrays.buildArray(field, 1); + dd[0] = zero.add(-6.0); + Map map = new HashMap(); + map.put("test-3", dd); + FieldSpacecraftState sO = new FieldSpacecraftState<>(state.getAbsPVA(), map); + Assert.assertEquals(-6.0, sO.getAdditionalState("test-3")[0].getReal(), 1.0e-15); + FieldSpacecraftState sOA = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), map); + Assert.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0].getReal(), 1.0e-15); + FieldSpacecraftState sOM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getMass(), map); + Assert.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0].getReal(), 1.0e-15); + FieldSpacecraftState sOAM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), state.getMass(), map); + Assert.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0].getReal(), 1.0e-15); + FieldSpacecraftState sFromDouble = new FieldSpacecraftState<>(field, sOAM.toSpacecraftState()); + Assert.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15); + + } @Before public void setUp(){ diff --git a/src/test/java/org/orekit/propagation/SpacecraftStateTest.java b/src/test/java/org/orekit/propagation/SpacecraftStateTest.java index 55d0752e09c45dcfeb188b3aa0b99e151096768d..b3d104c66740436ad8f18c77ca63f634bae961b0 100644 --- a/src/test/java/org/orekit/propagation/SpacecraftStateTest.java +++ b/src/test/java/org/orekit/propagation/SpacecraftStateTest.java @@ -34,6 +34,7 @@ import org.hipparchus.exception.MathIllegalStateException; import org.hipparchus.geometry.euclidean.threed.Rotation; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator; +import org.hipparchus.ode.nonstiff.DormandPrince853Integrator; import org.hipparchus.util.FastMath; import org.hipparchus.util.Precision; import org.junit.After; @@ -408,6 +409,93 @@ public class SpacecraftStateTest { Assert.assertEquals(3.0, deserialized.getAdditionalState("p2")[2], 1.0e-15); } + + @Test + public void testAdditionalStatesAbsPV() { + + double x_f = 0.8; + double y_f = 0.2; + double z_f = 0; + double vx_f = 0; + double vy_f = 0; + double vz_f = 0.1; + + AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 01, 01), + TimeComponents.H00, + TimeScalesFactory.getUTC()); + + PVCoordinates pva_f = new PVCoordinates(new Vector3D(x_f,y_f,z_f), new Vector3D(vx_f,vy_f,vz_f)); + + AbsolutePVCoordinates absPV_f = new AbsolutePVCoordinates(FramesFactory.getEME2000(), date, pva_f); + + NumericalPropagator prop = new NumericalPropagator(new DormandPrince853Integrator(0.1, 500, 0.001, 0.001)); + prop.setOrbitType(null); + + final SpacecraftState initialState = new SpacecraftState(absPV_f); + + prop.resetInitialState(initialState); + + final SpacecraftState state = prop.propagate(absPV_f.getDate().shiftedBy(60)); + double[] add = new double[2]; + add[0] = 1.; + add[1] = 2.; + final SpacecraftState extended = + state. + addAdditionalState("test-1", add). + addAdditionalState("test-2", 42.0); + Assert.assertEquals(0, state.getAdditionalStates().size()); + Assert.assertFalse(state.hasAdditionalState("test-1")); + try { + state.getAdditionalState("test-1"); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE); + Assert.assertEquals(oe.getParts()[0], "test-1"); + } + try { + state.ensureCompatibleAdditionalStates(extended); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE); + Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-")); + } + try { + extended.ensureCompatibleAdditionalStates(state); + Assert.fail("an exception should have been thrown"); + } catch (OrekitException oe) { + Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE); + Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-")); + } + try { + double[] kk = new double[7]; + extended.ensureCompatibleAdditionalStates(extended.addAdditionalState("test-2", kk)); + Assert.fail("an exception should have been thrown"); + } catch (MathIllegalStateException mise) { + Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier()); + Assert.assertEquals(7, ((Integer) mise.getParts()[0]).intValue()); + } + Assert.assertEquals(2, extended.getAdditionalStates().size()); + Assert.assertTrue(extended.hasAdditionalState("test-1")); + Assert.assertTrue(extended.hasAdditionalState("test-2")); + Assert.assertEquals( 1.0, extended.getAdditionalState("test-1")[0], 1.0e-15); + Assert.assertEquals( 2.0, extended.getAdditionalState("test-1")[1], 1.0e-15); + Assert.assertEquals(42.0, extended.getAdditionalState("test-2")[0], 1.0e-15); + + // test various constructors + double[] dd = new double[1]; + dd[0] = -6.0; + Map map = new HashMap(); + map.put("test-3", dd); + SpacecraftState sO = new SpacecraftState(state.getAbsPVA(), map); + Assert.assertEquals(-6.0, sO.getAdditionalState("test-3")[0], 1.0e-15); + SpacecraftState sOA = new SpacecraftState(state.getAbsPVA(), state.getAttitude(), map); + Assert.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0], 1.0e-15); + SpacecraftState sOM = new SpacecraftState(state.getAbsPVA(), state.getMass(), map); + Assert.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0], 1.0e-15); + SpacecraftState sOAM = new SpacecraftState(state.getAbsPVA(), state.getAttitude(), state.getMass(), map); + Assert.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0], 1.0e-15); + + } @Before public void setUp() { diff --git a/src/test/java/org/orekit/utils/FieldAbsolutePVCoordinatesTest.java b/src/test/java/org/orekit/utils/FieldAbsolutePVCoordinatesTest.java new file mode 100644 index 0000000000000000000000000000000000000000..2e14baf577e0c609b739acf5a3051615066347ce --- /dev/null +++ b/src/test/java/org/orekit/utils/FieldAbsolutePVCoordinatesTest.java @@ -0,0 +1,469 @@ +/* Copyright 2002-2019 CS Systèmes d'Information + * Licensed to CS Systèmes d'Information (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.utils; + +import static org.junit.Assert.assertEquals; + +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +import org.hipparchus.Field; +import org.hipparchus.RealFieldElement; +import org.hipparchus.analysis.differentiation.FieldDerivativeStructure; +import org.hipparchus.analysis.polynomials.PolynomialFunction; +import org.hipparchus.geometry.euclidean.threed.FieldVector3D; +import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.util.Decimal64Field; +import org.hipparchus.util.FastMath; +import org.junit.Assert; +import org.junit.Before; +import org.junit.Test; +import org.orekit.Utils; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.time.FieldAbsoluteDate; + +public class FieldAbsolutePVCoordinatesTest { + + @Before + public void setUp() { + Utils.setDataRoot("regular-data"); + } + + @Test + public void TestPVOnlyConstructor() { + doTestPVOnlyConstructor(Decimal64Field.getInstance()); + } + + @Test + public void testPVCoordinatesCopyConstructor() { + doTestPVCoordinatesCopyConstructor(Decimal64Field.getInstance()); + } + + @Test + public void testLinearConstructors() { + doTestLinearConstructors(Decimal64Field.getInstance()); + } + + @Test + public void testToDerivativeStructureVector2() { + doTestToDerivativeStructureVector2(Decimal64Field.getInstance()); + } + + @Test + public void testShift() { + doTestShift(Decimal64Field.getInstance()); + } + + @Test + public void testToString() { + doTestToString(Decimal64Field.getInstance()); + } + + @Test + public void testInterpolatePolynomialPVA() { + doTestInterpolatePolynomialPVA(Decimal64Field.getInstance()); + } + + @Test + public void testInterpolatePolynomialPV() { + doTestInterpolatePolynomialPV(Decimal64Field.getInstance()); + } + + @Test + public void testInterpolatePolynomialPositionOnly() { + doTestInterpolatePolynomialPositionOnly(Decimal64Field.getInstance()); + } + + @Test + public void testInterpolateNonPolynomial() { + doTestInterpolateNonPolynomial(Decimal64Field.getInstance()); + } + + @Test + public void testSamePV() { + doTestSamePV(Decimal64Field.getInstance()); + } + + @Test + public void testTaylorProvider() { + doTestTaylorProvider(Decimal64Field.getInstance()); + } + + private > void doTestPVOnlyConstructor(Field field) { + //setup + FieldAbsoluteDate date = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + final T one = field.getOne(); + FieldVector3D p = new FieldVector3D<>(one, one.multiply(2.0), one.multiply(3.0)); + FieldVector3D v = new FieldVector3D<>(one.multiply(4.0), one.multiply(5.0), one.multiply(6.0)); + + //action + FieldAbsolutePVCoordinates actual = new FieldAbsolutePVCoordinates<>(frame, date, p, v); + + //verify + Assert.assertEquals(date, actual.getDate()); + Assert.assertEquals(1.0, actual.getPosition().getX().getReal(), 0.0); + Assert.assertEquals(2.0, actual.getPosition().getY().getReal(), 0.0); + Assert.assertEquals(3.0, actual.getPosition().getZ().getReal(), 0.0); + Assert.assertEquals(4.0, actual.getVelocity().getX().getReal(), 0.0); + Assert.assertEquals(5.0, actual.getVelocity().getY().getReal(), 0.0); + Assert.assertEquals(6.0, actual.getVelocity().getZ().getReal(), 0.0); + Assert.assertEquals(FieldVector3D.getZero(field), actual.getAcceleration()); + } + + private > void doTestPVCoordinatesCopyConstructor(Field field) { + //setup + FieldAbsoluteDate date = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + final T one = field.getOne(); + FieldPVCoordinates pv = new FieldPVCoordinates<>(new FieldVector3D<>(one, one.multiply(2), one.multiply(3)), new FieldVector3D<>(one.multiply(4), one.multiply(5), one.multiply(6))); + + //action + FieldAbsolutePVCoordinates actual = new FieldAbsolutePVCoordinates<>(frame, date, pv); + + //verify + Assert.assertEquals(date, actual.getDate()); + Assert.assertEquals(1.0, actual.getPosition().getX().getReal(), 0.0); + Assert.assertEquals(2.0, actual.getPosition().getY().getReal(), 0.0); + Assert.assertEquals(3.0, actual.getPosition().getZ().getReal(), 0.0); + Assert.assertEquals(4.0, actual.getVelocity().getX().getReal(), 0.0); + Assert.assertEquals(5.0, actual.getVelocity().getY().getReal(), 0.0); + Assert.assertEquals(6.0, actual.getVelocity().getZ().getReal(), 0.0); + Assert.assertEquals(FieldVector3D.getZero(field), actual.getAcceleration()); + } + + private > void doTestLinearConstructors(Field field) { + Frame frame = FramesFactory.getEME2000(); + final T one = field.getOne(); + FieldAbsolutePVCoordinates pv1 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getCCSDSEpoch(field), + new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)), + new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)), + new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))); + FieldAbsolutePVCoordinates pv2 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getFiftiesEpoch(field), + new FieldVector3D<>(one.multiply(2.0), one.multiply(0.2), one.multiply(20.0)), + new FieldVector3D<>(one.multiply(-2.0), one.multiply(-0.2), one.multiply(-20.0)), + new FieldVector3D<>(one.multiply(20.0), one.multiply(-2.0), one.multiply(-200.0))); + FieldAbsolutePVCoordinates pv3 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getGalileoEpoch(field), + new FieldVector3D<>(one.multiply(3.0), one.multiply(0.3), one.multiply(30.0)), + new FieldVector3D<>(one.multiply(-3.0), one.multiply(-0.3), one.multiply(-30.0)), + new FieldVector3D<>(one.multiply(30.0), one.multiply(-3.0), one.multiply(-300.0))); + FieldAbsolutePVCoordinates pv4 = new FieldAbsolutePVCoordinates<>(frame,FieldAbsoluteDate.getJulianEpoch(field), + new FieldVector3D<>(one.multiply(4.0), one.multiply(0.4), one.multiply(40.0)), + new FieldVector3D<>(one.multiply(-4.0), one.multiply(-0.4), one.multiply(-40.0)), + new FieldVector3D<>(one.multiply(40.0), one.multiply(-4.0), one.multiply(-400.0))); + checkPV(pv4, new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getJulianEpoch(field), one.multiply(4.0), pv1), 1.0e-15); + checkPV(pv2, new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getFiftiesEpoch(field), pv1, pv3), 1.0e-15); + checkPV(pv3, new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getGalileoEpoch(field), one, pv1, one, pv2), 1.0e-15); + checkPV(new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(2.0), pv4), + new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(3.0), pv1, one, pv2, one, pv3), + 1.0e-15); + checkPV(new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(3.0), pv3), + new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(3.0), pv1, one, pv2, one, pv4), + 1.0e-15); + checkPV(new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(5.0), pv4), + new FieldAbsolutePVCoordinates(FieldAbsoluteDate.getJ2000Epoch(field), one.multiply(4.0), pv1, one.multiply(3.0), pv2, one.multiply(2.0), pv3, one, pv4), + 1.0e-15); + } + + private > void doTestToDerivativeStructureVector2(Field field) { + final T one = field.getOne(); + FieldVector3D> fv = + new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), + FieldAbsoluteDate.getGalileoEpoch(field), + new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)), + new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)), + new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))).toDerivativeStructureVector(2); + + Assert.assertEquals(1, fv.getX().getFreeParameters()); + Assert.assertEquals(2, fv.getX().getOrder()); + Assert.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10); + Assert.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10); + Assert.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10); + Assert.assertEquals( -1.0, fv.getX().getPartialDerivative(1).getReal(), 1.0e-15); + Assert.assertEquals( -0.1, fv.getY().getPartialDerivative(1).getReal(), 1.0e-15); + Assert.assertEquals( -10.0, fv.getZ().getPartialDerivative(1).getReal(), 1.0e-15); + Assert.assertEquals( 10.0, fv.getX().getPartialDerivative(2).getReal(), 1.0e-15); + Assert.assertEquals( -1.0, fv.getY().getPartialDerivative(2).getReal(), 1.0e-15); + Assert.assertEquals(-100.0, fv.getZ().getPartialDerivative(2).getReal(), 1.0e-15); + checkPV(new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), + FieldAbsoluteDate.getGalileoEpoch(field), + new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)), + new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)), + new FieldVector3D<>(one.multiply(10.0), one.multiply(-1.0), one.multiply(-100.0))), + new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), + FieldAbsoluteDate.getGalileoEpoch(field), fv), 1.0e-15); + + for (double dt = 0; dt < 10; dt += 0.125) { + Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10), + new Vector3D(-1, -0.1, -10), + new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition(); + Assert.assertEquals(p.getX(), fv.getX().taylor(dt).getReal(), 1.0e-14); + Assert.assertEquals(p.getY(), fv.getY().taylor(dt).getReal(), 1.0e-14); + Assert.assertEquals(p.getZ(), fv.getZ().taylor(dt).getReal(), 1.0e-14); + } + } + + + private > void doTestShift(Field field) { + final T one = field.getOne(); + FieldVector3D p1 = new FieldVector3D<>(one, one.multiply(0.1), one.multiply(10.0)); + FieldVector3D v1 = new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10)); + FieldVector3D a1 = new FieldVector3D<>(one.multiply(10.0), one, one.multiply(100.0)); + FieldVector3D p2 = new FieldVector3D<>(one.multiply(7.0), one.multiply(0.7), one.multiply(70.0)); + FieldVector3D v2 = new FieldVector3D<>(one.multiply(-11.0), one.multiply(-1.1), one.multiply(-110.0)); + FieldVector3D a2 = new FieldVector3D<>(one.multiply(10.0), one, one.multiply(100.0)); + checkPV(new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field), p2, v2, a2), + new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field).shiftedBy(1.0), p1, v1, a1).shiftedBy(one.multiply(-1.0)), 1.0e-15); + Assert.assertEquals(0.0, FieldAbsolutePVCoordinates.estimateVelocity(p1, p2, -1.0).subtract(new Vector3D(-6, -0.6, -60)).getNorm().getReal(), 1.0e-15); + } + + private > void doTestToString(Field field) { + final T one = field.getOne(); + FieldAbsolutePVCoordinates pv = + new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), + FieldAbsoluteDate.getJ2000Epoch(field), + new FieldVector3D<>(one.multiply(1.0), one.multiply(0.1), one.multiply(10.0)), + new FieldVector3D<>(one.multiply(-1.0), one.multiply(-0.1), one.multiply(-10.0)), + new FieldVector3D<>(one.multiply(10.0), one.multiply(1.0), one.multiply(100.0))); + Assert.assertEquals("{2000-01-01T11:58:55.816, P(1.0, 0.1, 10.0), V(-1.0, -0.1, -10.0), A(10.0, 1.0, 100.0)}", pv.toString()); + } + + + private > void doTestInterpolatePolynomialPVA(Field field) { + final T one = field.getOne(); + Random random = new Random(0xfe3945fcb8bf47cel); + FieldAbsoluteDate t0 = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + for (int i = 0; i < 20; ++i) { + + PolynomialFunction px = randomPolynomial(5, random); + PolynomialFunction py = randomPolynomial(5, random); + PolynomialFunction pz = randomPolynomial(5, random); + PolynomialFunction pxDot = px.polynomialDerivative(); + PolynomialFunction pyDot = py.polynomialDerivative(); + PolynomialFunction pzDot = pz.polynomialDerivative(); + PolynomialFunction pxDotDot = pxDot.polynomialDerivative(); + PolynomialFunction pyDotDot = pyDot.polynomialDerivative(); + PolynomialFunction pzDotDot = pzDot.polynomialDerivative(); + + List> sample = new ArrayList>(); + for (double dt : new double[] { 0.0, 0.5, 1.0 }) { + FieldVector3D position = new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)), one.multiply(pz.value(dt))); + FieldVector3D velocity = new FieldVector3D<>(one.multiply(pxDot.value(dt)), one.multiply(pyDot.value(dt)), one.multiply(pzDot.value(dt))); + FieldVector3D acceleration = new FieldVector3D<>(one.multiply(pxDotDot.value(dt)), one.multiply(pyDotDot.value(dt)), one.multiply(pzDotDot.value(dt))); + sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity, acceleration)); + } + + for (double dt = 0; dt < 1.0; dt += 0.01) { + FieldAbsolutePVCoordinates interpolated = + FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(one.multiply(dt)), CartesianDerivativesFilter.USE_PVA, sample.stream()); + FieldVector3D p = interpolated.getPosition(); + FieldVector3D v = interpolated.getVelocity(); + FieldVector3D a = interpolated.getAcceleration(); + Assert.assertEquals(px.value(dt), p.getX().getReal(), 4.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(py.value(dt), p.getY().getReal(), 4.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 4.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 9.0e-16 * v.getNorm().getReal()); + Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 9.0e-16 * v.getNorm().getReal()); + Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 9.0e-16 * v.getNorm().getReal()); + Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 9.0e-15 * a.getNorm().getReal()); + Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 9.0e-15 * a.getNorm().getReal()); + Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 9.0e-15 * a.getNorm().getReal()); + } + + } + + } + + private > void doTestInterpolatePolynomialPV(Field field) { + final T one = field.getOne(); + Random random = new Random(0xae7771c9933407bdl); + FieldAbsoluteDate t0 = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + for (int i = 0; i < 20; ++i) { + + PolynomialFunction px = randomPolynomial(5, random); + PolynomialFunction py = randomPolynomial(5, random); + PolynomialFunction pz = randomPolynomial(5, random); + PolynomialFunction pxDot = px.polynomialDerivative(); + PolynomialFunction pyDot = py.polynomialDerivative(); + PolynomialFunction pzDot = pz.polynomialDerivative(); + PolynomialFunction pxDotDot = pxDot.polynomialDerivative(); + PolynomialFunction pyDotDot = pyDot.polynomialDerivative(); + PolynomialFunction pzDotDot = pzDot.polynomialDerivative(); + + List> sample = new ArrayList>(); + for (double dt : new double[] { 0.0, 0.5, 1.0 }) { + FieldVector3D position = new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)), one.multiply(pz.value(dt))); + FieldVector3D velocity = new FieldVector3D<>(one.multiply(pxDot.value(dt)), one.multiply(pyDot.value(dt)), one.multiply(pzDot.value(dt))); + sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity, FieldVector3D.getZero(field))); + } + + for (double dt = 0; dt < 1.0; dt += 0.01) { + FieldAbsolutePVCoordinates interpolated = + FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(dt), CartesianDerivativesFilter.USE_PV, sample.stream()); + FieldVector3D p = interpolated.getPosition(); + FieldVector3D v = interpolated.getVelocity(); + FieldVector3D a = interpolated.getAcceleration(); + Assert.assertEquals(px.value(dt), p.getX().getReal(), 4.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(py.value(dt), p.getY().getReal(), 4.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 4.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 9.0e-16 * v.getNorm().getReal()); + Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 9.0e-16 * v.getNorm().getReal()); + Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 9.0e-16 * v.getNorm().getReal()); + Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 1.0e-14 * a.getNorm().getReal()); + Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 1.0e-14 * a.getNorm().getReal()); + Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 1.0e-14 * a.getNorm().getReal()); + } + + } + + } + + + private > void doTestInterpolatePolynomialPositionOnly(Field field) { + final T one = field.getOne(); + Random random = new Random(0x88740a12e4299003l); + FieldAbsoluteDate t0 = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + for (int i = 0; i < 20; ++i) { + + PolynomialFunction px = randomPolynomial(5, random); + PolynomialFunction py = randomPolynomial(5, random); + PolynomialFunction pz = randomPolynomial(5, random); + PolynomialFunction pxDot = px.polynomialDerivative(); + PolynomialFunction pyDot = py.polynomialDerivative(); + PolynomialFunction pzDot = pz.polynomialDerivative(); + PolynomialFunction pxDotDot = pxDot.polynomialDerivative(); + PolynomialFunction pyDotDot = pyDot.polynomialDerivative(); + PolynomialFunction pzDotDot = pzDot.polynomialDerivative(); + + List> sample = new ArrayList>(); + for (double dt : new double[] { 0.0, 0.2, 0.4, 0.6, 0.8, 1.0 }) { + FieldVector3Dposition = new FieldVector3D<>(one.multiply(px.value(dt)), one.multiply(py.value(dt)), one.multiply(pz.value(dt))); + sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, FieldVector3D.getZero(field), FieldVector3D.getZero(field))); + } + + for (double dt = 0; dt < 1.0; dt += 0.01) { + FieldAbsolutePVCoordinates interpolated = + FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(one.multiply(dt)), CartesianDerivativesFilter.USE_P, sample.stream()); + FieldVector3D p = interpolated.getPosition(); + FieldVector3D v = interpolated.getVelocity(); + FieldVector3D a = interpolated.getAcceleration(); + Assert.assertEquals(px.value(dt), p.getX().getReal(), 5.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(py.value(dt), p.getY().getReal(), 5.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(pz.value(dt), p.getZ().getReal(), 5.0e-16 * p.getNorm().getReal()); + Assert.assertEquals(pxDot.value(dt), v.getX().getReal(), 7.0e-15 * v.getNorm().getReal()); + Assert.assertEquals(pyDot.value(dt), v.getY().getReal(), 7.0e-15 * v.getNorm().getReal()); + Assert.assertEquals(pzDot.value(dt), v.getZ().getReal(), 7.0e-15 * v.getNorm().getReal()); + Assert.assertEquals(pxDotDot.value(dt), a.getX().getReal(), 2.0e-13 * a.getNorm().getReal()); + Assert.assertEquals(pyDotDot.value(dt), a.getY().getReal(), 2.0e-13 * a.getNorm().getReal()); + Assert.assertEquals(pzDotDot.value(dt), a.getZ().getReal(), 2.0e-13 * a.getNorm().getReal()); + } + + } + } + + private > void doTestInterpolateNonPolynomial(Field field) { + final T one = field.getOne(); + FieldAbsoluteDate t0 = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + + List> sample = new ArrayList>(); + for (double dt : new double[] { 0.0, 0.5, 1.0 }) { + FieldVector3D position = new FieldVector3D<>( one.multiply(FastMath.cos(dt)), one.multiply(FastMath.sin(dt)), one.multiply(0.0)); + FieldVector3D velocity = new FieldVector3D<>( one.multiply(-FastMath.sin(dt)), one.multiply(FastMath.cos(dt)), one.multiply(0.0)); + FieldVector3D acceleration = new FieldVector3D<>( one.multiply(-FastMath.cos(dt)), one.multiply(-FastMath.sin(dt)), one.multiply(0.0)); + sample.add(new FieldAbsolutePVCoordinates<>(frame, t0.shiftedBy(one.multiply(dt)), position, velocity, acceleration)); + } + + for (double dt = 0; dt < 1.0; dt += 0.01) { + FieldAbsolutePVCoordinates interpolated = + FieldAbsolutePVCoordinates.interpolate(frame, t0.shiftedBy(one.multiply(dt)), CartesianDerivativesFilter.USE_PVA, sample.stream()); + FieldVector3D p = interpolated.getPosition(); + FieldVector3D v = interpolated.getVelocity(); + FieldVector3D a = interpolated.getAcceleration(); + Assert.assertEquals( FastMath.cos(dt), p.getX().getReal(), 3.0e-10 * p.getNorm().getReal()); + Assert.assertEquals( FastMath.sin(dt), p.getY().getReal(), 3.0e-10 * p.getNorm().getReal()); + Assert.assertEquals(0, p.getZ().getReal(), 3.0e-10 * p.getNorm().getReal()); + Assert.assertEquals(-FastMath.sin(dt), v.getX().getReal(), 3.0e-9 * v.getNorm().getReal()); + Assert.assertEquals( FastMath.cos(dt), v.getY().getReal(), 3.0e-9 * v.getNorm().getReal()); + Assert.assertEquals(0, v.getZ().getReal(), 3.0e-9 * v.getNorm().getReal()); + Assert.assertEquals(-FastMath.cos(dt), a.getX().getReal(), 4.0e-8 * a.getNorm().getReal()); + Assert.assertEquals(-FastMath.sin(dt), a.getY().getReal(), 4.0e-8 * a.getNorm().getReal()); + Assert.assertEquals(0, a.getZ().getReal(), 4.0e-8 * a.getNorm().getReal()); + } + + } + + private > void doTestSamePV(Field field) { + //setup + final T one = field.getOne(); + FieldAbsoluteDate date = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + FieldVector3D p = new FieldVector3D<>(one.multiply(1), one.multiply(2), one.multiply(3)); + FieldVector3D v = new FieldVector3D<>(one.multiply(4), one.multiply(5), one.multiply(6)); + + //action + FieldAbsolutePVCoordinates actual = new FieldAbsolutePVCoordinates<>(frame, date, p, v); + + //verify + assertEquals(actual.getPVCoordinates().toString(), actual.getPVCoordinates(frame).toString()); + assertEquals(actual.getPVCoordinates(frame).toString(), actual.getPVCoordinates(date, frame).toString()); + } + + + private > void doTestTaylorProvider(Field field) { + //setup + final T one = field.getOne(); + FieldAbsoluteDate date = FieldAbsoluteDate.getJ2000Epoch(field); + Frame frame = FramesFactory.getEME2000(); + FieldVector3D p = new FieldVector3D<>(one.multiply(1), one.multiply(2), one.multiply(3)); + FieldVector3D v = new FieldVector3D<>(one.multiply(4), one.multiply(5), one.multiply(6)); + + //action + FieldAbsolutePVCoordinates actual = new FieldAbsolutePVCoordinates<>(frame, date, p, v); + final FieldPVCoordinatesProvider pv = actual.toTaylorProvider(); + + //verify + Assert.assertEquals(actual.getPVCoordinates(date, frame).toString(), pv.getPVCoordinates(date, frame).toString()); + } + + private PolynomialFunction randomPolynomial(int degree, Random random) { + double[] coeff = new double[ 1 + degree]; + for (int j = 0; j < degree; ++j) { + coeff[j] = random.nextDouble(); + } + return new PolynomialFunction(coeff); + } + + private > void checkPV(FieldAbsolutePVCoordinates expected, FieldAbsolutePVCoordinates real, double epsilon) { + Assert.assertEquals(expected.getDate(), real.getDate()); + Assert.assertEquals(expected.getPosition().getX().getReal(), real.getPosition().getX().getReal(), epsilon); + Assert.assertEquals(expected.getPosition().getY().getReal(), real.getPosition().getY().getReal(), epsilon); + Assert.assertEquals(expected.getPosition().getZ().getReal(), real.getPosition().getZ().getReal(), epsilon); + Assert.assertEquals(expected.getVelocity().getX().getReal(), real.getVelocity().getX().getReal(), epsilon); + Assert.assertEquals(expected.getVelocity().getY().getReal(), real.getVelocity().getY().getReal(), epsilon); + Assert.assertEquals(expected.getVelocity().getZ().getReal(), real.getVelocity().getZ().getReal(), epsilon); + Assert.assertEquals(expected.getAcceleration().getX().getReal(), real.getAcceleration().getX().getReal(), epsilon); + Assert.assertEquals(expected.getAcceleration().getY().getReal(), real.getAcceleration().getY().getReal(), epsilon); + Assert.assertEquals(expected.getAcceleration().getZ().getReal(), real.getAcceleration().getZ().getReal(), epsilon); + } + +}