From 88828dfc691f6307aebf9fe3d4ce450966e5ac9e Mon Sep 17 00:00:00 2001 From: Francis Giraldeau Date: Thu, 12 May 2016 13:57:00 -0400 Subject: [PATCH] Add shortest path implementation Signed-off-by: Francis Giraldeau --- evnav/evnav.cpp | 118 ++++++++++++++++++++++++++++++++-- graph/edge.h | 7 +- graph/graph.pro | 6 +- graph/shortestpath.cpp | 64 ++++++++++++++++++ graph/shortestpath.h | 25 +++++++ tests/graph/tst_graphtest.cpp | 39 ++++++++++- 6 files changed, 249 insertions(+), 10 deletions(-) create mode 100644 graph/shortestpath.cpp create mode 100644 graph/shortestpath.h diff --git a/evnav/evnav.cpp b/evnav/evnav.cpp index 13a5bba..cc78bd2 100644 --- a/evnav/evnav.cpp +++ b/evnav/evnav.cpp @@ -1,5 +1,6 @@ -#include "evnav.h" #include +#include "evnav.h" +#include "graph.h" #define _USE_MATH_DEFINES #include @@ -123,12 +124,119 @@ void Evnav::checkCachePerformance() qDebug() << "hist:" << hist; } -#include -using namespace boost; -typedef adjacency_list > Graph; +double computeEnergy(Trip &trip, double eff) +{ + // FIXME: take into account the speed + return (trip.dist_m / 1000) * eff; // kwh +} + +double computeChargingTime(double energy, double power) +{ + return (energy / power) * 3600.0; // s +} + +double computeSetupTime() +{ + return 5 * 60.0; // 5 minutes to s +} + +double computeTripTimeWithCharging(Trip &trip, double energy, double power) +{ + return trip.time_s + + computeSetupTime() + + computeChargingTime(energy, power); +} void Evnav::route(Coordinate &src, Coordinate &dst) { + Trip trip; + Graph g; + + double eff = 0.150; // kWh/km + double power = 50.0; // kW + double batt = 18.0; // kWh + double SOC_act = 0.95; + double SOC_min = 0.05; + double SOC_max = 0.80; + double SOC_dyn = SOC_max - SOC_min; + double batt_avail = batt * SOC_act; // kWh + + if (computeTrip(src, dst, trip) == Status::Ok) { + double e = computeEnergy(trip, eff); + double e_otw = 0; // kWh + if (e > batt_avail) { + e_otw = e - batt_avail; + } + qDebug() << "energy required : " << e << "kWh"; + qDebug() << "energy start : " << batt_avail << "kWh"; + qDebug() << "energy on the way: " << e_otw << "kWh"; + if (e < batt_avail) { + qDebug() << "reaching destination without charging"; + return; + } else { + int min_stops = std::ceil(e_otw / (batt * SOC_dyn)); + qDebug() << "charging min_stops:" << min_stops; + qDebug() << "charging min_time :" << computeChargingTime(e_otw, power); + } + } + + // Add edge from the source to all chargers + // FIXME: create waypoint class with chargers that extends it + VertexId srcId = -1; + VertexId dstId = -2; + + qDebug() << "source to chargers:"; + for (Charger &a : m_provider.chargers()) { + if (computeTrip(src, a.loc(), trip) == Status::Ok) { + double e = computeEnergy(trip, eff); + if (e < batt_avail) { + qDebug() << "can reach charger:" << a.name(); + g.addEdge(Edge{srcId, a.id(), (double)trip.time_s}); + } + } + } + + // Add all intermediate chargers + qDebug() << "intermediate chargers"; + chargerMatrix([&](Charger &a, Charger &b, Trip &t) { + // do not add edges between chargers that are too close + if (t.dist_m < 1000) { + return; + } + double e = computeEnergy(t, eff); + if (e < (batt * SOC_dyn)) { + //double charge_time = computeChargingTime(e, power); + double total_time = computeTripTimeWithCharging(trip, e, power); + Edge edge{a.id(), b.id(), total_time}; + qDebug() << a.name() << " -> " << b.name(); + /* + << "distance:" << (t.dist_m / 1000.0) + << "travel time:" << (t.time_s / 3600.0) + << "charge time:" << (charge_time / 3600.0) + << "total time :" << (total_time / 3600.0); + */ + g.addEdge(edge); + } + }); + + // Add edge from all chargers to the destination + qDebug() << "chargers to destination"; + for (Charger &a : m_provider.chargers()) { + if (computeTrip(a.loc(), dst, trip) == Status::Ok) { + double e = computeEnergy(trip, eff); + if (e < (batt * SOC_dyn)) { + qDebug() << "can reach charger:" << a.name(); + double total_time = computeTripTimeWithCharging(trip, e, power); + g.addEdge(Edge{a.id(), dstId, total_time}); + } + } + } + + qDebug() << "graph size:" << g.E(); + + // TODO: write the graph as Json + + // compute the shortest path + // compute detail of the trip } diff --git a/graph/edge.h b/graph/edge.h index 7350a0b..952b549 100644 --- a/graph/edge.h +++ b/graph/edge.h @@ -8,18 +8,21 @@ typedef int EdgeId; class Edge { public: - Edge() : m_from(-1), m_to(-1), m_weight(0) {} + Edge() : m_from(INT_MAX), m_to(INT_MAX), m_weight(0) {} Edge(VertexId u, VertexId v, double weight); VertexId from() const { return m_from; } VertexId to() const { return m_to; } double weight() const { return m_weight; } + bool operator!= (const Edge &e) { + return m_from != e.m_from || m_to != e.m_to; + } + private: VertexId m_from; VertexId m_to; double m_weight; }; - QDebug operator<<(QDebug dbg, const Edge &point); diff --git a/graph/graph.pro b/graph/graph.pro index b57e217..e9ebab8 100644 --- a/graph/graph.pro +++ b/graph/graph.pro @@ -14,10 +14,12 @@ TEMPLATE = lib CONFIG += staticlib SOURCES += graph.cpp \ - edge.cpp + edge.cpp \ + shortestpath.cpp HEADERS += graph.h \ - edge.h + edge.h \ + shortestpath.h unix { target.path = /usr/lib INSTALLS += target diff --git a/graph/shortestpath.cpp b/graph/shortestpath.cpp new file mode 100644 index 0000000..837bce2 --- /dev/null +++ b/graph/shortestpath.cpp @@ -0,0 +1,64 @@ +#include "shortestpath.h" + +ShortestPath::ShortestPath(Graph &g, VertexId src) : + m_graph(g), m_src(src) +{ + m_distTo[src] = 0; + QList queue; + QSet marked; + + marked.insert(src); + queue.append(src); + while(!queue.isEmpty()) { + int v = queue.takeFirst(); + for (Edge &e : g.adj(v)) { + relax(e); + if (!marked.contains(e.to())) { + marked.insert(e.to()); + queue.append(e.to()); + } + } + } +} + +double ShortestPath::distTo(VertexId dst) +{ + if (m_distTo.contains(dst)) + return m_distTo[dst]; + return INFINITY; +} + +bool ShortestPath::hasPathTo(VertexId dst) +{ + return m_distTo.contains(dst); +} + +QList ShortestPath::pathTo(VertexId dst) +{ + QList path; + if (hasPathTo(dst)) { + Edge nullEdge{}; + for (Edge &e = m_edgeTo[dst]; + e != nullEdge; + e = m_edgeTo[e.from()]) { + path.push_front(e); + } + } + return path; +} + +void ShortestPath::relax(Edge &e) +{ + double old_dist = INFINITY; + double new_dist = INFINITY; + VertexId v = e.from(); + VertexId w = e.to(); + if (m_distTo.contains(v)) + new_dist = m_distTo[v] + e.weight(); + if (m_distTo.contains(w)) + old_dist = m_distTo[w]; + if (old_dist > new_dist) { + m_distTo[w] = new_dist; + m_edgeTo[w] = e; + } +} diff --git a/graph/shortestpath.h b/graph/shortestpath.h new file mode 100644 index 0000000..21f8f57 --- /dev/null +++ b/graph/shortestpath.h @@ -0,0 +1,25 @@ +#ifndef SHORTESTPATH_H +#define SHORTESTPATH_H + +#include "graph.h" + +class ShortestPath +{ +public: + // FIXME: the graph must exists for the lifespan of this instance. + // convert graph to refcount for safety + ShortestPath(Graph &graph, VertexId srcId); + double distTo(VertexId dst); + bool hasPathTo(VertexId dst); + QList pathTo(VertexId dst); + +private: + void relax(Edge &e); + + Graph& m_graph; + VertexId m_src; + QMap m_distTo; + QMap m_edgeTo; +}; + +#endif // SHORTESTPATH_H diff --git a/tests/graph/tst_graphtest.cpp b/tests/graph/tst_graphtest.cpp index 0a1d756..c1cc8bd 100644 --- a/tests/graph/tst_graphtest.cpp +++ b/tests/graph/tst_graphtest.cpp @@ -1,8 +1,9 @@ #include -#include #include +#include #include "graph.h" +#include "shortestpath.h" class GraphTest : public QObject { @@ -13,6 +14,7 @@ class GraphTest : public QObject private Q_SLOTS: void testCase1(); + void testCase2(); }; GraphTest::GraphTest() @@ -35,6 +37,41 @@ void GraphTest::testCase1() QVERIFY2(g.V() == 3, "total vertices"); } +static Edge g1[] = { + { 4, 5, 0.35 }, + { 5, 4, 0.35 }, + { 4, 7, 0.37 }, + { 5, 7, 0.28 }, + { 7, 5, 0.28 }, + { 5, 1, 0.32 }, + { 0, 4, 0.38 }, + { 0, 2, 0.26 }, + { 7, 3, 0.39 }, + { 1, 3, 0.29 }, + { 2, 7, 0.34 }, + { 6, 2, 0.40 }, + { 3, 6, 0.52 }, + { 6, 0, 0.58 }, + { 6, 4, 0.93 }, +}; +int g1n = sizeof(g1) / sizeof(Edge); + +void GraphTest::testCase2() +{ + Graph g; + for (int i = 0; i < g1n; ++i) { + g.addEdge(g1[i]); + } + + ShortestPath sp(g, 0); + + QVERIFY2(sp.hasPathTo(1), "path to v1 exists"); + QVERIFY2(std::fabs(sp.distTo(1) - 1.05) < 0.0001, "wrong distance to v1"); + QList path = sp.pathTo(1); + QVERIFY2(path.size() == 3, "path has 3 edges"); + +} + QTEST_APPLESS_MAIN(GraphTest) #include "tst_graphtest.moc"