private RoadNode GetFirstNode(Waypoint startWp)
{
RoadNode node = new RoadNode(startWp);
node.GetNexts();
return(node);
}
public void Add(RoadNode newNode)
{
if (!newNode.beenAddedToGraph)
{
nodes.Add(newNode);
newNode.beenAddedToGraph = true;
numOfNodes++;
newNode.id = numOfNodes;
}
}
public RoadEdge NodeToEdge(RoadNode target)
{
foreach (RoadEdge edge in nextEdges)
{
if (edge.to == target)
{
return(edge);
}
}
return(null);
}
public RoadEdge[] Djikstra(Waypoint start, Waypoint target)
{
// Find some explanations here: https://www.geeksforgeeks.org/dijkstras-shortest-path-algorithm-greedy-algo-7/
ResetGraph();
RoadNode startingNode = ClosestFrom(start);
RoadNode endNode = ClosestFrom(target);
startingNode.minDist = 0f;
startingNode.minDistFrom = null;
// Hash function just returns node id. Wastes some memory, but quick to implement
RoadNode[] checkedList = new RoadNode[graph.nodes.Count];
SimplePriorityQueue <RoadNode> priorityQueue = new SimplePriorityQueue <RoadNode>();
foreach (RoadNode node in graph.nodes)
{
priorityQueue.Enqueue(node, node.minDist);
}
bool stopCondition = GetStopCondition(checkedList, endNode);
while (!stopCondition)
{
RoadNode checkedNode = priorityQueue.Dequeue();
checkedList[checkedNode.id] = checkedNode;
for (int i = 0; i < checkedNode.nexts.Count; i++)
{
if (checkedNode.minDist + checkedNode.nextEdges[i].weight < checkedNode.nexts[i].minDist)
{
checkedNode.nexts[i].minDist = checkedNode.minDist + checkedNode.nextEdges[i].weight;
checkedNode.nexts[i].minDistFrom = checkedNode.nextEdges[i];
}
}
stopCondition = GetStopCondition(checkedList, endNode);
}
List <RoadEdge> reversePath = new List <RoadEdge>();
RoadNode currentRoad = endNode;
while (currentRoad.minDist != 0)
{
reversePath.Add(currentRoad.minDistFrom);
currentRoad = currentRoad.minDistFrom.from;
}
RoadEdge[] path = reversePath.ToArray();
Array.Reverse(path, 0, path.Length);
return(path);
}
private void AssignNexts(Waypoint[] paths)
{
foreach (Waypoint path in paths)
{
Waypoint[] waypointsToNearestNext = ClosestNodableWaypoint(path);
Waypoint nextWaypoint = waypointsToNearestNext[waypointsToNearestNext.Length - 1];
if (nextWaypoint != null)
{
if (nextWaypoint.graphNode == null)
{
RoadNode next = new RoadNode(nextWaypoint);
nextWaypoint.graphNode = next;
}
nexts.Add(nextWaypoint.graphNode);
nextWaypoint.graphNode.prevs.Add(this);
RoadEdge edge = new RoadEdge(this, nextWaypoint.graphNode, waypointsToNearestNext);
nextEdges.Add(edge);
nextWaypoint.graphNode.prevEdges.Add(edge);
}
}
}
private bool GetStopCondition(RoadNode[] chckedList, RoadNode target)
{
return(chckedList[target.id] == target);
}
