private float? GetNodePenalty(PathNode node, PathNode nextNode)
{
if (character == null) { return 0.0f; }
float penalty = 0.0f;
if (nextNode.Waypoint.ConnectedGap != null && nextNode.Waypoint.ConnectedGap.Open < 0.9f)
{
if (nextNode.Waypoint.ConnectedDoor == null)
{
penalty = 100.0f;
}
else if (!canBreakDoors)
{
//door closed and the character can't open doors -> node can't be traversed
if (!canOpenDoors || character.LockHands) { return null; }
var doorButtons = nextNode.Waypoint.ConnectedDoor.Item.GetConnectedComponents<Controller>();
if (!doorButtons.Any())
{
if (!nextNode.Waypoint.ConnectedDoor.HasRequiredItems(character, false)) { return null; }
}
foreach (Controller button in doorButtons)
{
if (Math.Sign(button.Item.Position.X - nextNode.Waypoint.Position.X) !=
Math.Sign(node.Position.X - nextNode.Position.X)) { continue; }
if (!button.HasRequiredItems(character, false)) { return null; }
}
}
}
//non-humanoids can't climb up ladders
if (!(character.AnimController is HumanoidAnimController))
{
if (node.Waypoint.Ladders != null && nextNode.Waypoint.Ladders != null &&
nextNode.Position.Y - node.Position.Y > 1.0f && //more than one sim unit to climb up
nextNode.Waypoint.CurrentHull != null && nextNode.Waypoint.CurrentHull.Surface < nextNode.Waypoint.Position.Y) //upper node not underwater
{
return null;
}
}
if (node.Waypoint != null && node.Waypoint.CurrentHull != null)
{
var hull = node.Waypoint.CurrentHull;
if (hull.FireSources.Count > 0)
{
foreach (FireSource fs in hull.FireSources)
{
penalty += fs.Size.X * 10.0f;
}
}
if (character.NeedsAir && hull.WaterVolume / hull.Rect.Width > 100.0f) penalty += 500.0f;
if (character.PressureProtection < 10.0f && hull.WaterVolume > hull.Volume) penalty += 1000.0f;
}
return penalty;
}
private float?GetNodePenalty(PathNode node, PathNode nextNode)
{
if (character == null)
{
return(0.0f);
}
if (nextNode.Waypoint.isObstructed)
{
return(null);
}
float penalty = 0.0f;
if (nextNode.Waypoint.ConnectedGap != null && nextNode.Waypoint.ConnectedGap.Open < 0.9f)
{
var door = nextNode.Waypoint.ConnectedDoor;
if (door == null)
{
penalty = 100.0f;
}
else
{
if (!CanAccessDoor(door, button =>
{
// Ignore buttons that are on the wrong side of the door
if (door.IsHorizontal)
{
if (Math.Sign(button.Item.WorldPosition.Y - door.Item.WorldPosition.Y) != Math.Sign(character.WorldPosition.Y - door.Item.WorldPosition.Y))
{
return(false);
}
}
else
{
if (Math.Sign(button.Item.WorldPosition.X - door.Item.WorldPosition.X) != Math.Sign(character.WorldPosition.X - door.Item.WorldPosition.X))
{
return(false);
}
}
return(true);
}))
{
return(null);
}
}
}
//non-humanoids can't climb up ladders
if (!(character.AnimController is HumanoidAnimController))
{
if (node.Waypoint.Ladders != null && nextNode.Waypoint.Ladders != null && (nextNode.Waypoint.Ladders.Item.NonInteractable || character.LockHands) ||
(nextNode.Position.Y - node.Position.Y > 1.0f && //more than one sim unit to climb up
nextNode.Waypoint.CurrentHull != null && nextNode.Waypoint.CurrentHull.Surface < nextNode.Waypoint.Position.Y)) //upper node not underwater
{
return(null);
}
}
if (node.Waypoint != null && node.Waypoint.CurrentHull != null)
{
var hull = node.Waypoint.CurrentHull;
if (hull.FireSources.Count > 0)
{
foreach (FireSource fs in hull.FireSources)
{
penalty += fs.Size.X * 10.0f;
}
}
if (character.NeedsAir && hull.WaterVolume / hull.Rect.Width > 100.0f)
{
if (!HumanAIController.HasDivingSuit(character))
{
penalty += 500.0f;
}
}
if (character.PressureProtection < 10.0f && hull.WaterVolume > hull.Volume)
{
penalty += 1000.0f;
}
}
float yDist = Math.Abs(node.Position.Y - nextNode.Position.Y);
if (node.Waypoint.Ladders == null && nextNode.Waypoint.Ladders == null)
{
penalty += yDist * 10.0f;
}
return(penalty);
}
private SteeringPath FindPath(PathNode start, PathNode end)
{
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 = 10000.0f;
foreach (PathNode node in nodes)
{
if (node.state != 1)
{
continue;
}
if (node.F < dist)
{
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)
{
//path not found
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 (GameMain.NilMod.ShowPathfindingErrors)
{
DebugConsole.ThrowError("Pathfinding error: constructing final path failed");
}
return(new SteeringPath(true));
}
path.Cost += pathNode.F;
pathNode = pathNode.Parent;
}
finalPath.Add(start.Waypoint);
finalPath.Reverse();
foreach (WayPoint wayPoint in finalPath)
{
path.AddNode(wayPoint);
}
return(path);
}
public SteeringPath FindPath(Vector2 start, Vector2 end)
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
float closestDist = 0.0f;
PathNode startNode = null;
foreach (PathNode node in nodes)
{
Vector2 nodePos = node.Position;
float dist = System.Math.Abs(start.X - nodePos.X) +
System.Math.Abs(start.Y - nodePos.Y) * 10.0f; //higher cost for vertical movement
//prefer nodes that are closer to the end position
dist += Vector2.Distance(end, nodePos) / 10.0f;
if (dist < closestDist || startNode == null)
{
//if searching for a path inside the sub, make sure the waypoint is visible
if (insideSubmarine)
{
var body = Submarine.PickBody(
start, node.Waypoint.SimPosition, null,
Physics.CollisionWall | Physics.CollisionLevel | Physics.CollisionStairs | Physics.CollisionPlatform);
if (body != null)
{
if (body.UserData is Structure && !((Structure)body.UserData).IsPlatform)
{
continue;
}
if (body.UserData is Item && body.FixtureList[0].CollisionCategories.HasFlag(Physics.CollisionWall))
{
continue;
}
}
}
closestDist = dist;
startNode = node;
}
}
if (startNode == null)
{
if (GameMain.NilMod.ShowPathfindingErrors)
{
DebugConsole.NewMessage("Pathfinding error, couldn't find a start node", Color.DarkRed);
}
return(new SteeringPath());
}
closestDist = 0.0f;
PathNode endNode = null;
foreach (PathNode node in nodes)
{
Vector2 nodePos = node.Position;
float dist = Vector2.Distance(end, nodePos);
if (dist < closestDist || endNode == null)
{
//if searching for a path inside the sub, make sure the waypoint is visible
if (insideSubmarine)
{
var body = Submarine.CheckVisibility(end, node.Waypoint.SimPosition);
if (body != null && body.UserData is Structure)
{
continue;
}
}
closestDist = dist;
endNode = node;
}
}
if (endNode == null)
{
if (GameMain.NilMod.ShowPathfindingErrors)
{
DebugConsole.NewMessage("Pathfinding error, couldn't find an end node", Color.DarkRed);
}
return(new SteeringPath());
}
var path = FindPath(startNode, endNode);
sw.Stop();
System.Diagnostics.Debug.WriteLine("findpath: " + sw.ElapsedMilliseconds + " ms");
return(path);
}
private SteeringPath FindPath(PathNode start, PathNode end, Func <PathNode, bool> filter = null, string errorMsgStr = "")
{
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)
{
continue;
}
if (IndoorsSteering && node.Waypoint.isObstructed)
{
continue;
}
if (filter != null && !filter(node))
{
continue;
}
if (node.F < dist)
{
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
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);
}
public SteeringPath FindPath(Vector2 start, Vector2 end, Submarine hostSub = null, string errorMsgStr = null, Func <PathNode, bool> startNodeFilter = null, Func <PathNode, bool> endNodeFilter = null, Func <PathNode, bool> nodeFilter = null, bool checkVisibility = true)
{
//sort nodes roughly according to distance
sortedNodes.Clear();
foreach (PathNode node in nodes)
{
node.TempPosition = node.Position;
if (hostSub != null)
{
Vector2 diff = hostSub.SimPosition - node.Waypoint.Submarine.SimPosition;
node.TempPosition -= diff;
}
float xDiff = Math.Abs(start.X - node.TempPosition.X);
float yDiff = Math.Abs(start.Y - node.TempPosition.Y);
if (yDiff > 1.0f && node.Waypoint.Ladders == null && node.Waypoint.Stairs == null)
{
yDiff += 10.0f;
}
node.TempDistance = xDiff + (InsideSubmarine ? yDiff * 10.0f : yDiff); //higher cost for vertical movement when inside the sub
//much higher cost to waypoints that are outside
if (node.Waypoint.CurrentHull == null && ApplyPenaltyToOutsideNodes)
{
node.TempDistance *= 10.0f;
}
//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
PathNode startNode = null;
foreach (PathNode node in sortedNodes)
{
if (startNode == null || node.TempDistance < startNode.TempDistance)
{
if (nodeFilter != null && !nodeFilter(node))
{
continue;
}
if (startNodeFilter != null && !startNodeFilter(node))
{
continue;
}
//if searching for a path inside the sub, make sure the waypoint is visible
if (IndoorsSteering)
{
if (node.Waypoint.isObstructed)
{
continue;
}
// Always check the visibility for the start node
var body = Submarine.PickBody(
start, node.TempPosition, null,
Physics.CollisionWall | Physics.CollisionLevel | Physics.CollisionStairs);
if (body != null)
{
if (body.UserData is Structure && !((Structure)body.UserData).IsPlatform)
{
continue;
}
if (body.UserData is Item && body.FixtureList[0].CollisionCategories.HasFlag(Physics.CollisionWall))
{
continue;
}
}
}
startNode = node;
}
}
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();
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;
}
//avoid stopping at a ladder
if (node.Waypoint.Ladders != null)
{
node.TempDistance *= 10.0f;
}
}
int i = 0;
while (i < sortedNodes.Count && sortedNodes[i].TempDistance < node.TempDistance)
{
i++;
}
sortedNodes.Insert(i, node);
}
//find the most suitable end node, starting from the ones closest to the end position
PathNode endNode = null;
foreach (PathNode node in sortedNodes)
{
if (endNode == null || node.TempDistance < endNode.TempDistance)
{
if (nodeFilter != null && !nodeFilter(node))
{
continue;
}
if (endNodeFilter != null && !endNodeFilter(node))
{
continue;
}
if (IndoorsSteering)
{
if (node.Waypoint.isObstructed)
{
continue;
}
//if searching for a path inside the sub, make sure the waypoint is visible
if (checkVisibility)
{
// Only check the visibility for the end node when allowed (fix leaks)
var body = Submarine.PickBody(end, node.TempPosition, null,
Physics.CollisionWall | Physics.CollisionLevel | Physics.CollisionStairs);
if (body != null)
{
if (body.UserData is Structure && !((Structure)body.UserData).IsPlatform)
{
continue;
}
if (body.UserData is Item && body.FixtureList[0].CollisionCategories.HasFlag(Physics.CollisionWall))
{
continue;
}
}
}
}
endNode = node;
}
}
if (endNode == null)
{
#if DEBUG
DebugConsole.NewMessage("Pathfinding error, couldn't find an end node. " + errorMsgStr, Color.DarkRed);
#endif
return(new SteeringPath(true));
}
var path = FindPath(startNode, endNode, nodeFilter, errorMsgStr);
return(path);
}
