tree.c
Go to the documentation of this file.
00001 
00002 #include "config/compiler.h"
00003 #include "core/include/global.h"
00004 #include "config/compiler.h"
00005 #include "core/include/prim.h"
00006 #include "core/include/wmp_utils.h"
00007 #include "core/include/rssi_average.h"
00008 
00009 static char ** tmp_lqm;
00010 static char * tree, *tree_bak;
00011 
00012 void print_matrix(char* txt, char **lqm){
00013         return;
00014         char q[1024];
00015         lqmToString(lqm,q,status.N_NODES);
00016    WMP_ERROR(stderr,"@%s@\n%s#\n",txt,q);
00017 }
00018 
00019 void print_vector(char*txt, char *t){
00020         return;
00021         int i, n=status.N_NODES;
00022    WMP_ERROR(stderr,"@%s@\n",txt);
00023         for (i=0;i<n;i++){
00024       WMP_ERROR(stderr,"\t%d",i);
00025         }
00026    WMP_ERROR(stderr,"\n");
00027         for (i=0;i<n;i++){
00028       WMP_ERROR(stderr,"\t%d",t[i]);
00029         }
00030    WMP_ERROR(stderr,"#\n");
00031 }
00032 
00033 void tree_save_tree(char * cp){
00034         memcpy(tree_bak,cp,status.N_NODES);
00035 }
00036 
00037 void tree_create_lqm_from_tree(char ** lqm, char * tree){
00038         int i,j;
00039         for (i=0;i<status.N_NODES;i++){
00040                 for (j=0;j<status.N_NODES;j++){
00041                         lqm[i][j]=0;
00042                 }
00043         }
00044         //FPRINTF(stderr,"Cleaned!\n");
00045         print_vector("kkk",tree);
00046 
00047         for (i=0;i<status.N_NODES;i++){
00048                 if (tree[i]>=0 && tree[i]< status.N_NODES){
00049                         lqm[i][(int)tree[i]]=100;
00050                         lqm[(int)tree[i]][i]=100;
00051                 }
00052         }
00053         print_matrix("kkk",lqm);
00054 }
00055 int error_happened=0;
00056 void tree_create_tree_from_lqm1(char * tree, char ** lqm){
00057         int i,j;
00058 
00059         char occupied[status.N_NODES];
00060         for (i=0;i<status.N_NODES;i++){
00061                  tree[i]=-1;
00062                  occupied[i]=0;
00063         }
00064         for (i=0;i<status.N_NODES;i++){
00065                 for (j=0;j<i;j++){
00066                         if (lqm[i][j]>0){
00067                                 if (occupied[i]==0){
00068                                         tree[i]=j;
00069                                         occupied[i]=1;
00070                                         if (j==0) tree[j]=i;
00071                                 }else{
00072                                         if (occupied[j]==0){
00073                                                 tree[j]=i;
00074                                                 if (i==0) tree[i]=j;
00075                                         }else{
00076                                                 WMP_DEBUG(stderr,"compression error\n");
00077                                                 //assert(0);
00078                                                 error_happened++;
00079                                         }
00080                                 }
00081                         }
00082                 }
00083         }
00084 }
00085 
00086 void tree_create_tree_from_lqm2(char * tree, char ** lqm){
00087         int i,j,k,cnt;
00088 
00089         char occupied[status.N_NODES];
00090         for (i=0;i<status.N_NODES;i++){
00091                  tree[i]=-1;
00092                  occupied[i]=0;
00093         }
00094         for (i=0;i<status.N_NODES;i++){
00095                 for (j=0;j<status.N_NODES;j++){
00096                         for (k=j, cnt=0 ;k<status.N_NODES;k++){
00097                                 /* count the number of elements in a line */
00098                                 cnt += (lqm[i][k] > 0 ? 1 : 0);
00099                         }
00100                         if (lqm[i][j]>0 && j!=i){
00101                                         if (cnt>1 && tree[j]==i) continue;
00102                                         tree[i]=j;
00103                                         break;
00104                         }
00105                 }
00106         }
00107 }
00108 
00109 int tree_copy_lqm(char ** src_lqm, char ** dst_lqm){
00110    int i,j;
00111    for (i=0;i<status.N_NODES;i++){
00112       for (j=0;j<status.N_NODES;j++){
00113          dst_lqm[i][j]=src_lqm[i][j];
00114       }
00115    }
00116 }
00117 
00118 void tree_create_tree_from_lqm(char * tree, char ** lqm){
00119         int i,j,k;
00120         tree_copy_lqm(lqm,tmp_lqm);
00121         char occupied[status.N_NODES];
00122         for (i=0;i<status.N_NODES;i++){
00123                  tree[i]=-1;
00124                  occupied[i]=0;
00125         }
00126         for (k = 0; k < status.N_NODES; k++) {
00127                 for (i = 0; i < status.N_NODES; i++) {
00128                         int idx = 0, val = -1;
00129                         for (j = 0; j < status.N_NODES; j++) {
00130                                 if (tmp_lqm[i][j] > 0) {
00131                                         idx++;
00132                                         val = j;
00133                                 }
00134                         }
00135                         if (idx == 1) {
00136                                 tree[i] = val;
00137                                 tmp_lqm[val][i] = 0;
00138                         }
00139                 }
00140         }
00141 }
00142 
00143 void tree_init(void){
00144    int i;
00145 
00146         tmp_lqm=(char **) MALLOC(status.N_NODES*sizeof(char*));
00147         tree=(char *) MALLOC(status.N_NODES*sizeof(char));
00148         tree_bak = (char *) MALLOC(status.N_NODES*sizeof(char));
00149 
00150         for (i=0;i<status.N_NODES;i++){
00151                 tmp_lqm[i]=(char *) MALLOC(status.N_NODES*sizeof(char));
00152         }
00153 }
00154 
00155 void tree_free(void){
00156    int i;
00157 
00158    for (i=0;i<status.N_NODES;i++){
00159       FREE(tmp_lqm[i]);
00160    }
00161    FREE(tmp_lqm);
00162    FREE(tree_bak);
00163    FREE(tree);
00164 }
00165 
00166 void tree_set_next_tree(char * cp){
00167         memcpy(tree,cp,status.N_NODES);
00168 }
00169 char * tree_next_tree_get_ptr(void){
00170         return tree;
00171 }
00172 
00173 char * tree_get_saved_tree(void){
00174         return tree_bak;
00175 }
00176 
00177 void tree_get_next_tree(char * cp){
00178         int i;
00179         tree_create_lqm_from_tree(tmp_lqm,cp);
00180         print_matrix("before all",tmp_lqm);
00181         for (i=0;i<status.N_NODES;i++){
00182                 tmp_lqm[status.id][i]=rssi_get_averaged_rssi(i);
00183                 tmp_lqm[i][status.id]=rssi_get_averaged_rssi(i);
00184         }
00185         print_matrix("before prim",tmp_lqm);
00186         char ** improved_lqm=prim(tmp_lqm);
00187         print_matrix("after prim",improved_lqm);
00188         tree_create_tree_from_lqm(tree,improved_lqm);
00189         memcpy(cp,tree,status.N_NODES);
00190         print_vector("ap",cp);
00191 }
00192 
00193 int tree_which_best(char * t1, char * t2) {
00194         int broken1 = 0, broken2 = 0, i;
00195         for (i = 0; i < status.N_NODES; i++) {
00196                 if (t1[i] < 0){
00197                         broken1++;
00198                 }
00199                 if (t2[i] < 0){
00200                         broken2++;
00201                 }
00202         }
00203         if (broken1 < broken2) {
00204                 return 1;
00205         } else {
00206                 return 2;
00207         }
00208 }
00209 
00210 int tree_get_tree_path(char ** lqm, int id, char * path) {
00211         char reached[32];
00212         int path_idx = 1, path_pnt = 1, sum = 0, i, done;
00213         memset(reached, 0, sizeof(reached));
00214 
00215         path[0] = id;
00216         reached[id] = 1;
00217 
00218         while (sum < status.N_NODES) {
00219                 done = 0;
00220                 for (i = 0; i < status.N_NODES; i++) {
00221                         if (lqm[id][i] && !reached[i]) {
00222                                 id = i;
00223                                 done = 1;
00224                                 reached[id] = 1;
00225                                 path[path_idx] = id;
00226                                 path_idx++;
00227                                 path_pnt++;
00228                                 break;
00229                         }
00230                 }
00231                 if (!done) {
00232                         path_pnt -= 1;
00233                         int nid = path[path_pnt];
00234                         if (nid != id) {
00235                                 path[path_idx] = nid;
00236                                 path_idx++;
00237                         }
00238                         id = nid;
00239                 }
00240 
00241                 sum = 0;
00242                 for (i = 0; i < status.N_NODES; i++) {
00243                         sum += reached[i];
00244                 }
00245         }
00246 
00247 //      for (i = 0; i < path_idx; i++) {
00248 //              fprintf(stderr, "%d ", path[i]);
00249 //      }
00250         return path_idx;
00251 }


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Fri Jan 3 2014 12:07:56