TorqueFree.java

  1. /* Copyright 2002-2024 Luc Maisonobe
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.attitudes;

  18. import java.util.HashMap;
  19. import java.util.Map;

  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.Field;
  22. import org.hipparchus.geometry.euclidean.threed.FieldRotation;
  23. import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  24. import org.hipparchus.geometry.euclidean.threed.Rotation;
  25. import org.hipparchus.geometry.euclidean.threed.RotationConvention;
  26. import org.hipparchus.geometry.euclidean.threed.RotationOrder;
  27. import org.hipparchus.geometry.euclidean.threed.Vector3D;
  28. import org.hipparchus.ode.DenseOutputModel;
  29. import org.hipparchus.ode.FieldDenseOutputModel;
  30. import org.hipparchus.ode.FieldExpandableODE;
  31. import org.hipparchus.ode.FieldODEState;
  32. import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
  33. import org.hipparchus.ode.ODEState;
  34. import org.hipparchus.ode.OrdinaryDifferentialEquation;
  35. import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
  36. import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
  37. import org.hipparchus.special.elliptic.jacobi.CopolarN;
  38. import org.hipparchus.special.elliptic.jacobi.FieldCopolarN;
  39. import org.hipparchus.special.elliptic.jacobi.FieldJacobiElliptic;
  40. import org.hipparchus.special.elliptic.jacobi.JacobiElliptic;
  41. import org.hipparchus.special.elliptic.jacobi.JacobiEllipticBuilder;
  42. import org.hipparchus.special.elliptic.legendre.LegendreEllipticIntegral;
  43. import org.hipparchus.util.FastMath;
  44. import org.hipparchus.util.MathArrays;
  45. import org.orekit.frames.Frame;
  46. import org.orekit.time.AbsoluteDate;
  47. import org.orekit.time.FieldAbsoluteDate;
  48. import org.orekit.utils.FieldPVCoordinatesProvider;
  49. import org.orekit.utils.PVCoordinatesProvider;
  50. import org.orekit.utils.TimeStampedAngularCoordinates;
  51. import org.orekit.utils.TimeStampedFieldAngularCoordinates;


  52. /** This class handles torque-free motion of a general (non-symmetrical) body.
  53.  * <p>
  54.  * This attitude model is analytical, it can be called at any arbitrary date
  55.  * before or after the date of the initial attitude. Despite being an analytical
  56.  * model, it is <em>not</em> an approximation. It provides the attitude exactly
  57.  * in O(1) time.
  58.  * </p>
  59.  * <p>
  60.  * The equations are based on Landau and Lifchitz Course of Theoretical Physics,
  61.  * Mechanics vol 1, chapter 37. Some adaptations have been made to Landau and
  62.  * Lifchitz equations:
  63.  * </p>
  64.  * <ul>
  65.  *   <li>inertia can be in any order</li>
  66.  *   <li>initial conditions can be arbitrary</li>
  67.  *   <li>signs of several equations have been fixed to work for all initial conditions</li>
  68.  *   <li>equations have been rewritten to work in all octants</li>
  69.  *   <li>the φ angle model is based on a precomputed quadrature over one period computed
  70.  *   at construction (the Landau and Lifchitz equations 37.17 to 37.20 seem to be wrong)</li>
  71.  * </ul>
  72.  * <p>
  73.  * The precomputed quadrature is performed numerically, but as it is performed only once at
  74.  * construction and the full integrated model over one period is saved, it can be applied
  75.  * analytically later on for any number of periods, hence we consider this attitude mode
  76.  * to be analytical.
  77.  * </p>
  78.  * @author Luc Maisonobe
  79.  * @author Lucas Girodet
  80.  * @since 12.0
  81.  */
  82. public class TorqueFree implements AttitudeProvider {

  83.     /** Initial attitude. */
  84.     private final Attitude initialAttitude;

  85.     /** Spacecraft inertia. */
  86.     private final Inertia inertia;

  87.     /** Regular model for primitive double arguments. */
  88.     private final DoubleModel doubleModel;

  89.     /** Cached field-based models. */
  90.     private final transient Map<Field<? extends CalculusFieldElement<?>>, FieldModel<? extends CalculusFieldElement<?>>> cachedModels;

  91.     /** Simple constructor.
  92.      * @param initialAttitude initial attitude
  93.      * @param inertia spacecraft inertia
  94.      */
  95.     public TorqueFree(final Attitude initialAttitude, final Inertia inertia) {

  96.         this.initialAttitude = initialAttitude;
  97.         this.inertia         = inertia;

  98.         // prepare the regular model
  99.         this.doubleModel  = new DoubleModel();

  100.         // set an empty cache for field-based models that will be lazily build
  101.         this.cachedModels = new HashMap<>();

  102.     }

  103.     /** Get the initial attitude.
  104.      * @return initial attitude
  105.      */
  106.     public Attitude getInitialAttitude() {
  107.         return initialAttitude;
  108.     }

  109.     /** Get the spacecraft inertia.
  110.      * @return spacecraft inertia
  111.      */
  112.     public Inertia getInertia() {
  113.         return inertia;
  114.     }

  115.     /** {@inheritDoc} */
  116.     public Attitude getAttitude(final PVCoordinatesProvider pvProv,
  117.                                 final AbsoluteDate date, final Frame frame) {

  118.         // attitude from model
  119.         final Attitude attitude =
  120.                         new Attitude(initialAttitude.getReferenceFrame(), doubleModel.evaluate(date));

  121.         // fix frame
  122.         return attitude.withReferenceFrame(frame);

  123.     }

  124.     /** {@inheritDoc} */
  125.     public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
  126.                                                                             final FieldAbsoluteDate<T> date,
  127.                                                                             final Frame frame) {

  128.         // get the model for specified field
  129.         @SuppressWarnings("unchecked")
  130.         FieldModel<T> fm = (FieldModel<T>) cachedModels.get(date.getField());
  131.         if (fm == null) {
  132.             // create a model for this field
  133.             fm = new FieldModel<>(date.getField());
  134.             cachedModels.put(date.getField(), fm);
  135.         }

  136.         // attitude from model
  137.         final FieldAttitude<T> attitude =
  138.                         new FieldAttitude<>(initialAttitude.getReferenceFrame(), fm.evaluate(date));

  139.         // fix frame
  140.         return attitude.withReferenceFrame(frame);

  141.     }

  142.     /** Torque-free model. */
  143.     private class DoubleModel {

  144.         /** Inertia sorted to get a motion about axis 3. */
  145.         private final Inertia sortedInertia;

  146.         /** State scaling factor. */
  147.         private final double o1Scale;

  148.         /** State scaling factor. */
  149.         private final double o2Scale;

  150.         /** State scaling factor. */
  151.         private final double o3Scale;

  152.         /** Jacobi elliptic function. */
  153.         private final JacobiElliptic jacobi;

  154.         /** Time scaling factor. */
  155.         private final double tScale;

  156.         /** Time reference for rotation rate. */
  157.         private final AbsoluteDate tRef;

  158.         /** Offset rotation  between initial inertial frame and the frame with moment vector and Z axis aligned. */
  159.         private final Rotation inertToAligned;

  160.         /** Rotation to switch to the converted axes frame. */
  161.         private final Rotation sortedToBody;

  162.         /** Period of rotation rate. */
  163.         private final double period;

  164.         /** Slope of the linear part of the phi model. */
  165.         private final double phiSlope;

  166.         /** DenseOutputModel of phi. */
  167.         private final DenseOutputModel phiQuadratureModel;

  168.         /** Integral part of quadrature model over one period. */
  169.         private final double integOnePeriod;

  170.         /** Simple constructor.
  171.          */
  172.         DoubleModel() {

  173.             // build inertia tensor
  174.             final double   i1  = inertia.getInertiaAxis1().getI();
  175.             final Vector3D a1  = inertia.getInertiaAxis1().getA();
  176.             final double   i2  = inertia.getInertiaAxis2().getI();
  177.             final Vector3D a2  = inertia.getInertiaAxis2().getA();
  178.             final double   i3  = inertia.getInertiaAxis3().getI();
  179.             final Vector3D a3  = inertia.getInertiaAxis3().getA();
  180.             final Vector3D n1  = a1.normalize();
  181.             final Vector3D n2  = a2.normalize();
  182.             final Vector3D n3  = Vector3D.dotProduct(Vector3D.crossProduct(a1, a2), a3) > 0 ?
  183.                                   a3.normalize() : a3.normalize().negate();

  184.             final Vector3D omega0 = initialAttitude.getSpin();
  185.             final Vector3D m0     = new Vector3D(i1 * Vector3D.dotProduct(omega0, n1), n1,
  186.                                                  i2 * Vector3D.dotProduct(omega0, n2), n2,
  187.                                                  i3 * Vector3D.dotProduct(omega0, n3), n3);

  188.             // sort axes in increasing moments of inertia order
  189.             Inertia tmpInertia = new Inertia(new InertiaAxis(i1, n1), new InertiaAxis(i2, n2), new InertiaAxis(i3, n3));
  190.             if (tmpInertia.getInertiaAxis1().getI() > tmpInertia.getInertiaAxis2().getI()) {
  191.                 tmpInertia = tmpInertia.swap12();
  192.             }
  193.             if (tmpInertia.getInertiaAxis2().getI() > tmpInertia.getInertiaAxis3().getI()) {
  194.                 tmpInertia = tmpInertia.swap23();
  195.             }
  196.             if (tmpInertia.getInertiaAxis1().getI() > tmpInertia.getInertiaAxis2().getI()) {
  197.                 tmpInertia = tmpInertia.swap12();
  198.             }

  199.             // in order to simplify implementation, we want the motion to be about axis 3
  200.             // which is either the minimum or the maximum inertia axis
  201.             final double  o1                 = Vector3D.dotProduct(omega0, n1);
  202.             final double  o2                 = Vector3D.dotProduct(omega0, n2);
  203.             final double  o3                 = Vector3D.dotProduct(omega0, n3);
  204.             final double  o12                = o1 * o1;
  205.             final double  o22                = o2 * o2;
  206.             final double  o32                = o3 * o3;
  207.             final double   twoE              = i1 * o12 + i2 * o22 + i3 * o32;
  208.             final double   m2                = i1 * i1 * o12 + i2 * i2 * o22 + i3 * i3 * o32;
  209.             final double   separatrixInertia = (twoE == 0) ? 0.0 : m2 / twoE;
  210.             final boolean  clockwise;
  211.             if (separatrixInertia < tmpInertia.getInertiaAxis2().getI()) {
  212.                 // motion is about minimum inertia axis
  213.                 // we swap axes to put them in decreasing moments order
  214.                 // motion will be clockwise about axis 3
  215.                 clockwise = true;
  216.                 tmpInertia   = tmpInertia.swap13();
  217.             } else {
  218.                 // motion is about maximum inertia axis
  219.                 // we keep axes in increasing moments order
  220.                 // motion will be counter-clockwise about axis 3
  221.                 clockwise = false;
  222.             }
  223.             sortedInertia = tmpInertia;

  224.             final double i1C = tmpInertia.getInertiaAxis1().getI();
  225.             final double i2C = tmpInertia.getInertiaAxis2().getI();
  226.             final double i3C = tmpInertia.getInertiaAxis3().getI();
  227.             final double i32 = i3C - i2C;
  228.             final double i31 = i3C - i1C;
  229.             final double i21 = i2C - i1C;

  230.             // convert initial conditions to Euler angles such the M is aligned with Z in sorted computation frame
  231.             sortedToBody   = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J,
  232.                                           tmpInertia.getInertiaAxis1().getA(), tmpInertia.getInertiaAxis2().getA());
  233.             final Vector3D omega0Sorted = sortedToBody.applyInverseTo(omega0);
  234.             final Vector3D m0Sorted     = sortedToBody.applyInverseTo(m0);
  235.             final double   phi0         = 0; // this angle can be set arbitrarily, so 0 is a fair value (Eq. 37.13 - 37.14)
  236.             final double   theta0       = FastMath.acos(m0Sorted.getZ() / m0Sorted.getNorm());
  237.             final double   psi0         = FastMath.atan2(m0Sorted.getX(), m0Sorted.getY()); // it is really atan2(x, y), not atan2(y, x) as usual!

  238.             // compute offset rotation between inertial frame aligned with momentum and regular inertial frame
  239.             final Rotation alignedToSorted0 = new Rotation(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
  240.                                                            phi0, theta0, psi0);
  241.             inertToAligned = alignedToSorted0.
  242.                              applyInverseTo(sortedToBody.applyInverseTo(initialAttitude.getRotation()));

  243.             // Ω is always o1Scale * cn((t-tref) * tScale), o2Scale * sn((t-tref) * tScale), o3Scale * dn((t-tref) * tScale)
  244.             tScale  = FastMath.copySign(FastMath.sqrt(i32 * (m2 - twoE * i1C) / (i1C * i2C * i3C)),
  245.                                         clockwise ? -omega0Sorted.getZ() : omega0Sorted.getZ());
  246.             o1Scale = FastMath.sqrt((twoE * i3C - m2) / (i1C * i31));
  247.             o2Scale = FastMath.sqrt((twoE * i3C - m2) / (i2C * i32));
  248.             o3Scale = FastMath.copySign(FastMath.sqrt((m2 - twoE * i1C) / (i3C * i31)), omega0Sorted.getZ());

  249.             final double k2 = (twoE == 0) ? 0.0 : i21 * (twoE * i3C - m2) / (i32 * (m2 - twoE * i1C));
  250.             jacobi = JacobiEllipticBuilder.build(k2);
  251.             period = 4 * LegendreEllipticIntegral.bigK(k2) / tScale;

  252.             final double dtRef;
  253.             if (o1Scale == 0) {
  254.                 // special case where twoE * i3C = m2, then o2Scale is also zero
  255.                 // motion is exactly along one axis
  256.                 dtRef = 0;
  257.             } else {
  258.                 if (FastMath.abs(omega0Sorted.getX()) >= FastMath.abs(omega0Sorted.getY())) {
  259.                     if (omega0Sorted.getX() >= 0) {
  260.                         // omega is roughly towards +I
  261.                         dtRef = -jacobi.arcsn(omega0Sorted.getY() / o2Scale) / tScale;
  262.                     } else {
  263.                         // omega is roughly towards -I
  264.                         dtRef = jacobi.arcsn(omega0Sorted.getY() / o2Scale) / tScale - 0.5 * period;
  265.                     }
  266.                 } else {
  267.                     if (omega0Sorted.getY() >= 0) {
  268.                         // omega is roughly towards +J
  269.                         dtRef = -jacobi.arccn(omega0Sorted.getX() / o1Scale) / tScale;
  270.                     } else {
  271.                         // omega is roughly towards -J
  272.                         dtRef = jacobi.arccn(omega0Sorted.getX() / o1Scale) / tScale;
  273.                     }
  274.                 }
  275.             }
  276.             tRef = initialAttitude.getDate().shiftedBy(dtRef);

  277.             phiSlope           = FastMath.sqrt(m2) / i3C;
  278.             phiQuadratureModel = computePhiQuadratureModel(dtRef);
  279.             integOnePeriod     = phiQuadratureModel.getInterpolatedState(phiQuadratureModel.getFinalTime()).getPrimaryState()[0];

  280.         }

  281.         /** Compute the model for φ angle.
  282.          * @param dtRef start time
  283.          * @return model for φ angle
  284.          */
  285.         private DenseOutputModel computePhiQuadratureModel(final double dtRef) {

  286.             final double i1C = sortedInertia.getInertiaAxis1().getI();
  287.             final double i2C = sortedInertia.getInertiaAxis2().getI();
  288.             final double i3C = sortedInertia.getInertiaAxis3().getI();

  289.             final double i32 = i3C - i2C;
  290.             final double i31 = i3C - i1C;
  291.             final double i21 = i2C - i1C;

  292.             // coefficients for φ model
  293.             final double b = phiSlope * i32 * i31;
  294.             final double c = i1C * i32;
  295.             final double d = i3C * i21;

  296.             // integrate the quadrature phi term over one period
  297.             final DormandPrince853Integrator integ = new DormandPrince853Integrator(1.0e-6 * period, 1.0e-2 * period,
  298.                                                                                     phiSlope * period * 1.0e-13, 1.0e-13);
  299.             final DenseOutputModel model = new DenseOutputModel();
  300.             integ.addStepHandler(model);

  301.             integ.integrate(new OrdinaryDifferentialEquation() {

  302.                 /** {@inheritDoc} */
  303.                 @Override
  304.                 public int getDimension() {
  305.                     return 1;
  306.                 }

  307.                 /** {@inheritDoc} */
  308.                 @Override
  309.                 public double[] computeDerivatives(final double t, final double[] y) {
  310.                     final double sn = jacobi.valuesN((t - dtRef) * tScale).sn();
  311.                     return new double[] {
  312.                         b / (c + d * sn * sn)
  313.                     };
  314.                 }

  315.             }, new ODEState(0, new double[1]), period);

  316.             return model;

  317.         }

  318.         /** Evaluate torque-free motion model.
  319.          * @param date evaluation date
  320.          * @return body orientation at date
  321.          */
  322.         public TimeStampedAngularCoordinates evaluate(final AbsoluteDate date) {

  323.             // angular velocity
  324.             final CopolarN valuesN     = jacobi.valuesN(date.durationFrom(tRef) * tScale);
  325.             final Vector3D omegaSorted = new Vector3D(o1Scale * valuesN.cn(), o2Scale * valuesN.sn(), o3Scale * valuesN.dn());
  326.             final Vector3D omegaBody   = sortedToBody.applyTo(omegaSorted);

  327.             // acceleration
  328.             final Vector3D accelerationSorted = new Vector3D(o1Scale * tScale *                  valuesN.cn() * valuesN.dn(),
  329.                                                              o2Scale * tScale *                 -valuesN.sn() * valuesN.dn(),
  330.                                                              o3Scale * tScale * -jacobi.getM() * valuesN.sn() * valuesN.cn());
  331.             final Vector3D accelerationBody   = sortedToBody.applyTo(accelerationSorted);

  332.             // first Euler angles are directly linked to angular velocity
  333.             final double   dt          = date.durationFrom(initialAttitude.getDate());
  334.             final double   psi         = FastMath.atan2(sortedInertia.getInertiaAxis1().getI() * omegaSorted.getX(),
  335.                                                         sortedInertia.getInertiaAxis2().getI() * omegaSorted.getY());
  336.             final double   theta       = FastMath.acos(omegaSorted.getZ() / phiSlope);
  337.             final double   phiLinear   = phiSlope * dt;

  338.             // third Euler angle results from a quadrature
  339.             final int    nbPeriods     = (int) FastMath.floor(dt / period);
  340.             final double tStartInteg   = nbPeriods * period;
  341.             final double integPartial  = phiQuadratureModel.getInterpolatedState(dt - tStartInteg).getPrimaryState()[0];
  342.             final double phiQuadrature = nbPeriods * integOnePeriod + integPartial;
  343.             final double phi           = phiLinear + phiQuadrature;

  344.             // rotation between computation frame (aligned with momentum) and sorted computation frame
  345.             // (it is simply the angles equations provided by Landau & Lifchitz)
  346.             final Rotation alignedToSorted = new Rotation(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
  347.                                                           phi, theta, psi);

  348.             // combine with offset rotation to get back from regular inertial frame to body frame
  349.             final Rotation inertToBody = sortedToBody.applyTo(alignedToSorted.applyTo(inertToAligned));

  350.             return new TimeStampedAngularCoordinates(date, inertToBody, omegaBody, accelerationBody);
  351.         }

  352.     }

  353.     /** Torque-free model.
  354.      * @param <T> type of the field elements
  355.      */
  356.     private class FieldModel <T extends CalculusFieldElement<T>> {

  357.         /** Inertia sorted to get a motion about axis 3. */
  358.         private final FieldInertia<T> sortedInertia;

  359.         /** State scaling factor. */
  360.         private final T o1Scale;

  361.         /** State scaling factor. */
  362.         private final T o2Scale;

  363.         /** State scaling factor. */
  364.         private final T o3Scale;

  365.         /** Jacobi elliptic function. */
  366.         private final FieldJacobiElliptic<T> jacobi;

  367.         /** Time scaling factor. */
  368.         private final T tScale;

  369.         /** Time reference for rotation rate. */
  370.         private final FieldAbsoluteDate<T> tRef;

  371.         /** Offset rotation  between initial inertial frame and the frame with moment vector and Z axis aligned. */
  372.         private final FieldRotation<T> inertToAligned;

  373.         /** Rotation to switch to the converted axes frame. */
  374.         private final FieldRotation<T> sortedToBody;

  375.         /** Period of rotation rate. */
  376.         private final T period;

  377.         /** Slope of the linear part of the phi model. */
  378.         private final T phiSlope;

  379.         /** DenseOutputModel of phi. */
  380.         private final FieldDenseOutputModel<T> phiQuadratureModel;

  381.         /** Integral part of quadrature model over one period. */
  382.         private final T integOnePeriod;

  383.         /** Simple constructor.
  384.          * @param field field to which elements belong
  385.          */
  386.         FieldModel(final Field<T> field) {

  387.             final double   i1  = inertia.getInertiaAxis1().getI();
  388.             final Vector3D a1  = inertia.getInertiaAxis1().getA();
  389.             final double   i2  = inertia.getInertiaAxis2().getI();
  390.             final Vector3D a2  = inertia.getInertiaAxis2().getA();
  391.             final double   i3  = inertia.getInertiaAxis3().getI();
  392.             final Vector3D a3  = inertia.getInertiaAxis3().getA();

  393.             final T                zero = field.getZero();
  394.             final T                fI1  = zero.newInstance(i1);
  395.             final FieldVector3D<T> fA1  = new FieldVector3D<>(field, a1);
  396.             final T                fI2  = zero.newInstance(i2);
  397.             final FieldVector3D<T> fA2  = new FieldVector3D<>(field, a2);
  398.             final T                fI3  = zero.newInstance(i3);
  399.             final FieldVector3D<T> fA3  = new FieldVector3D<>(field, a3);

  400.             // build inertia tensor
  401.             final FieldVector3D<T> n1  = fA1.normalize();
  402.             final FieldVector3D<T> n2  = fA2.normalize();
  403.             final FieldVector3D<T> n3  = Vector3D.dotProduct(Vector3D.crossProduct(a1, a2), a3) > 0 ?
  404.                                          fA3.normalize() : fA3.normalize().negate();

  405.             final FieldVector3D<T> omega0 = new FieldVector3D<>(field, initialAttitude.getSpin());
  406.             final FieldVector3D<T> m0 = new FieldVector3D<>(fI1.multiply(FieldVector3D.dotProduct(omega0, n1)), n1,
  407.                                                             fI2.multiply(FieldVector3D.dotProduct(omega0, n2)), n2,
  408.                                                             fI3.multiply(FieldVector3D.dotProduct(omega0, n3)), n3);

  409.             // sort axes in increasing moments of inertia order
  410.             FieldInertia<T> tmpInertia = new FieldInertia<>(new FieldInertiaAxis<>(fI1, n1),
  411.                                                             new FieldInertiaAxis<>(fI2, n2),
  412.                                                             new FieldInertiaAxis<>(fI3, n3));
  413.             if (tmpInertia.getInertiaAxis1().getI().subtract(tmpInertia.getInertiaAxis2().getI()).getReal() > 0) {
  414.                 tmpInertia = tmpInertia.swap12();
  415.             }
  416.             if (tmpInertia.getInertiaAxis2().getI().subtract(tmpInertia.getInertiaAxis3().getI()).getReal() > 0) {
  417.                 tmpInertia = tmpInertia.swap23();
  418.             }
  419.             if (tmpInertia.getInertiaAxis1().getI().subtract(tmpInertia.getInertiaAxis2().getI()).getReal() > 0) {
  420.                 tmpInertia = tmpInertia.swap12();
  421.             }

  422.             // in order to simplify implementation, we want the motion to be about axis 3
  423.             // which is either the minimum or the maximum inertia axis
  424.             final T       o1                = FieldVector3D.dotProduct(omega0, n1);
  425.             final T       o2                = FieldVector3D.dotProduct(omega0, n2);
  426.             final T       o3                = FieldVector3D.dotProduct(omega0, n3);
  427.             final T       o12               = o1.multiply(o1);
  428.             final T       o22               = o2.multiply(o2);
  429.             final T       o32               = o3.multiply(o3);
  430.             final T       twoE              = fI1.multiply(o12).add(fI2.multiply(o22)).add(fI3.multiply(o32));
  431.             final T       m2                = fI1.multiply(fI1).multiply(o12).add(fI2.multiply(fI2).multiply(o22)).add(fI3.multiply(fI3).multiply(o32));
  432.             final T       separatrixInertia = (twoE.isZero()) ? zero : m2.divide(twoE);
  433.             final boolean clockwise;
  434.             if (separatrixInertia.subtract(tmpInertia.getInertiaAxis2().getI()).getReal() < 0) {
  435.                 // motion is about minimum inertia axis
  436.                 // we swap axes to put them in decreasing moments order
  437.                 // motion will be clockwise about axis 3
  438.                 clockwise  = true;
  439.                 tmpInertia = tmpInertia.swap13();
  440.             } else {
  441.                 // motion is about maximum inertia axis
  442.                 // we keep axes in increasing moments order
  443.                 // motion will be counter-clockwise about axis 3
  444.                 clockwise = false;
  445.             }
  446.             sortedInertia = tmpInertia;

  447.             final T i1C = tmpInertia.getInertiaAxis1().getI();
  448.             final T i2C = tmpInertia.getInertiaAxis2().getI();
  449.             final T i3C = tmpInertia.getInertiaAxis3().getI();
  450.             final T i32 = i3C.subtract(i2C);
  451.             final T i31 = i3C.subtract(i1C);
  452.             final T i21 = i2C.subtract(i1C);

  453.             // convert initial conditions to Euler angles such the M is aligned with Z in sorted computation frame
  454.             sortedToBody   = new FieldRotation<>(FieldVector3D.getPlusI(field),
  455.                                                  FieldVector3D.getPlusJ(field),
  456.                                                  tmpInertia.getInertiaAxis1().getA(),
  457.                                                  tmpInertia.getInertiaAxis2().getA());
  458.             final FieldVector3D<T> omega0Sorted = sortedToBody.applyInverseTo(omega0);
  459.             final FieldVector3D<T> m0Sorted     = sortedToBody.applyInverseTo(m0);
  460.             final T                phi0         = zero; // this angle can be set arbitrarily, so 0 is a fair value (Eq. 37.13 - 37.14)
  461.             final T                theta0       = FastMath.acos(m0Sorted.getZ().divide(m0Sorted.getNorm()));
  462.             final T                psi0         = FastMath.atan2(m0Sorted.getX(), m0Sorted.getY()); // it is really atan2(x, y), not atan2(y, x) as usual!

  463.             // compute offset rotation between inertial frame aligned with momentum and regular inertial frame
  464.             final FieldRotation<T> alignedToSorted0 = new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
  465.                                                                           phi0, theta0, psi0);
  466.             inertToAligned = alignedToSorted0.
  467.                              applyInverseTo(sortedToBody.applyInverseTo(new FieldRotation<>(field, initialAttitude.getRotation())));

  468.             // Ω is always o1Scale * cn((t-tref) * tScale), o2Scale * sn((t-tref) * tScale), o3Scale * dn((t-tref) * tScale)
  469.             tScale  = FastMath.copySign(FastMath.sqrt(i32.multiply(m2.subtract(twoE.multiply(i1C))).divide(i1C.multiply(i2C).multiply(i3C))),
  470.                                         clockwise ? omega0Sorted.getZ().negate() : omega0Sorted.getZ());
  471.             o1Scale = FastMath.sqrt(twoE.multiply(i3C).subtract(m2).divide(i1C.multiply(i31)));
  472.             o2Scale = FastMath.sqrt(twoE.multiply(i3C).subtract(m2).divide(i2C.multiply(i32)));
  473.             o3Scale = FastMath.copySign(FastMath.sqrt(m2.subtract(twoE.multiply(i1C)).divide(i3C.multiply(i31))),
  474.                                         omega0Sorted.getZ());

  475.             final T k2 = (twoE.isZero()) ?
  476.                          zero :
  477.                          i21.multiply(twoE.multiply(i3C).subtract(m2)).
  478.                          divide(i32.multiply(m2.subtract(twoE.multiply(i1C))));
  479.             jacobi = JacobiEllipticBuilder.build(k2);
  480.             period = LegendreEllipticIntegral.bigK(k2).multiply(4).divide(tScale);

  481.             final T dtRef;
  482.             if (o1Scale.isZero()) {
  483.                 // special case where twoE * i3C = m2, then o2Scale is also zero
  484.                 // motion is exactly along one axis
  485.                 dtRef = zero;
  486.             } else {
  487.                 if (FastMath.abs(omega0Sorted.getX()).subtract(FastMath.abs(omega0Sorted.getY())).getReal() >= 0) {
  488.                     if (omega0Sorted.getX().getReal() >= 0) {
  489.                         // omega is roughly towards +I
  490.                         dtRef = jacobi.arcsn(omega0Sorted.getY().divide(o2Scale)).divide(tScale).negate();
  491.                     } else {
  492.                         // omega is roughly towards -I
  493.                         dtRef = jacobi.arcsn(omega0Sorted.getY().divide(o2Scale)).divide(tScale).subtract(period.multiply(0.5));
  494.                     }
  495.                 } else {
  496.                     if (omega0Sorted.getY().getReal() >= 0) {
  497.                         // omega is roughly towards +J
  498.                         dtRef = jacobi.arccn(omega0Sorted.getX().divide(o1Scale)).divide(tScale).negate();
  499.                     } else {
  500.                         // omega is roughly towards -J
  501.                         dtRef = jacobi.arccn(omega0Sorted.getX().divide(o1Scale)).divide(tScale);
  502.                     }
  503.                 }
  504.             }
  505.             tRef = new FieldAbsoluteDate<>(field, initialAttitude.getDate()).shiftedBy(dtRef);

  506.             phiSlope           = FastMath.sqrt(m2).divide(i3C);
  507.             phiQuadratureModel = computePhiQuadratureModel(dtRef);
  508.             integOnePeriod     = phiQuadratureModel.getInterpolatedState(phiQuadratureModel.getFinalTime()).getPrimaryState()[0];

  509.         }

  510.         /** Compute the model for φ angle.
  511.          * @param dtRef start time
  512.          * @return model for φ angle
  513.          */
  514.         private FieldDenseOutputModel<T> computePhiQuadratureModel(final T dtRef) {

  515.             final T zero = dtRef.getField().getZero();

  516.             final T i1C = sortedInertia.getInertiaAxis1().getI();
  517.             final T i2C = sortedInertia.getInertiaAxis2().getI();
  518.             final T i3C = sortedInertia.getInertiaAxis3().getI();

  519.             final T i32 = i3C.subtract(i2C);
  520.             final T i31 = i3C.subtract(i1C);
  521.             final T i21 = i2C.subtract(i1C);

  522.             // coefficients for φ model
  523.             final T b = phiSlope.multiply(i32).multiply(i31);
  524.             final T c = i1C.multiply(i32);
  525.             final T d = i3C.multiply(i21);

  526.             // integrate the quadrature phi term on one period
  527.             final DormandPrince853FieldIntegrator<T> integ = new DormandPrince853FieldIntegrator<>(dtRef.getField(),
  528.                                                                                                    1.0e-6 * period.getReal(),
  529.                                                                                                    1.0e-2 * period.getReal(),
  530.                                                                                                    phiSlope.getReal() * period.getReal() * 1.0e-13,
  531.                                                                                                    1.0e-13);
  532.             final FieldDenseOutputModel<T> model = new FieldDenseOutputModel<>();
  533.             integ.addStepHandler(model);

  534.             integ.integrate(new FieldExpandableODE<T>(new FieldOrdinaryDifferentialEquation<T>() {

  535.                 /** {@inheritDoc} */
  536.                 @Override
  537.                 public int getDimension() {
  538.                     return 1;
  539.                 }

  540.                 /** {@inheritDoc} */
  541.                 @Override
  542.                 public T[] computeDerivatives(final T t, final T[] y) {
  543.                     final T sn = jacobi.valuesN(t.subtract(dtRef).multiply(tScale)).sn();
  544.                     final T[] yDot = MathArrays.buildArray(dtRef.getField(), 1);
  545.                     yDot[0] = b.divide(c.add(d.multiply(sn).multiply(sn)));
  546.                     return yDot;
  547.                 }

  548.             }), new FieldODEState<T>(zero, MathArrays.buildArray(dtRef.getField(), 1)), period);

  549.             return model;

  550.         }

  551.         /** Evaluate torque-free motion model.
  552.          * @param date evaluation date
  553.          * @return body orientation at date
  554.          */
  555.         public TimeStampedFieldAngularCoordinates<T> evaluate(final FieldAbsoluteDate<T> date) {

  556.             // angular velocity
  557.             final FieldCopolarN<T> valuesN     = jacobi.valuesN(date.durationFrom(tRef).multiply(tScale));
  558.             final FieldVector3D<T> omegaSorted = new FieldVector3D<>(valuesN.cn().multiply(o1Scale),
  559.                                                                      valuesN.sn().multiply(o2Scale),
  560.                                                                      valuesN.dn().multiply(o3Scale));
  561.             final FieldVector3D<T> omegaBody   = sortedToBody.applyTo(omegaSorted);

  562.             // acceleration
  563.             final FieldVector3D<T> accelerationSorted =
  564.                             new FieldVector3D<>(o1Scale.multiply(tScale).multiply(valuesN.cn()).multiply(valuesN.dn()),
  565.                                                 o2Scale.multiply(tScale).multiply(valuesN.sn().negate()).multiply(valuesN.dn()),
  566.                                                 o3Scale.multiply(tScale).multiply(jacobi.getM().negate()).multiply(valuesN.sn()).multiply(valuesN.cn()));
  567.             final FieldVector3D<T> accelerationBody   = sortedToBody.applyTo(accelerationSorted);

  568.             // first Euler angles are directly linked to angular velocity
  569.             final T   dt          = date.durationFrom(initialAttitude.getDate());
  570.             final T   psi         = FastMath.atan2(sortedInertia.getInertiaAxis1().getI().multiply(omegaSorted.getX()),
  571.                                                    sortedInertia.getInertiaAxis2().getI().multiply(omegaSorted.getY()));
  572.             final T   theta       = FastMath.acos(omegaSorted.getZ().divide(phiSlope));
  573.             final T   phiLinear   = dt.multiply(phiSlope);

  574.             // third Euler angle results from a quadrature
  575.             final int nbPeriods   = (int) FastMath.floor(dt.divide(period)).getReal();
  576.             final T tStartInteg   = period.multiply(nbPeriods);
  577.             final T integPartial  = phiQuadratureModel.getInterpolatedState(dt.subtract(tStartInteg)).getPrimaryState()[0];
  578.             final T phiQuadrature = integOnePeriod.multiply(nbPeriods).add(integPartial);
  579.             final T phi           = phiLinear.add(phiQuadrature);

  580.             // rotation between computation frame (aligned with momentum) and sorted computation frame
  581.             // (it is simply the angles equations provided by Landau & Lifchitz)
  582.             final FieldRotation<T> alignedToSorted = new FieldRotation<>(RotationOrder.ZXZ, RotationConvention.FRAME_TRANSFORM,
  583.                                                                          phi, theta, psi);

  584.             // combine with offset rotation to get back from regular inertial frame to body frame
  585.             final FieldRotation<T> inertToBody = sortedToBody.applyTo(alignedToSorted.applyTo(inertToAligned));

  586.             return new TimeStampedFieldAngularCoordinates<>(date, inertToBody, omegaBody, accelerationBody);

  587.         }

  588.     }

  589. }