Diff: navigation.cpp

Revision:

0:fd72f6df078c

Child:

1:a53d97b74fab

diff -r 000000000000 -r fd72f6df078c navigation.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/navigation.cpp	Fri Oct 30 15:48:48 2015 +0000
@@ -0,0 +1,183 @@
+#include "navigation.h"
+#include "stdint.h"
+#include <iostream>
+#include <fstream>
+#include <stack>
+#include <set>
+
+// FUNCTION:
+//      Navigation(int size = DEFAULT_SIZE)
+// IN-PARAMETERS:
+//      Number of initial vertices (DEFAULT = 1)
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Default and 1-parameter constructor.
+Navigation::Navigation(int size = DEFAULT_SIZE) : vertex(DEFAULT_VERTEX),
+                                                        angle(DEFAULT_ANGLE)
+{
+    graph.resize(size);
+}
+
+// FUNCTION:
+//      addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
+//                   uint16_t angle)
+// IN-PARAMETERS:
+//      Source node (uint16_t), target node (uint16_t), edge distance
+//      (uint16_t), and angle (uint16_t)
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Adds a graph node to the graph.
+void Navigation::addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
+                                 uint16_t angle)
+{
+    graph[src].push_back(graphNode(target, dist, angle));
+}
+
+// FUNCTION:
+//      addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
+//                   uint16_t angle)
+// IN-PARAMETERS:
+//      Source node (uint16_t)
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Adds a graph node to the graph, with target, dist, and angle
+//      initialized to infinity
+void Navigation::addGraphNode(uint16_t src)
+{
+    graph[src].push_back(graphNode());
+}
+
+// FUNCTION:
+//      int loadMap(char* infile);
+// IN-PARAMETERS:
+//      Input file name (*char[])
+// OUT-PARAMETERS:
+//      Returns 1 if file doesn't open
+//      Returns 0 if file opens succesfully
+// DESCRIPTION:
+//      Loads contents of a file as a weighted graph. First entry in the file
+//      should be the number of vertices. All subsequent entries indicate edges
+//      by listing a source vertex, a target vertex, the distance between them
+//      and the angle from source to target (0 - 359 degrees) . Only edges
+//      are listed. Assumes that inverse angles/edges are included in file.
+int Navigation::loadMap(char* inputFile)
+{
+    std::ifstream inFile;
+    inFile.open(inputFile);
+    if (!inFile)
+        return 1; // returns 1 if file fails to open
+
+    int vertices;
+    inFile >> vertices; // reads first entry as number of vertices
+    graph.resize(vertices); // resizes vector as necessary
+
+    uint16_t src;
+    uint16_t target;
+    uint16_t dist;
+    uint16_t angle;
+
+    while (!inFile.eof() )
+    {
+        inFile >> src;
+        inFile >> target;
+        inFile >> dist;
+        inFile >> angle;
+
+        addGraphNode(src, target, dist, angle);
+    } // loads all data from file into graph
+
+    return 0;
+}
+
+// FUNCTION:
+//      void getShortestPath(uint16_t src, uint16_t destination)
+// IN-PARAMETERS:
+//      Target vertex (uint16_t)
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Implementation of Dijkstra's algorithm. Uses current vertex as source
+//      and calculates shortest path information for all other vertices.
+//      Subroutine ends when shortest path for target vertex has been found
+//      and then loads the path into route.
+void Navigation::getShortestPath(uint16_t destination)
+{
+    uint16_t src = vertex;
+    uint16_t v; // stores temporary vertex
+    int n = graph.size();
+    minDistance.clear();
+    minDistance.resize(n, MAX_DIST);
+    minDistance[src] = 0;
+    previous.clear();
+    previous.resize(n, -1);
+    std::set<std::pair<uint16_t, uint16_t> > vertex_queue;
+    vertex_queue.insert(std::make_pair(minDistance[src], src));
+
+    while (!vertex_queue.empty())
+    {
+        uint16_t dist = vertex_queue.begin()->first;
+        uint16_t u = vertex_queue.begin()->second;
+        vertex_queue.erase(vertex_queue.begin());
+
+        // Visit each edge exiting u
+        const std::vector<graphNode> &neighbors = graph[u];
+        for (std::vector<graphNode>::const_iterator neighbor_iter = neighbors.begin();
+             neighbor_iter != neighbors.end();
+             neighbor_iter++)
+        {
+            v = neighbor_iter->neighbor;
+            uint16_t weight = neighbor_iter->distance;
+            uint16_t distance_through_u = dist + weight;
+
+            if (distance_through_u < minDistance[v])
+            {
+                vertex_queue.erase(std::make_pair(minDistance[v], v));
+
+                minDistance[v] = distance_through_u;
+                previous[v] = u;
+                vertex_queue.insert(std::make_pair(minDistance[v], v));
+            }
+            if (v == destination)
+                break;
+        }
+        if (v == destination)
+            break;
+    }
+    while (v != src)
+    {
+        route.push(v);
+        v = previous[v];
+    }
+}
+
+// utility function
+void Navigation::printPrevious(Serial &pc)
+{
+    for(int i = 0; i < previous.size(); i++)
+    {
+        pc.printf("Node %i is prev to node %i\n", previous[i], i);
+    }
+}
+
+// utility function
+void Navigation::printRoute(Serial &pc)
+{
+    while (!route.empty() )
+    {
+        pc.printf("Go to node %i\n", route.top() );
+        route.pop();
+    }
+}
+
+// utility function
+void Navigation::printGraph(Serial &pc)
+{
+    for (int i = 0; i < graph.size(); i++)
+        for (int j = 0; j < graph[i].size(); j++)
+        {
+            pc.printf("Node %i to %i = dist - %i, angle - %i\n", i, j, graph[i][j].distance, graph[i][j].angle);
+        }
+}