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; }