Torque Models

// Create a new type derived from TorqueModel. This type represents a time-varying torque
// that is equal to the InitialTorque at time Epoch but varies linearly over time
// at a rate equal to TorqueRate.
public class LinearTorque : TorqueModel
{
    public LinearTorque(Cartesian initialTorque, Cartesian torqueRate, JulianDate epoch, Axes referenceAxes)
    {
        m_initialTorque = initialTorque;
        m_torqueRate = torqueRate;
        m_epoch = epoch;
        m_referenceAxes = referenceAxes;
    }

    // The copy constructor is used to make a copy of the object. Copy all of the fields of the
    // "existing instance" to the new object. Reference types should
    // be passed through a call to UpdateReference so that the depth of the copy
    // can be controlled by the user.  See the documentation of the ICloneWithContext
    // interface for more information.
    private LinearTorque(LinearTorque existingInstance, CopyContext context)
        : base(existingInstance, context)
    {
        m_initialTorque = existingInstance.m_initialTorque;
        m_torqueRate = existingInstance.m_torqueRate;
        m_epoch = existingInstance.m_epoch;
        m_referenceAxes = context.UpdateReference(existingInstance.m_referenceAxes);
    }

    // This is called to make a copy of the object, which it does by calling the
    // copy constructor above.
    public override object Clone(CopyContext context)
    {
        return new LinearTorque(this, context);
    }

    // This method is only called by the IsSameDefinition method in the base class to
    // determine if two torque models are equivalent. Derived classes MUST override this method and
    // check all new fields introduced by the derived class for definitional equivalence. It
    // is NOT necessary to check base class fields because the base class will already have
    // done that.
    protected sealed override bool CheckForSameDefinition(TorqueModel other)
    {
        var o = other as LinearTorque;
        return o != null &&
               AreSameDefinition(m_initialTorque, o.m_initialTorque) &&
               AreSameDefinition(m_torqueRate, o.m_torqueRate) &&
               AreSameDefinition(m_epoch, o.m_epoch) &&
               AreSameDefinition(m_referenceAxes, o.m_referenceAxes) &&
               CheckForSameDefinition(o);
    }

    // This method allows all types derived from LinearTorque to continue the process
    // of making unique CheckForSameDefinition methods.
    protected virtual bool CheckForSameDefinition(LinearTorque other)
    {
        return other != null && other.GetType() == typeof(LinearTorque);
    }

    // Called to determine a hash code for the current configuration of this object.
    // Derived classes MUST override this method and compute a hash code that combines: 
    // a unique hash code seed, the base implementation result, and the 
    // hash codes of all new fields introduced by the derived class which are used 
    // in the CheckForSameDefinition method.
    protected override int ComputeCurrentDefinitionHashCode()
    {
        return HashCode.Combine(typeof(LinearTorque).GetHashCode(),
                                base.ComputeCurrentDefinitionHashCode(),
                                GetDefinitionHashCode(m_initialTorque),
                                GetDefinitionHashCode(m_torqueRate),
                                GetDefinitionHashCode(m_epoch),
                                GetDefinitionHashCode(m_referenceAxes));
    }

    // Called to enumerate all of the other objects on which this object depends.  This
    // allows clients to navigate the graph of objects related to a computation.
    public override void EnumerateDependencies(DependencyEnumerator enumerator)
    {
        base.EnumerateDependencies(enumerator);
        enumerator.Enumerate(m_referenceAxes);
    }

    // The time in which the torque is equal to the InitialTorque.
    public JulianDate Epoch
    {
        get { return m_epoch; }
        set
        {
            // Called to ensure that properties cannot be set if
            // the object is frozen.
            ThrowIfFrozen(); 
            m_epoch = value;
        }
    }

    // The torque vector at the Epoch.
    public Cartesian InitialTorque
    {
        get { return m_initialTorque; }
        set
        {
            ThrowIfFrozen();
            m_initialTorque = value;
        }
    }

    // The rate at which the torque changes over time.
    public Cartesian TorqueRate
    {
        get { return m_torqueRate; }
        set
        {
            ThrowIfFrozen();
            m_torqueRate = value;
        }
    }

    // The reference axes that the torque is defined relative to.
    // These are usually the body axes of the rigid body.
    public Axes ReferenceAxes
    {
        get { return m_referenceAxes; }
        set
        {
            ThrowIfFrozen();
            m_referenceAxes = value;
        }
    }

    // This method is responsible for returning an instance of the private
    // LinearTorqueEvaluator class. It should ensure that the properties are not null or
    // in an invalid state, and then use the evaluator group when it constructs
    // and returns an instance of the LinearTorqueEvaluator.
    public override TorqueEvaluator GetTorqueEvaluator(EvaluatorGroup group)
    {
        if (group == null)
            throw new ArgumentNullException("group");
        if (ReferenceAxes == null)
            throw new PropertyInvalidException("ReferenceAxes", PropertyInvalidException.PropertyCannotBeNull);
        return group.CreateEvaluator<LinearTorqueEvaluator>(CreateEvaluator);
    }

    // This method, which is passed to the evaluator group in the method above as a delegate,
    // will only be called by the delegate if the evaluator does not yet exist in the group
    // and needs to be created.
    private LinearTorqueEvaluator CreateEvaluator(EvaluatorGroup group)
    {
        return new LinearTorqueEvaluator(group, m_epoch, m_initialTorque, m_torqueRate, m_referenceAxes);
    }

    // This method is used by PropagationEulerianAxes to combine all the torques
    // that are applied upon the rigid body.
    public override void BuildTorqueEvaluator(ResultantTorqueBuilder builder, EvaluatorGroup group)
    {
        if (builder == null)
            throw new ArgumentNullException("builder");
        if (group == null)
            throw new ArgumentNullException("group");
        builder.AddTorque(GetTorqueEvaluator(group));
    }

    private Cartesian m_initialTorque;
    private Cartesian m_torqueRate;
    private JulianDate m_epoch;
    private Axes m_referenceAxes;

    // This is the definition of the Evaluator that is used to actually evaluate this TorqueModel.
    // Because it is a private, nested class, it is not visible outside of the
    // LinearTorque class. This is ok, though, because once it is created users only
    // interact with it via a reference to its base class: TorqueEvaluator.
    private sealed class LinearTorqueEvaluator : TorqueEvaluator
    {
        // An evaluator should not store any data that the user will be able to change
        // after creating the evaluator.  This sometimes requires that data required by the
        // evaluator be copied or frozen using the IFreezable interface.
        public LinearTorqueEvaluator(EvaluatorGroup group, JulianDate epoch, Cartesian initialTorque,
                                     Cartesian torqueRate, Axes referenceAxes)
            : base(group)
        {
            m_initialTorque = initialTorque;
            m_torqueRate = torqueRate;
            m_epoch = epoch;
            m_referenceAxes = referenceAxes;
        }

        // The LinearTorqueEvaluator's copy constructor will be called from the Clone method.
        // Don't forget to call the base class implementation!
        private LinearTorqueEvaluator(LinearTorqueEvaluator existingInstance, CopyContext context)
            : base(existingInstance, context)
        {
            m_initialTorque = existingInstance.m_initialTorque;
            m_torqueRate = existingInstance.m_torqueRate;
            m_epoch = existingInstance.m_epoch;

            // Use the UpdateReference method on the CopyContext to perform any updates to the
            // reference that are required by the context.
            m_referenceAxes = context.UpdateReference(existingInstance.m_referenceAxes);
        }

        // This method is used by the EvaluatorGroup system to avoid redundant evaluations. The
        // EvaluatorGroup may call it on your evaluator in order to replace your evaluator's
        // reference to another evaluator with a reference to a version that caches its last
        // result.
        // 
        // In this example, there are no internal evaluators so the method does nothing.
        public override void UpdateEvaluatorReferences(CopyContext context)
        {
        }

        // The Clone method should call the copy constructor.
        public override object Clone(CopyContext context)
        {
            return new LinearTorqueEvaluator(this, context);
        }

        // Override the Dispose method to call the Dispose() method on any nested
        // evaluators or other disposable nested types.
        protected override void Dispose(bool disposing)
        {
        }

        // This method returns a collection of time intervals when data is 
        // available from this Evaluator.
        public override TimeIntervalCollection GetAvailabilityIntervals(TimeIntervalCollection consideredIntervals)
        {
            return consideredIntervals;
        }

        // This method determines if there is data available from this Evaluator at 
        // the specified date.
        public override bool IsAvailable(JulianDate date)
        {
            return true;
        }

        // This property determines if this Evaluator result changes depending on the time at
        // which it is evaluated.
        public override bool IsTimeVarying
        {
            // In this example, the torque is time-varying as long as the torque rate is not zero.
            get { return m_torqueRate != Cartesian.Zero; }
        }

        // This property determines if this Evaluator can safely be used from multiple threads
        // simultaneously. If the evaluator stores data during the Evaluate call, it is not thread
        // safe. Otherwise, it generally is thread safe as long as any nested evaluators it uses
        // are thread safe.
        public override bool IsThreadSafe
        {
            get { return true; }
        }

        // This is where we do the actual evaluation of the torque at a particular time.
        public override Cartesian Evaluate(JulianDate date)
        {
            // The initial torque vector is added to the torque rate multiplied
            // by the time elapsed since the reference epoch to form the linear
            // torque model.
            return m_initialTorque + m_epoch.SecondsDifference(date) * m_torqueRate;
        }

        // The evaluator is defined to produce torques that are defined
        // with respect to the reference axes, which is usually the body
        // axes of the rigid body.
        public override Axes DefinedIn
        {
            get { return m_referenceAxes; }
        }

        private readonly Cartesian m_initialTorque;
        private readonly Cartesian m_torqueRate;
        private readonly JulianDate m_epoch;
        private readonly Axes m_referenceAxes;
    }
}

// The first step of creating a NumericalPropagator is to create and populate
// a NumericalPropagatorDefinition.
NumericalPropagatorDefinition definition = new NumericalPropagatorDefinition();
// Epoch is September 5, 2022 at 11:30:00.000 UTC.
JulianDate epoch = new GregorianDate(2022, 9, 5, 11, 30, 0.0).ToJulianDate();
definition.Epoch = epoch;

// A PropagationNewtonianPoint is defined to provide the position of a
// gravity-gradient satellite in an equatorial circular orbit with an
// altitude of 300 km.
KeplerianElements elements = new KeplerianElements(WorldGeodeticSystem1984.SemimajorAxis + 300000.0,
                                                   0.0,
                                                   0.0,
                                                   0.0,
                                                   0.0,
                                                   0.0,
                                                   WorldGeodeticSystem1984.GravitationalParameter);
Motion<Cartesian> initialOrbitalState = elements.ToCartesian();
PropagationNewtonianPoint point = new PropagationNewtonianPoint();
point.InitialPosition = initialOrbitalState.Value;
point.InitialVelocity = initialOrbitalState.FirstDerivative;
point.AppliedForces.Add(new TwoBodyGravity(point.IntegrationPoint));
point.Mass = 100; // kg

// The attitude of the spacecraft is defined by a PropagationEulerianAxes.
Cartesian initialAngularVelocity = new Cartesian(0.05, 0.0, 0.01); // rad/s
UnitQuaternion initialAttitudeQuaternion = UnitQuaternion.Identity; // The body frame and inertial frame are initially aligned.
const double transverseMomentOfInertia = 200.0; // kg*m^2
const double spinMomentOfInertia = 100.0; // kg*m^2
Matrix3By3Symmetric axisymmetricInertiaMatrix = Matrix3By3Symmetric.DiagonalMatrix(transverseMomentOfInertia,
                                                                                   transverseMomentOfInertia,
                                                                                   spinMomentOfInertia);
PropagationEulerianAxes attitude = new PropagationEulerianAxes();
attitude.InitialAttitudeQuaternion = initialAttitudeQuaternion;
attitude.InitialAngularVelocity = initialAngularVelocity;
attitude.InertiaMatrix = axisymmetricInertiaMatrix;

// Add linear torque, magnetic coil torque, and gravity-gradient torque to PropagationEulerianAxes.
LinearTorque linearTorque
    = new LinearTorque(Cartesian.Zero, 0.00000001 * UnitCartesian.UnitZ, epoch, attitude.IntegrationAxes);

// The number of loops is the number of times the magnetic coil winds around its axis.
const double numberOfLoops = 3.0;

// The electric current is set to be constant at 10 amps.
Scalar current = new ScalarFixed(10.0);

// The directed area is 1.0 square meters in the body Z-axis. This means
// that the current is flowing through the loops in the counter-clockwise
// direction in the +Z axis of the body frame.
Cartesian directedArea = (Cartesian)UnitCartesian.UnitZ;
Vector directedAreaVector = new VectorFixed(attitude.IntegrationAxes, directedArea);

// The WMM model can be used to define Earth's magnetic field.
MagneticCoilTorque magneticCoilTorque = MagneticCoilTorque.CreateMagneticCoilTorqueUsingWmmModel(wmmPath,
                                                                                                 point.IntegrationPoint,
                                                                                                 numberOfLoops,
                                                                                                 current,
                                                                                                 directedAreaVector,
                                                                                                 attitude.IntegrationAxes);

// The IGRF model can also be used. It can be defined by either a .txt file or a .COF file.

GravityGradientTorque gravityGradientTorque
    = new GravityGradientTorque(point.IntegrationPoint, axisymmetricInertiaMatrix, attitude.IntegrationAxes);
attitude.AppliedTorques.Add(linearTorque);
attitude.AppliedTorques.Add(magneticCoilTorque);
attitude.AppliedTorques.Add(gravityGradientTorque);

// Add point and attitude to NumericalPropagatorDefinition.
definition.IntegrationElements.Add(point);
definition.IntegrationElements.Add(attitude);

// Define integrator.
RungeKutta4Integrator integrator = new RungeKutta4Integrator();
integrator.InitialStepSize = 1.0; // 1 second steps.
integrator.Direction = IntegrationSense.Increasing;
definition.Integrator = integrator;

// Create a propagator and propagate for 1 day.
NumericalPropagator propagator = definition.CreatePropagator();
NumericalPropagationStateHistory history = propagator.Propagate(new Duration(1, 0.0), 1);

// Extract results for the location and attitude of the gravity-gradient satellite.
DateMotionCollection<Cartesian> locationHistory = history.GetDateMotionCollection<Cartesian>(point.Identification);
DateMotionCollection<RotationVectorAngularVelocity> rotationHistory
    = history.GetDateMotionCollection<RotationVectorAngularVelocity>(attitude.Identification);

// Transform rotation history to a more usable form.
DateMotionCollection<UnitQuaternion, Cartesian> transformedRotationHistory
    = RotationVectorAngularVelocity.ToDateMotionCollectionUnitQuaternionCartesian(rotationHistory, 1);

// Extract the final Motion value from this collection.
Motion<UnitQuaternion, Cartesian> finalMotion = transformedRotationHistory.Motions[transformedRotationHistory.Count - 1];
UnitQuaternion finalAttitude = finalMotion.Value;
Cartesian finalAngularVelocity = finalMotion.FirstDerivative; // rad/s, in inertial coordinates.