public List <Vector3> ConstructPath(NavNode goal, Dictionary <NavNode, NavNode> parents)
{
List <Vector3> path = new List <Vector3>(parents.Count + 1);
List <NavNode> pathToPrint = new List <NavNode>(parents.Count + 1);
path.Insert(0, goal.Position);
pathToPrint.Insert(0, goal);
bool done = false;
NavNode curr = goal;
g_PathLengthNodes++;
while (!done)
{
if (parents[curr].Equals(curr))
{
done = true;
}
curr.SetHighlightTile(true, curr == goal ? Color.green : Color.yellow, 0.8f);
curr = parents[curr];
path.Insert(0, curr.Position);
pathToPrint.Insert(0, curr);
g_PathLengthNodes++;
if (g_PathLengthNodes > Mathf.Pow(Mathf.Max(g_Grid.transform.localScale.x, g_Grid.transform.localScale.z), 3.0f))
{
throw new System.Exception("Impossible to construct path");
}
}
if (g_Grid != null)
{
g_Grid.WritePathToFile(pathToPrint);
}
return(path);
}
public void DryRunAlgorithm()
{
if (DryRunAlgo)
{
RaycastHit hit;
List <Vector3> pathList = new List <Vector3>();
if (Physics.Raycast(new Ray(transform.position + (transform.up * 0.2f), -1 * transform.up), out hit))
{
g_Grid = hit.collider.GetComponent <NavGrid>();
int succeed = 0;
while (succeed < DryRunTests)
{
int x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)),
y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.y - 1));
g_FromNode = g_Grid.GetGridNode(x, y);
if (g_FromNode != null)
{
NavNode to = null;
do
{
x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1));
y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1));
to = g_Grid.GetGridNode(x, y);
} while (to == null);
g_FromNode.SetHighlightTile(true, Color.black, 1f);
FindPath(g_FromNode.Position, to.Position);
succeed++;
}
}
}
g_NPCController.Debug("Finished algorithm dry run");
}
DryRunAlgo = false;
g_FromNode = null;
}
public void AddINPCPathfinderNode(INPCPathfinder ipf, NavNode node)
{
if (g_WalkedOnNodes.ContainsKey(ipf) && g_WalkedOnNodes[ipf] != node)
{
if (Application.isPlaying)
{
g_WalkedOnNodes[ipf].SetHighlightTile(false, Color.grey, 0.5f);
}
g_WalkedOnNodes.Remove(ipf);
}
else if (!g_WalkedOnNodes.ContainsKey(ipf))
{
g_WalkedOnNodes.Add(ipf, node);
if (Application.isPlaying)
{
node.SetHighlightTile(true, Color.red, 0.5f);
}
}
}
public List <Vector3> ConstructPath(NavNode goal, Dictionary <NavNode, NavNode> parents)
{
List <Vector3> path = new List <Vector3>(parents.Count + 1);
List <NavNode> pathToPrint = new List <NavNode>(parents.Count + 1);
path.Insert(0, goal.Position);
pathToPrint.Insert(0, goal);
bool done = false;
NavNode curr = goal;
g_PathLengthNodes++;
while (!done)
{
if (parents.ContainsKey(curr))
{
if (parents[curr].Equals(curr))
{
done = true;
}
curr.SetHighlightTile(true, curr == goal ? Color.green : Color.yellow, 0.8f);
curr = parents[curr];
path.Insert(0, curr.Position);
pathToPrint.Insert(0, curr);
g_PathLengthNodes++;
}
else
{
done = true;
}
}
if (g_Grid != null)
{
g_Grid.WritePathToFile(pathToPrint);
}
return(path);
}
public void Benchmark()
{
if (BenchmarkAll)
{
List <float> times = new List <float>();
for (int r = 0; r < 5; r++)
{
float globalBenchMarkTime = Time.realtimeSinceStartup;
Dictionary <string, int> hCounter = new Dictionary <string, int>();
Dictionary <string, float> timeAvg = new Dictionary <string, float>();
RaycastHit hit;
List <Vector3> pathList = new List <Vector3>();
if (Physics.Raycast(new Ray(transform.position + (transform.up * 0.2f), -1 * transform.up), out hit))
{
g_Grid = hit.collider.GetComponent <NavGrid>();
int succeed = 0;
while (succeed < DryRunTests)
{
int x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)),
y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.y - 1));
g_FromNode = g_Grid.GetGridNode(x, y);
if (g_FromNode != null)
{
NavNode to = null;
do
{
x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1));
y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1));
to = g_Grid.GetGridNode(x, y);
} while (to == null);
NavNode from = g_FromNode;
// benchmark each path
for (int i = 0; i < 10; ++i)
{
string log = "";
float w2 = 1f;
if (i == 0)
{
foreach (NavSeqAStar n in g_Pathfinders)
{
n.g_SecondaryHeuristicsWeight = w2;
}
}
else if (i == 1)
{
foreach (NavSeqAStar n in g_Pathfinders)
{
w2 += 0.25f;
n.g_SecondaryHeuristicsWeight = w2;
}
}
else if (i == 2)
{
foreach (NavSeqAStar n in g_Pathfinders)
{
w2 += 0.5f;
n.g_SecondaryHeuristicsWeight = w2;
}
}
else if (i == 3)
{
foreach (NavSeqAStar n in g_Pathfinders)
{
w2 += 0.75f;
n.g_SecondaryHeuristicsWeight = w2;
}
}
else
{
foreach (NavSeqAStar n in g_Pathfinders)
{
w2 += 1f;
n.g_SecondaryHeuristicsWeight = w2;
}
}
float now = Time.realtimeSinceStartup;
from.SetHighlightTile(true, Color.black, 1f);
FindPath(from.Position, to.Position);
if (hCounter.ContainsKey(g_Winner))
{
hCounter[g_Winner] += 1;
}
else
{
hCounter.Add(g_Winner, 1);
}
log += "W2: " + w2 + "\n";
float t = (Time.realtimeSinceStartup - now);
log += "Time: " + t + "ms \n";
if (timeAvg.ContainsKey(g_Winner))
{
timeAvg[g_Winner] += t;
}
else
{
timeAvg.Add(g_Winner, t);
}
UnityEngine.Debug.Log(log);
g_CurrentPathValue = 0f;
g_PathLengthNodes = 0;
}
foreach (string s in hCounter.Keys)
{
Debug.Log(s + " average time: " + timeAvg[s] / hCounter[s] + "\n");
}
succeed++;
timeAvg.Clear();
hCounter.Clear();
}
}
}
times.Add((Time.realtimeSinceStartup - globalBenchMarkTime));
}
foreach (float f in times)
{
Debug.Log(f);
}
}
DryRunAlgo = false;
g_FromNode = null;
}
public List <Vector3> FindPath(Vector3 from, Vector3 to)
{
ClearPath();
foreach (NavSeqAStar s in g_Pathfinders)
{
s.ClearPath();
}
if (DynamicWeight)
{
HeuristicWeight = 1.0f;
}
RaycastHit hit;
g_PathList = new List <Vector3>();
if (Physics.Raycast(new Ray(transform.position + (transform.up * 0.2f), -1 * transform.up), out hit))
{
// Ensure we are on a grid
g_Grid = hit.collider.GetComponent <NavGrid>();
if (CleanPathOnRestart)
{
g_Grid.CleanAll();
}
g_FromNode = g_FromNode == null?g_Grid.GetOccupiedNode(this) : g_FromNode;
if (g_FromNode == null)
{
g_NPCController.Debug("NavAStar --> Agent is currently navigating in between nodes, try again please");
return(g_PathList);
}
/* Initialize all heuristics here */
InitializeFinder(g_FromNode, to, g_Grid);
foreach (NavSeqAStar s in g_Pathfinders)
{
s.InitializeFinder(g_FromNode, to, g_Grid);
}
while (g_OpenList.Count > 0)
{
// get next optimal node
NavNode n = g_Fringe.Values[0];
// Then Heuristic-based search
foreach (NavSeqAStar nav in g_Pathfinders)
{
float va = nav.g_gVal.Count == 0 ? float.MaxValue : nav.g_gVal[nav.g_Fringe.Values[0]];
if (va < (nav.g_SecondaryHeuristicsWeight * g_gVal[g_Fringe.Values[0]]))
{
if (IsCurrentStateGoal(new System.Object[] { nav.g_Fringe.Values[0] }))
{
g_Winner = "Heuristic won: " + nav.Name();
n = nav.g_Fringe.Values[0];
n.SetHighlightTile(true, Color.green, 1f);
g_GoalNode = n;
g_PathList = ConstructPath(n, nav.g_Parents);
g_CurrentPathValue = nav.g_gVal[n];
g_NPCController.Debug(g_Winner);
goto exit_pathfinding;
}
else
{
NavNode s = nav.g_Fringe.Values[0];
nav.ExpandNode(s);
nav.g_ClosedList.Add(s);
}
}
else if (IsCurrentStateGoal(new System.Object[] { n }))
{
g_Winner = "Search won: " + nav.Name();
n.SetHighlightTile(true, Color.green, 1f);
g_GoalNode = n;
g_PathList = ConstructPath(g_GoalNode, g_Parents);
g_CurrentPathValue = g_gVal[n];
g_NPCController.Debug(g_Winner);
goto exit_pathfinding;
}
else
{
ExpandNode(n);
g_ClosedList.Add(n);
}
}
}
}
else
{
g_NPCController.Debug("NavAStar --> Pathfinder not on grid");
}
exit_pathfinding:
g_FromNode = null;
return(g_PathList);
}