X-Git-Url: https://fleuret.org/cgi-bin/gitweb/gitweb.cgi?p=mtp.git;a=blobdiff_plain;f=mtp_graph.cc;h=2cbb33848a3589007528a6e3b1098357dbf3d3df;hp=12017c20a1b9039e1e62ebd5a96ed9ee0a90ac92;hb=HEAD;hpb=2b3a2e10ec226f1610b9c39abd20f0899a34a652 diff --git a/mtp_graph.cc b/mtp_graph.cc index 12017c2..2cbb338 100644 --- a/mtp_graph.cc +++ b/mtp_graph.cc @@ -24,6 +24,7 @@ #include "mtp_graph.h" +#include #include using namespace std; @@ -43,19 +44,18 @@ public: class Vertex { public: - Edge *leaving_edges; scalar_t distance_from_source; Edge *pred_edge_toward_source; - int last_change; // Used to mark which edges have already been - // processed in some methods - - Vertex **heap_position; + Edge *leaving_edge_list_root; + Vertex **heap_slot; Vertex(); inline void add_leaving_edge(Edge *e); inline void del_leaving_edge(Edge *e); + inline void decrease_distance_in_heap(Vertex **heap); + inline void increase_distance_in_heap(Vertex **heap, Vertex **heap_bottom); }; ////////////////////////////////////////////////////////////////////// @@ -65,27 +65,27 @@ void Edge::invert() { positivized_length = - positivized_length; origin_vertex->del_leaving_edge(this); terminal_vertex->add_leaving_edge(this); - Vertex *t = terminal_vertex; - terminal_vertex = origin_vertex; - origin_vertex = t; + swap(terminal_vertex, origin_vertex); } ////////////////////////////////////////////////////////////////////// Vertex::Vertex() { - leaving_edges = 0; + leaving_edge_list_root = 0; } void Vertex::add_leaving_edge(Edge *e) { - e->next_leaving_edge = leaving_edges; + e->next_leaving_edge = leaving_edge_list_root; e->pred_leaving_edge = 0; - if(leaving_edges) { leaving_edges->pred_leaving_edge = e; } - leaving_edges = e; + if(leaving_edge_list_root) { + leaving_edge_list_root->pred_leaving_edge = e; + } + leaving_edge_list_root = e; } void Vertex::del_leaving_edge(Edge *e) { - if(e == leaving_edges) { - leaving_edges = e->next_leaving_edge; + if(e == leaving_edge_list_root) { + leaving_edge_list_root = e->next_leaving_edge; } if(e->pred_leaving_edge) { e->pred_leaving_edge->next_leaving_edge = e->next_leaving_edge; @@ -95,12 +95,48 @@ void Vertex::del_leaving_edge(Edge *e) { } } -////////////////////////////////////////////////////////////////////// +void Vertex::decrease_distance_in_heap(Vertex **heap) { + Vertex **p, **h; + h = heap_slot; + while(1) { + if(h <= heap) break; + p = heap + ((h - heap + 1) >> 1) - 1; + if((*p)->distance_from_source <= distance_from_source) break; + swap((*p)->heap_slot, heap_slot); + swap(*p, *h); + h = p; + } +} -static int compare_vertex(const void *v1, const void *v2) { - return (*((Vertex **) v1))->last_change - (*((Vertex **) v2))->last_change; +void Vertex::increase_distance_in_heap(Vertex **heap, Vertex **heap_bottom) { + Vertex **c1, **c2, **h; + h = heap_slot; + while(1) { + c1 = heap + 2 * (h - heap) + 1; + if(c1 >= heap_bottom) break; + c2 = c1 + 1; + if((*c1)->distance_from_source < distance_from_source) { + if(c2 < heap_bottom && (*c2)->distance_from_source < (*c1)->distance_from_source) { + swap((*c2)->heap_slot, heap_slot); + swap(*c2, *h); + h = c2; + } else { + swap((*c1)->heap_slot, heap_slot); + swap(*c1, *h); + h = c1; + } + } else { + if(c2 < heap_bottom && (*c2)->distance_from_source < distance_from_source) { + swap((*c2)->heap_slot, heap_slot); + swap(*c2, *h); + h = c2; + } else break; + } + } } +////////////////////////////////////////////////////////////////////// + MTPGraph::MTPGraph(int nb_vertices, int nb_edges, int *vertex_from, int *vertex_to, int source, int sink) { @@ -116,30 +152,21 @@ MTPGraph::MTPGraph(int nb_vertices, int nb_edges, _sink = &_vertices[sink]; for(int e = 0; e < nb_edges; e++) { - _vertices[vertex_from[e]].add_leaving_edge(_edges + e); + _vertices[vertex_from[e]].add_leaving_edge(&_edges[e]); _edges[e].occupied = 0; - _edges[e].origin_vertex = _vertices + vertex_from[e]; - _edges[e].terminal_vertex = _vertices + vertex_to[e]; + _edges[e].origin_vertex = &_vertices[vertex_from[e]]; + _edges[e].terminal_vertex = &_vertices[vertex_to[e]]; } for(int v = 0; v < _nb_vertices; v++) { _heap[v] = &_vertices[v]; - _vertices[v].heap_position = &_heap[v]; + _vertices[v].heap_slot = &_heap[v]; } paths = 0; nb_paths = 0; - if(is_dag()) { - // Here the last_change field of every vertex tells us how many - // iterations of DP we need to reach it. Hence we only have to - // process the vertex in that order. - for(int v = 0; v < _nb_vertices; v++) { _dp_order[v] = &_vertices[v]; } - qsort(_dp_order, _nb_vertices, sizeof(Vertex *), compare_vertex); - } else { - cerr << __FILE__ << ": This graph is not a DAG." << endl; - abort(); - } + compute_dp_ordering(); } MTPGraph::~MTPGraph() { @@ -155,15 +182,12 @@ MTPGraph::~MTPGraph() { void MTPGraph::print(ostream *os) { for(int k = 0; k < _nb_edges; k++) { - Edge *e = _edges + k; + Edge *e = &_edges[k]; (*os) << e->origin_vertex - _vertices - << " -> " - << e->terminal_vertex - _vertices - << " " - << e->length; - if(e->occupied) { - (*os) << " *"; - } + << " -> " + << e->terminal_vertex - _vertices + << " (" << e->length << ")"; + if(e->occupied) { (*os) << " *"; } (*os) << endl; } } @@ -176,7 +200,7 @@ void MTPGraph::print_dot(ostream *os) { (*os) << " " << _source - _vertices << " [peripheries=2];" << endl; (*os) << " " << _sink - _vertices << " [peripheries=2];" << endl; for(int k = 0; k < _nb_edges; k++) { - Edge *e = _edges + k; + Edge *e = &_edges[k]; (*os) << " " << e->origin_vertex - _vertices << " -> " @@ -194,7 +218,7 @@ void MTPGraph::print_dot(ostream *os) { void MTPGraph::update_positivized_lengths() { for(int k = 0; k < _nb_edges; k++) { - Edge *e = _edges + k; + Edge *e = &_edges[k]; e->positivized_length += e->origin_vertex->distance_from_source - e->terminal_vertex->distance_from_source; } @@ -206,170 +230,22 @@ void MTPGraph::force_positivized_lengths() { scalar_t max_error = 0.0; #endif for(int k = 0; k < _nb_edges; k++) { - Edge *e = _edges + k; + Edge *e = &_edges[k]; if(e->positivized_length < 0) { - #ifdef VERBOSE - if((e->origin_vertex->last_change < 0 && e->terminal_vertex->last_change >= 0) || - (e->origin_vertex->last_change >= 0 && e->terminal_vertex->last_change < 0)) { - cout << "Inconsistent non-connexity (this should never happen)." << endl; - abort(); - } - if(e->origin_vertex->last_change >= 0 && - e->terminal_vertex->last_change >= 0 && - e->positivized_length < 0) { - residual_error -= e->positivized_length; - max_error = max(max_error, - e->positivized_length); - } + residual_error -= e->positivized_length; + max_error = max(max_error, - e->positivized_length); #endif e->positivized_length = 0.0; } } #ifdef VERBOSE - cerr << "residual_error " << residual_error << " max_error " << residual_error << endl; + cerr << __FILE__ << ": residual_error " << residual_error << " max_error " << max_error << endl; #endif } -int MTPGraph::is_dag() { - Vertex *v; - Edge *e; - - Vertex **active = new Vertex *[_nb_vertices]; - - // We put everybody in the active - for(int k = 0; k < _nb_vertices; k++) { - _vertices[k].last_change = 0; - active[k] = &_vertices[k]; - } - - int iteration = 1; - int nb_active = _nb_vertices, pred_nb_active; - - do { - // We set the last_change field of all the vertices with incoming - // edges to the current iteration value - for(int f = 0; f < nb_active; f++) { - v = active[f]; - for(e = v->leaving_edges; e; e = e->next_leaving_edge) { - e->terminal_vertex->last_change = iteration; - } - } - - pred_nb_active = nb_active; - nb_active = 0; - - // We keep all the vertices with incoming nodes - for(int f = 0; f < pred_nb_active; f++) { - v = active[f]; - if(v->last_change == iteration) { - active[nb_active++] = v; - } - } - - iteration++; - } while(nb_active < pred_nb_active); - - delete[] active; - - return nb_active == 0; -} - -#ifdef DEBUG -void print_heap(int first, int heap_size, Vertex **heap, int depth, Vertex *vertices) { - // if(depth == 0) cout << "** START_HEAP ********************************************************" << endl; - // if(first < heap_size) { - // print_heap((first + 1) * 2 - 1, heap_size, heap, depth + 1, vertices); - // for(int d = 0; d < depth; d++) cout << " "; - // cout << "[" << heap[first] - vertices << "] "; - // if(heap[first]->distance_from_source == FLT_MAX) cout << "inf"; - // else cout << heap[first]->distance_from_source; - // cout << endl; - // print_heap((first + 1) * 2, heap_size, heap, depth + 1, vertices); - // } - // if(depth == 0) cout << "** END_HEAP **********************************************************" << endl; -} - -void check_heap(Vertex **heap, int heap_size, Vertex *vertices, int nb_vertices) { - Vertex **p, **h; - int *used = new int[nb_vertices]; - for(int v = 0; v < nb_vertices; v++) used[v] = 0; - for(int k = 0; k < heap_size; k++) { - used[heap[k] - vertices]++; - h = heap + k; - if(h != (*h)->heap_position) abort(); - if(k > 0) { - p = heap + (h - heap + 1) / 2 - 1; - if((*h)->distance_from_source < (*p)->distance_from_source) abort(); - } - } - for(int v = 0; v < nb_vertices; v++) { - cout << used[v]; - if(used[v] > 1) abort(); - } - cout << endl; - delete[] used; -} -#endif - -void MTPGraph::decrease_distance_in_heap(Vertex *v) { -#ifdef DEBUG - // cout << "START decrease_distance_in_heap" << endl; - // print_heap(0, _heap_size, _heap, 0, _vertices); -#endif - Vertex **p, **h; - // There is some beauty in that - h = v->heap_position; - while(h > _heap && - (p = _heap + (h - _heap + 1) / 2 - 1, - (*p)->distance_from_source > (*h)->distance_from_source)) { -// #warning REMOVE - // cout << "SWAP [" << (*p) - _vertices << " | " << (*h) - _vertices << "]" << endl; - // if((*p) - _vertices == 6 && (*h) - _vertices == 96) abort(); - swap(*p, *h); - swap((*p)->heap_position, (*h)->heap_position); - h = p; - } -#ifdef DEBUG - // check_heap(_heap, _heap_size, _vertices, _nb_vertices); - // print_heap(0, _heap_size, _heap, 0, _vertices); -#endif -} - -void MTPGraph::increase_distance_in_heap(Vertex *v) { -#ifdef DEBUG - // cout << "START increase_distance_in_heap" << endl; - // print_heap(0, _heap_size, _heap, 0, _vertices); -#endif - Vertex **c1, **c2, **h; - // There is some beauty in that - h = v->heap_position; - while(c1 = _heap + 2 * (h - _heap + 1) - 1, c2 = c1 + 1, - (c1 < _heap + _heap_size && - (*c1)->distance_from_source < (*h)->distance_from_source) - || - (c2 < _heap + _heap_size && - (*c2)->distance_from_source < (*h)->distance_from_source) - ) { - if(c1 < _heap + _heap_size && - !(c2 < _heap + _heap_size && - (*c2)->distance_from_source < (*c1)->distance_from_source)){ - swap(*c1, *h); - swap((*c1)->heap_position, (*h)->heap_position); - h = c1; - } else { - swap(*c2, *h); - swap((*c2)->heap_position, (*h)->heap_position); - h = c2; - } - } -#ifdef DEBUG - // check_heap(_heap, _heap_size, _vertices, _nb_vertices); - // print_heap(0, _heap_size, _heap, 0, _vertices); -#endif -} - -void MTPGraph::dp_distance_propagation() { +void MTPGraph::dp_compute_distances() { Vertex *v, *tv; Edge *e; scalar_t d; @@ -383,99 +259,64 @@ void MTPGraph::dp_distance_propagation() { for(int k = 0; k < _nb_vertices; k++) { v = _dp_order[k]; - for(e = v->leaving_edges; e; e = e->next_leaving_edge) { + for(e = v->leaving_edge_list_root; 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->pred_edge_toward_source = e; - decrease_distance_in_heap(tv); } } } - -#ifdef DEBUG - for(int k = 0; k < _nb_vertices; k++) { - if(_vertices[k].distance_from_source == FLT_MAX) abort(); - } -#endif } -// This method does not change the edge occupation. It only set +// This method does not change the edge occupation. It only sets // properly, for every vertex, the fields distance_from_source and // pred_edge_toward_source. void MTPGraph::find_shortest_path() { - Vertex *v, *tv; + int heap_size; + Vertex *v, *tv, **last_slot; Edge *e; scalar_t d; -#ifdef DEBUG - if(is_dag()) { - cout << "find_shortest_path: DAG -> ok" << endl; - } else { - for(int e = 0; e < _nb_edges; e++) { - if(_edges[e].positivized_length < 0) abort(); - } - cout << "find_shortest_path: All positivized_length are positive -> ok" << endl; - } -#endif - for(int k = 0; k < _nb_vertices; k++) { _vertices[k].distance_from_source = FLT_MAX; _vertices[k].pred_edge_toward_source = 0; } - _heap_size = _nb_vertices; + heap_size = _nb_vertices; _source->distance_from_source = 0; - decrease_distance_in_heap(_source); - - do { -#ifdef DEBUG - for(int k = 0; k < _heap_size; k++) { - if(_heap[0]->distance_from_source > _heap[k]->distance_from_source) abort(); - } - // cout << "OK!" << endl; -#endif + _source->decrease_distance_in_heap(_heap); + while(heap_size > 1) { + // Get the closest to the source v = _heap[0]; - _heap_size--; - Vertex **a = _heap, **b = _heap + _heap_size; - swap(*a, *b); swap((*a)->heap_position, (*b)->heap_position); - increase_distance_in_heap(_heap[0]); - for(e = v->leaving_edges; e; e = e->next_leaving_edge) { + // Remove it from the heap (swap it with the last_slot in the heap, and + // update the distance of that one) + heap_size--; + last_slot = _heap + heap_size; + swap(*_heap, *last_slot); swap((*_heap)->heap_slot, (*last_slot)->heap_slot); + (*_heap)->increase_distance_in_heap(_heap, last_slot); + + // Now update the neighbors of the node currently closest to the + // source + for(e = v->leaving_edge_list_root; e; e = e->next_leaving_edge) { d = v->distance_from_source + e->positivized_length; tv = e->terminal_vertex; if(d < tv->distance_from_source) { - ASSERT(tv->heap_position - _heap < _heap_size); + ASSERT(tv->heap_slot < last_slot); tv->distance_from_source = d; tv->pred_edge_toward_source = e; - decrease_distance_in_heap(tv); + tv->decrease_distance_in_heap(_heap); } -#ifdef DEBUG - for(int k = 0; k < _heap_size; k++) { - if(_heap[0]->distance_from_source > _heap[k]->distance_from_source) abort(); - } -#endif - } - } while(_heap_size > 0); - -#ifdef DEBUG - for(int k = 0; k < _nb_vertices; k++) { - v = &_vertices[k]; - 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) abort(); } } -#endif - cout << "DONE!" << endl; } void MTPGraph::find_best_paths(scalar_t *lengths) { - scalar_t total_length; + scalar_t shortest_path_length; Vertex *v; Edge *e; @@ -485,19 +326,21 @@ void MTPGraph::find_best_paths(scalar_t *lengths) { _edges[e].positivized_length = _edges[e].length; } - // // We call find_shortest_path here to set properly the distances to - // // the source, so that we can make all the edge lengths positive at - // // the first iteration. - // find_shortest_path(); - - dp_distance_propagation(); + // Compute the distance of all the nodes from the source by just + // visiting them in the proper DAG ordering we computed when + // building the graph + dp_compute_distances(); do { + // Use the current distance from the source to make all edge + // lengths positive update_positivized_lengths(); + // Fix numerical errors force_positivized_lengths(); + find_shortest_path(); - total_length = 0.0; + shortest_path_length = 0.0; // Do we reach the sink? if(_sink->pred_edge_toward_source) { @@ -505,13 +348,13 @@ void MTPGraph::find_best_paths(scalar_t *lengths) { // original edge lengths v = _sink; while(v->pred_edge_toward_source) { - total_length += v->pred_edge_toward_source->length; + shortest_path_length += v->pred_edge_toward_source->length; v = v->pred_edge_toward_source->origin_vertex; } // If that length is negative - if(total_length < 0.0) { + if(shortest_path_length < 0.0) { #ifdef VERBOSE - cerr << "Found a path of length " << total_length << endl; + cerr << __FILE__ << ": Found a path of length " << shortest_path_length << endl; #endif // Invert all the edges along the best path v = _sink; @@ -526,80 +369,152 @@ void MTPGraph::find_best_paths(scalar_t *lengths) { } } - } while(total_length < 0.0); + } while(shortest_path_length < 0.0); // Put back the graph in its original state (i.e. invert edges which // have been inverted in the process) for(int k = 0; k < _nb_edges; k++) { - e = _edges + k; + e = &_edges[k]; if(e->occupied) { e->invert(); } } } -int MTPGraph::retrieve_one_path(Edge *e, Path *path) { +int MTPGraph::retrieve_one_path(Edge *e, Path *path, int *used_edges) { Edge *f, *next = 0; int l = 0, nb_occupied_next; if(path) { - path->nodes[l++] = e->origin_vertex - _vertices; + path->nodes[l++] = int(e->origin_vertex - _vertices); path->length = e->length; } else l++; while(e->terminal_vertex != _sink) { if(path) { - path->nodes[l++] = e->terminal_vertex - _vertices; + path->nodes[l++] = int(e->terminal_vertex - _vertices); path->length += e->length; } else l++; nb_occupied_next = 0; - for(f = e->terminal_vertex->leaving_edges; f; f = f->next_leaving_edge) { - if(f->occupied) { nb_occupied_next++; next = f; } + for(f = e->terminal_vertex->leaving_edge_list_root; f; f = f->next_leaving_edge) { + if(f->occupied && !used_edges[f - _edges]) { + nb_occupied_next++; next = f; + } } #ifdef DEBUG if(nb_occupied_next == 0) { - cerr << "retrieve_one_path: Non-sink end point." << endl; - abort(); - } - - else if(nb_occupied_next > 1) { - cerr << "retrieve_one_path: Non node-disjoint paths." << endl; + cerr << __FILE__ << ": retrieve_one_path: Non-sink end point." << endl; abort(); } #endif + if(path) { used_edges[next - _edges] = 1; } + e = next; } if(path) { - path->nodes[l++] = e->terminal_vertex - _vertices; + path->nodes[l++] = int(e->terminal_vertex - _vertices); path->length += e->length; } else l++; return l; } +////////////////////////////////////////////////////////////////////// + +void MTPGraph::compute_dp_ordering() { + Vertex *v; + Edge *e; + int ntv; + + // This method orders the nodes by putting first the ones with no + // predecessors, then going on adding nodes whose predecessors have + // all been already added. Computing the distances from the source + // by visiting nodes in that order is equivalent to DP. + + int *nb_predecessors = new int[_nb_vertices]; + + Vertex **already_processed = _dp_order, **front = _dp_order, **new_front = _dp_order; + + for(int k = 0; k < _nb_vertices; k++) { + nb_predecessors[k] = 0; + } + + for(int k = 0; k < _nb_vertices; k++) { + v = &_vertices[k]; + for(e = v->leaving_edge_list_root; e; e = e->next_leaving_edge) { + ntv = int(e->terminal_vertex - _vertices); + nb_predecessors[ntv]++; + } + } + + for(int k = 0; k < _nb_vertices; k++) { + if(nb_predecessors[k] == 0) { + *(front++) = _vertices + k; + } + } + + while(already_processed < front) { + // Here, nodes before already_processed can be ignored, nodes + // before front were set to 0 predecessors during the previous + // iteration. During this new iteration, we have to visit the + // successors of these ones only, since they are the only ones + // which may end up with no predecessors. + new_front = front; + while(already_processed < front) { + v = *(already_processed++); + for(e = v->leaving_edge_list_root; e; e = e->next_leaving_edge) { + ntv = int(e->terminal_vertex - _vertices); + nb_predecessors[ntv]--; + ASSERT(nb_predecessors[ntv] >= 0); + if(nb_predecessors[ntv] == 0) { + *(new_front++) = e->terminal_vertex; + } + } + } + front = new_front; + } + + if(already_processed < _dp_order + _nb_vertices) { + cerr << __FILE__ << ": The graph is not a DAG." << endl; + abort(); + } + + delete[] nb_predecessors; +} + +////////////////////////////////////////////////////////////////////// + void MTPGraph::retrieve_disjoint_paths() { Edge *e; int p, l; + int *used_edges; for(int p = 0; p < nb_paths; p++) delete paths[p]; delete[] paths; nb_paths = 0; - for(e = _source->leaving_edges; e; e = e->next_leaving_edge) { + for(e = _source->leaving_edge_list_root; e; e = e->next_leaving_edge) { if(e->occupied) { nb_paths++; } } paths = new Path *[nb_paths]; + used_edges = new int[_nb_edges]; + for(int e = 0; e < _nb_edges; e++) { + used_edges[e] = 0; + } p = 0; - for(e = _source->leaving_edges; e; e = e->next_leaving_edge) { - if(e->occupied) { - l = retrieve_one_path(e, 0); + for(e = _source->leaving_edge_list_root; e; e = e->next_leaving_edge) { + if(e->occupied && !used_edges[e - _edges]) { + l = retrieve_one_path(e, 0, used_edges); paths[p] = new Path(l); - retrieve_one_path(e, paths[p]); + retrieve_one_path(e, paths[p], used_edges); + used_edges[e - _edges] = 1; p++; } } + + delete[] used_edges; }