#define RUNGE_KUTTA_4_INTEGRATION
using System;
namespace ConsoleApplication {
#region Runge Kutta 4 Integrator
/// <summary>Integrates position and velocity using the Runge Kutta 4 method</summary>
public struct RungeKutta4 {
#region struct State
/// <summary>Stores the current state of the system</summary>
private struct State {
/// <summary>Position of the object</summary>
public float Position;
/// <summary>Velocity the object is moving at</summary>
public float Velocity;
}
#endregion // struct State
#region struct Derivative
/// <summary>Stores the derivative state of the system</summary>
private struct Derivative {
/// <summary>Velocity of the object</summary>
public float Velocity;
/// <summary>Acceleration the object's velocity is increasing at</summary>
public float Acceleration;
}
#endregion // struct Derivative
/// <summary>Performs one Runge Kutta 4 simulation step</summary>
/// <param name="velocity">Velocity of the object</param>
/// <param name="acceleration">Acceleration of the object at (t + dt)</param>
/// <param name="deltaTime">Amount of time that will be simulated</param>
/// <returns>The translation of the object after the specified delta time</returns>
public float Step(ref float velocity, float acceleration, float deltaTime) {
var state = new State();
state.Position = 0.0f;
state.Velocity = velocity;
this.acceleration = acceleration;
integrate(ref state, deltaTime);
velocity = state.Velocity;
return state.Position;
}
/// <summary>Integrates the state over the specified delta time</summary>
/// <param name="state">State that will be integrated</param>
/// <param name="deltaTime">Amount of time that has passed</param>
private void integrate(ref State state, float deltaTime) {
Derivative a, b, c, d;
a = evaluate(state, 0.0f, new Derivative());
b = evaluate(state, deltaTime * 0.5f, a);
c = evaluate(state, deltaTime * 0.5f, b);
d = evaluate(state, deltaTime, c);
float velocity = (
1.0f / 6.0f * (a.Velocity + 2.0f * (b.Velocity + c.Velocity) + d.Velocity)
);
float acceleration = (
1.0f / 6.0f * (a.Acceleration + 2.0f * (b.Acceleration + c.Acceleration) + d.Acceleration)
);
state.Position += velocity * deltaTime;
state.Velocity += acceleration * deltaTime;
}
/// <summary>Linearly integrates a single state over the specified time</summary>
/// <param name="initial">State at the beginning of the simulation</param>
/// <param name="deltaTime">Amount of time that should be simulated</param>
/// <param name="derivative">Derivative describing how the state changes</param>
/// <returns>The state after the specified amount of time has passed</returns>
private Derivative evaluate(State initial, float deltaTime, Derivative derivative) {
State state;
state.Position = initial.Position + derivative.Velocity * deltaTime;
state.Velocity = initial.Velocity + derivative.Acceleration * deltaTime;
Derivative output;
output.Velocity = state.Velocity;
output.Acceleration = this.acceleration;
return output;
}
/// <summary>Acceleration (constant over each step)</summary>
private float acceleration;
}
#endregion // Runge Kutta 4 Integrator
internal class Program {
static void Test() {
float delta = 1f/120f;
float height = .001f;
float impulse = 6.264f;
float gravity = -9.81f;
float mass = 80f;
float velocity = 0;
float peakHeight = 0;
while (height > 0)
{
#if RUNGE_KUTTA_4_INTEGRATION
float acceleration = gravity/mass;
float impulses = impulse/mass;
impulse = 0;
acceleration += impulses/delta;
var rk4 = new RungeKutta4();
height += rk4.Step(ref velocity, acceleration * mass, delta);
#else // EULER_INTEGRATION
float acceleration = gravity/mass;
float impulses = impulse/mass;
impulse = 0;
acceleration += impulses/delta;
velocity += (acceleration*mass)*delta/2f;
height += velocity*delta;
velocity += (acceleration*mass)*delta/2f;
#endif
if (height > peakHeight)
peakHeight = height;
}
Console.WriteLine("PeakHeight: " + peakHeight);
}
public static void Main(string[] args) {
Test();
}
}
}