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