Debugged a stupid bug.

[mtp.git] / mtp_graph.cc

diff --git a/mtp_graph.cc b/mtp_graph.cc
index c213a07..cc816c9 100644 (file)
--- a/mtp_graph.cc
+++ b/mtp_graph.cc
@@ -155,6 +155,9 @@ void MTPGraph::print_dot(ostream *os) {
   (*os) << "  node[shape=circle];" << endl;
   for(int k = 0; k < _nb_edges; k++) {
     Edge *e = _edges + k;
+    // (*os) << "  " << e->origin_vertex->id << " -> " << e->terminal_vertex->id
+          // << ";"
+          // << endl;
     if(e->occupied) {
       (*os) << "  " << e->origin_vertex->id << " -> " << e->terminal_vertex->id
            << " [style=bold,color=black,label=\"" << e->length << "\"];" << endl;
@@ -185,26 +188,29 @@ void MTPGraph::initialize_positivized_lengths_with_min() {
 void MTPGraph::update_positivized_lengths() {
   for(int k = 0; k < _nb_edges; k++) {
     Edge *e = _edges + k;
-    e->positivized_length += e->terminal_vertex->distance_from_source - e->terminal_vertex->distance_from_source;
+    e->positivized_length +=
+      e->origin_vertex->distance_from_source - e->terminal_vertex->distance_from_source;
   }
 }
 
 void MTPGraph::force_positivized_lengths() {
 #ifdef VERBOSE
   scalar_t residual_error = 0.0;
+  scalar_t max_error = 0.0;
 #endif
   for(int n = 0; n < _nb_vertices; n++) {
     for(Edge *e = _vertices[n].leaving_edges; e; e = e->next_leaving_edge) {
       if(e->positivized_length < 0) {
 #ifdef VERBOSE
         residual_error -= e->positivized_length;
+        max_error = max(max_error, fabs(e->positivized_length));
 #endif
         e->positivized_length = 0.0;
       }
     }
   }
 #ifdef VERBOSE
-  cerr << "residual_error " << residual_error << endl;
+  cerr << "residual_error " << residual_error << " max_error " << residual_error << endl;
 #endif
 }
 
@@ -232,6 +238,32 @@ void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
   do {
     _new_front_size = 0;
     iteration++;
+
+    // for(int k = 0; k < _nb_edges; k++) {
+      // Edge *e = _edges + k;
+      // d = e->origin_vertex->distance_from_source + e->positivized_length;
+      // if(d < e->terminal_vertex->distance_from_source) {
+        // e->terminal_vertex->distance_from_source = d;
+        // _new_front_size++;
+      // }
+    // }
+
+    // for(int n = 0; n < _nb_vertices; n++) {
+      // v = &_vertices[n];
+      // for(e = v->leaving_edges; e; e = e->next_leaving_edge) {
+        // d = v->distance_from_source + e->positivized_length;
+        // tv = e->terminal_vertex;
+        // if(d < tv->distance_from_source) {
+          // tv->distance_from_source = d;
+          // tv->best_pred_edge_to_source = e;
+          // if(tv->iteration < iteration) {
+            // _new_front[_new_front_size++] = tv;
+            // tv->iteration = iteration;
+          // }
+        // }
+      // }
+    // }
+
     for(int f = 0; f < _front_size; f++) {
       v = _front[f];
       for(e = v->leaving_edges; e; e = e->next_leaving_edge) {
@@ -256,6 +288,19 @@ void MTPGraph::find_shortest_path(Vertex **_front, Vertex **_new_front) {
     _new_front_size = _front_size;
     _front_size = tmp_front_size;
   } while(_front_size > 0);
+
+#ifdef DEBUG
+  scalar_t min_delta = 0, delta;
+  for(int k = 0; k < _nb_edges; k++) {
+    Edge *e = _edges + k;
+    // d = e->origin_vertex->distance_from_source + e->positivized_length;
+    // if(d > e->terminal_vertex->distance_from_source) { abort(); }
+    delta = e->positivized_length +
+               (e->origin_vertex->distance_from_source - e->terminal_vertex->distance_from_source);
+    min_delta = min(delta, min_delta);
+  }
+  cout << "min_delta = " << delta << endl;
+#endif
 }
 
 void MTPGraph::find_best_paths(scalar_t *lengths) {
@@ -269,6 +314,9 @@ void MTPGraph::find_best_paths(scalar_t *lengths) {
     _edges[e].positivized_length = _edges[e].length;
   }
 
+  cout << "********************************************************" << endl;
+  // print_dot(&cout);
+
   // We use one iteration of find_shortest_path simply to propagate
   // the distance to make all the edge lengths positive.
   find_shortest_path(_front, _new_front);
@@ -319,7 +367,7 @@ void MTPGraph::find_best_paths(scalar_t *lengths) {
 }
 
 int MTPGraph::retrieve_one_path(Edge *e, int *nodes) {
-  Edge *f, *next;
+  Edge *f, *next = 0;
   int l = 0;
 
   if(nodes) { nodes[l++] = e->origin_vertex->id; }