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)));
}
