2 ///////////////////////////////////////////////////////////////////////////
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. //
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. //
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/>. //
17 // Written by and Copyright (C) Francois Fleuret //
18 // Contact <francois.fleuret@idiap.ch> for comments & bug reports //
21 ///////////////////////////////////////////////////////////////////////////
34 typedef float scalar_t;
37 #define ASSERT(x) if(!(x)) { \
38 std::cerr << "ASSERT FAILED IN " << __FILE__ << ":" << __LINE__ << endl; \
50 scalar_t length, fixed_length;
51 Vertex *terminal_vertex;
57 // These are the leaving edges
64 Vertex() { first_edge = 0; }
66 inline void add_edge(Edge *e) {
67 if(first_edge) { first_edge->pred = e; }
73 inline void del_edge(Edge *e) {
74 if(e == first_edge) { first_edge = e->next; }
75 if(e->pred) { e->pred->next = e->next; }
76 if(e->next) { e->next->pred = e->pred; }
85 Vertex *source, *sink;
87 Graph(int nb_vertices, int nb_edges, int *from, int *to, scalar_t *lengths,
88 int source, int sink);
91 void initialize_fixed_lengths();
92 void update_fixed_length();
93 void find_shortest_path();
94 void find_best_paths();
99 for(int n = 0; n < nb_vertices; n++) {
100 cout << "VERTEX [" << n << "] ";
101 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
102 cout << " " << e->length;
108 Graph::Graph(int nb_vrt, int nb_edges,
109 int *from, int *to, scalar_t *lengths,
111 nb_vertices = nb_vrt;
113 edge_heap = new Edge[nb_edges];
114 vertices = new Vertex[nb_vertices];
116 source = &vertices[src];
117 sink = &vertices[snk];
119 for(int e = 0; e < nb_edges; e++) {
120 vertices[from[e]].add_edge(&edge_heap[e]);
121 edge_heap[e].occupied = 0;
122 edge_heap[e].length = lengths[e];
123 edge_heap[e].terminal_vertex = &vertices[to[e]];
132 void Graph::initialize_fixed_lengths() {
133 scalar_t length_min = 0;
134 for(int n = 0; n < nb_vertices; n++) {
135 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
136 length_min = min(e->length, length_min);
139 for(int n = 0; n < nb_vertices; n++) {
140 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
141 e->fixed_length = e->length - length_min;
146 void Graph::update_fixed_length() {
147 for(int n = 0; n < nb_vertices; n++) {
148 scalar_t d = vertices[n].distance;
149 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
150 e->fixed_length += d - e->terminal_vertex->distance;
155 void Graph::find_shortest_path() {
156 Vertex **front = new Vertex *[nb_vertices];
157 Vertex **new_front = new Vertex *[nb_vertices];
164 for(int n = 0; n < nb_vertices; n++) {
165 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
166 if(e->fixed_length < 0) {
167 cerr << "DEBUG error in find_shortest_path: Edge fixed lengths have to be positive."
175 for(int v = 0; v < nb_vertices; v++) {
176 vertices[v].distance = FLT_MAX;
177 vertices[v].pred_vertex = 0;
178 vertices[v].pred_edge = 0;
181 int front_size = 0, new_front_size;
182 front[front_size++] = source;
183 source->distance = 0;
187 for(int f = 0; f < front_size; f++) {
189 for(Edge *e = v->first_edge; e; e = e->next) {
190 d = v->distance + e->fixed_length;
191 tv = e->terminal_vertex;
192 if(d < tv->distance) {
196 new_front[new_front_size++] = tv;
201 tmp_front = new_front;
205 tmp_front_size = new_front_size;
206 new_front_size = front_size;
207 front_size = tmp_front_size;
208 cout << "front_size = " << front_size << endl;
209 } while(front_size > 0);
215 void Graph::find_best_paths() {
216 scalar_t total_length;
218 initialize_fixed_lengths();
224 find_shortest_path();
225 update_fixed_length();
227 // Do we reach the sink?
228 if(sink->pred_edge) {
231 cout << "VERBOSE there is a path reaching the sink" << endl;
234 // If yes, compute the length of the best path
235 for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
236 total_length += v->pred_edge->length;
240 cout << "VERBOSE total_length " << total_length << endl;
243 // If that length is negative
244 if(total_length < 0.0) {
245 // Invert all the edges along the best path
246 for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
247 Edge *e = v->pred_edge;
248 e->terminal_vertex = v->pred_vertex;
249 e->occupied = 1 - e->occupied;
250 e->length = - e->length;
251 e->fixed_length = - e->fixed_length;
252 v->pred_vertex->del_edge(e);
254 cout << "INVERT!" << endl;
258 } while(total_length < 0.0);
261 //////////////////////////////////////////////////////////////////////
263 int main(int argc, char **argv) {
266 cerr << argv[0] << " <graph file>" << endl;
270 ifstream *file = new ifstream(argv[1]);
272 int nb_edges, nb_vertices;
277 (*file) >> nb_vertices >> nb_edges;
278 (*file) >> source >> sink;
280 cout << "INPUT nb_edges " << nb_edges << endl;
281 cout << "INPUT nb_vertices " << nb_vertices << endl;
282 cout << "INPUT source " << source << endl;
283 cout << "INPUT sink " << sink << endl;
285 scalar_t *el = new scalar_t[nb_edges];
286 int *ea = new int[nb_edges];
287 int *eb = new int[nb_edges];
289 for(int e = 0; e < nb_edges; e++) {
290 (*file) >> ea[e] >> eb[e] >> el[e];
291 cout << "INPUT_EDGE " << ea[e] << " " << eb[e] << " " << el[e] << endl;
294 Graph graph(nb_vertices, nb_edges, ea, eb, el, source, sink);
296 graph.find_best_paths();
304 cerr << "Can not open " << argv[1] << endl;