1 /* Copyright 2002-2019 CS Systèmes d'Information 2 * Licensed to CS Systèmes d'Information (CS) under one or more 3 * contributor license agreements. See the NOTICE file distributed with 4 * this work for additional information regarding copyright ownership. 5 * CS licenses this file to You under the Apache License, Version 2.0 6 * (the "License"); you may not use this file except in compliance with 7 * the License. You may obtain a copy of the License at 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 */ 17 package org.orekit.utils; 18 19 import java.util.Collection; 20 21 import org.hipparchus.analysis.differentiation.DerivativeStructure; 22 import org.hipparchus.analysis.interpolation.HermiteInterpolator; 23 import org.hipparchus.geometry.euclidean.threed.FieldRotation; 24 import org.hipparchus.geometry.euclidean.threed.Rotation; 25 import org.hipparchus.geometry.euclidean.threed.RotationConvention; 26 import org.hipparchus.geometry.euclidean.threed.Vector3D; 27 import org.hipparchus.util.FastMath; 28 import org.hipparchus.util.MathArrays; 29 import org.orekit.errors.OrekitException; 30 import org.orekit.errors.OrekitInternalError; 31 import org.orekit.errors.OrekitMessages; 32 import org.orekit.time.AbsoluteDate; 33 import org.orekit.time.TimeStamped; 34 35 /** {@link TimeStamped time-stamped} version of {@link AngularCoordinates}. 36 * <p>Instances of this class are guaranteed to be immutable.</p> 37 * @author Luc Maisonobe 38 * @since 7.0 39 */ 40 public class TimeStampedAngularCoordinates extends AngularCoordinates implements TimeStamped { 41 42 /** Serializable UID. */ 43 private static final long serialVersionUID = 20140723L; 44 45 /** The date. */ 46 private final AbsoluteDate date; 47 48 /** Builds a rotation/rotation rate pair. 49 * @param date coordinates date 50 * @param rotation rotation 51 * @param rotationRate rotation rate Ω (rad/s) 52 * @param rotationAcceleration rotation acceleration dΩ/dt (rad²/s²) 53 */ 54 public TimeStampedAngularCoordinates(final AbsoluteDate date, 55 final Rotation rotation, 56 final Vector3D rotationRate, 57 final Vector3D rotationAcceleration) { 58 super(rotation, rotationRate, rotationAcceleration); 59 this.date = date; 60 } 61 62 /** Build the rotation that transforms a pair of pv coordinates into another pair. 63 64 * <p><em>WARNING</em>! This method requires much more stringent assumptions on 65 * its parameters than the similar {@link Rotation#Rotation(Vector3D, Vector3D, 66 * Vector3D, Vector3D) constructor} from the {@link Rotation Rotation} class. 67 * As far as the Rotation constructor is concerned, the {@code v₂} vector from 68 * the second pair can be slightly misaligned. The Rotation constructor will 69 * compensate for this misalignment and create a rotation that ensure {@code 70 * v₁ = r(u₁)} and {@code v₂ ∈ plane (r(u₁), r(u₂))}. <em>THIS IS NOT 71 * TRUE ANYMORE IN THIS CLASS</em>! As derivatives are involved and must be 72 * preserved, this constructor works <em>only</em> if the two pairs are fully 73 * consistent, i.e. if a rotation exists that fulfill all the requirements: {@code 74 * v₁ = r(u₁)}, {@code v₂ = r(u₂)}, {@code dv₁/dt = dr(u₁)/dt}, {@code dv₂/dt 75 * = dr(u₂)/dt}, {@code d²v₁/dt² = d²r(u₁)/dt²}, {@code d²v₂/dt² = d²r(u₂)/dt²}.</p> 76 77 * @param date coordinates date 78 * @param u1 first vector of the origin pair 79 * @param u2 second vector of the origin pair 80 * @param v1 desired image of u1 by the rotation 81 * @param v2 desired image of u2 by the rotation 82 * @param tolerance relative tolerance factor used to check singularities 83 */ 84 public TimeStampedAngularCoordinates(final AbsoluteDate date, 85 final PVCoordinates#PVCoordinates">PVCoordinates u1, final PVCoordinates u2, 86 final PVCoordinates#PVCoordinates">PVCoordinates v1, final PVCoordinates v2, 87 final double tolerance) { 88 super(u1, u2, v1, v2, tolerance); 89 this.date = date; 90 } 91 92 /** Build one of the rotations that transform one pv coordinates into another one. 93 94 * <p>Except for a possible scale factor, if the instance were 95 * applied to the vector u it will produce the vector v. There is an 96 * infinite number of such rotations, this constructor choose the 97 * one with the smallest associated angle (i.e. the one whose axis 98 * is orthogonal to the (u, v) plane). If u and v are collinear, an 99 * arbitrary rotation axis is chosen.</p> 100 101 * @param date coordinates date 102 * @param u origin vector 103 * @param v desired image of u by the rotation 104 */ 105 public TimeStampedAngularCoordinates(final AbsoluteDate date, 106 final PVCoordinatesl#PVCoordinates">PVCoordinates u, final PVCoordinates v) { 107 super(u, v); 108 this.date = date; 109 } 110 111 /** Builds a TimeStampedAngularCoordinates from a {@link FieldRotation}<{@link DerivativeStructure}>. 112 * <p> 113 * The rotation components must have time as their only derivation parameter and 114 * have consistent derivation orders. 115 * </p> 116 * @param date coordinates date 117 * @param r rotation with time-derivatives embedded within the coordinates 118 */ 119 public TimeStampedAngularCoordinates(final AbsoluteDate date, 120 final FieldRotation<DerivativeStructure> r) { 121 super(r); 122 this.date = date; 123 } 124 125 /** {@inheritDoc} */ 126 public AbsoluteDate getDate() { 127 return date; 128 } 129 130 /** Revert a rotation/rotation rate pair. 131 * Build a pair which reverse the effect of another pair. 132 * @return a new pair whose effect is the reverse of the effect 133 * of the instance 134 */ 135 public TimeStampedAngularCoordinates revert() { 136 return new TimeStampedAngularCoordinates(date, 137 getRotation().revert(), 138 getRotation().applyInverseTo(getRotationRate().negate()), 139 getRotation().applyInverseTo(getRotationAcceleration().negate())); 140 } 141 142 /** Get a time-shifted state. 143 * <p> 144 * The state can be slightly shifted to close dates. This shift is based on 145 * a simple linear model. It is <em>not</em> intended as a replacement for 146 * proper attitude propagation but should be sufficient for either small 147 * time shifts or coarse accuracy. 148 * </p> 149 * @param dt time shift in seconds 150 * @return a new state, shifted with respect to the instance (which is immutable) 151 */ 152 public TimeStampedAngularCoordinates shiftedBy(final double dt) { 153 final AngularCoordinates sac = super.shiftedBy(dt); 154 return new TimeStampedAngularCoordinates(date.shiftedBy(dt), 155 sac.getRotation(), sac.getRotationRate(), sac.getRotationAcceleration()); 156 157 } 158 159 /** Add an offset from the instance. 160 * <p> 161 * We consider here that the offset rotation is applied first and the 162 * instance is applied afterward. Note that angular coordinates do <em>not</em> 163 * commute under this operation, i.e. {@code a.addOffset(b)} and {@code 164 * b.addOffset(a)} lead to <em>different</em> results in most cases. 165 * </p> 166 * <p> 167 * The two methods {@link #addOffset(AngularCoordinates) addOffset} and 168 * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed 169 * so that round trip applications are possible. This means that both {@code 170 * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code 171 * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1. 172 * </p> 173 * @param offset offset to subtract 174 * @return new instance, with offset subtracted 175 * @see #subtractOffset(AngularCoordinates) 176 */ 177 @Override 178 public TimeStampedAngularCoordinates addOffset(final AngularCoordinates offset) { 179 final Vector3D rOmega = getRotation().applyTo(offset.getRotationRate()); 180 final Vector3D rOmegaDot = getRotation().applyTo(offset.getRotationAcceleration()); 181 return new TimeStampedAngularCoordinates(date, 182 getRotation().compose(offset.getRotation(), RotationConvention.VECTOR_OPERATOR), 183 getRotationRate().add(rOmega), 184 new Vector3D( 1.0, getRotationAcceleration(), 185 1.0, rOmegaDot, 186 -1.0, Vector3D.crossProduct(getRotationRate(), rOmega))); 187 } 188 189 /** Subtract an offset from the instance. 190 * <p> 191 * We consider here that the offset rotation is applied first and the 192 * instance is applied afterward. Note that angular coordinates do <em>not</em> 193 * commute under this operation, i.e. {@code a.subtractOffset(b)} and {@code 194 * b.subtractOffset(a)} lead to <em>different</em> results in most cases. 195 * </p> 196 * <p> 197 * The two methods {@link #addOffset(AngularCoordinates) addOffset} and 198 * {@link #subtractOffset(AngularCoordinates) subtractOffset} are designed 199 * so that round trip applications are possible. This means that both {@code 200 * ac1.subtractOffset(ac2).addOffset(ac2)} and {@code 201 * ac1.addOffset(ac2).subtractOffset(ac2)} return angular coordinates equal to ac1. 202 * </p> 203 * @param offset offset to subtract 204 * @return new instance, with offset subtracted 205 * @see #addOffset(AngularCoordinates) 206 */ 207 @Override 208 public TimeStampedAngularCoordinates subtractOffset(final AngularCoordinates offset) { 209 return addOffset(offset.revert()); 210 } 211 212 /** Interpolate angular coordinates. 213 * <p> 214 * The interpolated instance is created by polynomial Hermite interpolation 215 * on Rodrigues vector ensuring rotation rate remains the exact derivative of rotation. 216 * </p> 217 * <p> 218 * This method is based on Sergei Tanygin's paper <a 219 * href="http://www.agi.com/downloads/resources/white-papers/Attitude-interpolation.pdf">Attitude 220 * Interpolation</a>, changing the norm of the vector to match the modified Rodrigues 221 * vector as described in Malcolm D. Shuster's paper <a 222 * href="http://www.ladispe.polito.it/corsi/Meccatronica/02JHCOR/2011-12/Slides/Shuster_Pub_1993h_J_Repsurv_scan.pdf">A 223 * Survey of Attitude Representations</a>. This change avoids the singularity at π. 224 * There is still a singularity at 2π, which is handled by slightly offsetting all rotations 225 * when this singularity is detected. Another change is that the mean linear motion 226 * is first removed before interpolation and added back after interpolation. This allows 227 * to use interpolation even when the sample covers much more than one turn and even 228 * when sample points are separated by more than one turn. 229 * </p> 230 * <p> 231 * Note that even if first and second time derivatives (rotation rates and acceleration) 232 * from sample can be ignored, the interpolated instance always includes 233 * interpolated derivatives. This feature can be used explicitly to 234 * compute these derivatives when it would be too complex to compute them 235 * from an analytical formula: just compute a few sample points from the 236 * explicit formula and set the derivatives to zero in these sample points, 237 * then use interpolation to add derivatives consistent with the rotations. 238 * </p> 239 * @param date interpolation date 240 * @param filter filter for derivatives from the sample to use in interpolation 241 * @param sample sample points on which interpolation should be done 242 * @return a new position-velocity, interpolated at specified date 243 */ 244 public static TimeStampedAngularCoordinates interpolate(final AbsoluteDate date, 245 final AngularDerivativesFilter filter, 246 final Collection<TimeStampedAngularCoordinates> sample) { 247 248 // set up safety elements for 2π singularity avoidance 249 final double epsilon = 2 * FastMath.PI / sample.size(); 250 final double threshold = FastMath.min(-(1.0 - 1.0e-4), -FastMath.cos(epsilon / 4)); 251 252 // set up a linear model canceling mean rotation rate 253 final Vector3D meanRate; 254 if (filter != AngularDerivativesFilter.USE_R) { 255 Vector3D sum = Vector3D.ZERO; 256 for (final TimeStampedAngularCoordinates datedAC : sample) { 257 sum = sum.add(datedAC.getRotationRate()); 258 } 259 meanRate = new Vector3D(1.0 / sample.size(), sum); 260 } else { 261 if (sample.size() < 2) { 262 throw new OrekitException(OrekitMessages.NOT_ENOUGH_DATA_FOR_INTERPOLATION, 263 sample.size()); 264 } 265 Vector3D sum = Vector3D.ZERO; 266 TimeStampedAngularCoordinates previous = null; 267 for (final TimeStampedAngularCoordinates datedAC : sample) { 268 if (previous != null) { 269 sum = sum.add(estimateRate(previous.getRotation(), datedAC.getRotation(), 270 datedAC.date.durationFrom(previous.date))); 271 } 272 previous = datedAC; 273 } 274 meanRate = new Vector3D(1.0 / (sample.size() - 1), sum); 275 } 276 TimeStampedAngularCoordinates offset = 277 new TimeStampedAngularCoordinates(date, Rotation.IDENTITY, meanRate, Vector3D.ZERO); 278 279 boolean restart = true; 280 for (int i = 0; restart && i < sample.size() + 2; ++i) { 281 282 // offset adaptation parameters 283 restart = false; 284 285 // set up an interpolator taking derivatives into account 286 final HermiteInterpolator interpolator = new HermiteInterpolator(); 287 288 // add sample points 289 double sign = +1.0; 290 Rotation previous = Rotation.IDENTITY; 291 292 for (final TimeStampedAngularCoordinates ac : sample) { 293 294 // remove linear offset from the current coordinates 295 final double dt = ac.date.durationFrom(date); 296 final TimeStampedAngularCoordinates fixed = ac.subtractOffset(offset.shiftedBy(dt)); 297 298 // make sure all interpolated points will be on the same branch 299 final double dot = MathArrays.linearCombination(fixed.getRotation().getQ0(), previous.getQ0(), 300 fixed.getRotation().getQ1(), previous.getQ1(), 301 fixed.getRotation().getQ2(), previous.getQ2(), 302 fixed.getRotation().getQ3(), previous.getQ3()); 303 sign = FastMath.copySign(1.0, dot * sign); 304 previous = fixed.getRotation(); 305 306 // check modified Rodrigues vector singularity 307 if (fixed.getRotation().getQ0() * sign < threshold) { 308 // the sample point is close to a modified Rodrigues vector singularity 309 // we need to change the linear offset model to avoid this 310 restart = true; 311 break; 312 } 313 314 final double[][] rodrigues = fixed.getModifiedRodrigues(sign); 315 switch (filter) { 316 case USE_RRA: 317 // populate sample with rotation, rotation rate and acceleration data 318 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1], rodrigues[2]); 319 break; 320 case USE_RR: 321 // populate sample with rotation and rotation rate data 322 interpolator.addSamplePoint(dt, rodrigues[0], rodrigues[1]); 323 break; 324 case USE_R: 325 // populate sample with rotation data only 326 interpolator.addSamplePoint(dt, rodrigues[0]); 327 break; 328 default : 329 // this should never happen 330 throw new OrekitInternalError(null); 331 } 332 } 333 334 if (restart) { 335 // interpolation failed, some intermediate rotation was too close to 2π 336 // we need to offset all rotations to avoid the singularity 337 offset = offset.addOffset(new AngularCoordinates(new Rotation(Vector3D.PLUS_I, 338 epsilon, 339 RotationConvention.VECTOR_OPERATOR), 340 Vector3D.ZERO, Vector3D.ZERO)); 341 } else { 342 // interpolation succeeded with the current offset 343 final double[][] p = interpolator.derivatives(0.0, 2); 344 final AngularCoordinates ac = createFromModifiedRodrigues(p); 345 return new TimeStampedAngularCoordinates(offset.getDate(), 346 ac.getRotation(), 347 ac.getRotationRate(), 348 ac.getRotationAcceleration()).addOffset(offset); 349 } 350 351 } 352 353 // this should never happen 354 throw new OrekitInternalError(null); 355 356 } 357 358 }