private void RunIteration()
{
// this places a lot of trust in the algorithm always finishing
WaypointSearchNode currentBest = searchNodes.Min;
evaluationsThisFrame = 0;
if (currentBest.inGround)
{
AddGroundProbeNodes(currentBest);
}
else if (currentBest.isGroundProbe)
{
HandleGroundProbe();
}
else
{
HandleAirWaypoints();
}
if (iterations++ > MAX_ITERATIONS || noImprovementFrames > MAX_NO_IMPROVEMENT_FRAMES)
{
searchFailed = true;
if (!searchNodes.Min.isGroundProbe)
{
searchNodes.Clear();
// take the closest guess we got
searchNodes.Add(visited.Min);
}
}
}
private void AddGroundProbeNodes(WaypointSearchNode parent)
{
visited.Clear();
searchNodes.Clear();
WaypointSearchNode grandparent = parent.parent;
Vector2 targetDirection = parent.position - grandparent.position;
WaypointSearchNode probeNodeCW = new WaypointSearchNode(grandparent.position, 0, grandparent)
{
parentPos = parent.position,
isGroundProbe = true,
targetDirection = targetDirection,
isClockwise = true,
velocity = targetDirection.rotate90CCW()
};
WaypointSearchNode probeNodeCCW = new WaypointSearchNode(grandparent.position, 1, grandparent)
{
parentPos = parent.position,
isGroundProbe = true,
targetDirection = targetDirection,
isClockwise = false,
velocity = targetDirection.rotate90CW()
};
// back up the node before the ground by one block
grandparent.position -= targetDirection;
searchNodes.Add(probeNodeCW);
searchNodes.Add(probeNodeCCW);
}
private WaypointSearchNode AddNode(WaypointSearchNode parent)
{
Vector2 newPosition;
bool inGround = false;
bool outOfBounds = false;
if (parent == null)
{
newPosition = player.Center;
}
else
{
newPosition = parent.position;
Vector2 distance = waypointPosition - newPosition;
Vector2 angleOffset;
if (Math.Abs(distance.X) > Math.Abs(distance.Y))
{
angleOffset = new Vector2(DISTANCE_STEP * Math.Sign(distance.X), 0);
}
else
{
angleOffset = new Vector2(0, DISTANCE_STEP * Math.Sign(distance.Y));
}
newPosition += angleOffset;
if (WaypointSearchNode.TileAtLocation(newPosition, ref outOfBounds))
{
inGround = true;
}
}
if (outOfBounds)
{
return(null);
}
float distanceHeuristic = Vector2.DistanceSquared(waypointPosition, newPosition);
bool hasLOS = false;
if (distanceHeuristic < LOS_CHECK_THRESHOLD * LOS_CHECK_THRESHOLD &&
Collision.CanHitLine(waypointPosition, 1, 1, newPosition, 1, 1))
{
hasLOS = true;
}
WaypointSearchNode newNode = new WaypointSearchNode(newPosition, distanceHeuristic, parent)
{
hasLOS = hasLOS,
inGround = inGround
};
if (parent == null || (!parent.IsBacktracking(newNode) && !visited.Contains(newNode)))
{
return(newNode);
}
else
{
return(null);
}
}
private void HandleAirWaypoints()
{
WaypointSearchNode currentBest = searchNodes.Min;
while (evaluationsThisFrame < EVALUATIONS_PER_FRAME)
{
currentBest = searchNodes.Min;
if (currentBest.inGround)
{
return;
}
if (AddNode(currentBest) is WaypointSearchNode newNode)
{
if (newNode.hasLOS)
{
// terminate the algorithm here, we've found a direct path
searchNodes.Clear();
searchNodes.Add(newNode);
return;
}
else
{
searchNodes.Add(newNode);
}
}
;
evaluationsThisFrame++;
if (searchNodes.Count > MAX_PENDING_QUEUE_SIZE)
{
searchNodes.Remove(searchNodes.Max);
}
}
if (visited.Min != null && currentBest.distanceHeuristic > visited.Min.distanceHeuristic)
{
noImprovementFrames++;
}
else
{
noImprovementFrames = 0;
}
searchNodes.Remove(currentBest);
visited.Add(currentBest);
}
// reduce the path to the minimum number of nodes with LOS to each other
public void CleanupPath()
{
if (pathFinalized)
{
return;
}
pathFinalized = true;
WaypointSearchNode currNode = searchNodes.Min;
pathLength = 0;
List <Vector2> allNodes = new List <Vector2>();
while (currNode != null)
{
allNodes.Add(currNode.position);
currNode = currNode.parent;
}
allNodes.Reverse();
if (searchSucceeded)
{
allNodes.Add(waypointPosition);
PrunePath(allNodes);
}
}
// return whether the algorithm needs to be run
// and do a bunch of side effects
internal bool InEndState()
{
waypointPosition = WaypointPos();
// if the waypoint isn't active, return
if (waypointPosition == default)
{
lastWaypointPosition = default;
return(true);
}
if (waypointPosition != lastWaypointPosition)
{
if (searchSucceeded && lastWaypointPosition != default &&
Collision.CanHitLine(waypointPosition, 1, 1, lastWaypointPosition, 1, 1))
{
// if we're able to 'patch' the path by just moving the endpoint
orderedPath.Add(waypointPosition);
// lazy copy
PrunePath(orderedPath);
lastWaypointPosition = waypointPosition;
return(true);
}
else
{
// reset state if the waypoint moved to a point where we can't see it
ResetState();
lastWaypointPosition = waypointPosition;
}
}
else if (searchSucceeded &&
Main.GameUpdateCount - lastPlayerMovementFrame > PLAYER_MOVEMENT_RATE_LIMIT &&
Vector2.DistanceSquared(player.Center, orderedPath[0]) > PLAYER_MOVEMENT_THRESHOLD * PLAYER_MOVEMENT_THRESHOLD)
{
// check if we can 'patch' the path by replacing the current few starting nodes
// with the player's new position
for (int i = 1; i < Math.Min(3, orderedPath.Count); i++)
{
if (Collision.CanHitLine(player.Center, 1, 1, orderedPath[1], 1, 1))
{
// can just patch the current path
orderedPath[i - 1] = player.Center;
PrunePath(orderedPath.Skip(i - 1).ToList());
return(true);
}
}
// fall through, need to fully recalculate
ResetState();
}
if (searchNodes.Count == 0 || searchNodes.Min is null)
{
// this shouldn't happen, error out
searchFailed = true;
return(true);
}
WaypointSearchNode currentBest = searchNodes.Min;
if (!currentBest.hasLOS)
{
// do one full LOS check per iteration, since this can eliminate some cases
// where the path converges along a weird axis
currentBest.hasLOS = Collision.CanHitLine(currentBest.position, 1, 1, waypointPosition, 1, 1);
}
searchSucceeded |= currentBest.hasLOS;
if (searchSucceeded || searchFailed)
{
CleanupPath();
DrawPath();
return(true);
}
return(false);
}
