public List <PathNode> FindPath(Double3 beginPoint, Double3 targetPoint)
{
Int3 beginBlock = world.GetGound(beginPoint);
Int3 targetBlock = world.GetGound(targetPoint);
var startNode = new PathNode(beginBlock, 0, beginBlock.Distance(targetBlock), MoveActions.Walk);
openList = new List <PathNode> {
startNode
};
closedList = new List <Int3>();
startNode.GCost = 0;
startNode.HCost = CalculateDistanceCost(startNode.Position, targetBlock);
while (openList.Count > 0 && Discoverd.Count < maxNodes)
{
PathNode currentNode = GetLowestFCostNode(openList);
if (currentNode.Position == targetBlock)
{
// Reached final node
return(CalculatePath(currentNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode.Position);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode.Position))
{
continue;
}
int tententiveGCos = currentNode.GCost + CalculateDistanceCost(currentNode.Position, neighbourNode.Position);
if (tententiveGCos < neighbourNode.GCost && tententiveGCos < maxEstimatePath)
{
neighbourNode.cameFromNode = currentNode;
neighbourNode.GCost = tententiveGCos;
neighbourNode.HCost = CalculateDistanceCost(neighbourNode.Position, targetBlock);
neighbourNode.CalculateFCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
return(null);
}