public List <Vector3> FindPath(Node deptNode, Node destNode)
{
NodeHeap openHeap = new NodeHeap();
HashSet <Node> closedSet = new HashSet <Node>();
openHeap.Add(deptNode);
deptNode.fromCost = deptNode.toCost = 0;
deptNode.from = destNode.from = null;
while (openHeap.Count > 0)
{
Node nodeToVisit = openHeap.Pop();
closedSet.Add(nodeToVisit);
if (nodeToVisit == destNode)
{
break;
}
foreach (var adjacent in GetAdjacents(nodeToVisit))
{
if (closedSet.Contains(adjacent))
{
continue;
}
Node fromNode = null;
if (nodeToVisit.from != null && nodeToVisit.from.IsLineOfSight(adjacent))
{
fromNode = nodeToVisit.from;
}
else
{
fromNode = nodeToVisit;
}
int toAdjacentCost = fromNode.toCost + PredictDistanceCost(fromNode, adjacent);
if (!openHeap.Contains(adjacent) || toAdjacentCost < adjacent.toCost)
{
adjacent.toCost = toAdjacentCost;
adjacent.fromCost = PredictDistanceCost(adjacent, destNode);
adjacent.from = fromNode;
if (openHeap.Contains(adjacent))
{
openHeap.UpdateItem(adjacent);
}
else
{
openHeap.Add(adjacent);
}
}
}
}
List <Node> path = new List <Node>();
if (destNode.from != null)
{
Node currentNode = destNode;
while (currentNode != null)
{
path.Add(currentNode);
currentNode = currentNode.from;
}
path.Reverse();
}
// PS
// SmoothingPath(path);
List <Vector3> positionPath = new List <Vector3>();
// Visit departure node looks wired when path is changed
// Skip first(departure) node
for (int i = 1; i < path.Count; i++)
{
positionPath.Add(path[i].tile.transform.position);
}
return(positionPath);
}
public List <Vector3> GetSafeRandomPath(VoronoiNode start, float SafetyThreshold, out VoronoiNode safeSpot)
{
List <Vector3> path = new List <Vector3>();
openHeap.ResetHeap();
openHeap.Push(start.Id);
int MaxNumLoops = 5;
while (!openHeap.IsEmpty())
{
MaxNumLoops--;
VoronoiNode least = openHeap.Pop();
if (MaxNumLoops < 0)
{
safeSpot = least;
int numStops = 1;
List <VoronoiNode> nodeList = new List <VoronoiNode>();
while (least.Id != start.Id)
{
numStops++;
path.Add(least.Position);
nodeList.Add(least);
least = openHeap.GetNode(least.Parent);
}
nodeList.Add(start);
path.Add(start.Position);
roughMoves = path;
VoronoiNode[] nodeArr = new VoronoiNode[numStops];
int i = 0;
foreach (VoronoiNode n in nodeList)
{
nodeArr[i] = n;
i++;
}
path = GetFunnelPath(nodeArr);
smoothMoves = path;
return(path);
}
int[] neighbors = least.GetNeighbors();
for (int i = 0; i < 3; i++)
{
VoronoiNode nbr = openHeap.GetNode(neighbors[i]);
if (!nbr.Open && !nbr.Closed)
{
float nbrSafety = 1 - (navMesh.distanceFromPlayer[nbr.Id] / navMesh.maxDistanceFromPlayer);
openHeap.SetGiven(nbr.Id, least.Id, nbrSafety);
openHeap.SetCost(nbr.Id);
openHeap.Push(nbr.Id);
}
}
}
safeSpot = start;
return(path);
}