public VoronoiNode GetClosestNode(Vector3 position)
{
VoronoiNode ClosestNode = openHeap.GetNode(0);
openHeap.ResetHeap();
openHeap.Push(0);
int numFurther = 0;
float closestDist = 10000;
int MaxNumLoops = 100000;
while (!openHeap.IsEmpty() && MaxNumLoops > 0)
{
MaxNumLoops--;
VoronoiNode least = openHeap.Pop();
float distToPos = Vector3.SqrMagnitude(least.Position - position);
if (distToPos < closestDist)
{
ClosestNode = least;
closestDist = distToPos;
numFurther = 0;
}
else
{
numFurther++;
if (numFurther > 10)
{
return(ClosestNode);
}
}
int[] neighbors = least.GetNeighbors();
for (int i = 0; i < 3; i++)
{
VoronoiNode nbr = openHeap.GetNode(neighbors[i]);
if (!nbr.Closed && !nbr.Open)
{
openHeap.SetHeuristic(nbr.Id, distToPos);
openHeap.SetCost(nbr.Id);
openHeap.Push(nbr.Id);
}
}
}
if (MaxNumLoops == 0)
{
Debug.Log("error: exited due to too many iterations in pathfinding loop");
}
else
{
Debug.Log("error: no path found");
}
return(ClosestNode);
}
public VoronoiNode GetFurthestNode(Vector3 position, float distToFlee)
{
VoronoiNode FurthestNode = openHeap.GetNode(Random.Range(0, navMesh.nodes.Length - 1));
openHeap.ResetHeap();
openHeap.Push(0);
int MaxNumLoops = 100000;
while (!openHeap.IsEmpty() && MaxNumLoops > 0)
{
MaxNumLoops--;
VoronoiNode least = openHeap.Pop();
float distToPos = Vector3.SqrMagnitude(least.Position - position);
if (distToPos > distToFlee)
{
return(FurthestNode);
}
int[] neighbors = least.GetNeighbors();
for (int i = 0; i < 3; i++)
{
VoronoiNode nbr = openHeap.GetNode(neighbors[i]);
if (!nbr.Closed && !nbr.Open)
{
openHeap.SetHeuristic(nbr.Id, 1 / distToPos);
openHeap.SetCost(nbr.Id);
openHeap.Push(nbr.Id);
}
}
}
if (MaxNumLoops == 0)
{
Debug.Log("error: exited due to too many iterations in pathfinding loop");
}
else
{
Debug.Log("error: no path found");
}
return(FurthestNode);
}
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);
}
public List <Vector3> GetPath(VoronoiNode start, VoronoiNode end)
{
List <Vector3> path = new List <Vector3>();
if (start.Id == end.Id)
{
return(path);
}
openHeap.ResetHeap();
openHeap.Push(start.Id);
int MaxNumLoops = 100000;
while (!openHeap.IsEmpty() && MaxNumLoops > 0)
{
MaxNumLoops--;
VoronoiNode least = openHeap.Pop();
if (least.Id == end.Id)
{
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;
//funnel path!!
return(path);
}
int[] neighbors = least.GetNeighbors();
for (int i = 0; i < 3; i++)
{
VoronoiNode nbr = openHeap.GetNode(neighbors[i]);
if (nbr.Open)
{
float DistToNbr = Vector3.Distance(nbr.Position, least.Position);
if (nbr.Given > least.Given + DistToNbr)
{
openHeap.SetGiven(nbr.Id, least.Id, least.Given + DistToNbr);
openHeap.SetCost(nbr.Id);
openHeap.UpdateNode(nbr.Id);
}
}
else if (!nbr.Closed)
{
openHeap.SetHeuristic(nbr.Id, Vector3.Distance(nbr.Position, end.Position));
openHeap.SetGiven(nbr.Id, least.Id, least.Given + Vector3.Distance(nbr.Position, least.Position));
openHeap.SetCost(nbr.Id);
openHeap.Push(nbr.Id);
}
}
}
if (MaxNumLoops == 0)
{
Debug.Log("error: exited due to too many iterations in pathfinding loop");
}
else
{
Debug.Log("error: no path found");
}
return(path);
}
public List <Vector3> GetPathToSafeSpot(VoronoiNode start, float SafetyThreshold, out VoronoiNode safeSpot)
{
List <Vector3> path = new List <Vector3>();
safeSpot = start;
if (navMesh.nodeVisability[start.Id] < SafetyThreshold)
{
VoronoiNode ss;
path = GetSafeRandomPath(start, SafetyThreshold, out ss);
safeSpot = ss;
return(path);
}
openHeap.ResetHeap();
openHeap.Push(start.Id);
int MaxNumLoops = 100000;
while (!openHeap.IsEmpty() && MaxNumLoops > 0)
{
MaxNumLoops--;
VoronoiNode least = openHeap.Pop();
if (navMesh.nodeVisability[least.Id] < SafetyThreshold)
{
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 nbrVisibility = navMesh.nodeVisability[nbr.Id];
openHeap.SetGiven(nbr.Id, least.Id, least.Given + nbrVisibility * Vector3.Distance(nbr.Position, least.Position));
openHeap.SetCost(nbr.Id);
openHeap.Push(nbr.Id);
}
}
}
if (MaxNumLoops == 0)
{
Debug.Log("error: exited due to too many iterations in pathfinding loop");
}
else
{
Debug.Log("error: no path found");
}
return(path);
}
