Derivative Evaluate(State initial, float t, float dt, Derivative d)
{
State newState;
newState.x = initial.x + d.dx * dt;
newState.v = initial.v + d.dv * dt;
Derivative der;
der.dx = state.v;
der.dv = Acceleration(state, t + dt);
return(der);
}
//RK4 very close to complete
public void Update(float t, float dt)
{
preVelocity = state.v;
Derivative a = Evaluate(state, t, 0.0f, new Derivative());
Derivative b = Evaluate(state, t, dt * 0.5f, a);
Derivative c = Evaluate(state, t, dt * 0.5f, b);
Derivative d = Evaluate(state, t, dt, c);
Vector3 dxdt = 1.0f / 6.0f * (a.dx + 2.0f * (b.dx + c.dx) + d.dx);
Vector3 dvdt = 1.0f / 6.0f * (a.dv + 2.0f * (b.dv + c.dv) + d.dv);
state.x += dxdt * dt;
state.v += dvdt * dt;
bounds.Center = state.x;
}
