public void SerializationJson()
{
PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
PoseD pose2;
string fileName = "SerializationPoseD.json";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
var serializer = new DataContractJsonSerializer(typeof(PoseD));
using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
serializer.WriteObject(stream, pose1);
using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
pose2 = (PoseD)serializer.ReadObject(stream);
Assert.AreEqual(pose1, pose2);
}
public void SerializationXml()
{
PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
PoseD pose2;
string fileName = "SerializationPoseD.xml";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
var serializer = new DataContractSerializer(typeof(PoseD));
using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
using (var writer = XmlDictionaryWriter.CreateTextWriter(stream, Encoding.UTF8))
serializer.WriteObject(writer, pose1);
using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
using (var reader = XmlDictionaryReader.CreateTextReader(stream, new XmlDictionaryReaderQuotas()))
pose2 = (PoseD)serializer.ReadObject(reader);
Assert.AreEqual(pose1, pose2);
}
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));
}
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)));
}
private void OnComponentChanged()
{
if (_isUpdating)
return;
_isUpdating = true;
try
{
if (_valueType == typeof(Pose))
{
Value = new Pose(new Vector3F((float)X, (float)Y, (float)Z),
new Matrix33F((float)M00, (float)M01, (float)M02,
(float)M10, (float)M11, (float)M12,
(float)M20, (float)M21, (float)M22));
}
else if (_valueType == typeof(PoseD))
{
Value = new PoseD(new Vector3D(X, Y, Z),
new Matrix33D(M00, M01, M02,
M10, M11, M12,
M20, M21, M22));
}
}
finally
{
_isUpdating = false;
}
}
private PoseD AddToAllComponents(PoseD pose, double epsilon)
{
var result = pose;
result.Position.X += epsilon;
result.Position.Y += epsilon;
result.Position.Z += epsilon;
result.Orientation.M00 += epsilon;
result.Orientation.M01 += epsilon;
result.Orientation.M02 += epsilon;
result.Orientation.M10 += epsilon;
result.Orientation.M11 += epsilon;
result.Orientation.M12 += epsilon;
result.Orientation.M20 += epsilon;
result.Orientation.M21 += epsilon;
result.Orientation.M22 += epsilon;
return result;
}
public void ToPose()
{
PoseD poseD = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
Pose pose = new Pose(new Vector3F(1, 2, 3), new Matrix33F(4, 5, 6, 7, 8, 9, 10, 11, 12));
Assert.AreEqual(pose, (Pose)poseD);
Assert.AreEqual(pose, poseD.ToPose());
}
public void SerializationXml()
{
PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
PoseD pose2;
string fileName = "SerializationPoseD.xml";
if (File.Exists(fileName))
File.Delete(fileName);
var serializer = new DataContractSerializer(typeof(PoseD));
using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
using (var writer = XmlDictionaryWriter.CreateTextWriter(stream, Encoding.UTF8))
serializer.WriteObject(writer, pose1);
using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
using (var reader = XmlDictionaryReader.CreateTextReader(stream, new XmlDictionaryReaderQuotas()))
pose2 = (PoseD)serializer.ReadObject(reader);
Assert.AreEqual(pose1, pose2);
}
public void SerializationJson()
{
PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
PoseD pose2;
string fileName = "SerializationPoseD.json";
if (File.Exists(fileName))
File.Delete(fileName);
var serializer = new DataContractJsonSerializer(typeof(PoseD));
using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
serializer.WriteObject(stream, pose1);
using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
pose2 = (PoseD)serializer.ReadObject(stream);
Assert.AreEqual(pose1, pose2);
}
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 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));
}
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)));
}
public void GetHashCodeTest()
{
PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
PoseD p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
Assert.AreEqual(p1.GetHashCode(), p2.GetHashCode());
p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
p2 = new PoseD(new Vector3D(2, 1, 3), QuaternionD.CreateRotationY(0.3));
Assert.AreNotEqual(p1.GetHashCode(), p2.GetHashCode());
// Too bad two rotation matrices that differ only by the sign of the angle
// (+/- angle with same axis) have the same hashcodes. See KB -> .NET --> GetHashCode
//p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
//p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(-0.3));
//Assert.AreNotEqual(p1.GetHashCode(), p2.GetHashCode());
}
public void Equals()
{
PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
PoseD p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
Assert.AreEqual(p1, p2);
Assert.IsTrue(p1.Equals((object)p2));
Assert.IsTrue(p1.Equals(p2));
Assert.IsFalse(p1.Equals(p2.ToMatrix44D()));
}