Institut de Robòtica i Informàtica Industrial
KRD Group

The CuikSuite Project

box_list.c

Go to the documentation of this file.
00001 #include "box_list.h"
00002 
00003 #include "interval.h"
00004 
00005 #include "boolean.h"
00006 #include "defines.h"
00007 
00008 #include <stdio.h>
00009 #include <stdlib.h>
00010 #include <string.h>
00011 
00021 /*
00022  * Starts a list of boxes
00023  */
00024 void InitListOfBoxes(Tlist *l)
00025 {
00026   InitList(sizeof(Tbox),l);
00027 }
00028 
00029 void CopyListOfBoxes(Tlist *l_dst,Tlist *l_src)
00030 {
00031   Titerator i;
00032   Tbox b;
00033 
00034   InitListOfBoxes(l_dst);
00035 
00036   InitIterator(&i,l_src);
00037   First(&i);  
00038   while(!EndOfList(&i))
00039     {
00040       CopyBox(&b,(Tbox *)GetCurrent(&i));
00041       AddLastElement((void *)&b,l_dst);
00042       Advance(&i);
00043     }
00044 }
00045 
00046 /*
00047  * Returns the total volume of the box stored in the list
00048  * (normalized using t_trans and t_rot)
00049  */
00050 double ListOfBoxesVolume(boolean *used,Tlist *l)
00051 {
00052   double v;
00053   Titerator i;
00054 
00055   InitIterator(&i,l);
00056   First(&i); 
00057   v=0.0;
00058   while(!EndOfList(&i))
00059     {
00060       v+=GetBoxVolume(used,(Tbox *)GetCurrent(&i));
00061       Advance(&i);
00062     }
00063 
00064   return(v);
00065 }
00066 
00067 /*
00068  * Returns the maximum diagonal for all boxes in the list
00069  */
00070 double ListOfBoxesMaxDiagonal(boolean *used,Tlist *l)
00071 {
00072   double d,d_max;
00073   Titerator i;
00074 
00075   InitIterator(&i,l);
00076   First(&i);
00077   d_max=0.0;
00078   while(!EndOfList(&i))
00079     {
00080       d=GetBoxDiagonal(used,(Tbox *)GetCurrent(&i));
00081       if (d>d_max) d_max=d;
00082       Advance(&i);
00083     }
00084 
00085   return(d_max);
00086 }
00087 
00088 /*
00089  * Returns the maximum size for all boxes in the list
00090  */
00091 double ListOfBoxesMaxSize(boolean *used,Tlist *l)
00092 {
00093   double s,s_max;
00094   Titerator i;
00095 
00096   InitIterator(&i,l);
00097   First(&i);
00098   s_max=0.0;
00099   while(!EndOfList(&i))
00100     {
00101       s=GetBoxSize(used,(Tbox *)GetCurrent(&i));
00102       if (s>s_max) s_max=s;
00103       Advance(&i);
00104     }
00105 
00106   return(s_max);
00107 }
00108 
00109 /*
00110  * Returns a box that fully includes all boxes in the list.
00111  * If the list is empty so is the box.
00112  */
00113 void ListOfBoxesBB(boolean *used,Tbox *b,Tlist *l)
00114 {
00115   if (ListSize(l)==0)
00116     InitBox(0,NULL,b);
00117   else
00118     {
00119         Titerator i;
00120 
00121         InitIterator(&i,l);
00122         First(&i);
00123         CopyBox(b,(Tbox *)GetCurrent(&i));
00124         Advance(&i);  
00125         while(!EndOfList(&i))
00126           {
00127             BoxUnion(used,(Tbox *)GetCurrent(&i),b,b);
00128             Advance(&i);
00129           }
00130     }
00131 }
00132 
00133 
00134 /*
00135  * Returns a list with the bounding boxes for all clusters of boxes
00136  * in l_in. 
00137  */
00138 void ListOfBoxesCluster(boolean *used,Tlist *l_out,Tlist *l_in)
00139 {
00140   unsigned int k,n,l,t;
00141   Titerator i;
00142   Titerator j;
00143   Titerator c;
00144   Tbox *b2,*b3,b_out,*cb;
00145   boolean modified;
00146   unsigned int *cluster,nc;
00147 
00148   /*Number of boxes to be clustered*/
00149   n=ListSize(l_in);
00150 
00151  /*Initially we have no cluster*/
00152   NEW(cluster,n,unsigned int);
00153   nc=0;
00154   for(k=0;k<n;k++)
00155     cluster[k]=(unsigned int)(-1);
00156 
00157   /*Initially the output is empty*/
00158   InitListOfBoxes(l_out);
00159 
00160   /*Proceed for all boxes in the input list not yet clustered*/
00161   InitIterator(&i,l_in);
00162   First(&i);
00163   k=0;
00164   while(!EndOfList(&i))
00165     {
00166       if (cluster[k]==(unsigned int)(-1)) /*= not clustered*/
00167         {
00168           /*Start a new cluster*/
00169           nc++;
00170           cluster[k]=nc; 
00171           NEW(cb,1,Tbox);
00172           CopyBox(cb,(Tbox *)GetCurrent(&i)); /*'cb' is the bounding box for all boxes in the cluster*/
00173 
00174           do {
00175 
00176             /*For all boxes still to be clustered*/
00177             modified=FALSE; /*Initally we assume the cluster to remain as it is now*/
00178 
00179             /*and we check for the rest of boxes in the input list whether or not they
00180               can be included in the current cluster*/
00181             InitIterator(&j,l_in);
00182             First(&j);
00183             l=0;
00184             while(!EndOfList(&j))
00185               {
00186                 if (cluster[l]==(unsigned int)(-1)) /*the box in not yet assigned to any cluster*/
00187                   {
00188                     b2=(Tbox *)GetCurrent(&j); /*this is the box candidate to joint the current cluster*/
00189 
00190                     /*Check if the non-clustered box has contact with one of the
00191                       boxes already in cluster 'nc'=cluster[k]*/
00192                     InitIterator(&c,l_in);
00193                     First(&c);
00194                     t=0;
00195                     while(!EndOfList(&c))
00196                       {
00197                         if (cluster[t]==cluster[k])
00198                           {
00199                             b3=(Tbox *)GetCurrent(&c); /*This is a box already in cluster 'nc'*/
00200                             CopyBox(&b_out,b3);
00201                             if (BoxesIntersection(used,b2,b3,&b_out))
00202                               {
00203                                 cluster[l]=nc; /*box b2 (number 'l') is included in the current cluster*/
00204                                 BoxUnion(NULL,cb,b2,cb); /*Enlarge the cluster bounding box*/
00205 
00206                                 modified=TRUE; /*Indicate the current cluster has been modified*/
00207                               }
00208                             DeleteBox(&b_out); /*bout is an auxiliary box that must be deleted*/
00209                           }
00210                         Advance(&c);
00211                         t++;
00212                       }
00213                   }
00214                 Advance(&j);
00215                 l++;
00216               }
00217           } while(modified); /* If we added one or more boxex to the cluster we have to check whether
00218                                 or not the cluster can be still extended*/
00219 
00220 
00221           AddLastElement((void *)cb,l_out); /*Add the bounding box of the current cluster to the output list*/
00222         }
00223       Advance(&i);
00224       k++;
00225     }
00226   free(cluster);
00227 }
00228 
00229 
00230 void ConcatListOfBoxes(Tlist *l_new,Tlist *l)
00231 {
00232   Titerator i;
00233   Tbox b;
00234 
00235   InitIterator(&i,l_new);
00236   First(&i);
00237   while(!EndOfList(&i))
00238     { 
00239       CopyBox(&b,(Tbox *)GetCurrent(&i));
00240       AddLastElement((void *)&b,l);
00241       Advance(&i);
00242     }
00243 }
00244 
00245 void ReverseListOfBoxes(Tlist *l_orig,Tlist *l)
00246 {
00247   Titerator i;
00248   Tbox b;
00249 
00250   InitListOfBoxes(l);
00251 
00252   InitIterator(&i,l_orig);
00253   First(&i);
00254   while(!EndOfList(&i))
00255     { 
00256       CopyBox(&b,(Tbox *)GetCurrent(&i));
00257       AddFirstElement((void *)&b,l);
00258       Advance(&i);
00259     }
00260 }
00261 
00262 /*
00263  * Prints a given list of boxes
00264  */
00265 void PrintListOfBoxes(FILE *f,boolean *used,char *heading,Tlist *l)
00266 {
00267   Titerator i;
00268   unsigned int k;
00269 
00270   InitIterator(&i,l);
00271   First(&i);
00272   k=1;
00273   while(!EndOfList(&i))
00274     {          
00275       if (heading!=NULL) fprintf(f,"%s ",heading);
00276       fprintf(f,"%u : ",k++);
00277       PrintBoxSubset(f,used,NULL,(Tbox *)GetCurrent(&i));
00278       Advance(&i);
00279     }
00280   fflush(f);
00281 }
00282 
00283 /*
00284  * Read a list of boxes from a file (wrote by cuik).
00285  */
00286 boolean ReadListOfBoxes(char *filename,Tlist *l)
00287 {
00288   FILE *PSin;
00289   int token=~EOF; 
00290   Tbox *box;
00291 
00292   InitList(sizeof(Tbox),l);
00293 
00294   PSin=fopen(filename,"r");
00295   if (PSin)
00296     {
00297       do {
00298         //read a box from the file
00299         NEW(box,1,Tbox);
00300         token=ReadBox(PSin,box);
00301         //if we got a proper box (no end of file)...
00302         if (token!=EOF)
00303           {
00304             //and add the box to the list
00305             AddLastElement((void *)box,l);
00306           }
00307       } while (token!=EOF);
00308       return(TRUE);
00309     }
00310   else
00311     return(FALSE);
00312 }
00313 
00314 /* Now the binary alternatives to save/load boxes */
00315 void SaveListOfBoxes(FILE *f,Tlist *list)
00316 {
00317   unsigned int n;
00318   Titerator i;
00319 
00320   n=ListSize(list);
00321   fwrite(&n,sizeof(unsigned int),1,f);
00322 
00323   InitIterator(&i,list);
00324   First(&i);
00325   n=0;
00326   while(!EndOfList(&i))
00327     {
00328       SaveBox(f,(Tbox *)GetCurrent(&i));
00329       Advance(&i);
00330     }
00331 }
00332 
00333 void LoadListOfBoxes(FILE *f,Tlist *list)
00334 {
00335   unsigned int i,n;
00336   Tbox box;
00337 
00338   fread(&n,sizeof(unsigned int),1,f);
00339 
00340   InitListOfBoxes(list);
00341 
00342   for(i=0;i<n;i++)
00343     {
00344       LoadBox(f,&box);
00345       AddLastElement((void *)&box,list);
00346     }
00347 }
00348 
00349 /*
00350  * Deletes a given list of boxes
00351  */
00352 void DeleteListOfBoxes(Tlist *l)
00353 {
00354   Titerator i;
00355 
00356   InitIterator(&i,l);
00357   First(&i);
00358   while(!EndOfList(&i))
00359     {
00360       DeleteBox((Tbox *)GetCurrent(&i));
00361       DeleteCurrent(&i);
00362     }
00363 }
00364