public void MultiplyOperator()
{
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(Vector4D.AreNumericallyEqual(
p1.ToMatrix44D() * p2.ToMatrix44D() * new Vector4D(1, 2, 3, 1),
p1 * p2 * new Vector4D(1, 2, 3, 1)));
}
public void TestIsZero()
{
Vector4D nonZero = new Vector4D(0.001, 0.001, 0.0, 0.001);
Assert.IsFalse(Vector4D.AreNumericallyEqual(nonZero, Vector4D.Zero));
Vector4D zero = new Vector4D(0.0000000000001, 0.0000000000001, 0.0, 0.0000000000001);
Assert.IsTrue(Vector4D.AreNumericallyEqual(zero, Vector4D.Zero));
}
public void AreEqualWithEpsilon()
{
double epsilon = 0.001;
Vector4D u = new Vector4D(1.0, 2.0, 3.0, 4.0);
Vector4D v = new Vector4D(1.002, 2.002, 3.002, 4.002);
Vector4D w = new Vector4D(1.0001, 2.0001, 3.0001, 4.0001);
Assert.IsTrue(Vector4D.AreNumericallyEqual(u, u, epsilon));
Assert.IsFalse(Vector4D.AreNumericallyEqual(u, v, epsilon));
Assert.IsTrue(Vector4D.AreNumericallyEqual(u, w, epsilon));
}
public void CosineInterpolationVector4D()
{
Vector4D v = new Vector4D(1.0, 10.0, 100.0, 1000.0);
Vector4D w = new Vector4D(2.0, 20.0, 200.0, 2000.0);
Vector4D lerp0 = InterpolationHelper.CosineInterpolation(v, w, 0.0);
Vector4D lerp1 = InterpolationHelper.CosineInterpolation(v, w, 1.0);
Vector4D lerp05 = InterpolationHelper.CosineInterpolation(v, w, 0.5);
Assert.AreEqual(v, lerp0);
Assert.AreEqual(w, lerp1);
Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(1.5, 15.0, 150.0, 1500.0), lerp05));
}
public void AreEqual()
{
double originalEpsilon = Numeric.EpsilonD;
Numeric.EpsilonD = 1e-8;
Vector4D u = new Vector4D(1.0, 2.0, 3.0, 4.0);
Vector4D v = new Vector4D(1.000001, 2.000001, 3.000001, 4.000001);
Vector4D w = new Vector4D(1.00000001, 2.00000001, 3.00000001, 4.00000001);
Assert.IsTrue(Vector4D.AreNumericallyEqual(u, u));
Assert.IsFalse(Vector4D.AreNumericallyEqual(u, v));
Assert.IsTrue(Vector4D.AreNumericallyEqual(u, w));
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);
}