void MoveFrame(float distance)
{
while (distance > 0)
{
var direction = Entity.Position.DirectionTo(CurrentWaypoint);
var distanceToWaypoint = Entity.Position.DistanceTo(CurrentWaypoint);
var cell = Waypoints.Peek();
if (distanceToWaypoint > distance)
{
Entity.Position += direction * distance;
return;
}
Entity.Position = CurrentWaypoint;
this.Log($"Pathfinding reached waypoint {CurrentWaypoint}");
Waypoints.Dequeue();
if (!HasWaypoints)
{
this.Log($"Pathfinding reached destination");
DestinationReached?.Invoke(this, new CellEventArgs(cell));
return;
}
distance -= distanceToWaypoint;
}
}
public void MoveTo(Vector2 nextEpisilon, double speed)
{
string instructions = (ID + 1) + " ";
// Check we're overshooting our destination.
// Basically if vectors are pointing in the same direction everything's still alright but as soon as they're opposite we can't move forward anymore.
if (Vector2.Dot((Waypoints.Peek() - Position), (Waypoints.Peek() - nextEpisilon)) > 0)
{
Position = nextEpisilon;
}
else
{
Position = Waypoints.Peek();
Waypoints.Dequeue();
}
instructions += "M," + Math.Round(Position.X, 2) + "," + Math.Round(Position.Y, 2) + "," +
+speed;
_pythonCall.Send(instructions);
}