Adding graph routines for the arm path planning.
This is guaranteed not to perform any runtime allocations, and
allows constructing a list of vertexes, and directed edges with costs
and then traversing this graph optimally based on those costs.
Change-Id: I17614b794c3ec5c3e1569cc75a030acf40a942b0
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/y2018/control_loops/superstructure/arm/graph.cc
new file mode 100644
index 0000000..a1e6fb2
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/graph.cc
@@ -0,0 +1,65 @@
+#include "y2018/control_loops/superstructure/arm/graph.h"
+
+#include <assert.h>
+#include <algorithm>
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+SearchGraph::SearchGraph(size_t num_vertexes, const std::vector<Edge> &edges)
+ : edges_(edges) {
+ vertexes_.resize(num_vertexes);
+ size_t i = 0;
+ for (const auto &edge : edges) {
+ assert(edge.start < num_vertexes);
+ assert(edge.end < num_vertexes);
+ assert(edge.start != edge.end);
+ assert(edge.cost > 0);
+ vertexes_[edge.start].forward.emplace_back(HalfEdge{edge.end, i});
+ vertexes_[edge.end].reverse.emplace_back(HalfEdge{edge.start, i});
+ ++i;
+ }
+ for (auto &vertex : vertexes_) {
+ std::sort(vertex.forward.begin(), vertex.forward.end());
+ std::sort(vertex.reverse.begin(), vertex.reverse.end());
+ }
+ vertex_heap_.Reserve(num_vertexes);
+}
+
+SearchGraph::~SearchGraph() {}
+
+void SearchGraph::SetGoal(size_t vertex) {
+ assert(vertex < vertexes_.size());
+ vertex_heap_.Clear();
+
+ for (auto &vertex : vertexes_) {
+ vertex.cached_distance = std::numeric_limits<double>::infinity();
+ }
+
+ vertexes_[vertex].cached_distance = 0.0;
+ vertex_heap_.InsertOrDecrease(&vertexes_[vertex]);
+
+ while (!vertex_heap_.Empty()) {
+ Vertex *vertex = vertex_heap_.PopMin();
+ for (const auto &half_edge : vertex->reverse) {
+ Vertex *to_adjust = &vertexes_[half_edge.dest];
+ double adjust_cost = vertex->cached_distance + GetCost(half_edge);
+ if (adjust_cost < to_adjust->cached_distance) {
+ to_adjust->cached_distance = adjust_cost;
+ vertex_heap_.InsertOrDecrease(to_adjust);
+ }
+ }
+ }
+}
+
+double SearchGraph::GetCostToGoal(size_t vertex) {
+ assert(vertex < vertexes_.size());
+ return vertexes_[vertex].cached_distance;
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018