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