public void Test1()
{
Pose p = Pose.Identity;
Assert.AreEqual(Matrix44F.Identity, p.ToMatrix44F());
Assert.AreEqual(Matrix33F.Identity, p.Orientation);
Assert.AreEqual(Vector3F.Zero, p.Position);
p.Position = new Vector3F(1, 2, 3);
p.Orientation = Matrix33F.CreateRotation(new Vector3F(3, -4, 9), 0.49f);
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitX), 0), p * new Vector4F(1, 0, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitY), 0), p * new Vector4F(0, 1, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldDirection(Vector3F.UnitZ), 0), p * new Vector4F(0, 0, 1, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitX), 1), p * new Vector4F(1, 0, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitY), 1), p * new Vector4F(0, 1, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToWorldPosition(Vector3F.UnitZ), 1), p * new Vector4F(0, 0, 1, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitX), 0), p.Inverse * new Vector4F(1, 0, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitY), 0), p.Inverse * new Vector4F(0, 1, 0, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalDirection(Vector3F.UnitZ), 0), p.Inverse * new Vector4F(0, 0, 1, 0)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitX), 1), p.Inverse * new Vector4F(1, 0, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitY), 1), p.Inverse * new Vector4F(0, 1, 0, 1)));
Assert.IsTrue(Vector4F.AreNumericallyEqual(new Vector4F(p.ToLocalPosition(Vector3F.UnitZ), 1), p.Inverse * new Vector4F(0, 0, 1, 1)));
Pose p2 = Pose.FromMatrix(new Matrix44F(p.Orientation, Vector3F.Zero));
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.IsTrue(Vector3F.AreNumericallyEqual(p2.Position, Vector3F.Zero));
Matrix44F m = p2;
m.SetColumn(3, new Vector4F(p.Position, 1));
p2 = Pose.FromMatrix(m);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(p.Orientation, p2.Orientation));
Assert.AreEqual(p.Position, p2.Position);
//Assert.IsTrue(Vector3F.AreNumericallyEqual(p.Position, p2.Position));
// Test other constructors.
Assert.AreEqual(Vector3F.Zero, new Pose(QuaternionF.CreateRotationX(0.3f)).Position);
Assert.AreEqual(Matrix33F.CreateRotationX(0.3f), new Pose(Matrix33F.CreateRotationX(0.3f)).Orientation);
Assert.AreEqual(new Vector3F(1, 2, 3), new Pose(new Vector3F(1, 2, 3)).Position);
Assert.AreEqual(Matrix33F.Identity, new Pose(new Vector3F(1, 2, 3)).Orientation);
}
public void SetColumnException2()
{
Matrix44F m = new Matrix44F(rowMajor, MatrixOrder.RowMajor);
m.SetColumn(4, Vector4F.One);
}
public void SetColumn()
{
Matrix44F m = new Matrix44F(rowMajor, MatrixOrder.RowMajor);
m.SetColumn(0, new Vector4F(0.1f, 0.2f, 0.3f, 0.4f));
Assert.AreEqual(new Vector4F(0.1f, 0.2f, 0.3f, 0.4f), m.GetColumn(0));
Assert.AreEqual(new Vector4F(2.0f, 6.0f, 10.0f, 14.0f), m.GetColumn(1));
Assert.AreEqual(new Vector4F(3.0f, 7.0f, 11.0f, 15.0f), m.GetColumn(2));
Assert.AreEqual(new Vector4F(4.0f, 8.0f, 12.0f, 16.0f), m.GetColumn(3));
m.SetColumn(1, new Vector4F(0.4f, 0.5f, 0.6f, 0.7f));
Assert.AreEqual(new Vector4F(0.1f, 0.2f, 0.3f, 0.4f), m.GetColumn(0));
Assert.AreEqual(new Vector4F(0.4f, 0.5f, 0.6f, 0.7f), m.GetColumn(1));
Assert.AreEqual(new Vector4F(3.0f, 7.0f, 11.0f, 15.0f), m.GetColumn(2));
Assert.AreEqual(new Vector4F(4.0f, 8.0f, 12.0f, 16.0f), m.GetColumn(3));
m.SetColumn(2, new Vector4F(0.7f, 0.8f, 0.9f, 1.0f));
Assert.AreEqual(new Vector4F(0.1f, 0.2f, 0.3f, 0.4f), m.GetColumn(0));
Assert.AreEqual(new Vector4F(0.4f, 0.5f, 0.6f, 0.7f), m.GetColumn(1));
Assert.AreEqual(new Vector4F(0.7f, 0.8f, 0.9f, 1.0f), m.GetColumn(2));
Assert.AreEqual(new Vector4F(4.0f, 8.0f, 12.0f, 16.0f), m.GetColumn(3));
m.SetColumn(3, new Vector4F(1.1f, 1.8f, 1.9f, 1.2f));
Assert.AreEqual(new Vector4F(0.1f, 0.2f, 0.3f, 0.4f), m.GetColumn(0));
Assert.AreEqual(new Vector4F(0.4f, 0.5f, 0.6f, 0.7f), m.GetColumn(1));
Assert.AreEqual(new Vector4F(0.7f, 0.8f, 0.9f, 1.0f), m.GetColumn(2));
Assert.AreEqual(new Vector4F(1.1f, 1.8f, 1.9f, 1.2f), m.GetColumn(3));
}
