mtp.cc

   1
   2 ///////////////////////////////////////////////////////////////////////////
   3 // This program is free software: you can redistribute it and/or modify  //
   4 // it under the terms of the version 3 of the GNU General Public License //
   5 // as published by the Free Software Foundation.                         //
   6 //                                                                       //
   7 // This program is distributed in the hope that it will be useful, but   //
   8 // WITHOUT ANY WARRANTY; without even the implied warranty of            //
   9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      //
  10 // General Public License for more details.                              //
  11 //                                                                       //
  12 // You should have received a copy of the GNU General Public License     //
  13 // along with this program. If not, see <http://www.gnu.org/licenses/>.  //
  14 //                                                                       //
  15 // Written by and Copyright (C) Francois Fleuret                         //
  16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
  17 ///////////////////////////////////////////////////////////////////////////
  18
  19 // Multi-Tracked Path
  20
  21 // #define VERBOSE
  22
  23 #include <iostream>
  24 #include <fstream>
  25 #include <cmath>
  26 #include <stdio.h>
  27 #include <stdlib.h>
  28 #include <float.h>
  29
  30 using namespace std;
  31
  32 typedef float scalar_t;
  33
  34 #ifdef DEBUG
  35 #define ASSERT(x) if(!(x)) { \
  36   std::cerr << "ASSERT FAILED IN " << __FILE__ << ":" << __LINE__ << endl; \
  37   abort(); \
  38 }
  39 #else
  40 #define ASSERT(x)
  41 #endif
  42
  43 class Vertex;
  44
  45 class Edge {
  46 public:
  47   int occupied;
  48   scalar_t length, work_length;
  49   Vertex *terminal_vertex;
  50   Edge *next, *pred;
  51 };
  52
  53 class Vertex {
  54 public:
  55   int id;
  56
  57   Edge *first_edge;
  58   scalar_t distance_from_source;
  59
  60   Vertex *pred_vertex;
  61   Edge *pred_edge;
  62
  63   Vertex() { first_edge = 0; }
  64
  65   inline void add_edge(Edge *e) {
  66     e->next = first_edge;
  67     e->pred = 0;
  68     if(first_edge) { first_edge->pred = e; }
  69     first_edge = e;
  70   }
  71
  72   inline void del_edge(Edge *e) {
  73     if(e == first_edge) { first_edge = e->next; }
  74     if(e->pred) { e->pred->next = e->next; }
  75     if(e->next) { e->next->pred = e->pred; }
  76   }
  77 };
  78
  79 class Graph {
  80   void initialize_work_lengths();
  81   void update_work_length();
  82   void find_shortest_path(Vertex **front, Vertex **new_front);
  83
  84   int nb_vertices;
  85   Edge *edge_heap;
  86   Vertex *vertices;
  87   Vertex *source, *sink;
  88
  89 public:
  90   Graph(int nb_vertices, int nb_edges, int *from, int *to, scalar_t *lengths,
  91         int source, int sink);
  92
  93   ~Graph();
  94
  95   void find_best_paths();
  96   void print();
  97   void print_occupied_edges();
  98   void dot_print();
  99 };
 100
 101 void Graph::print() {
 102   for(int n = 0; n < nb_vertices; n++) {
 103     for(Edge *e = vertices[n].first_edge; e; e = e->next) {
 104       cout << n << " -> " << e->terminal_vertex->id << " " << e->length << endl;
 105     }
 106   }
 107 }
 108
 109 void Graph::print_occupied_edges() {
 110   for(int n = 0; n < nb_vertices; n++) {
 111     for(Edge *e = vertices[n].first_edge; e; e = e->next) {
 112       if(e->occupied) {
 113         int a = n, b = e->terminal_vertex->id;
 114         if(a > b) { int c = a; a = b; b = c; }
 115         cout << a << " " << b << endl;
 116       }
 117     }
 118   }
 119 }
 120
 121 void Graph::dot_print() {
 122   cout << "digraph {" << endl;
 123   cout << "  node[shape=circle];" << endl;
 124   for(int n = 0; n < nb_vertices; n++) {
 125     for(Edge *e = vertices[n].first_edge; e; e = e->next) {
 126       int a = n, b = e->terminal_vertex->id;
 127       if(e->occupied) {
 128         int c = a; a = b; b = c;
 129         cout << "  " << a << " -> " << b << " [style=bold,color=black,label=\"" << -e->length << "\"];" << endl;
 130       } else {
 131         cout << "  " << a << " -> " << b << " [color=gray,label=\"" << e->length << "\"];" << endl;
 132       }
 133     }
 134   }
 135   cout << "}" << endl;
 136 }
 137
 138 Graph::Graph(int nb_vrt, int nb_edges,
 139              int *from, int *to, scalar_t *lengths,
 140              int src, int snk) {
 141   nb_vertices = nb_vrt;
 142
 143   edge_heap = new Edge[nb_edges];
 144   vertices = new Vertex[nb_vertices];
 145
 146   source = &vertices[src];
 147   sink = &vertices[snk];
 148
 149   for(int v = 0; v < nb_vertices; v++) {
 150     vertices[v].id = v;
 151   }
 152
 153   for(int e = 0; e < nb_edges; e++) {
 154     vertices[from[e]].add_edge(&edge_heap[e]);
 155     edge_heap[e].occupied = 0;
 156     edge_heap[e].length = lengths[e];
 157     edge_heap[e].terminal_vertex = &vertices[to[e]];
 158   }
 159 }
 160
 161 Graph::~Graph() {
 162   delete[] vertices;
 163   delete[] edge_heap;
 164 }
 165
 166 void Graph::initialize_work_lengths() {
 167   scalar_t length_min = 0;
 168   for(int n = 0; n < nb_vertices; n++) {
 169     for(Edge *e = vertices[n].first_edge; e; e = e->next) {
 170       length_min = min(e->length, length_min);
 171     }
 172   }
 173   for(int n = 0; n < nb_vertices; n++) {
 174     for(Edge *e = vertices[n].first_edge; e; e = e->next) {
 175       e->work_length = e->length - length_min;
 176     }
 177   }
 178 }
 179
 180 void Graph::update_work_length() {
 181   for(int n = 0; n < nb_vertices; n++) {
 182     scalar_t d = vertices[n].distance_from_source;
 183     for(Edge *e = vertices[n].first_edge; e; e = e->next) {
 184       e->work_length += d - e->terminal_vertex->distance_from_source;
 185     }
 186   }
 187 }
 188
 189 void Graph::find_shortest_path(Vertex **front, Vertex **new_front) {
 190   Vertex **tmp_front;
 191   int tmp_front_size;
 192   Vertex *v, *tv;
 193   scalar_t d;
 194
 195 #ifdef DEBUG
 196   for(int n = 0; n < nb_vertices; n++) {
 197     for(Edge *e = vertices[n].first_edge; e; e = e->next) {
 198       if(e->work_length < 0) {
 199         cerr << "DEBUG error in find_shortest_path: Edge fixed lengths have to be positive."
 200              << endl;
 201         abort();
 202       }
 203     }
 204   }
 205 #endif
 206
 207   for(int v = 0; v < nb_vertices; v++) {
 208     vertices[v].distance_from_source = FLT_MAX;
 209     vertices[v].pred_vertex = 0;
 210     vertices[v].pred_edge = 0;
 211   }
 212
 213   int front_size = 0, new_front_size;
 214   front[front_size++] = source;
 215   source->distance_from_source = 0;
 216
 217   do {
 218     new_front_size = 0;
 219     for(int f = 0; f < front_size; f++) {
 220       v = front[f];
 221       for(Edge *e = v->first_edge; e; e = e->next) {
 222         d = v->distance_from_source + e->work_length;
 223         tv = e->terminal_vertex;
 224         if(d < tv->distance_from_source) {
 225           tv->distance_from_source = d;
 226           tv->pred_vertex = v;
 227           tv->pred_edge = e;
 228           new_front[new_front_size++] = tv;
 229         }
 230       }
 231     }
 232
 233     tmp_front = new_front;
 234     new_front = front;
 235     front = tmp_front;
 236
 237     tmp_front_size = new_front_size;
 238     new_front_size = front_size;
 239     front_size = tmp_front_size;
 240   } while(front_size > 0);
 241 }
 242
 243 void Graph::find_best_paths() {
 244   Vertex **front = new Vertex *[nb_vertices];
 245   Vertex **new_front = new Vertex *[nb_vertices];
 246
 247   scalar_t total_length;
 248
 249   initialize_work_lengths();
 250
 251   do {
 252     total_length = 0.0;
 253     find_shortest_path(front, new_front);
 254     update_work_length();
 255
 256     // Do we reach the sink?
 257     if(sink->pred_edge) {
 258
 259       // If yes, compute the length of the best path
 260       for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
 261         total_length += v->pred_edge->length;
 262       }
 263
 264       // If that length is negative
 265       if(total_length < 0.0) {
 266         // Invert all the edges along the best path
 267         for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
 268           Edge *e = v->pred_edge;
 269           e->terminal_vertex = v->pred_vertex;
 270           e->occupied = 1 - e->occupied;
 271           e->length = - e->length;
 272           e->work_length = - e->work_length;
 273           v->pred_vertex->del_edge(e);
 274           v->add_edge(e);
 275         }
 276       }
 277     }
 278   } while(total_length < 0.0);
 279
 280   // // We put all occupied edges back to their original orientations
 281   // for(int n = 0; n < nb_vertices; n++) {
 282     // Vertex *v = &vertices[n];
 283     // for(Edge *e = v->first_edge; e; e = e->next) {
 284       // if(e->occupied) {
 285         // e->terminal_vertex = v->pred_vertex;
 286         // e->length = - e->length;
 287         // e->work_length = 0;
 288         // v->pred_vertex->del_edge(e);
 289         // v->add_edge(e);
 290       // }
 291     // }
 292   // }
 293
 294
 295   delete[] front;
 296   delete[] new_front;
 297 }
 298
 299 //////////////////////////////////////////////////////////////////////
 300
 301 int main(int argc, char **argv) {
 302
 303   if(argc < 2) {
 304     cerr << argv[0] << " <graph file>" << endl;
 305     exit(EXIT_FAILURE);
 306   }
 307
 308   ifstream *file = new ifstream(argv[1]);
 309
 310   int nb_edges, nb_vertices;
 311   int source, sink;
 312
 313   if(file->good()) {
 314
 315     (*file) >> nb_vertices >> nb_edges;
 316     (*file) >> source >> sink;
 317
 318     // cout << "INPUT nb_edges " << nb_edges << endl;
 319     // cout << "INPUT nb_vertices " << nb_vertices << endl;
 320     // cout << "INPUT source " << source << endl;
 321     // cout << "INPUT sink " << sink << endl;
 322
 323     scalar_t *el = new scalar_t[nb_edges];
 324     int *ea = new int[nb_edges];
 325     int *eb = new int[nb_edges];
 326
 327     for(int e = 0; e < nb_edges; e++) {
 328       (*file) >> ea[e] >> eb[e] >> el[e];
 329     }
 330
 331     // for(int e = 0; e < nb_edges; e++) {
 332       // cout << "INPUT_EDGE " << ea[e] << " " << eb[e] << " " << el[e] << endl;
 333     // }
 334
 335     Graph graph(nb_vertices, nb_edges, ea, eb, el, source, sink);
 336
 337     graph.find_best_paths();
 338     // graph.print_occupied_edges();
 339     graph.dot_print();
 340
 341     delete[] el;
 342     delete[] ea;
 343     delete[] eb;
 344
 345   } else {
 346
 347     cerr << "Can not open " << argv[1] << endl;
 348
 349     delete file;
 350     exit(EXIT_FAILURE);
 351
 352   }
 353
 354   delete file;
 355   exit(EXIT_SUCCESS);
 356 }