Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "config/compiler.h"
00038 #include "include/wmp_utils.h"
00039
00040 #define DBL_MAX 2^31
00041
00042 static char **tree;
00043 static int **graph;
00044 static char *vertexOnTree,N_ROBOTS;
00045
00046 void init_prim(int nnodes){
00047 int i;
00048 N_ROBOTS=nnodes;
00049 graph=(int **) MALLOC(N_ROBOTS*sizeof(int*));
00050 tree=(char **) MALLOC(N_ROBOTS*sizeof(char*));
00051
00052 vertexOnTree=(char *) MALLOC(N_ROBOTS*N_ROBOTS*sizeof(char));
00053 for (i=0;i<N_ROBOTS;i++){
00054 graph[i]=(int *) MALLOC(N_ROBOTS*sizeof(int));
00055 tree[i]=(char *) MALLOC(N_ROBOTS*sizeof(char));
00056 }
00057 }
00058
00059 void free_prim(void){
00060 int i;
00061
00062 for (i=0;i<N_ROBOTS;i++){
00063 FREE(graph[i]);
00064 FREE(tree[i]);
00065 }
00066 FREE(vertexOnTree);
00067 FREE(tree);
00068 FREE(graph);
00069 }
00070
00071
00072 char ** prim(char **lqm){
00073 int i,j, numVertexOnTree;
00074 char val;
00075 int fail = 0;
00076 for (i=0;i<N_ROBOTS;i++){
00077 for (j=0;j<N_ROBOTS;j++){
00078 tree[i][j]=0;
00079 val = lqm[i][j] < lqm[j][i] ? lqm[i][j]:lqm[j][i];
00080
00081 if (val > 0){
00082 graph[i][j]=100 - val;
00083 graph[j][i]=100 - val;
00084 } else{
00085 graph[i][j]=DBL_MAX;
00086 graph[j][i]=DBL_MAX;
00087 }
00088 WMP_DBG(PRIM,"lqm: %d :%d :%d %f\n",lqm[i][j],i,j,graph[i][j]);
00089 }
00090 }
00091
00092 numVertexOnTree=1;
00093 vertexOnTree[0]=0;
00094 while (numVertexOnTree < N_ROBOTS){
00095 int minWeight=DBL_MAX, v;
00096
00097 int minWeightIdxi=-1;
00098 int minWeightIdxj=-1;
00099 int i,j;
00100 for (i=0;i<numVertexOnTree;i++){
00101 int vertex=vertexOnTree[i];
00102 for (j=0;j<N_ROBOTS;j++){
00103 if (vertex==j) {
00104 continue;
00105 }
00106
00107 if (graph[vertex][j] <= minWeight){
00108 minWeight=graph[vertex][j];
00109 minWeightIdxi=vertex;
00110 minWeightIdxj=j;
00111 }
00112 }
00113 }
00114
00115 if (minWeightIdxi < 0 || minWeightIdxj < 0){
00116 static char txt[1000];
00117 lqmToString(lqm,txt, N_ROBOTS);
00118 WMP_DBG(PRIM,"PRIM Failed, LQM:\n %s\n",txt);
00119 fail = 1;
00120 break;
00121 }
00122 v=lqm[minWeightIdxi][minWeightIdxj];
00123 if (v>0) {
00124 v=99;
00125 } else {
00126 v=0;
00127 }
00128 tree[minWeightIdxi][minWeightIdxj]=v;
00129 tree[minWeightIdxj][minWeightIdxi]=v;
00130 for (i=0;i<numVertexOnTree;i++){
00131 graph[(int)vertexOnTree[i]][minWeightIdxj]=DBL_MAX;
00132 graph[minWeightIdxj][(int)vertexOnTree[i]]=DBL_MAX;
00133 }
00134 vertexOnTree[numVertexOnTree]=minWeightIdxj;
00135 numVertexOnTree++;
00136 }
00137 if (!fail){
00138 return tree;
00139 } else {
00140 return lqm;
00141 }
00142 }
00143
00144
00145
00146
00147
00148