public void AreNumericallyEqual()
{
var a = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(1, 2, 3, 4, 5, 6, 7, 8, 9));
var b = a;
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a));
b = AddToAllComponents(a, Numeric.EpsilonD / 10);
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
b = AddToAllComponents(a, Numeric.EpsilonD * 10);
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD * 100));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD * 100));
b = a;
b.Position.X -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Position.Y -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Position.Z -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M00 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M01 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M02 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M10 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M11 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M12 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M20 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M21 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
b = a;
b.Orientation.M22 -= 0.0001;
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
}
