ksp.cc

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