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