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