// Use this for initialization
public void SetUpPathPlanning(Vector3 start)
{
map = new GenerateGraph(start);
//print (map.ToStringWithNeighbors());
//print (map.nodes.Count);
print("Start Node: " + map.startNode.point.ToString());
print("Node Count: " + map.nodes.Count);
print("End Node: " + map.endNode.point.ToString());
determinePath(map.startNode, map.endNode);
}
// <summary>
// Given two Node objects, determines a path between the startNode and the endNode
// </summary>
// <param name="startNode"> the start Node </param>
// <param name="endNode"> the end Node </param>
public void determinePath(GenerateGraph.Node startNode, GenerateGraph.Node endNode)
{
if (startNode == null || endNode == null)
return;
PriorityQueue<GenerateGraph.Node> pq = new PriorityQueue<GenerateGraph.Node>(map.nodes.Count);
pq.queue(startNode);
Dictionary<GenerateGraph.Node, GenerateGraph.Node> came_from = new Dictionary<GenerateGraph.Node, GenerateGraph.Node>();
Dictionary<GenerateGraph.Node, float> cost_so_far = new Dictionary<GenerateGraph.Node, float> ();
came_from.Add(startNode, null);
cost_so_far.Add (startNode, 0);
Dictionary<GenerateGraph.Node, int> nodeToId = new Dictionary<GenerateGraph.Node, int>();
for (int i = 0; i < map.nodes.Count; i++) {
nodeToId.Add (map.nodes[i], i);
}
GenerateGraph.Node current;
while (pq.getSize() > 0) {
current = pq.dequeue();
print(current.point.ToString());
print("Neighbor Count: " + current.neighbors.Count);
if (current.Equals(endNode)) {
print ("True");
break;
}
for (int i = 0; i < current.neighbors.Count; i++) {
print("Current Neighbor: " + current.neighbors[i].point.ToString());
float new_cost = cost_so_far[current] +
distanceBetweenNodes(current, current.neighbors[i]);
if (cost_so_far.ContainsKey(current.neighbors[i]) == false ||
new_cost < cost_so_far[current.neighbors[i]]) {
cost_so_far[current.neighbors[i]] = new_cost;
current.neighbors[i].priority = new_cost;
pq.queue(current.neighbors[i]);
came_from[current.neighbors[i]] = current;
}
}
}
//Put nodes of the path into the list
path = new List<GenerateGraph.Node> ();
GenerateGraph.Node currentNode = endNode;
path.Add (currentNode);
while (currentNode.Equals(startNode) == false) {
currentNode = came_from[currentNode];
path.Add (currentNode);
}
path.Reverse();
print ("Path Nodes: ");
for (int i = 0; i < path.Count; i++) {
print(nodeToId[path[i]] + "\n");
}
}
// <summary>
// Given two Node objects, determines the distance between them. Specifically, the
// distance between the centroids of their triangles is returned.
// </summary>
// <param name="n1"> the first given Node </param>
// <param name="n2"> the second given Node </param>
public float distanceBetweenNodes(GenerateGraph.Node n1, GenerateGraph.Node n2)
{
return Vector3.Distance(n1.point, n2.point);
}
// Use this for initialization
public void SetUpPathPlanning(Vector3 start)
{
map = new GenerateGraph(start);
//print (map.ToStringWithNeighbors());
//print (map.nodes.Count);
print("Start Node: " + map.startNode.point.ToString());
print ("Node Count: " + map.nodes.Count);
print("End Node: " + map.endNode.point.ToString ());
determinePath(map.startNode, map.endNode);
}