public RigidBody6DOF(IKinematics initialState, double mass, Matrix3 inertiaBody)
{
if (initialState == null) throw new ArgumentNullException(nameof(initialState));
if (mass <= 0) throw new ArgumentException(nameof(mass) + " must be larger than zero");
InvMass = 1.0 / mass;
InvIBody = inertiaBody.InverseMatrix();
kinematicBody = new EuclideanKinematicBody(initialState.Transform);
kinematicBody.V = initialState.V;
kinematicBody.Omega = initialState.Omega;
P = Mass * kinematicBody.V;
L = I * kinematicBody.Omega;
}
public RigidBody6DOF(IKinematics initialState, double mass, Matrix3 inertiaBody)
{
if (initialState == null)
{
throw new ArgumentNullException(nameof(initialState));
}
if (mass <= 0)
{
throw new ArgumentException(nameof(mass) + " must be larger than zero");
}
InvMass = 1.0 / mass;
InvIBody = inertiaBody.InverseMatrix();
kinematicBody = new EuclideanKinematicBody(initialState.Transform);
kinematicBody.V = initialState.V;
kinematicBody.Omega = initialState.Omega;
P = Mass * kinematicBody.V;
L = I * kinematicBody.Omega;
}
