Numerical Integration

// Define a function that returns the array of derivatives of vector y, given y and
// its dependent variable x. Since our system is one-dimensional, y has length 1, and
// so does our return value.
private static double[] Derivative(double x, double[] y)
{
    return new[] { Math.Cos(x) };
}

// Instantiate a system by using the derivative function and specifying the order of the dependent variable (y) in the differential equation.
DependentVariableDifferentialEquation systemOfEquations = new DependentVariableDifferentialEquation(new OrdinaryDifferentialEquationSystem(Derivative, 1));

// Instantiate a numerical integrator.
RungeKutta4Integrator rk4Integrator = new RungeKutta4Integrator(systemOfEquations);

RungeKuttaCashKarp45Integrator rkckIntegrator = new RungeKuttaCashKarp45Integrator(systemOfEquations, 0.1);

// Specify that we want to adapt step size to mitigate error.
rkckIntegrator.StepSizeBehavior = KindOfStepSize.Relative;

// Specify the tolerance we want to use.
rkckIntegrator.AbsoluteTolerance = Constants.Epsilon10;

rkckIntegrator.Initialize(0.1, new[] { 0.0 });

while (rkckIntegrator.FinalIndependentVariableValue < 4.7)
{
    rkckIntegrator.Integrate();
}

// Since the while loop guarantees that we reached or overshot our desired endpoint (x = 4.7),
// we reintegrate with a specific step size to reach exactly 4.7. Since the step size
// here is guaranteed to be smaller than our last step (which overshot 4.7),
// our computed value is at least as accurate as the value computed for our last step.
rkckIntegrator.Reintegrate(4.7 - rkckIntegrator.InitialIndependentVariableValue);
y = rkckIntegrator.FinalDependentVariableValues;

private static double[] PiecewiseDerivative(double x, double[] y)
{
    if ((int)x % 2 == 0)
    {
        return new[] { 10.0 };
    }
    return new[] { -10.0 };
}

// Define a function that returns the array of derivatives of vector y, given y and
// its dependent variable x.
private static double[] Derivatives(double x, double[] y)
{
    return new[] { y[0] * 1e-5 * x, y[1] * 1e-5 * x, y[2] * 1e-5 * x };
}

// Here we specify the order of each dependent variable (i.e. each component of y) in the differential equation.
systemOfEquations = new DependentVariableDifferentialEquation(new OrdinaryDifferentialEquationSystem(Derivatives, 1, 1, 1));

// Instantiate the integrator.
RungeKuttaFehlberg78Integrator rkfIntegrator = new RungeKuttaFehlberg78Integrator
{
    InitialStepSize = 1,
    StepSizeBehavior = KindOfStepSize.Relative,
    SystemOfEquations = systemOfEquations
};

// Specify initial conditions and integrate.
rkfIntegrator.Initialize(0.1, new[] { -1, 0.1, 3.124 });

while (rkfIntegrator.FinalIndependentVariableValue < 61.1)
{
    rkfIntegrator.Integrate();
}

rkfIntegrator.Reintegrate(61.1 - rkfIntegrator.InitialIndependentVariableValue);
y = rkfIntegrator.FinalDependentVariableValues;

// Define a function that returns the first derivative of the first component of y and the
// second derivative of the second component of y, given y and its dependent variable x.
// Note the format of y: y1, y1', y2. In general, a dependent variable's derivatives' values
// immediately follow its own value.
private static double[] SecondOrderDerivatives(double x, double[] y)
{
    return new[] { -g, y[2] * 1e-5 * x };
}

// Here we specify that y's first component has order 2, and its second component has order 1.
systemOfEquations = new DependentVariableDifferentialEquation(new OrdinaryDifferentialEquationSystem(SecondOrderDerivatives, 2, 1));

// Instantiate the integrator.
rk4Integrator = new RungeKutta4Integrator(systemOfEquations);

// Specify initial conditions.
rk4Integrator.Initialize(0.0, new[] { 0.0, g, 0.1 });

rk4Integrator.Integrate();
rk4Integrator.Integrate();

y = rk4Integrator.FinalDependentVariableValues;

private static readonly JulianDate s_startDate = GregorianDate.Parse("20000831T124242.00").ToJulianDate();
private PointEvaluator m_pointEvaluator;
private ForceEvaluator m_gravityEvaluator;
private ForceEvaluator m_thrustEvaluator;
private ScalarEvaluator m_massEvaluator;
private const double StepSize = 0.1;

private void GenerateEvaluatorsFromPropagation()
{
    CentralBody earth = CentralBodiesFacet.GetFromContext().Earth;

    // Create a newtonian point to store force models for the numerical propagator.
    PropagationNewtonianPoint newtonianPoint = new PropagationNewtonianPoint();
    newtonianPoint.Identification = "myPoint";

    // Initialize the point approximately at the Earth's surface, at rest.
    newtonianPoint.InitialPosition = new Cartesian(0, 0, 6.37e6); 
    newtonianPoint.InitialVelocity = new Cartesian(0, 0, 0); 

    // Set the point's mass to a custom scalar that decreases with time, representing the rocket's fuel depletion.
    newtonianPoint.Mass = new MassScalar(); 

    ForceModel gravityModel = new TwoBodyGravity(newtonianPoint.IntegrationPoint);
    ForceModel thrustModel = new ConstantForce(earth.FixedFrame.Axes, new Cartesian(0, 0, 30e6));

    newtonianPoint.AppliedForces.Add(gravityModel);
    newtonianPoint.AppliedForces.Add(thrustModel);

    // Create a propagator and get a motion collection on a 20 second interval.
    NumericalPropagatorDefinition def = new NumericalPropagatorDefinition();
    def.Integrator = new RungeKutta4Integrator
    {
        InitialStepSize = StepSize
    };
    def.Epoch = s_startDate;
    def.IntegrationElements.Add(newtonianPoint);

    NumericalPropagator propagator = def.CreatePropagator();
    NumericalPropagationStateHistory history = propagator.Propagate(new Duration(0, 20), 1);
    DateMotionCollection<Cartesian> motions = history.GetDateMotionCollection<Cartesian>("myPoint");

    // Get a point representing the propagation that we can use for later analysis.
    Point point = new PointInterpolator(earth.FixedFrame, InterpolationAlgorithmType.Linear, 1, motions);
    m_pointEvaluator = point.GetEvaluator();

    // Get evaluators for later analysis.
    m_gravityEvaluator = new TwoBodyGravity(point).GetForceEvaluator();
    m_thrustEvaluator = thrustModel.GetForceEvaluator();
    m_massEvaluator = new MassScalar().GetEvaluator();
}

// Recall that the derivative of work is power, i.e. force times velocity.
private double[] ThrustPower(double t, double[] w)
{
    JulianDate evalDate = s_startDate.Add(new Duration(0, t));
    return new[] { m_thrustEvaluator.Evaluate(evalDate).Dot(m_pointEvaluator.Evaluate(evalDate, 1).FirstDerivative) };
}

GenerateEvaluatorsFromPropagation();

DependentVariableDifferentialEquation systemOfEquations = new DependentVariableDifferentialEquation(new OrdinaryDifferentialEquationSystem(ThrustPower, 1));
RungeKutta4Integrator rk4Integrator = new RungeKutta4Integrator(systemOfEquations);
rk4Integrator.InitialStepSize = StepSize;

// At t=0, the rocket hasn't done any work yet.
rk4Integrator.Initialize(0, new[] { 0.0 });

// Populate a list of work values along a 10 second interval.
List<double> thrustWorks = new List<double>();
for (int i = 0; i < 100; i++)
{
    rk4Integrator.Integrate();
    double[] w = rk4Integrator.FinalDependentVariableValues;
    thrustWorks.Add(w[0]);
}

private double[] GravityPower(double t, double[] w)
{
    JulianDate evalDate = s_startDate.Add(new Duration(0, t));
    // TwoBodyGravity is defined as a specific force, so we must multiply by mass to get the force in Newtons.
    return new[] { m_massEvaluator.Evaluate(evalDate) * m_gravityEvaluator.Evaluate(evalDate).Dot(m_pointEvaluator.Evaluate(evalDate, 1).FirstDerivative) };
}

systemOfEquations = new DependentVariableDifferentialEquation(new OrdinaryDifferentialEquationSystem(GravityPower, 1));
rk4Integrator = new RungeKutta4Integrator(systemOfEquations);
rk4Integrator.InitialStepSize = StepSize;

rk4Integrator.Initialize(0, new[] { 0.0 });

List<double> gravityWorks = new List<double>();
for (int i = 0; i < 100; i++)
{
    rk4Integrator.Integrate();
    double[] w = rk4Integrator.FinalDependentVariableValues;
    gravityWorks.Add(w[0]);
}