public void MultiplyMatrix()
{
Matrix33D m = new Matrix33D(12, 23, 45,
67, 89, 90,
43, 65, 87);
Assert.AreEqual(Matrix33D.Zero, Matrix33D.Multiply(m, Matrix33D.Zero));
Assert.AreEqual(Matrix33D.Zero, Matrix33D.Multiply(Matrix33D.Zero, m));
Assert.AreEqual(m, Matrix33D.Multiply(m, Matrix33D.Identity));
Assert.AreEqual(m, Matrix33D.Multiply(Matrix33D.Identity, m));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, Matrix33D.Multiply(m, m.Inverse)));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, Matrix33D.Multiply(m.Inverse, m)));
Matrix33D m1 = new Matrix33D(columnMajor, MatrixOrder.ColumnMajor);
Matrix33D m2 = new Matrix33D(12, 23, 45,
67, 89, 90,
43, 65, 87);
Matrix33D result = Matrix33D.Multiply(m1, m2);
for (int column = 0; column < 3; column++)
{
for (int row = 0; row < 3; row++)
{
Assert.AreEqual(Vector3D.Dot(m1.GetRow(row), m2.GetColumn(column)), result[row, column]);
}
}
}
public void InverseWithNearSingularMatrix()
{
Matrix33D m = new Matrix33D(0.0001, 0, 0,
0, 0.0001, 0,
0, 0, 0.0001);
Vector3D v = Vector3D.One;
Vector3D w = m * v;
Assert.IsTrue(Vector3D.AreNumericallyEqual(v, m.Inverse * w));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, m * m.Inverse));
}
public void CreateRotationZ()
{
double angle = (double)MathHelper.ToRadians(30);
Matrix33D m = Matrix33D.CreateRotationZ(angle);
Assert.IsTrue(Vector3D.AreNumericallyEqual(new Vector3D((double)Math.Cos(angle), (double)Math.Sin(angle), 0), m * Vector3D.UnitX));
QuaternionD q = QuaternionD.CreateRotation(Vector3D.UnitZ, angle);
Assert.IsTrue(Vector3D.AreNumericallyEqual(q.Rotate(Vector3D.One), m * Vector3D.One));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.CreateRotation(Vector3D.UnitZ, angle), m));
}
public void Inverse()
{
Assert.AreEqual(Matrix33D.Identity, Matrix33D.Identity.Inverse);
Matrix33D m = new Matrix33D(1, 2, 3,
2, 5, 8,
7, 6, -1);
Vector3D v = Vector3D.One;
Vector3D w = m * v;
Assert.IsTrue(Vector3D.AreNumericallyEqual(v, m.Inverse * w));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, m * m.Inverse));
}
public void AreEqualWithEpsilon()
{
double epsilon = 0.001;
Matrix33D m0 = new Matrix33D(columnMajor, MatrixOrder.ColumnMajor);
Matrix33D m1 = new Matrix33D(columnMajor, MatrixOrder.ColumnMajor);
m1 += new Matrix33D(0.002);
Matrix33D m2 = new Matrix33D(columnMajor, MatrixOrder.ColumnMajor);
m2 += new Matrix33D(0.0001);
Assert.IsTrue(Matrix33D.AreNumericallyEqual(m0, m0, epsilon));
Assert.IsFalse(Matrix33D.AreNumericallyEqual(m0, m1, epsilon));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(m0, m2, epsilon));
}
public void Interpolate()
{
PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
PoseD p2 = new PoseD(new Vector3D(-4, 5, -6), QuaternionD.CreateRotationZ(-0.1));
Assert.IsTrue(Vector3D.AreNumericallyEqual(p1.Position, PoseD.Interpolate(p1, p2, 0).Position));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p1.Orientation, PoseD.Interpolate(p1, p2, 0).Orientation));
Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, PoseD.Interpolate(p1, p2, 1).Position));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p2.Orientation, PoseD.Interpolate(p1, p2, 1).Orientation));
Assert.IsTrue(Vector3D.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3), PoseD.Interpolate(p1, p2, 0.3).Position));
Assert.IsTrue(
QuaternionD.AreNumericallyEqual(
InterpolationHelper.Lerp(QuaternionD.CreateRotation(p1.Orientation), QuaternionD.CreateRotation(p2.Orientation), 0.3),
QuaternionD.CreateRotation(PoseD.Interpolate(p1, p2, 0.3).Orientation)));
}
public void AreEqual()
{
double originalEpsilon = Numeric.EpsilonD;
Numeric.EpsilonD = 1e-8;
Matrix33D m0 = new Matrix33D(columnMajor, MatrixOrder.ColumnMajor);
Matrix33D m1 = new Matrix33D(columnMajor, MatrixOrder.ColumnMajor);
m1 += new Matrix33D(0.000001);
Matrix33D m2 = new Matrix33D(columnMajor, MatrixOrder.ColumnMajor);
m2 += new Matrix33D(0.00000001);
Assert.IsTrue(Matrix33D.AreNumericallyEqual(m0, m0));
Assert.IsFalse(Matrix33D.AreNumericallyEqual(m0, m1));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(m0, m2));
Numeric.EpsilonD = originalEpsilon;
}
public void Test1()
{
PoseD p = PoseD.Identity;
Assert.AreEqual(Matrix44D.Identity, p.ToMatrix44D());
Assert.AreEqual(Matrix33D.Identity, p.Orientation);
Assert.AreEqual(Vector3D.Zero, p.Position);
p.Position = new Vector3D(1, 2, 3);
p.Orientation = Matrix33D.CreateRotation(new Vector3D(3, -4, 9), 0.49);
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitX), 0), p * new Vector4D(1, 0, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitY), 0), p * new Vector4D(0, 1, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitZ), 0), p * new Vector4D(0, 0, 1, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitX), 1), p * new Vector4D(1, 0, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitY), 1), p * new Vector4D(0, 1, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitZ), 1), p * new Vector4D(0, 0, 1, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitX), 0), p.Inverse * new Vector4D(1, 0, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitY), 0), p.Inverse * new Vector4D(0, 1, 0, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitZ), 0), p.Inverse * new Vector4D(0, 0, 1, 0)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitX), 1), p.Inverse * new Vector4D(1, 0, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitY), 1), p.Inverse * new Vector4D(0, 1, 0, 1)));
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitZ), 1), p.Inverse * new Vector4D(0, 0, 1, 1)));
PoseD p2 = PoseD.FromMatrix(new Matrix44D(p.Orientation, Vector3D.Zero));
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, Vector3D.Zero));
Matrix44D m = p2;
m.SetColumn(3, new Vector4D(p.Position, 1));
p2 = PoseD.FromMatrix(m);
Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.AreEqual(p.Position, p2.Position);
//Assert.IsTrue(Vector3D.AreNumericallyEqual(p.Position, p2.Position));
// Test other constructors.
Assert.AreEqual(Vector3D.Zero, new PoseD(QuaternionD.CreateRotationX(0.3)).Position);
Assert.AreEqual(Matrix33D.CreateRotationX(0.3), new PoseD(Matrix33D.CreateRotationX(0.3)).Orientation);
Assert.AreEqual(new Vector3D(1, 2, 3), new PoseD(new Vector3D(1, 2, 3)).Position);
Assert.AreEqual(Matrix33D.Identity, new PoseD(new Vector3D(1, 2, 3)).Orientation);
}
public void Orthogonalize()
{
var m = Matrix33D.CreateRotationX(0.1) * Matrix33D.CreateRotationX(20) * Matrix33D.CreateRotationZ(1000);
// Introduce error.
m.M01 += 0.1f;
m.M22 += 0.1f;
Assert.IsFalse(m.IsOrthogonal);
Assert.IsFalse(m.IsRotation);
m.Orthogonalize();
Assert.IsTrue(m.IsOrthogonal);
Assert.IsTrue(m.IsRotation);
// Orthogonalizing and orthogonal matrix does not change the matrix.
var n = m;
n.Orthogonalize();
Assert.IsTrue(Matrix33D.AreNumericallyEqual(m, n));
}
