public void TestMove()
{
var pose = new Pose(new Vector3(2, 3, 4), Vector3.UnitX, Vector3.UnitY);
pose = pose.Move(pose.Front, 0, 0, 0);
Assert.AreEqual(new Vector3(3, 3, 4), pose.Location);
pose = pose.Move(Vector3.Zero, 90, 0, 0);
Assert.AreEqual(Tuple.Create(Vector3.UnitY, -Vector3.UnitX), Tuple.Create(pose.Front, pose.Up));
pose = pose.Move(Vector3.Zero, 0, 90, 0);
Assert.AreEqual(Tuple.Create(-Vector3.UnitZ, -Vector3.UnitX), Tuple.Create(pose.Front, pose.Up));
}
private static bool IsCloseAhead(Pose pose, Vector3 target)
{
if (pose.Location.DistanceSquared(target) > 150 * 150) return false;
return (target - pose.Location).ToNormalized().DifferenceLessThan(pose.Front, 0.7f);
}