Something went wrong. Try again.
Distances on Directed Graphs in R
Something went wrong. Try again.
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517#include "pathfinders.h"#include "heaps/heap.h"
#include <algorithm> // std::fill
// Modified from code by Shane Saunders
// @param n Number of vertices in graph// @param heapD The type of heap used// @param g A DGraph object// @param twoheap If `TRUE`, uses a bi-directional search.PF::PathFinder::PathFinder(size_t n, const HeapDesc& heapD, std::shared_ptr<const DGraph> g, bool twoheap) : m_is_bidirectional(twoheap){ m_heap = heapD.newInstance (n); m_closed = new bool [n]; m_open = new bool [n]; if (twoheap) { m_heap_rev = heapD.newInstance (n); m_closed2 = new bool [n]; m_open2 = new bool [n]; } else { m_heap_rev = nullptr; m_closed2 = nullptr; m_open2 = nullptr; } init (g);}
PF::PathFinder::~PathFinder() { delete [] m_open; delete [] m_closed; delete m_heap; if (m_is_bidirectional) { delete [] m_open2; delete [] m_closed2; delete m_heap_rev; }}
void PF::PathFinder::init(std::shared_ptr<const DGraph> g) { m_graph = g;}
void PF::PathFinder::init_arrays ( std::vector <double>& d, std::vector <double>& w, std::vector <long int>& prev, bool *m_open_vec, bool *m_closed_vec, const size_t v, const size_t n){ m_heap->clear (); // safe no-op for non-BHeap types; enables PathFinder reuse
std::fill (w.begin (), w.end (), INFINITE_DOUBLE); std::fill (d.begin (), d.end (), INFINITE_DOUBLE); std::fill (prev.begin (), prev.end (), INFINITE_INT); w [v] = 0.0; d [v] = 0.0; prev [v] = -1;
std::fill (m_open_vec, m_open_vec + n, false); std::fill (m_closed_vec, m_closed_vec + n, false); m_open_vec [v] = true;}
void PF::PathFinder::scan_edges (const DGraphEdge *edge, std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, bool *m_open_vec, const bool *m_closed_vec, const size_t &v0){ while (edge) { size_t et = edge->target; if (!m_closed_vec [et]) { double wt = w [v0] + edge->wt; if (wt < w [et]) { d [et] = d [v0] + edge->dist; w [et] = wt; prev [et] = static_cast <int> (v0);
if (m_open_vec [et]) { m_heap->decreaseKey(et, wt); } else { m_heap->insert (et, wt); m_open_vec [et] = true; } } } edge = edge->nextOut; }}
void PF::PathFinder::scan_edges_heur (const DGraphEdge *edge, std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, bool *m_open_vec, const bool *m_closed_vec, const size_t &v0, const double target_x, const double target_y, const std::vector<double> &vx, const std::vector<double> &vy, const bool is_spatial){ while (edge) { size_t et = edge->target; if (!m_closed_vec [et]) { double wt = w [v0] + edge->wt; if (wt < w [et]) { d [et] = d [v0] + edge->dist; w [et] = wt; prev [et] = static_cast <int> (v0);
double heur = 0.0; if (is_spatial) { const double dx = vx [et] - target_x; const double dy = vy [et] - target_y; heur = sqrt (dx * dx + dy * dy); }
if (m_open_vec [et]) { m_heap->decreaseKey(et, wt + heur); } else { m_heap->insert (et, wt + heur); m_open_vec [et] = true; } } } edge = edge->nextOut; }}
/********************************************************************** ************************ PATH ALGORITHMS ************************* **********************************************************************/
void PF::PathFinder::Dijkstra ( std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, const size_t v0, const std::vector <size_t> &to_index){ const DGraphEdge *edge;
const size_t n = m_graph->nVertices(); const std::vector<DGraphVertex>& vertices = m_graph->vertices();
PF::PathFinder::init_arrays (d, w, prev, m_open, m_closed, v0, n); m_heap->insert (v0, 0.0);
size_t n_reached = 0; const size_t n_targets = to_index.size (); bool *is_target = new bool [n]; std::fill (is_target, is_target + n, false); for (auto t: to_index) is_target [t] = true;
while (m_heap->nItems() > 0) { size_t v = m_heap->deleteMin();
m_closed [v] = true; m_open [v] = false;
edge = vertices [v].outHead; scan_edges (edge, d, w, prev, m_open, m_closed, v);
if (is_target [v]) n_reached++; if (n_reached == n_targets) break; } // end while nItems > 0 delete [] is_target;}
// Modified Dijkstra that stops as soon as first target is reached. Used to find// minimal distance to nearest one of set of possible targets.void PF::PathFinder::DijkstraNearest ( std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, const size_t v0, const std::vector <size_t> &to_index){ const DGraphEdge *edge;
const size_t n = m_graph->nVertices(); const std::vector<DGraphVertex>& vertices = m_graph->vertices();
PF::PathFinder::init_arrays (d, w, prev, m_open, m_closed, v0, n); m_heap->insert (v0, 0.0);
bool *is_target = new bool [n]; std::fill (is_target, is_target + n, false); for (auto t: to_index) is_target [t] = true;
while (m_heap->nItems() > 0) { size_t v = m_heap->deleteMin();
m_closed [v] = true; m_open [v] = false;
edge = vertices [v].outHead; scan_edges (edge, d, w, prev, m_open, m_closed, v);
if (is_target [v]) break; } // end while nItems > 0 delete [] is_target;}
// Modified pathfinder only out to specified distance limit. Done as separate// routine to avoid costs of the `if` clause in general non-limited case.void PF::PathFinder::DijkstraLimit ( std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, const size_t v0, const double &dlim){ const DGraphEdge *edge;
const size_t n = m_graph->nVertices(); const std::vector<DGraphVertex>& vertices = m_graph->vertices();
PF::PathFinder::init_arrays (d, w, prev, m_open, m_closed, v0, n); m_heap->insert (v0, 0.0);
while (m_heap->nItems() > 0) { size_t v = m_heap->deleteMin();
m_closed [v] = true; m_open [v] = false;
// explore the OUT set of v only if distances are < threshold bool explore = false; edge = vertices [v].outHead; while (edge) { if ((d [v] + edge->dist) <= dlim) { explore = true; break; } edge = edge->nextOut; }
if (explore) { edge = vertices [v].outHead; scan_edges (edge, d, w, prev, m_open, m_closed, v); } } // end while nItems > 0}
void PF::PathFinder::AStar (std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, const size_t v0, const std::vector <size_t> &to_index, const double target_x, const double target_y, const std::vector<double> &vx, const std::vector<double> &vy, const bool is_spatial){ const DGraphEdge *edge;
const size_t n = m_graph->nVertices(); const std::vector<DGraphVertex>& vertices = m_graph->vertices();
PF::PathFinder::init_arrays (d, w, prev, m_open, m_closed, v0, n);
double h0 = 0.0; if (is_spatial) { const double dx = vx [v0] - target_x; const double dy = vy [v0] - target_y; h0 = sqrt (dx * dx + dy * dy); } m_heap->insert (v0, h0);
size_t n_reached = 0; const size_t n_targets = to_index.size (); bool *is_target = new bool [n]; std::fill (is_target, is_target + n, false); for (auto t: to_index) is_target [t] = true;
while (m_heap->nItems() > 0) { size_t v = m_heap->deleteMin();
m_closed [v] = true; m_open [v] = false;
edge = vertices [v].outHead; scan_edges_heur (edge, d, w, prev, m_open, m_closed, v, target_x, target_y, vx, vy, is_spatial);
if (is_target [v]) n_reached++; if (n_reached == n_targets) break; } // end while m_heap->nItems delete [] is_target;}
// Dijkstra with C++ std::set, modified from the above to use EdgeSet edge_set// instead of m_heap, and so all routines hard-coded here.void PF::PathFinder::Dijkstra_set (std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, size_t v0){ const DGraphEdge *edge;
const size_t n = m_graph->nVertices(); const std::vector<DGraphVertex>& vertices = m_graph->vertices();
PF::PathFinder::init_arrays (d, w, prev, m_open, m_closed, v0, n); m_heap->insert(v0, 0.0);
edge_set.insert (DijkstraEdge (0.0, v0));
while (edge_set.size () > 0) { EdgeSet::iterator ei = edge_set.begin(); size_t v = ei->geti(); edge_set.erase (ei);
m_closed [v] = true; m_open [v] = false;
edge = vertices [v].outHead; while (edge) { size_t et = edge->target;
if (!m_closed [et]) { double wt = w [v] + edge->wt; if (wt < w [et]) { d [et] = d [v] + edge->dist; double wold = w [et]; w [et] = wt; prev [et] = static_cast <int> (v); if (m_open [et]) { DijkstraEdge de (wold, et); if (edge_set.find (de) != edge_set.end ()) edge_set.erase (edge_set.find (de)); } else m_open [et] = true; edge_set.insert (DijkstraEdge (w [et], et)); } }
edge = edge->nextOut; } // end while edge } // end while edge_set.size}void PF::PathFinder::scan_edges_heur_rev (const DGraphEdge *edge, std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, bool *m_open_vec, const bool *m_closed_vec, const size_t &v0, const double target_x, const double target_y, const std::vector<double> &vx, const std::vector<double> &vy, const double h_max, const bool is_spatial){ while (edge) { size_t et = edge->source; if (!m_closed_vec [et]) { double wt = w [v0] + edge->wt; if (wt < w [et]) { d [et] = d [v0] + edge->dist; w [et] = wt; prev [et] = static_cast <int> (v0);
double heur_et = 0.0; if (is_spatial) { const double dx = vx [et] - target_x; const double dy = vy [et] - target_y; heur_et = h_max - sqrt (dx * dx + dy * dy); } const double priority = wt + heur_et;
if (m_open_vec [et]) { m_heap_rev->decreaseKey(et, priority); } else { m_heap_rev->insert (et, priority); m_open_vec [et] = true; } } } edge = edge->nextIn; }}
void PF::PathFinder::AStar2 (std::vector<double>& d, std::vector<double>& w, std::vector<long int>& prev, const size_t v0, const size_t v1, std::vector<double>& d_rev, std::vector<double>& w_rev, std::vector<long int>& prev_rev, const double target_x, const double target_y, const std::vector<double> &vx, const std::vector<double> &vy, const bool is_spatial){ const size_t n = m_graph->nVertices(); const std::vector<DGraphVertex>& vertices = m_graph->vertices();
if (v0 == v1) { d[v0] = 0.0; w[v0] = 0.0; return; }
PF::PathFinder::init_arrays (d, w, prev, m_open, m_closed, v0, n); PF::PathFinder::init_arrays (d_rev, w_rev, prev_rev, m_open2, m_closed2, v1, n);
double h0 = 0.0; double h1 = 0.0; if (is_spatial) { const double dx0 = vx [v0] - target_x; const double dy0 = vy [v0] - target_y; h0 = sqrt (dx0 * dx0 + dy0 * dy0);
const double dx1 = vx [v1] - target_x; const double dy1 = vy [v1] - target_y; h1 = sqrt (dx1 * dx1 + dy1 * dy1); }
const double h_max = h0;
m_heap->insert (v0, h0); m_heap_rev->insert (v1, h_max - h1);
double meeting_distance = INFINITE_DOUBLE;
while (m_heap->nItems() > 0 || m_heap_rev->nItems() > 0) { double min_f = (m_heap->nItems() > 0 ? m_heap->getmin() : INFINITE_DOUBLE); double min_r = (m_heap_rev->nItems() > 0 ? m_heap_rev->getmin() : INFINITE_DOUBLE);
if (min_f + min_r >= meeting_distance + h_max) { break; }
if (m_heap->nItems() > 0 && (m_heap_rev->nItems() == 0 || min_f <= min_r)) { size_t v = m_heap->deleteMin(); m_closed [v] = true; m_open [v] = false;
if (w_rev[v] < INFINITE_DOUBLE && w[v] + w_rev[v] < meeting_distance) { meeting_distance = w[v] + w_rev[v]; }
const DGraphEdge *edge = vertices [v].outHead; scan_edges_heur (edge, d, w, prev, m_open, m_closed, v, target_x, target_y, vx, vy, is_spatial); } else { size_t v = m_heap_rev->deleteMin(); m_closed2 [v] = true; m_open2 [v] = false;
if (w[v] < INFINITE_DOUBLE && w[v] + w_rev[v] < meeting_distance) { meeting_distance = w[v] + w_rev[v]; }
const DGraphEdge *edge = vertices [v].inHead; scan_edges_heur_rev (edge, d_rev, w_rev, prev_rev, m_open2, m_closed2, v, target_x, target_y, vx, vy, h_max, is_spatial); } }
double best_w = INFINITE_DOUBLE; size_t best_meet = static_cast<size_t>(-1); for (size_t i = 0; i < n; i++) { if (w[i] < INFINITE_DOUBLE && w_rev[i] < INFINITE_DOUBLE) { if (w[i] + w_rev[i] < best_w) { best_w = w[i] + w_rev[i]; best_meet = i; } } }
if (best_meet != static_cast<size_t>(-1)) { w[v1] = best_w; d[v1] = d[best_meet] + d_rev[best_meet];
size_t curr = best_meet; while (curr != v1 && curr != static_cast<size_t>(-1)) { long int p_long = prev_rev[curr]; if (p_long == -1 || p_long == INFINITE_INT) break; size_t p = static_cast<size_t>(p_long);
prev[p] = static_cast<long int>(curr); curr = p; } }}