dijkstra_alg.c
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           RT-WMP              --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V7.0B  11/05/10
00005  *
00006  *
00007  *  File: ./src/core/dijkstra_alg.c
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2010, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 
00037 #include "config/compiler.h"
00038 #include "include/global.h"
00039 #define DIJ_INFINITY 10000
00040 
00041 static int n; /* The number of nodes in the graph */
00042 static long **dist; /* dist[i][j] is the distance between node i and j; or 0 if there is no direct connection */
00043 static long *d; /* d[i] is the length of the shortest path between the source (s) and node i */
00044 static int *prev; /* prev[i] is the node that comes right before i in the shortest path from the source to i*/
00045 static int *visited;
00046 
00047 void dijkstra(int s) {
00048         int i, k, mini;
00049 
00050         for (i = 0; i < n; ++i) {
00051                 d[i] = DIJ_INFINITY;
00052                 prev[i] = -1; /* no path has yet been found to i */
00053                 visited[i] = 0; /* the i-th element has not yet been visited */
00054         }
00055 
00056         d[s] = 0;
00057 
00058         for (k = 0; k < n; ++k) {
00059                 mini = -1;
00060                 for (i = 0; i < n; ++i)
00061                         if (!visited[i] && ((mini == -1) || (d[i] < d[mini])))
00062                                 mini = i;
00063 
00064                 visited[mini] = 1;
00065 
00066                 for (i = 0; i < n; ++i)
00067                         if (dist[mini][i])
00068                                 if (d[mini] + dist[mini][i] < d[i]) {
00069                                         d[i] = d[mini] + dist[mini][i];
00070                                         prev[i] = mini;
00071                                 }
00072         }
00073 }
00074 
00075 void dij_init(int matrix_size) {
00076 
00077         int i;
00078         n = matrix_size;
00079         dist= (long**) MALLOC(n*sizeof(long*));
00080         for (i=0;i<n;i++){
00081                 dist[i]=(long *) MALLOC(n*sizeof(long));
00082         }
00083         d=(long *) MALLOC(n*sizeof(long));
00084         prev=(int *) MALLOC(n*sizeof(long));
00085         visited=(int *) MALLOC(n*sizeof(int));
00086 }
00087 
00088 void dij_free (void) {
00089    int i;
00090 
00091    for (i=0;i<n;i++){
00092       FREE(dist[i]);
00093    }
00094    FREE(dist);
00095    FREE(d);
00096    FREE(prev);
00097    FREE(visited);
00098 }
00099 
00100 static int check_range(int i, int j){
00101         if (i >= 0 && i < status.N_NODES && j >= 0 && j < status.N_NODES){
00102                 return 1;
00103         }else{
00104                 return 0;
00105         }
00106 }
00107 
00108 long ** dij_get_ptr(void){
00109         return dist;
00110 }
00111 
00112 void dij_set(int i, int j, long val) {
00113         if (check_range(i,j)){
00114                 dist[i][j] = val;
00115         }
00116 }
00117 long dij_get(int i, int j) {
00118         if (check_range(i,j)){
00119                 return dist[i][j];
00120         } else{
00121                 return -1;
00122         }
00123 }
00124 
00125 
00126 int dij_getPath(int src, int dest, char* path) {
00127         int idx = 0, j = 0;
00128         if (!check_range(src,dest)){
00129                 return -1;
00130         }
00131         dijkstra(src);
00132         if (d[dest] == DIJ_INFINITY) {
00133                 WMP_DBG(DIJKSTRA,"No PATH");
00134                 return -1;
00135         }
00136         while (prev[dest] != -1) {
00137                 path[idx] = dest;
00138                 idx = idx + 1;
00139                 dest = prev[dest];
00140         }
00141         path[idx] = dest;
00142 
00143         for (j = 0; j <= idx / 2; j++) {
00144                 int a = path[j];
00145                 path[j] = path[idx - j];
00146                 path[idx - j] = a;
00147 
00148         }
00149         return idx;
00150 }
00151 
00152 int dij_getDist(int src, int dest) {
00153         if (!check_range(src,dest)){
00154                 return -1;
00155         }
00156         dijkstra(src);
00157         return d[dest];
00158 }
00159 
00160 
00161 
00162 
00163 int dij_isConnected(void) {
00164         int i=0, connected=1;
00165         dijkstra(0);
00166         for (i=0; i<n; i++){
00167                 connected = connected && (d[i]<DIJ_INFINITY);
00168         }
00169         return connected;
00170 }
00171 
00172 int dij_isIsolated(int src) {
00173         // This way I'll control if the node can reach me
00174         //dijkstra(src);
00175         //return (d[status.id] ==  DIJ_INFINITY);
00176 
00177         // This way I'll control if I can reach the node
00178         if (!check_range(src,src)){
00179                 return 1;
00180         }
00181         dijkstra(status.id);
00182         return (d[src] ==  DIJ_INFINITY);
00183 }
00184 
00185 
00186 
00187 int dij_isIsolated2(int src) {
00188         dijkstra(src);
00189         return (d[status.id] ==  DIJ_INFINITY);
00190 }
00191 
00192 
00193 
00194 


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:09