public void SetPath(SteeringPath path)
{
currentPath = path;
if (path.Nodes.Any()) currentTarget = path.Nodes[path.Nodes.Count - 1].SimPosition;
findPathTimer = 1.0f;
IsPathDirty = false;
}
private Vector2 CalculateSteeringSeek(Vector2 target, float weight, Func <PathNode, bool> startNodeFilter = null, Func <PathNode, bool> endNodeFilter = null, Func <PathNode, bool> nodeFilter = null)
{
bool needsNewPath = character.Params.PathFinderPriority > 0.5f && (currentPath == null || currentPath.Unreachable || currentPath.Finished || Vector2.DistanceSquared(target, currentTarget) > 1);
//find a new path if one hasn't been found yet or the target is different from the current target
if (needsNewPath || findPathTimer < -1.0f)
{
IsPathDirty = true;
if (findPathTimer > 0.0f)
{
return(Vector2.Zero);
}
currentTarget = target;
Vector2 currentPos = host.SimPosition;
if (character != null && character.Submarine == null)
{
var targetHull = Hull.FindHull(ConvertUnits.ToDisplayUnits(target), null, false);
if (targetHull != null && targetHull.Submarine != null)
{
currentPos -= targetHull.Submarine.SimPosition;
}
}
pathFinder.InsideSubmarine = character.Submarine != null;
var newPath = pathFinder.FindPath(currentPos, target, character.Submarine, "(Character: " + character.Name + ")", startNodeFilter, endNodeFilter, nodeFilter);
bool useNewPath = currentPath == null || needsNewPath || currentPath.Finished;
if (!useNewPath && currentPath != null && currentPath.CurrentNode != null && newPath.Nodes.Any() && !newPath.Unreachable)
{
// It's possible that the current path was calculated from a start point that is no longer valid.
// Therefore, let's accept also paths with a greater cost than the current, if the current node is much farther than the new start node.
useNewPath = newPath.Cost <currentPath.Cost ||
Vector2.DistanceSquared(character.WorldPosition, currentPath.CurrentNode.WorldPosition)> Math.Pow(Vector2.Distance(character.WorldPosition, newPath.Nodes.First().WorldPosition) * 3, 2);
}
if (useNewPath)
{
currentPath = newPath;
}
float priority = MathHelper.Lerp(3, 1, character.Params.PathFinderPriority);
findPathTimer = priority * Rand.Range(1.0f, 1.2f);
IsPathDirty = false;
return(DiffToCurrentNode());
}
Vector2 diff = DiffToCurrentNode();
var collider = character.AnimController.Collider;
//if not in water and the waypoint is between the top and bottom of the collider, no need to move vertically
if (!character.AnimController.InWater && !character.IsClimbing && diff.Y < collider.height / 2 + collider.radius)
{
diff.Y = 0.0f;
}
//if (diff.LengthSquared() < 0.001f) { return -host.Steering; }
if (diff == Vector2.Zero)
{
return(Vector2.Zero);
}
return(Vector2.Normalize(diff) * weight);
}
protected override Vector2 DoSteeringSeek(Vector2 target, float weight)
{
//find a new path if one hasn't been found yet or the target is different from the current target
if (currentPath == null || Vector2.Distance(target, currentTarget) > 1.0f || findPathTimer < -1.0f)
{
IsPathDirty = true;
if (findPathTimer > 0.0f)
{
return(Vector2.Zero);
}
currentTarget = target;
Vector2 pos = host.SimPosition;
if (character != null && character.Submarine == null)
{
var targetHull = Hull.FindHull(FarseerPhysics.ConvertUnits.ToDisplayUnits(target), null, false);
if (targetHull != null && targetHull.Submarine != null)
{
pos -= targetHull.Submarine.SimPosition;
}
}
currentPath = pathFinder.FindPath(pos, target, "(Character: " + character.Name + ")");
findPathTimer = Rand.Range(1.0f, 1.2f);
IsPathDirty = false;
return(DiffToCurrentNode());
}
Vector2 diff = DiffToCurrentNode();
var collider = character.AnimController.Collider;
//if not in water and the waypoint is between the top and bottom of the collider, no need to move vertically
if (!character.AnimController.InWater && !character.IsClimbing && diff.Y < collider.height / 2 + collider.radius)
{
diff.Y = 0.0f;
}
if (diff.LengthSquared() < 0.001f)
{
return(-host.Steering);
}
return(Vector2.Normalize(diff) * weight);
}
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);
}
protected override Vector2 DoSteeringSeek(Vector2 target, float weight)
{
bool needsNewPath = currentPath != null && currentPath.Unreachable || Vector2.DistanceSquared(target, currentTarget) > 1;
//find a new path if one hasn't been found yet or the target is different from the current target
if (currentPath == null || needsNewPath || findPathTimer < -1.0f)
{
IsPathDirty = true;
if (findPathTimer > 0.0f)
{
return(Vector2.Zero);
}
currentTarget = target;
Vector2 pos = host.SimPosition;
// TODO: remove this and handle differently?
if (character != null && character.Submarine == null)
{
var targetHull = Hull.FindHull(FarseerPhysics.ConvertUnits.ToDisplayUnits(target), null, false);
if (targetHull != null && targetHull.Submarine != null)
{
pos -= targetHull.Submarine.SimPosition;
}
}
var newPath = pathFinder.FindPath(pos, target, character.Submarine, "(Character: " + character.Name + ")");
bool useNewPath = currentPath == null || needsNewPath;
if (!useNewPath && currentPath != null && currentPath.CurrentNode != null && newPath.Nodes.Any() && !newPath.Unreachable)
{
// It's possible that the current path was calculated from a start point that is no longer valid.
// Therefore, let's accept also paths with a greater cost than the current, if the current node is much farther than the new start node.
useNewPath = newPath.Cost <currentPath.Cost ||
Vector2.DistanceSquared(character.WorldPosition, currentPath.CurrentNode.WorldPosition)> Math.Pow(Vector2.Distance(character.WorldPosition, newPath.Nodes.First().WorldPosition) * 2, 2);
}
if (useNewPath)
{
currentPath = newPath;
}
findPathTimer = Rand.Range(1.0f, 1.2f);
IsPathDirty = false;
return(DiffToCurrentNode());
}
Vector2 diff = DiffToCurrentNode();
var collider = character.AnimController.Collider;
//if not in water and the waypoint is between the top and bottom of the collider, no need to move vertically
if (!character.AnimController.InWater && !character.IsClimbing && diff.Y < collider.height / 2 + collider.radius)
{
diff.Y = 0.0f;
}
if (diff.LengthSquared() < 0.001f)
{
return(-host.Steering);
}
return(Vector2.Normalize(diff) * weight);
}
private Vector2 CalculateSteeringSeek(Vector2 target, float weight, float minGapSize = 0, Func <PathNode, bool> startNodeFilter = null, Func <PathNode, bool> endNodeFilter = null, Func <PathNode, bool> nodeFilter = null, bool checkVisibility = true)
{
bool needsNewPath = currentPath == null || currentPath.Unreachable || currentPath.Finished;
if (!needsNewPath && character.Submarine != null && character.Params.PathFinderPriority > 0.5f)
{
Vector2 targetDiff = target - currentTarget;
if (currentPath != null && currentPath.Nodes.Any() && character.Submarine != null)
{
//target in a different sub than where the character is now
//take that into account when calculating if the target has moved
Submarine currentPathSub = currentPath?.CurrentNode?.Submarine;
if (currentPathSub == character.Submarine)
{
currentPathSub = currentPath?.Nodes.LastOrDefault()?.Submarine;
}
if (currentPathSub != character.Submarine && targetDiff.LengthSquared() > 1 && currentPathSub != null)
{
Vector2 subDiff = character.Submarine.SimPosition - currentPathSub.SimPosition;
targetDiff += subDiff;
}
}
if (targetDiff.LengthSquared() > 1)
{
needsNewPath = true;
}
}
//find a new path if one hasn't been found yet or the target is different from the current target
if (needsNewPath || findPathTimer < -1.0f)
{
IsPathDirty = true;
if (findPathTimer < 0)
{
SkipCurrentPathNodes();
currentTarget = target;
Vector2 currentPos = host.SimPosition;
pathFinder.InsideSubmarine = character.Submarine != null && !character.Submarine.Info.IsRuin;
pathFinder.ApplyPenaltyToOutsideNodes = character.Submarine != null && character.PressureProtection <= 0;
var newPath = pathFinder.FindPath(currentPos, target, character.Submarine, "(Character: " + character.Name + ")", minGapSize, startNodeFilter, endNodeFilter, nodeFilter, checkVisibility: checkVisibility);
bool useNewPath = needsNewPath || currentPath == null || currentPath.CurrentNode == null || character.Submarine != null && findPathTimer < -1 && Math.Abs(character.AnimController.TargetMovement.Combine()) <= 0;
if (newPath.Unreachable || newPath.Nodes.None())
{
useNewPath = false;
}
else if (!useNewPath && currentPath != null && currentPath.CurrentNode != null)
{
// Check if the new path is the same as the old, in which case we just ignore it and continue using the old path (or the progress would reset).
if (IsIdenticalPath())
{
useNewPath = false;
}
else
{
// Use the new path if it has significantly lower cost (don't change the path if it has marginally smaller cost. This reduces navigating backwards due to new path that is calculated from the node just behind us).
float t = (float)currentPath.CurrentIndex / (currentPath.Nodes.Count - 1);
useNewPath = newPath.Cost < currentPath.Cost * MathHelper.Lerp(0.95f, 0, t);
if (!useNewPath && character.Submarine != null && !character.IsClimbing)
{
// It's possible that the current path was calculated from a start point that is no longer valid.
// Therefore, let's accept also paths with a greater cost than the current, if the current node is much farther than the new start node.
// This is a special case for cases e.g. where the character falls and thus needs a new path.
// Don't do this outside or when climbing ladders, because both cause issues.
useNewPath = Vector2.DistanceSquared(character.WorldPosition, currentPath.CurrentNode.WorldPosition) > Math.Pow(Vector2.Distance(character.WorldPosition, newPath.Nodes.First().WorldPosition) * 3, 2);
}
}
bool IsIdenticalPath()
{
int nodeCount = newPath.Nodes.Count;
if (nodeCount == currentPath.Nodes.Count)
{
for (int i = 0; i < nodeCount - 1; i++)
{
if (newPath.Nodes[i] != currentPath.Nodes[i])
{
return(false);
}
}
return(true);
}
return(false);
}
}
if (useNewPath)
{
if (currentPath != null)
{
CheckDoorsInPath();
}
currentPath = newPath;
}
float priority = MathHelper.Lerp(3, 1, character.Params.PathFinderPriority);
findPathTimer = priority * Rand.Range(1.0f, 1.2f);
IsPathDirty = false;
return(DiffToCurrentNode());
void SkipCurrentPathNodes()
{
if (!character.AnimController.InWater || character.Submarine != null)
{
return;
}
if (CurrentPath == null || CurrentPath.Unreachable || CurrentPath.Finished)
{
return;
}
if (CurrentPath.CurrentIndex < 0 || CurrentPath.CurrentIndex >= CurrentPath.Nodes.Count - 1)
{
return;
}
// Check if we could skip ahead to NextNode when the character is swimming and using waypoints outside.
// Do this to optimize the old path before creating and evaluating a new path.
// In general, this is to avoid behavior where:
// a) the character goes back to first reach CurrentNode when the second node would be closer; or
// b) the character moves along the path when they could cut through open space to reduce the total distance.
float pathDistance = Vector2.Distance(character.WorldPosition, CurrentPath.CurrentNode.WorldPosition);
pathDistance += CurrentPath.GetLength(startIndex: CurrentPath.CurrentIndex);
for (int i = CurrentPath.Nodes.Count - 1; i > CurrentPath.CurrentIndex + 1; i--)
{
var waypoint = CurrentPath.Nodes[i];
float directDistance = Vector2.DistanceSquared(character.WorldPosition, waypoint.WorldPosition);
if (directDistance > (pathDistance * pathDistance) || Submarine.PickBody(host.SimPosition, waypoint.SimPosition, collisionCategory: Physics.CollisionLevel | Physics.CollisionWall) != null)
{
pathDistance -= CurrentPath.GetLength(startIndex: i - 1, endIndex: i);
continue;
}
CurrentPath.SkipToNode(i);
break;
}
}
}
}
Vector2 diff = DiffToCurrentNode();
var collider = character.AnimController.Collider;
// Only humanoids can climb ladders
bool canClimb = character.AnimController is HumanoidAnimController;
//if not in water and the waypoint is between the top and bottom of the collider, no need to move vertically
if (canClimb && !character.AnimController.InWater && !character.IsClimbing && diff.Y < collider.height / 2 + collider.radius)
{
diff.Y = 0.0f;
}
if (diff == Vector2.Zero)
{
return(Vector2.Zero);
}
return(Vector2.Normalize(diff) * weight);
}
public void ResetPath()
{
currentPath = null;
IsPathDirty = true;
}
private Vector2 CalculateSteeringSeek(Vector2 target, float weight, Func <PathNode, bool> startNodeFilter = null, Func <PathNode, bool> endNodeFilter = null, Func <PathNode, bool> nodeFilter = null, bool checkVisibility = true)
{
Vector2 targetDiff = target - currentTarget;
if (currentPath != null && currentPath.Nodes.Any())
{
//current path calculated relative to a different sub than where the character is now
//take that into account when calculating if the target has moved
Submarine currentPathSub = currentPath?.Nodes.First().Submarine;
if (currentPathSub != character.Submarine && character.Submarine != null)
{
Vector2 subDiff = character.Submarine.SimPosition - currentPathSub.SimPosition;
targetDiff += subDiff;
}
}
bool needsNewPath = character.Params.PathFinderPriority > 0.5f && (currentPath == null || currentPath.Unreachable || targetDiff.LengthSquared() > 1);
//find a new path if one hasn't been found yet or the target is different from the current target
if (needsNewPath || findPathTimer < -1.0f)
{
IsPathDirty = true;
if (findPathTimer < 0)
{
currentTarget = target;
Vector2 currentPos = host.SimPosition;
if (character != null && character.Submarine == null)
{
var targetHull = Hull.FindHull(ConvertUnits.ToDisplayUnits(target), null, false);
if (targetHull != null && targetHull.Submarine != null)
{
currentPos -= targetHull.Submarine.SimPosition;
}
}
pathFinder.InsideSubmarine = character.Submarine != null;
pathFinder.ApplyPenaltyToOutsideNodes = character.PressureProtection <= 0;
var newPath = pathFinder.FindPath(currentPos, target, character.Submarine, "(Character: " + character.Name + ")", startNodeFilter, endNodeFilter, nodeFilter, checkVisibility: checkVisibility);
bool useNewPath = needsNewPath || currentPath == null || currentPath.CurrentNode == null || findPathTimer < -1 && Math.Abs(character.AnimController.TargetMovement.X) <= 0;
if (!useNewPath && currentPath != null && currentPath.CurrentNode != null && newPath.Nodes.Any() && !newPath.Unreachable)
{
// Check if the new path is the same as the old, in which case we just ignore it and continue using the old path (or the progress would reset).
if (IsIdenticalPath())
{
useNewPath = false;
}
else
{
// Use the new path if it has significantly lower cost (don't change the path if it has marginally smaller cost. This reduces navigating backwards due to new path that is calculated from the node just behind us).
float t = (float)currentPath.CurrentIndex / (currentPath.Nodes.Count - 1);
useNewPath = newPath.Cost < currentPath.Cost * MathHelper.Lerp(0.95f, 0, t);
if (!useNewPath)
{
// It's possible that the current path was calculated from a start point that is no longer valid.
// Therefore, let's accept also paths with a greater cost than the current, if the current node is much farther than the new start node.
useNewPath = Vector2.DistanceSquared(character.WorldPosition, currentPath.CurrentNode.WorldPosition) > Math.Pow(Vector2.Distance(character.WorldPosition, newPath.Nodes.First().WorldPosition) * 3, 2);
}
}
bool IsIdenticalPath()
{
int nodeCount = newPath.Nodes.Count;
if (nodeCount == currentPath.Nodes.Count)
{
for (int i = 0; i < nodeCount - 1; i++)
{
if (newPath.Nodes[i] != currentPath.Nodes[i])
{
return(false);
}
}
return(true);
}
return(false);
}
}
if (useNewPath)
{
currentPath = newPath;
}
float priority = MathHelper.Lerp(3, 1, character.Params.PathFinderPriority);
findPathTimer = priority * Rand.Range(1.0f, 1.2f);
IsPathDirty = false;
return(DiffToCurrentNode());
}
}
Vector2 diff = DiffToCurrentNode();
var collider = character.AnimController.Collider;
// Only humanoids can climb ladders
bool canClimb = character.AnimController is HumanoidAnimController;
//if not in water and the waypoint is between the top and bottom of the collider, no need to move vertically
if (canClimb && !character.AnimController.InWater && !character.IsClimbing && diff.Y < collider.height / 2 + collider.radius)
{
diff.Y = 0.0f;
}
if (diff == Vector2.Zero)
{
return(Vector2.Zero);
}
return(Vector2.Normalize(diff) * weight);
}
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.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);
if (GetNodePenalty != null)
{
float?nodePenalty = GetNodePenalty(currNode, nextNode);
if (nodePenalty == null)
{
nextNode.state = -1;
continue;
}
nextNode.H += (float)nodePenalty;
}
nextNode.G = currNode.G + currNode.distances[i];
nextNode.F = nextNode.G + nextNode.H;
nextNode.Parent = currNode;
nextNode.state = 1;
}
//node that has been searched
else if (nextNode.state == 1)
{
float tempG = currNode.G + currNode.distances[i];
//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;
}
}
}
}
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
if (finalPath.Count > nodes.Count)
{
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);
}
