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 SetColumnException2()
{
Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor);
m.SetColumn(4, Vector4D.One);
}
public void SetColumn()
{
Matrix44D m = new Matrix44D(rowMajor, MatrixOrder.RowMajor);
m.SetColumn(0, new Vector4D(0.1, 0.2, 0.3, 0.4));
Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetColumn(0));
Assert.AreEqual(new Vector4D(2.0, 6.0, 10.0, 14.0), m.GetColumn(1));
Assert.AreEqual(new Vector4D(3.0, 7.0, 11.0, 15.0), m.GetColumn(2));
Assert.AreEqual(new Vector4D(4.0, 8.0, 12.0, 16.0), m.GetColumn(3));
m.SetColumn(1, new Vector4D(0.4, 0.5, 0.6, 0.7));
Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetColumn(0));
Assert.AreEqual(new Vector4D(0.4, 0.5, 0.6, 0.7), m.GetColumn(1));
Assert.AreEqual(new Vector4D(3.0, 7.0, 11.0, 15.0), m.GetColumn(2));
Assert.AreEqual(new Vector4D(4.0, 8.0, 12.0, 16.0), m.GetColumn(3));
m.SetColumn(2, new Vector4D(0.7, 0.8, 0.9, 1.0));
Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetColumn(0));
Assert.AreEqual(new Vector4D(0.4, 0.5, 0.6, 0.7), m.GetColumn(1));
Assert.AreEqual(new Vector4D(0.7, 0.8, 0.9, 1.0), m.GetColumn(2));
Assert.AreEqual(new Vector4D(4.0, 8.0, 12.0, 16.0), m.GetColumn(3));
m.SetColumn(3, new Vector4D(1.1, 1.8, 1.9, 1.2));
Assert.AreEqual(new Vector4D(0.1, 0.2, 0.3, 0.4), m.GetColumn(0));
Assert.AreEqual(new Vector4D(0.4, 0.5, 0.6, 0.7), m.GetColumn(1));
Assert.AreEqual(new Vector4D(0.7, 0.8, 0.9, 1.0), m.GetColumn(2));
Assert.AreEqual(new Vector4D(1.1, 1.8, 1.9, 1.2), m.GetColumn(3));
}
