public void DeterminantTest() { const double expectedDet = -152; var detExample = new Matrix3(0.5, -2, 4.4, 3, 0, 19, 4, 0, 0); Assert.AreEqual(expectedDet, detExample.Determinant); }
public void TransposeTest() { var transExample = new Matrix3(0.5, -2, 4.4, 3, 0, 19, 4, 0, 0); var transExpected = new Matrix3(0.5, 3, 4, -2, 0, 0, 4.4, 19, 0); Assert.AreEqual(transExpected, transExample.TransposeMatrix()); }
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; }
public void OrthogonalTest() { Assert.IsTrue(Matrix3.Identity.IsOrthoganol); Assert.IsTrue(Matrix3.Identity.MultScaler(-1).IsOrthoganol); var orthExample = new Matrix3(0, -0.8, -0.6, 0.8, -0.36, 0.48, 0.6, 0.48, -0.64); Assert.IsTrue(orthExample.IsOrthoganol); var orthExample2 = new Matrix3(0, -1, 0, 1, 0, 0, 0, 0, -1); Assert.IsTrue(orthExample2.IsOrthoganol); var notOrthExample = new Matrix3(0, -1, 12, 0, 5, 0, 0, 0, -1); Assert.IsFalse(notOrthExample.IsOrthoganol); }
public Matrix3 Add(Matrix3 m) { return new Matrix3(XX + m.XX, XY + m.XY, XZ + m.XZ, YX + m.YX, YY + m.YY, YZ + m.YZ, ZX + m.ZX, ZY + m.ZY, ZZ + m.ZZ); }
public bool Equals(Matrix3 m) { if (XX != m.XX) return false; if (XY != m.XY) return false; if (XZ != m.XZ) return false; if (YX != m.YX) return false; if (YY != m.YY) return false; if (YZ != m.YZ) return false; if (ZX != m.ZX) return false; if (ZY != m.ZY) return false; if (ZZ != m.ZZ) return false; return true; }
public Matrix3 Rotate(Matrix3 rotationMatrix) { if (!rotationMatrix.IsOrthoganol) throw new ArgumentException("rotationMatrix must be orthoganol"); return rotationMatrix * this * rotationMatrix.TransposeMatrix(); }
public Matrix3 MultMatrix(Matrix3 rhs) { return new Matrix3( // row 1 XX * rhs.XX + XY * rhs.YX + XZ * rhs.ZX, XX * rhs.XY + XY * rhs.YY + XZ * rhs.ZY, XX * rhs.XZ + XY * rhs.YZ + XZ * rhs.ZZ, // row 2 YX * rhs.XX + YY * rhs.YX + YZ * rhs.ZX, YX * rhs.XY + YY * rhs.YY + YZ * rhs.ZY, YX * rhs.XZ + YY * rhs.YZ + YZ * rhs.ZZ, // row 3 ZX * rhs.XX + ZY * rhs.YX + ZZ * rhs.ZX, ZX * rhs.XY + ZY * rhs.YY + ZZ * rhs.ZY, ZX * rhs.XZ + ZY * rhs.YZ + ZZ * rhs.ZZ); }
public Matrix3 Subtract(Matrix3 rhs) { return new Matrix3(XX - rhs.XX, XY - rhs.XY, XZ - rhs.XZ, YX - rhs.YX, YY - rhs.YY, YZ - rhs.YZ, ZX - rhs.ZX, ZY - rhs.ZY, ZZ - rhs.ZZ); }
private bool Eq(Matrix3 other) { return x == other.x && y == other.y && z == other.z; }
private bool Eq(Matrix3 other) { return(x == other.x && y == other.y && z == other.z); }
public Transform Rotate(Matrix3 m) => Rotate(Quaternion.FromRotMatrix(m));
public Transform(Vector3 pos, Matrix3 r) { Pos = pos; Q = Quaternion.FromRotMatrix(r); }