dijkstra.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.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 "include/dijkstra.h"
00038 #include "include/dijkstra_alg.h"
00039 #include "include/global.h"
00040 #include "include/lqm.h"
00041 
00042 void initDijkstra(int numNodes) {
00043         dij_init(numNodes);
00044 }
00045 
00046 void freeDijkstra(void) {
00047    dij_free();
00048 }
00049 
00050 #define likely(x)       __builtin_expect(!!(x), 1)
00051 
00052 void put_matrix(char ** matrix){
00053         int i, j;
00054         for (i = 0; i < status.N_NODES; i++) {
00055                 for (j = i; j < status.N_NODES; j++) {
00056                         if (matrix[i][j] > 0 && matrix[j][i] > 0) {
00057                                 if (matrix[i][j] < matrix[j][i]) {
00058                                         dij_set(i, j, (long) (lqm_get_f())(matrix[i][j]));
00059                                         dij_set(j, i, (long) (lqm_get_f())(matrix[i][j]));
00060                                 } else {
00061                                         dij_set(i, j, (long) (lqm_get_f())(matrix[j][i]));
00062                                         dij_set(j, i, (long) (lqm_get_f())(matrix[j][i]));
00063                                 }
00064                         } else {
00065                                 dij_set(i, j, 0);
00066                                 dij_set(j, i, 0);
00067                         }
00068                 }
00069         }
00070 }
00071 
00072 void dij_put_matrix(char ** matrix){
00073         put_matrix(matrix);
00074 }
00075 
00076 int nextStepWithCost(char ** matrix,int src, int dest, int* cost) {
00077         char path[32];
00078    int hops;
00079         //print_lqm();
00080         put_matrix(matrix);
00081         hops = dij_getPath(src,dest,path);
00082         if (hops > 0){
00083                 return path[1];
00084         }else{
00085                 return -1;
00086         }
00087 }
00088 
00089 int dijkstra_path_len(char ** matrix,int src, int dest) {
00090         char path[32];
00091    int hops;
00092         if (matrix != 0){
00093                 put_matrix(matrix);
00094         }
00095         hops = dij_getPath(src,dest,path);
00096         return hops;
00097 }
00098 
00099 int isConnected(char ** matrix) {
00100         int connected;
00101         put_matrix(matrix);
00102         connected = dij_isConnected();
00103         return connected;
00104 }
00105 
00106 int isIsolated(char ** matrix,int node) {
00107         int isolated;
00108         put_matrix(matrix);
00109         isolated = dij_isIsolated(node);
00110         return isolated;
00111 }
00112 
00113 int dijkstra_is_node_within_path(char ** matrix,int src, int dest, int node) {
00114         char path[32];
00115    int hops, i;
00116         put_matrix(matrix);
00117         hops = dij_getPath(src,dest,path);
00118         for (i=0;i<=hops;i++){
00119                 if (node == path[i]){
00120                         return 1;
00121                 }
00122         }
00123         return 0;
00124 }
00125 
00126 int getNeighbors(char ** matrix, int src, char * neighbors){
00127         char path[32];
00128         int hops, n_neighbors = 0, i;
00129         put_matrix(matrix);
00130         for (i = 0;  i < status.N_NODES; i++){
00131                 hops = dij_getPath(src,i,path);
00132                 if (hops == 1){
00133                         neighbors[n_neighbors] = path[1];
00134                         n_neighbors++;
00135                 }
00136         }
00137         return n_neighbors;
00138 }
00139 
00140 
00141 int getPath(char ** lqm, int src, int dest, char * path){
00142         put_matrix(lqm);
00143         return dij_getPath(src,dest,path);
00144 }
00145 


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