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