public void Integrate(Rigidbody body, float h)
{
State currentState = body.State;
Derivative k1, k2, k3, k4;
Derivative init = new Derivative
{
dPosition = body.State.Velocity,
dRotation = body.State.AngVelocity,
dVelocity = KVector2.Zero,
dAngVelocity = 0f
};
k1 = RK.FindK(body, 0, init);
k2 = RK.FindK(body, h * 0.5f, k1 * 0.5f);
k3 = RK.FindK(body, h * 0.5f, k2 * 0.5f);
k4 = RK.FindK(body, h, k3);
const float mult = 1.0f / 6.0f;
currentState.Transform.Position += mult * (k1.dPosition + 2.0f * (k2.dPosition + k3.dPosition) + k4.dPosition) * h;
currentState.Transform.Rotation += mult * (k1.dRotation + 2.0f * (k2.dRotation + k3.dRotation) + k4.dRotation) * h;
currentState.Velocity += mult * (k1.dVelocity + 2.0f * (k2.dVelocity + k3.dVelocity) + k4.dVelocity) * h;
currentState.AngVelocity += mult * (k1.dAngVelocity + 2.0f * (k2.dAngVelocity + k3.dAngVelocity) + k4.dAngVelocity) * h;
currentState.ClearAccumulator();
//Optimisation: Compute after angular and movement calculation
currentState.Transform.SyncMatrix();
}
public void Integrate(Rigidbody body, float h)
{
State state = body.State;
Derivative k1, k2;
Derivative init = new Derivative
{
dPosition = body.State.Velocity,
dRotation = body.State.AngVelocity,
dVelocity = KVector2.Zero,
dAngVelocity = 0f
};
k1 = RK.FindK(body, 0, init);
k2 = RK.FindK(body, h * 0.5f, k1 * 0.5f);
state.Transform.Position += (k2.dPosition + k1.dPosition) * 0.5f * h;
state.Transform.Rotation += (k2.dRotation + k1.dRotation) * 0.5f * h;
state.Velocity += (k2.dVelocity + k1.dVelocity) * 0.5f * h;
state.AngVelocity += (k2.dAngVelocity + k1.dAngVelocity) * 0.5f * h;
state.ClearAccumulator();
//Optimisation: Compute after angular and translation calculation
state.Transform.SyncMatrix();
}
public static Derivative Multiply(Derivative d, float k)
{
Derivative output;
output.dPosition = d.dPosition * k;
output.dVelocity = d.dVelocity * k;
output.dRotation = d.dRotation * k;
output.dAngVelocity = d.dAngVelocity * k;
return(output);
}
public static Derivative FindK(Rigidbody body, float h, Derivative d)
{
State state = new State();
state.Transform.Position = body.State.Transform.Position + d.dPosition * h;
state.Transform.Rotation = body.State.Transform.Rotation + d.dRotation * h;
state.Velocity = d.dPosition + d.dVelocity * h;
state.AngVelocity = d.dRotation + d.dAngVelocity * h;
Derivative output;
output.dPosition = state.Velocity;
output.dRotation = state.AngVelocity;
body.Acceleration(state, h, out output.dVelocity, out output.dAngVelocity);
return(output);
}