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