public bool FindPath(Waypoint w, ref List <Waypoint> path)
{
List <bool> visited = new List <bool>(m_graph.Size());
List <float> distance = new List <float>(m_graph.Size());
for (int i = 0; i < m_graph.Size(); ++i)
{
visited.Add(false);
distance.Add(-1);
if (m_graph.Waypoints()[i] == this)
{
distance[i] = 0;
}
}
DjikstraQueue queue = new DjikstraQueue();
queue.Push(0, this);
while (!queue.Empty())
{
Waypoint curPoint = queue.Pop();
if (visited[curPoint.m_index])
{
continue;
}
float curDistance = distance[curPoint.m_index];
if (curPoint == w)
{
Waypoint traceback = w;
while (traceback != null)
{
path.Add(traceback);
if (traceback == this)
{
break;
}
float minDistance = 0;
Waypoint nextNode = null;
foreach (var edge in traceback.m_edges)
{
Waypoint n = edge.Other(traceback);
if (!visited[n.m_index])
{
continue;
}
if (nextNode == null || (distance[n.m_index] < minDistance))
{
minDistance = distance[n.m_index];
nextNode = n;
}
}
traceback = nextNode;
}
path.Reverse();
return(true);
}
foreach (var edge in curPoint.m_edges)
{
Waypoint next = edge.Other(curPoint);
float nextDistance = curDistance + Vector3.Distance(curPoint.Position(), next.Position());
if (distance[next.m_index] == -1 || nextDistance < distance[next.m_index])
{
distance[next.m_index] = nextDistance;
queue.Push(nextDistance, next);
}
}
visited[curPoint.m_index] = true;
}
return(false);
}