private List <PathNode> FindPath(PathNode start, PathNode end, float blockageDPS, float moveSpeed)
{
List <PathNode> result = new List <PathNode>();
if (start == end)
{
return(result);
}
PathingInfo startInfo = new PathingInfo(start);
startInfo.G = 0;
startInfo.H = _heuristic.Calculate(start, end);
List <PathingInfo> closedSet = new List <PathingInfo>();
List <PathingInfo> openSet = new List <PathingInfo>()
{
startInfo
};
PathingInfo endInfo = null;
while (endInfo == null && openSet.Count > 0)
{
// find tile with lowest score
float minF = float.MaxValue;
int minIndex = -1;
for (int i = 0; i < openSet.Count; ++i)
{
float f = openSet[i].G + openSet[i].H;
if (f < minF)
{
minF = f;
minIndex = i;
}
}
PathingInfo nodeInfo = openSet[minIndex];
// move to closed set
closedSet.Add(nodeInfo);
openSet.RemoveAt(minIndex);
var neighbours = nodeInfo.node.Neighbours;
for (int i = 0; i < neighbours.Count; ++i)
{
var node = neighbours[i].Node;
// Check if in closed set
if (FindInSet(closedSet, node) != null)
{
continue;
}
float costToEnter = node.CostToEnter;
if (node.BlockageHealth > 0)
{
costToEnter += node.BlockageHealth /= blockageDPS;
}
if (neighbours[i].Distance > 0f)
{
costToEnter += neighbours[i].Distance / moveSpeed;
}
float newG = nodeInfo.G + costToEnter;
// Check if in open set
PathingInfo pi = FindInSet(openSet, node);
if (pi == null)
{
pi = new PathingInfo(node);
// Compute score
pi.G = newG;
pi.H = _heuristic.Calculate(node, end);
// set parent
pi.parent = nodeInfo;
// add to open set
openSet.Add(pi);
if (node == end)
{
// we have reached the end node
endInfo = pi;
}
}
else
{
// get F score
float prevF = pi.G + pi.H;
float newF = newG + pi.H;
if (prevF < newF)
{
// cost along current path is better - use that
pi.G = newG;
pi.parent = nodeInfo;
if (node == end)
{
// we have reached the end node
endInfo = pi;
}
}
}
}
}
// construct the result path
while (endInfo != null)
{
result.Insert(0, endInfo.node);
endInfo = endInfo.parent;
}
//Debug.Log("Path Length" + result.Count.ToString());
return(result);
}
