public void Integrate(Rigidbody body, float h) { //Setting Variable and calculating acceleration State currentState = body.State; body.Acceleration(currentState, h, out KVector2 acceleration, out double angularAcceleration); //Translation KVector2 velocity = currentState.Velocity + acceleration * h; KVector2 position = currentState.Transform.Position + currentState.Velocity * h; currentState.Velocity = velocity; currentState.Transform.Position = position; //Angular double angularVelocity = currentState.AngVelocity + angularAcceleration * h; double rotation = currentState.Transform.Rotation + currentState.AngVelocity * h; currentState.AngVelocity = angularVelocity; currentState.Transform.Rotation = rotation; currentState.ClearAccumulator(); currentState.Transform.SyncMatrix(); }
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); }