public void InverseTest()
{
var invExample = new Matrix3(0.5, -2, 4.4,
3, 0, 19,
4, 0, 0);
var invExpected = new Matrix3(0, 0, 0.25,
-0.5, 0.11578947368421054, -0.0243421052631579,
0, 0.052631578947368418, -0.039473684210526314);
Assert.AreEqual(invExpected, invExample.InverseMatrix());
}
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;
}
