protected Derivative evaluate(State initial, float t, float dt, Derivative d, Vector3 accel)
{
Vector3 Position = initial.Position + d.dx * dt;
Vector3 Velocity = initial.Velocity + d.dv * dt;
State state = new State(Position, Velocity);
Derivative output = new Derivative();
output.dx = state.Velocity;
output.dv = accel;
return output;
}
public override State integrate(State state, float t, float dt, Vector3 accel)
{
/*
Derivative d = evaluate(state, t, dt, new Derivative(), accel);
Vector3 dxdt = d.dx;
Vector3 dvdt = d.dv;
*/
state.Position = state.Position + state.Velocity * dt;
state.Velocity = state.Velocity + accel * dt;
return state;
}
public override State integrate(State state, float t, float dt, Vector3 accel)
{
Derivative a = evaluate(state, t, 0.0f, new Derivative(), accel);
Derivative b = evaluate(state, t + dt * 0.5f, dt * 0.5f, a, accel);
Derivative c = evaluate(state, t + dt * 0.5f, dt * 0.5f, b, accel);
Derivative d = evaluate(state, t + dt, dt, c, accel);
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);
Vector3 finalPosition = state.Position + dxdt * dt;
Vector3 finalVelocity = state.Velocity + dvdt * dt;
return new State(finalPosition, finalVelocity);
}
public abstract State integrate(State state, float t, float dt, Vector3 accel);
public override void Update(GameTime gameTime)
{
switch (game.gameState)
{
case GameState.Play:
float elapsedSeconds = (float)gameTime.ElapsedGameTime.TotalSeconds;
t += elapsedSeconds;
foreach (IPhysicsObject physicsObject in physicsObjects)
{
Vector3 initialPosition = physicsObject.GetPosition();
Vector3 initialVelocity = physicsObject.GetVelocity();
Vector3 gravity = PhysicsUtil.GravityConstant * Vector3.Down;
State initialState = new State(initialPosition, initialVelocity);
State finalState = integrator.integrate(initialState, t, elapsedSeconds, gravity);
Vector3 finalPosition = physicsObject.DoBoundsCheck() ? BoundsCheck(finalState.Position) : finalState.Position;
physicsObject.UpdatePosition(finalPosition);
physicsObject.UpdateVelocity(finalState.Velocity);
DoCollisionDetectionWithTerrain(physicsObject);
}
base.Update(gameTime);
break;
}
}
