lqm.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/lqm.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/lqm.h"
00038 #include "include/global.h"
00039 #include "include/dijkstra.h"
00040 #include "include/wmp_utils.h"
00041 #include "include/nstat.h"
00042 
00043 static char ** lqm, ** lqm_dist, ** lqm_copy, ** lqm_pruned;
00044 static int ** A0, ** A1, ** Next;
00045 static int size, net_connected = 0, informed = 0;
00046 static THREAD_SEM_T isconn;
00047 
00048 static char (*fp) (char);
00049 
00050 char f_lqm(char val) {
00051         /* val is a % value (0-100) */
00052         if (val == 0){
00053                 return 0;
00054         } else if (val < status.w100)
00055                 return 100;
00056         else if (val >= status.w100 && val < status.w3)
00057                 return 3;
00058         else if (val >= status.w2 && val < status.w1)
00059                 return 2;
00060         else
00061                 return 1;
00062 }
00063 
00064 
00065 void init_lqm(int m_size){
00066         int i;
00067         size=m_size;
00068         lqm=(char **) MALLOC(size*sizeof(char*));
00069         for (i=0;i<size;i++){
00070                 lqm[i]=(char *) MALLOC(size*sizeof(char));
00071         }
00072 
00073         /* init distance matrix */
00074         lqm_dist=(char**) MALLOC(size*sizeof(char*));
00075         for (i=0;i<size;i++){
00076                 lqm_dist[i]=(char *) MALLOC(size*sizeof(char));
00077                 memset(lqm_dist[i],-1,size*sizeof(char));
00078         }
00079 
00080         /* init distance matrix */
00081         lqm_copy=(char**) MALLOC(size*sizeof(char*));
00082         for (i=0;i<size;i++){
00083                 lqm_copy[i]=(char *) MALLOC(size*sizeof(char));
00084         }
00085         lqm_pruned=(char**) MALLOC(size*sizeof(char*));
00086         for (i=0;i<size;i++){
00087                 lqm_pruned[i]=(char *) MALLOC(size*sizeof(char));
00088         }
00089 
00090 
00091         A0=(int**) MALLOC(size*sizeof(int*));
00092         for (i=0;i<size;i++){
00093                 A0[i]=(int *) MALLOC(size*sizeof(int));
00094         }
00095         A1=(int**) MALLOC(size*sizeof(int*));
00096         for (i=0;i<size;i++){
00097                 A1[i]=(int *) MALLOC(size*sizeof(int));
00098         }
00099 
00100         Next=(int**) MALLOC(size*sizeof(int*));
00101         for (i=0;i<size;i++){
00102                 Next[i]=(int *) MALLOC(size*sizeof(int));
00103         }
00104 
00105         fp = f_lqm;
00106 
00107    THREAD_SEM_INIT_LOCKED(&isconn);
00108 }
00109 
00110 void free_lqm() {
00111         int i;
00112 
00113         for (i = 0; i < size; i++) {
00114                 FREE((char*)lqm[i]);
00115         }
00116         FREE((char*)lqm);
00117 
00118         for (i = 0; i < size; i++) {
00119                 FREE((char*)lqm_dist[i]);
00120         }
00121         FREE((char*)lqm_dist);
00122 
00123         for (i = 0; i < size; i++) {
00124                 FREE((char*)lqm_copy[i]);
00125         }
00126         FREE((char*)lqm_copy);
00127 
00128         for (i = 0; i < size; i++) {
00129                 FREE((char*)lqm_pruned[i]);
00130         }
00131         FREE((char*)lqm_pruned);
00132 
00133         for (i = 0; i < size; i++) {
00134                 FREE((int*)A0[i]);
00135         }
00136         FREE((int*)A0);
00137 
00138         for (i = 0; i < size; i++) {
00139                 FREE((int*)A1[i]);
00140         }
00141         FREE((int*)A1);
00142         for (i = 0; i < size; i++) {
00143                 FREE((int*)Next[i]);
00144         }
00145         FREE((int*)Next);
00146 
00147 }
00148 
00149 void fill_lqm(char val){
00150         int i,j;
00151         for (i=0;i<size;i++){
00152                 for (j=0;j<size;j++){
00153                         if (i!=j) lqm[i][j]=val;
00154                         else lqm[i][j]=0;
00155                 }
00156         }
00157 }
00158 
00159 char lqm_get_val(int i, int j){
00160         return lqm[i][j];
00161 }
00162 
00163 int lqm_get_num_hops(int i, int j){
00164         if (i<0 || j<0 || i>=size || j>=size){
00165                 return -1;
00166         }else{
00167                 return lqm_dist[i][j];
00168         }
00169 }
00170 
00171 void lqm_set_val(int i, int j, char val){
00172         lqm[i][j]=val;
00173 }
00174 
00175 char** lqm_get_ptr(void){
00176         return lqm;
00177 }
00178 
00179 int  wmpIsNetworkConnected(void){
00180         return net_connected;
00181 }
00182 
00183 int  wmpIsNetworkConnectedBlocking(int timeout_ms){
00184         int ret;
00185         informed = 0;
00186         if (timeout_ms > 0){
00187       ret = ( THREAD_SEM_WAIT_TIMED(isconn, timeout_ms) == 0 );
00188         } else{
00189       THREAD_SEM_WAIT(&isconn);
00190                 ret = 1;
00191         }
00192         return ret;
00193 }
00194 
00195 void lqm_calculate_distances(void){
00196         int i, j;
00197         net_connected = 1;
00198 
00199         /* simetriza la matriz */
00200         dij_put_matrix(lqm);
00201 
00202         for (i = 0; i < size; i++) {
00203                 /* NOV 30 j=0 */
00204                 for (j = 0; j < i; j++) {
00205                         if (i != j) {
00206                                 int len = dijkstra_path_len(0, i, j);
00207 
00208                                 if (nstat_isLost(i) || nstat_isLost(j)){
00209                                         len  = -1;
00210                                 }
00211 
00212                                 lqm_dist[i][j] = len;
00213                                 lqm_dist[j][i] = len;
00214                                 net_connected = (net_connected && (len > 0));// && (lqm[i][j] <= 100));
00215                         } else {
00216                                 lqm_dist[i][j] = 0;
00217                         }
00218 
00219                 }
00220         }
00221         if (net_connected && !informed) {
00222       THREAD_SEM_SIGNAL(&isconn);
00223                 informed = 1;
00224         }else{
00225                 informed = 0;
00226         }
00227 }
00228 
00229 int lqm_is_leaf(char id){
00230         int i, sum=0;
00231         for (i=0;i<size;i++){
00232                 if (lqm[(int)id][i]>0) sum+=1;
00233         }
00234         if (sum > 1) return 0;
00235         else if (sum ==1) return 1;
00236         else return -1;
00237 }
00238 static int copied=0;
00239 
00240 void lqm_backup(){
00241         int i,j;
00242         for (i=0;i<size;i++){
00243                 for (j=0;j<size;j++){
00244                         lqm_copy[i][j]=lqm[i][j];
00245                 }
00246         }
00247         copied = 1;
00248 }
00249 
00250 void lqm_restore(){
00251         if (copied)     {
00252                 int i,j;
00253                 for (i=0;i<size;i++){
00254                         for (j=0;j<size;j++){
00255                                 lqm[i][j]=lqm_copy[i][j];
00256                         }
00257                 }
00258         }
00259         copied=0;
00260 }
00261 
00262 int lqm_get_distance(char i, char j){
00263         return lqm_dist[(int)i][(int)j];
00264 }
00265 
00266 
00267 void print_lqm3(char* txt, char **lqm){
00268         char q[1020];
00269         lqmToString(lqm,q,status.N_NODES);
00270    WMP_ERROR(stderr,"@%s@\n%s#\n",txt,q);
00271 }
00272 
00273 char ** lqm_prune(char ** mlqm) {
00274    int i, j, val_ij, val_ji, found;
00275 
00276         if (!isConnected(lqm)) {
00277                 return lqm;
00278         }
00279 
00280         for (i = 0; i < size; i++) {
00281                 for (j = 0; j < size; j++) {
00282                         lqm_pruned[i][j] = mlqm[i][j];
00283                 }
00284         }
00285 
00286         //Quito el link más debil y veo si la red sigue conexa, luego el segundo más débil etc...
00287 
00288 
00289         found = 1;
00290         while (found) {
00291                 int umbral = 15, sel_i = 0, sel_j = 0;
00292                 found = 0;
00293                 for (i = 0; i < size; i++) {
00294                         for (j = 0; j < size; j++) {
00295                                 if (i == j){
00296                                         continue;
00297                                 }
00298                                 if (lqm_pruned[i][j] > 0 && lqm_pruned[i][j] < umbral) {
00299                                         sel_i = i;
00300                                         sel_j = j;
00301                                         found = 1;
00302                                         umbral = lqm_pruned[i][j];
00303                                 }
00304                         }
00305                 }
00306 
00307                 if (found){
00308                         //fprintf(stderr,"found seli:%d selj:%d umbral:%d\n",sel_i,sel_j,umbral);
00309 
00310                         val_ij = lqm_pruned[sel_i][sel_j];
00311                         val_ji = lqm_pruned[sel_j][sel_i];
00312 
00313                         lqm_pruned[sel_i][sel_j] = 0;
00314                         lqm_pruned[sel_j][sel_i] = 0;
00315 
00316                         if (!isConnected(lqm_pruned)) {
00317                                 // marco los elementos ya procesados
00318                                 lqm_pruned[sel_i][sel_j] = -val_ij;
00319                                 lqm_pruned[sel_j][sel_i] = -val_ji;
00320                         }
00321                 }
00322                 //print_lqm3("afterr",lqm_pruned);
00323         }
00324         for (i = 0; i < size; i++) {
00325                 for (j = 0; j < size; j++) {
00326                         if (i==j) {
00327                                 continue;
00328                         }
00329                         lqm_pruned[i][j] = abs(lqm_pruned[i][j]);
00330                 }
00331         }
00332 
00333         //print_lqm3("after",lqm_pruned);
00334 
00335         return lqm_pruned;
00336 }
00337 
00338 void lqm_copy_to(char ** dest, char ** src) {
00339         int i,j;
00340         for (i = 0; i < size; i++) {
00341                 for (j = 0; j < size; j++) {
00342                         if (i != j) {
00343                                 dest[i][j] = src[i][j];
00344                         }
00345                 }
00346         }
00347 }
00348 
00349 
00350 char (*lqm_get_f())(char){
00351         return fp;
00352 }
00353 
00354 
00355 void lqm_set_f( char (*f) (char)){
00356         fp = f;
00357 }
00358 
00359 void lqm_compute_prob(char ** lqm) {
00360         int i,j,k;
00361         for (i=0; i< size; i++){
00362                 for (j=0; j< size; j++){
00363                         Next[i][j]=j;
00364                         if (i==j){
00365                                 A0[i][j] = 1000;
00366                         }else{
00367                                 A0[i][j] = lqm[i][j]*10; //XXX:
00368                         }
00369                 }
00370         }
00371 
00372         for (k = 0; k < size; k++) {
00373                 for (i = 0; i < size; i++) {
00374                         for (j = 0; j < size; j++) {
00375                                 if (A0[i][k] * A0[k][k] * A0[k][j] / 1000000 > A0[i][j]) {
00376                                         Next[i][j] = Next[i][k];
00377                                         A1[i][j] = A0[i][k] * A0[k][k] * A0[k][j] / 1000000;
00378 
00379                                 } else {
00380                                         A1[i][j] = A0[i][j];
00381                                 }
00382                         }
00383                 }
00384                 memcpy(A0, A1, sizeof(A0));
00385         }
00386 }
00387 
00388 /* returns first hop (if exist) or -1 if not. If path is passed, the function returns the whole path */
00389 int lqm_prob_get_path(int src, int dest, char * path) {
00390         int res = src, exres = 0, idx = 0;
00391         while (res != dest) {
00392 
00393                 res = Next[res][dest];
00394 //              fprintf(stderr,"src:%d dst:%d res:%d\n",src,dest,res);
00395                 if (path != 0){
00396                         path[idx] = res;
00397                         idx++;
00398                 }
00399 //              if (res == exres){
00400 //                      return -1;
00401 //              }
00402                 exres = res;
00403         }
00404         return path[0];//Next[res][dest];
00405 }
00406 /* returns first hop (if exist) or -1 if not. If path is passed, the function returns the whole path */
00407 int lqm_prob_get_val(int src, int dest) {
00408         return A0[src][dest];
00409 }


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