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