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. //
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. //
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/>. //
15 // Written by and Copyright (C) Francois Fleuret //
16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports //
17 ///////////////////////////////////////////////////////////////////////////
32 typedef float scalar_t;
35 #define ASSERT(x) if(!(x)) { \
36 std::cerr << "ASSERT FAILED IN " << __FILE__ << ":" << __LINE__ << endl; \
48 scalar_t length, work_length;
49 Vertex *terminal_vertex;
56 // These are the leaving edges
63 Vertex() { first_edge = 0; }
65 inline void add_edge(Edge *e) {
66 if(first_edge) { first_edge->pred = e; }
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; }
84 Vertex *source, *sink;
86 Graph(int nb_vertices, int nb_edges, int *from, int *to, scalar_t *lengths,
87 int source, int sink);
90 void initialize_work_lengths();
91 void update_work_length();
92 void find_shortest_path();
93 void find_best_paths();
95 void print_occupied_edges();
99 for(int n = 0; n < nb_vertices; n++) {
100 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
101 cout << n << " -> " << e->terminal_vertex->id << " " << e->length << endl;
106 void Graph::print_occupied_edges() {
107 for(int n = 0; n < nb_vertices; n++) {
108 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
110 int a = n, b = e->terminal_vertex->id;
111 if(a > b) { int c = a; a = b; b = c; }
112 cout << a << " " << b << endl;
118 Graph::Graph(int nb_vrt, int nb_edges,
119 int *from, int *to, scalar_t *lengths,
121 nb_vertices = nb_vrt;
123 edge_heap = new Edge[nb_edges];
124 vertices = new Vertex[nb_vertices];
126 source = &vertices[src];
127 sink = &vertices[snk];
129 for(int v = 0; v < nb_vertices; v++) {
133 for(int e = 0; e < nb_edges; e++) {
134 vertices[from[e]].add_edge(&edge_heap[e]);
135 edge_heap[e].occupied = 0;
136 edge_heap[e].length = lengths[e];
137 edge_heap[e].terminal_vertex = &vertices[to[e]];
146 void Graph::initialize_work_lengths() {
147 scalar_t length_min = 0;
148 for(int n = 0; n < nb_vertices; n++) {
149 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
150 length_min = min(e->length, length_min);
153 for(int n = 0; n < nb_vertices; n++) {
154 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
155 e->work_length = e->length - length_min;
160 void Graph::update_work_length() {
161 for(int n = 0; n < nb_vertices; n++) {
162 scalar_t d = vertices[n].distance;
163 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
164 e->work_length += d - e->terminal_vertex->distance;
169 void Graph::find_shortest_path() {
170 Vertex **front = new Vertex *[nb_vertices];
171 Vertex **new_front = new Vertex *[nb_vertices];
178 for(int n = 0; n < nb_vertices; n++) {
179 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
180 if(e->work_length < 0) {
181 cerr << "DEBUG error in find_shortest_path: Edge fixed lengths have to be positive."
189 for(int v = 0; v < nb_vertices; v++) {
190 vertices[v].distance = FLT_MAX;
191 vertices[v].pred_vertex = 0;
192 vertices[v].pred_edge = 0;
195 int front_size = 0, new_front_size;
196 front[front_size++] = source;
197 source->distance = 0;
201 for(int f = 0; f < front_size; f++) {
203 for(Edge *e = v->first_edge; e; e = e->next) {
204 d = v->distance + e->work_length;
205 tv = e->terminal_vertex;
206 if(d < tv->distance) {
210 new_front[new_front_size++] = tv;
215 tmp_front = new_front;
219 tmp_front_size = new_front_size;
220 new_front_size = front_size;
221 front_size = tmp_front_size;
222 } while(front_size > 0);
228 void Graph::find_best_paths() {
229 scalar_t total_length;
231 initialize_work_lengths();
239 find_shortest_path();
240 update_work_length();
242 // Do we reach the sink?
243 if(sink->pred_edge) {
246 cout << "VERBOSE there is a path reaching the sink" << endl;
249 // If yes, compute the length of the best path
250 for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
251 total_length += v->pred_edge->length;
255 cout << "VERBOSE total_length " << total_length << endl;
258 // If that length is negative
259 if(total_length < 0.0) {
260 // Invert all the edges along the best path
261 for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
262 Edge *e = v->pred_edge;
263 e->terminal_vertex = v->pred_vertex;
264 e->occupied = 1 - e->occupied;
265 e->length = - e->length;
266 e->work_length = - e->work_length;
267 v->pred_vertex->del_edge(e);
272 } while(total_length < 0.0);
275 //////////////////////////////////////////////////////////////////////
277 int main(int argc, char **argv) {
280 cerr << argv[0] << " <graph file>" << endl;
284 ifstream *file = new ifstream(argv[1]);
286 int nb_edges, nb_vertices;
291 (*file) >> nb_vertices >> nb_edges;
292 (*file) >> source >> sink;
294 cout << "INPUT nb_edges " << nb_edges << endl;
295 cout << "INPUT nb_vertices " << nb_vertices << endl;
296 cout << "INPUT source " << source << endl;
297 cout << "INPUT sink " << sink << endl;
299 scalar_t *el = new scalar_t[nb_edges];
300 int *ea = new int[nb_edges];
301 int *eb = new int[nb_edges];
303 for(int e = 0; e < nb_edges; e++) {
304 (*file) >> ea[e] >> eb[e] >> el[e];
305 cout << "INPUT_EDGE " << ea[e] << " " << eb[e] << " " << el[e] << endl;
308 Graph graph(nb_vertices, nb_edges, ea, eb, el, source, sink);
310 graph.find_best_paths();
311 graph.print_occupied_edges();
319 cerr << "Can not open " << argv[1] << endl;