496 lines
19 KiB
C#
496 lines
19 KiB
C#
using Barotrauma.Extensions;
|
|
using Microsoft.Xna.Framework;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
|
|
namespace Barotrauma
|
|
{
|
|
class PathNode
|
|
{
|
|
public int state;
|
|
|
|
public PathNode Parent;
|
|
|
|
public float F, G, H;
|
|
|
|
public readonly List<PathNode> connections = new List<PathNode>();
|
|
public List<float> distances;
|
|
|
|
public Vector2 TempPosition;
|
|
public float TempDistance;
|
|
|
|
public readonly WayPoint Waypoint;
|
|
public readonly Vector2 Position;
|
|
public readonly int WayPointID;
|
|
|
|
public override string ToString()
|
|
{
|
|
return $"PathNode {WayPointID}";
|
|
}
|
|
|
|
public PathNode(WayPoint wayPoint)
|
|
{
|
|
Waypoint = wayPoint;
|
|
Position = wayPoint.SimPosition;
|
|
WayPointID = Waypoint.ID;
|
|
}
|
|
|
|
public static List<PathNode> GenerateNodes(List<WayPoint> wayPoints, bool removeOrphans)
|
|
{
|
|
var nodes = new Dictionary<int, PathNode>();
|
|
foreach (WayPoint wayPoint in wayPoints)
|
|
{
|
|
if (wayPoint == null) { continue; }
|
|
if (nodes.ContainsKey(wayPoint.ID))
|
|
{
|
|
#if DEBUG
|
|
DebugConsole.ThrowError("Error in PathFinder.GenerateNodes (duplicate ID \"" + wayPoint.ID + "\")");
|
|
#endif
|
|
continue;
|
|
}
|
|
nodes.Add(wayPoint.ID, new PathNode(wayPoint));
|
|
}
|
|
|
|
foreach (KeyValuePair<int, PathNode> node in nodes)
|
|
{
|
|
foreach (MapEntity linked in node.Value.Waypoint.linkedTo)
|
|
{
|
|
nodes.TryGetValue(linked.ID, out PathNode connectedNode);
|
|
if (connectedNode == null) { continue; }
|
|
if (!node.Value.connections.Contains(connectedNode)) { node.Value.connections.Add(connectedNode); }
|
|
if (!connectedNode.connections.Contains(node.Value)) { connectedNode.connections.Add(node.Value); }
|
|
}
|
|
}
|
|
|
|
var nodeList = nodes.Values.ToList();
|
|
if (removeOrphans)
|
|
{
|
|
nodeList.RemoveAll(n => n.connections.Count == 0);
|
|
}
|
|
foreach (PathNode node in nodeList)
|
|
{
|
|
node.distances = new List<float>();
|
|
for (int i = 0; i < node.connections.Count; i++)
|
|
{
|
|
node.distances.Add(Vector2.Distance(node.Position, node.connections[i].Position));
|
|
}
|
|
}
|
|
|
|
return nodeList;
|
|
}
|
|
|
|
private bool? blocked;
|
|
public bool IsBlocked()
|
|
{
|
|
if (blocked.HasValue) { return blocked.Value; }
|
|
blocked = false;
|
|
if (Waypoint.Submarine != null) { return blocked.Value; }
|
|
if (Waypoint.Tunnel?.Type != Level.TunnelType.Cave) { return blocked.Value; }
|
|
foreach (var w in Level.Loaded.ExtraWalls)
|
|
{
|
|
if (!w.IsPointInside(Waypoint.Position)) { continue; }
|
|
if (w is DestructibleLevelWall d)
|
|
{
|
|
blocked = !d.Destroyed;
|
|
}
|
|
if (blocked.Value) { break; }
|
|
}
|
|
return blocked.Value;
|
|
}
|
|
|
|
public void ResetBlocked()
|
|
{
|
|
blocked = null;
|
|
}
|
|
}
|
|
|
|
class PathFinder
|
|
{
|
|
public delegate float? GetNodePenaltyHandler(PathNode node, PathNode prevNode);
|
|
public GetNodePenaltyHandler GetNodePenalty;
|
|
public delegate float? GetSingleNodePenaltyHandler(PathNode node);
|
|
public GetSingleNodePenaltyHandler GetSingleNodePenalty;
|
|
|
|
private readonly List<PathNode> nodes;
|
|
private readonly bool isCharacter;
|
|
|
|
public bool InsideSubmarine { get; set; }
|
|
public bool ApplyPenaltyToOutsideNodes { get; set; }
|
|
|
|
public PathFinder(List<WayPoint> wayPoints, bool isCharacter)
|
|
{
|
|
var filtered = isCharacter ? wayPoints : wayPoints.FindAll(w => w.Submarine == null);
|
|
nodes = PathNode.GenerateNodes(filtered, removeOrphans: true);
|
|
foreach (WayPoint wp in wayPoints)
|
|
{
|
|
wp.OnLinksChanged += WaypointLinksChanged;
|
|
}
|
|
sortedNodes = new List<PathNode>(nodes.Count);
|
|
this.isCharacter = isCharacter;
|
|
}
|
|
|
|
void WaypointLinksChanged(WayPoint wp)
|
|
{
|
|
if (Submarine.Unloading) { return; }
|
|
|
|
var node = nodes.Find(n => n.Waypoint == wp);
|
|
if (node == null) { return; }
|
|
|
|
for (int i = node.connections.Count - 1; i >= 0; i--)
|
|
{
|
|
//remove connection if the waypoint isn't connected anymore
|
|
if (wp.linkedTo.FirstOrDefault(l => l == node.connections[i].Waypoint) == null)
|
|
{
|
|
node.connections.RemoveAt(i);
|
|
node.distances.RemoveAt(i);
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < wp.linkedTo.Count; i++)
|
|
{
|
|
if (!(wp.linkedTo[i] is WayPoint connected)) { continue; }
|
|
|
|
//already connected, continue
|
|
if (node.connections.Any(n => n.Waypoint == connected)) { continue; }
|
|
|
|
var matchingNode = nodes.Find(n => n.Waypoint == connected);
|
|
if (matchingNode == null)
|
|
{
|
|
#if DEBUG
|
|
DebugConsole.ThrowError("Waypoint connections were changed, no matching path node found in PathFinder");
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
node.connections.Add(matchingNode);
|
|
node.distances.Add(Vector2.Distance(node.Position, matchingNode.Position));
|
|
}
|
|
}
|
|
|
|
private readonly List<PathNode> sortedNodes;
|
|
|
|
public SteeringPath FindPath(Vector2 start, Vector2 end, Submarine hostSub = null, string errorMsgStr = null, float minGapSize = 0, Func<PathNode, bool> startNodeFilter = null, Func<PathNode, bool> endNodeFilter = null, Func<PathNode, bool> nodeFilter = null, bool checkVisibility = true)
|
|
{
|
|
foreach (PathNode node in nodes)
|
|
{
|
|
node.ResetBlocked();
|
|
}
|
|
|
|
// First calculate the temp positions for all nodes.
|
|
foreach (PathNode node in nodes)
|
|
{
|
|
node.TempPosition = node.Position;
|
|
var wpSub = node.Waypoint.Submarine;
|
|
if (hostSub != null && wpSub == null)
|
|
{
|
|
// inside and targeting outside
|
|
node.TempPosition -= hostSub.SimPosition;
|
|
}
|
|
else if (wpSub != null && hostSub != null && wpSub != hostSub)
|
|
{
|
|
// different subs
|
|
node.TempPosition -= hostSub.SimPosition - wpSub.SimPosition;
|
|
}
|
|
else if (hostSub == null && wpSub != null)
|
|
{
|
|
// Outside and targeting inside
|
|
node.TempPosition += wpSub.SimPosition;
|
|
}
|
|
}
|
|
|
|
//sort nodes roughly according to distance
|
|
sortedNodes.Clear();
|
|
PathNode startNode = null;
|
|
foreach (PathNode node in nodes)
|
|
{
|
|
float xDiff = Math.Abs(start.X - node.TempPosition.X);
|
|
float yDiff = Math.Abs(start.Y - node.TempPosition.Y);
|
|
if (InsideSubmarine && !(node.Waypoint.Submarine?.Info?.IsRuin ?? false))
|
|
{
|
|
//higher cost for vertical movement when inside the sub
|
|
if (yDiff > 1.0f && node.Waypoint.Ladders == null && node.Waypoint.Stairs == null)
|
|
{
|
|
yDiff += 10.0f;
|
|
}
|
|
node.TempDistance = xDiff + yDiff * 10.0f;
|
|
}
|
|
else
|
|
{
|
|
node.TempDistance = xDiff + yDiff;
|
|
}
|
|
|
|
//much higher cost to waypoints that are outside
|
|
if (node.Waypoint.CurrentHull == null && ApplyPenaltyToOutsideNodes) { node.TempDistance *= 10.0f; }
|
|
|
|
//optimization: node extremely far, don't try to use it as a start node
|
|
if (node.TempDistance > (InsideSubmarine ? 100.0f : 800.0f))
|
|
{
|
|
continue;
|
|
}
|
|
//optimization: node close enough. If it's valid, choose it as the start node and skip the more exhaustive search for the closest one
|
|
if (node.TempDistance < FarseerPhysics.ConvertUnits.ToSimUnits(AIObjectiveGetItem.DefaultReach))
|
|
{
|
|
if (IsValidStartNode(node))
|
|
{
|
|
startNode = node;
|
|
break;
|
|
}
|
|
}
|
|
//prefer nodes that are closer to the end position
|
|
node.TempDistance += (Math.Abs(end.X - node.TempPosition.X) + Math.Abs(end.Y - node.TempPosition.Y)) / 100.0f;
|
|
|
|
int i = 0;
|
|
while (i < sortedNodes.Count && sortedNodes[i].TempDistance < node.TempDistance)
|
|
{
|
|
i++;
|
|
}
|
|
sortedNodes.Insert(i, node);
|
|
}
|
|
|
|
//find the most suitable start node, starting from the ones that are the closest
|
|
if (startNode == null)
|
|
{
|
|
foreach (PathNode node in sortedNodes)
|
|
{
|
|
if (IsValidStartNode(node))
|
|
{
|
|
startNode = node;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (startNode == null)
|
|
{
|
|
#if DEBUG
|
|
DebugConsole.NewMessage("Pathfinding error, couldn't find a start node. "+ errorMsgStr, Color.DarkRed);
|
|
#endif
|
|
return new SteeringPath(true);
|
|
}
|
|
|
|
//sort nodes again, now based on distance from the end position
|
|
sortedNodes.Clear();
|
|
PathNode endNode = null;
|
|
foreach (PathNode node in nodes)
|
|
{
|
|
node.TempDistance = Vector2.DistanceSquared(end, node.TempPosition);
|
|
if (InsideSubmarine)
|
|
{
|
|
if (ApplyPenaltyToOutsideNodes)
|
|
{
|
|
//much higher cost to waypoints that are outside
|
|
if (node.Waypoint.CurrentHull == null) { node.TempDistance *= 10.0f; }
|
|
}
|
|
//avoid stopping at a doorway
|
|
if (node.Waypoint.ConnectedDoor != null) { node.TempDistance *= 10.0f; }
|
|
}
|
|
//optimization: node extremely far (> 100m / 800 m) from the end position, don't try to use it as an end node
|
|
if (node.TempDistance > (InsideSubmarine ? 100.0f * 100.0f : 800.0f * 800.0f))
|
|
{
|
|
continue;
|
|
}
|
|
//optimization: node extremely close (< 1 m). If it's valid, choose it as the end node and skip the more exhaustive search for the closest one
|
|
if (node.TempDistance < 1.0f)
|
|
{
|
|
if (IsValidEndNode(node))
|
|
{
|
|
endNode = node;
|
|
break;
|
|
}
|
|
}
|
|
int i = 0;
|
|
while (i < sortedNodes.Count && sortedNodes[i].TempDistance < node.TempDistance)
|
|
{
|
|
i++;
|
|
}
|
|
sortedNodes.Insert(i, node);
|
|
}
|
|
if (endNode == null)
|
|
{
|
|
//find the most suitable end node, starting from the ones closest to the end position
|
|
foreach (PathNode node in sortedNodes)
|
|
{
|
|
if (IsValidEndNode(node))
|
|
{
|
|
endNode = node;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (endNode == null)
|
|
{
|
|
#if DEBUG
|
|
DebugConsole.NewMessage("Pathfinding error, couldn't find an end node. " + errorMsgStr, Color.DarkRed);
|
|
#endif
|
|
return new SteeringPath(true);
|
|
}
|
|
return FindPath(startNode, endNode, nodeFilter, errorMsgStr, minGapSize);
|
|
|
|
bool IsValidStartNode(PathNode node) => IsValidNode(node, (isCharacter, start), startNodeFilter);
|
|
|
|
bool IsValidEndNode(PathNode node) => IsValidNode(node, (isCharacter && checkVisibility, end), endNodeFilter);
|
|
|
|
bool IsValidNode(PathNode node, (bool check, Vector2 start) visibilityCheck, Func<PathNode, bool> extraFilter)
|
|
{
|
|
if (nodeFilter != null && !nodeFilter(node)) { return false; }
|
|
if (extraFilter != null && !extraFilter(node)) { return false; }
|
|
if (GetSingleNodePenalty != null && GetSingleNodePenalty(node) == null) { return false; }
|
|
if (node.Waypoint.ConnectedGap != null)
|
|
{
|
|
if (!CanFitThroughGap(node.Waypoint.ConnectedGap, minGapSize)) { return false; }
|
|
}
|
|
if (visibilityCheck.check)
|
|
{
|
|
var body = Submarine.PickBody(visibilityCheck.start, node.TempPosition,
|
|
collisionCategory: Physics.CollisionWall | Physics.CollisionLevel | Physics.CollisionStairs);
|
|
if (body != null)
|
|
{
|
|
if (body.UserData is Submarine) { return false; }
|
|
if (body.UserData is Structure s && !s.IsPlatform) { return false; }
|
|
if (body.UserData is Voronoi2.VoronoiCell) { return false; }
|
|
if (body.UserData is Item && body.FixtureList[0].CollisionCategories.HasFlag(Physics.CollisionWall)) { return false; }
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
}
|
|
|
|
private SteeringPath FindPath(PathNode start, PathNode end, Func<PathNode, bool> filter = null, string errorMsgStr = "", float minGapSize = 0)
|
|
{
|
|
if (start == end)
|
|
{
|
|
var path1 = new SteeringPath();
|
|
path1.AddNode(start.Waypoint);
|
|
return path1;
|
|
}
|
|
|
|
foreach (PathNode node in nodes)
|
|
{
|
|
node.Parent = null;
|
|
node.state = 0;
|
|
node.F = 0.0f;
|
|
node.G = 0.0f;
|
|
node.H = 0.0f;
|
|
}
|
|
|
|
start.state = 1;
|
|
while (true)
|
|
{
|
|
PathNode currNode = null;
|
|
float dist = float.MaxValue;
|
|
foreach (PathNode node in nodes)
|
|
{
|
|
if (node.state != 1 || node.F > dist) { continue; }
|
|
if (filter != null && !filter(node)) { continue; }
|
|
if (node.Waypoint.ConnectedGap != null)
|
|
{
|
|
if (!CanFitThroughGap(node.Waypoint.ConnectedGap, minGapSize)) { continue; }
|
|
}
|
|
dist = node.F;
|
|
currNode = node;
|
|
}
|
|
|
|
if (currNode == null || currNode == end) { break; }
|
|
|
|
currNode.state = 2;
|
|
|
|
for (int i = 0; i < currNode.connections.Count; i++)
|
|
{
|
|
PathNode nextNode = currNode.connections[i];
|
|
|
|
//a node that hasn't been searched yet
|
|
if (nextNode.state == 0)
|
|
{
|
|
nextNode.H = Vector2.Distance(nextNode.Position, end.Position);
|
|
|
|
float penalty = 0.0f;
|
|
if (GetNodePenalty != null)
|
|
{
|
|
float? nodePenalty = GetNodePenalty(currNode, nextNode);
|
|
if (nodePenalty == null)
|
|
{
|
|
nextNode.state = -1;
|
|
continue;
|
|
}
|
|
penalty = nodePenalty.Value;
|
|
}
|
|
|
|
nextNode.G = currNode.G + currNode.distances[i] + penalty;
|
|
nextNode.F = nextNode.G + nextNode.H;
|
|
nextNode.Parent = currNode;
|
|
nextNode.state = 1;
|
|
}
|
|
//node that has been searched
|
|
else if (nextNode.state == 1 || nextNode.state == -1)
|
|
{
|
|
float tempG = currNode.G + currNode.distances[i];
|
|
|
|
if (GetNodePenalty != null)
|
|
{
|
|
float? nodePenalty = GetNodePenalty(currNode, nextNode);
|
|
if (nodePenalty == null) { continue; }
|
|
tempG += nodePenalty.Value;
|
|
}
|
|
|
|
//only use if this new route is better than the
|
|
//route the node was a part of
|
|
if (tempG < nextNode.G)
|
|
{
|
|
nextNode.G = tempG;
|
|
nextNode.F = nextNode.G + nextNode.H;
|
|
nextNode.Parent = currNode;
|
|
nextNode.state = 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (end.state == 0 || end.Parent == null)
|
|
{
|
|
#if DEBUG
|
|
if (errorMsgStr != null)
|
|
{
|
|
DebugConsole.NewMessage("Path not found. " + errorMsgStr, Color.Yellow);
|
|
}
|
|
#endif
|
|
return new SteeringPath(true);
|
|
}
|
|
|
|
SteeringPath path = new SteeringPath();
|
|
List<WayPoint> finalPath = new List<WayPoint>();
|
|
|
|
PathNode pathNode = end;
|
|
while (pathNode != start && pathNode != null)
|
|
{
|
|
finalPath.Add(pathNode.Waypoint);
|
|
|
|
//(there was one bug report that seems to have been caused by this loop never terminating:
|
|
//couldn't reproduce or figure out what caused it, but here's a workaround that prevents the game from crashing in case it happens again)
|
|
|
|
//should be fixed now, was most likely caused by the parent fields of the nodes not being cleared before starting the pathfinding
|
|
if (finalPath.Count > nodes.Count)
|
|
{
|
|
#if DEBUG
|
|
DebugConsole.ThrowError("Pathfinding error: constructing final path failed");
|
|
#endif
|
|
return new SteeringPath(true);
|
|
}
|
|
|
|
path.Cost += pathNode.F;
|
|
pathNode = pathNode.Parent;
|
|
}
|
|
|
|
finalPath.Add(start.Waypoint);
|
|
for (int i = finalPath.Count - 1; i >= 0; i--)
|
|
{
|
|
path.AddNode(finalPath[i]);
|
|
}
|
|
System.Diagnostics.Debug.Assert(finalPath.Count == path.Nodes.Count);
|
|
|
|
return path;
|
|
}
|
|
|
|
private bool CanFitThroughGap(Gap gap, float minWidth) => gap.IsHorizontal ? gap.RectHeight > minWidth : gap.RectWidth > minWidth;
|
|
}
|
|
} |