示例#1
0
        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]);
                }
            }
        }
示例#2
0
 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));
 }
示例#3
0
    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));
    }
示例#4
0
    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));
    }
示例#5
0
    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));
    }
示例#6
0
        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)));
        }
示例#7
0
    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;
    }
示例#8
0
        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);
        }
示例#9
0
    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));
    }