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;
58 scalar_t distance_from_source;
63 Vertex() { first_edge = 0; }
65 inline void add_edge(Edge *e) {
68 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; }
80 void initialize_work_lengths();
81 void update_work_length();
82 void find_shortest_path(Vertex **front, Vertex **new_front);
87 Vertex *source, *sink;
90 Graph(int nb_vertices, int nb_edges, int *from, int *to, scalar_t *lengths,
91 int source, int sink);
95 void find_best_paths();
97 void print_occupied_edges();
101 void Graph::print() {
102 for(int n = 0; n < nb_vertices; n++) {
103 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
104 cout << n << " -> " << e->terminal_vertex->id << " " << e->length << endl;
109 void Graph::print_occupied_edges() {
110 for(int n = 0; n < nb_vertices; n++) {
111 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
113 int a = n, b = e->terminal_vertex->id;
114 if(a > b) { int c = a; a = b; b = c; }
115 cout << a << " " << b << endl;
121 void Graph::dot_print() {
122 cout << "digraph {" << endl;
123 cout << " node[shape=circle];" << endl;
124 for(int n = 0; n < nb_vertices; n++) {
125 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
126 int a = n, b = e->terminal_vertex->id;
128 int c = a; a = b; b = c;
129 cout << " " << a << " -> " << b << " [style=bold,color=black,label=\"" << -e->length << "\"];" << endl;
131 cout << " " << a << " -> " << b << " [color=gray,label=\"" << e->length << "\"];" << endl;
138 Graph::Graph(int nb_vrt, int nb_edges,
139 int *from, int *to, scalar_t *lengths,
141 nb_vertices = nb_vrt;
143 edge_heap = new Edge[nb_edges];
144 vertices = new Vertex[nb_vertices];
146 source = &vertices[src];
147 sink = &vertices[snk];
149 for(int v = 0; v < nb_vertices; v++) {
153 for(int e = 0; e < nb_edges; e++) {
154 vertices[from[e]].add_edge(&edge_heap[e]);
155 edge_heap[e].occupied = 0;
156 edge_heap[e].length = lengths[e];
157 edge_heap[e].terminal_vertex = &vertices[to[e]];
166 void Graph::initialize_work_lengths() {
167 scalar_t length_min = 0;
168 for(int n = 0; n < nb_vertices; n++) {
169 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
170 length_min = min(e->length, length_min);
173 for(int n = 0; n < nb_vertices; n++) {
174 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
175 e->work_length = e->length - length_min;
180 void Graph::update_work_length() {
181 for(int n = 0; n < nb_vertices; n++) {
182 scalar_t d = vertices[n].distance_from_source;
183 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
184 e->work_length += d - e->terminal_vertex->distance_from_source;
189 void Graph::find_shortest_path(Vertex **front, Vertex **new_front) {
196 for(int n = 0; n < nb_vertices; n++) {
197 for(Edge *e = vertices[n].first_edge; e; e = e->next) {
198 if(e->work_length < 0) {
199 cerr << "DEBUG error in find_shortest_path: Edge fixed lengths have to be positive."
207 for(int v = 0; v < nb_vertices; v++) {
208 vertices[v].distance_from_source = FLT_MAX;
209 vertices[v].pred_vertex = 0;
210 vertices[v].pred_edge = 0;
213 int front_size = 0, new_front_size;
214 front[front_size++] = source;
215 source->distance_from_source = 0;
219 for(int f = 0; f < front_size; f++) {
221 for(Edge *e = v->first_edge; e; e = e->next) {
222 d = v->distance_from_source + e->work_length;
223 tv = e->terminal_vertex;
224 if(d < tv->distance_from_source) {
225 tv->distance_from_source = d;
228 new_front[new_front_size++] = tv;
233 tmp_front = new_front;
237 tmp_front_size = new_front_size;
238 new_front_size = front_size;
239 front_size = tmp_front_size;
240 } while(front_size > 0);
243 void Graph::find_best_paths() {
244 Vertex **front = new Vertex *[nb_vertices];
245 Vertex **new_front = new Vertex *[nb_vertices];
247 scalar_t total_length;
249 initialize_work_lengths();
253 find_shortest_path(front, new_front);
254 update_work_length();
256 // Do we reach the sink?
257 if(sink->pred_edge) {
259 // If yes, compute the length of the best path
260 for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
261 total_length += v->pred_edge->length;
264 // If that length is negative
265 if(total_length < 0.0) {
266 // Invert all the edges along the best path
267 for(Vertex *v = sink; v->pred_edge; v = v->pred_vertex) {
268 Edge *e = v->pred_edge;
269 e->terminal_vertex = v->pred_vertex;
270 e->occupied = 1 - e->occupied;
271 e->length = - e->length;
272 e->work_length = - e->work_length;
273 v->pred_vertex->del_edge(e);
278 } while(total_length < 0.0);
280 // // We put all occupied edges back to their original orientations
281 // for(int n = 0; n < nb_vertices; n++) {
282 // Vertex *v = &vertices[n];
283 // for(Edge *e = v->first_edge; e; e = e->next) {
285 // e->terminal_vertex = v->pred_vertex;
286 // e->length = - e->length;
287 // e->work_length = 0;
288 // v->pred_vertex->del_edge(e);
299 //////////////////////////////////////////////////////////////////////
301 int main(int argc, char **argv) {
304 cerr << argv[0] << " <graph file>" << endl;
308 ifstream *file = new ifstream(argv[1]);
310 int nb_edges, nb_vertices;
315 (*file) >> nb_vertices >> nb_edges;
316 (*file) >> source >> sink;
318 // cout << "INPUT nb_edges " << nb_edges << endl;
319 // cout << "INPUT nb_vertices " << nb_vertices << endl;
320 // cout << "INPUT source " << source << endl;
321 // cout << "INPUT sink " << sink << endl;
323 scalar_t *el = new scalar_t[nb_edges];
324 int *ea = new int[nb_edges];
325 int *eb = new int[nb_edges];
327 for(int e = 0; e < nb_edges; e++) {
328 (*file) >> ea[e] >> eb[e] >> el[e];
331 // for(int e = 0; e < nb_edges; e++) {
332 // cout << "INPUT_EDGE " << ea[e] << " " << eb[e] << " " << el[e] << endl;
335 Graph graph(nb_vertices, nb_edges, ea, eb, el, source, sink);
337 graph.find_best_paths();
338 // graph.print_occupied_edges();
347 cerr << "Can not open " << argv[1] << endl;