Add shortest path implementation

Signed-off-by: Francis Giraldeau <[email protected]>

evnav/evnav.cpp

@@ -1,5 +1,6 @@
-#include "evnav.h"
 #include <QElapsedTimer>
+#include "evnav.h"
+#include "graph.h"
 
 #define _USE_MATH_DEFINES
 #include <cmath>
@@ -123,12 +124,119 @@ void Evnav::checkCachePerformance()
     qDebug() << "hist:" << hist;
 }
 
-#include <boost/graph/adjacency_list.hpp>
-using namespace boost;
-typedef adjacency_list<vecS, vecS, directedS,
-    no_property, property<edge_index_t, std::size_t> > 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
 
 }

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);
 
 

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

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<VertexId> queue;
+    QSet<VertexId> 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<Edge> ShortestPath::pathTo(VertexId dst)
+{
+    QList<Edge> 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;
+    }
+}

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<Edge> pathTo(VertexId dst);
+
+private:
+    void relax(Edge &e);
+
+    Graph& m_graph;
+    VertexId m_src;
+    QMap<VertexId, double> m_distTo;
+    QMap<VertexId, Edge> m_edgeTo;
+};
+
+#endif // SHORTESTPATH_H

tests/graph/tst_graphtest.cpp

@@ -1,8 +1,9 @@
 #include <QString>
-#include <QDebug>
 #include <QtTest>
+#include <QDebug>
 
 #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<Edge> path = sp.pathTo(1);
+    QVERIFY2(path.size() == 3, "path has 3 edges");
+
+}
+
 QTEST_APPLESS_MAIN(GraphTest)
 
 #include "tst_graphtest.moc"