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

The CuikSuite Project

box.c

Go to the documentation of this file.
00001 #include "box.h"
00002 
00003 #include "defines.h"
00004 #include "error.h"
00005 
00006 #include <string.h>
00007 #include <stdlib.h>
00008 #include <math.h>
00009 
00010 
00019 /*
00020  * Inits an empty box in a  'dim' dimensional space
00021  */
00022 void InitBox(unsigned int dim,Tinterval *is,Tbox *b)
00023 { 
00024   b->level=1; /* Default level */
00025   b->n=dim;
00026   if (b->n==0)
00027     b->is=NULL;
00028   else
00029     {
00030       NEW(b->is,b->n,Tinterval);
00031       if (is!=NULL)
00032         memcpy(b->is,is,b->n*sizeof(Tinterval));
00033       else
00034         {
00035           unsigned int i;
00036           for(i=0;i<dim;i++)
00037             NewInterval(0.0,0.0,&(b->is[i]));
00038         }
00039     }
00040 }
00041 
00042 unsigned int GetBoxBufferSize(Tbox *b)
00043 {
00044   return(b->n*2+4);
00045 }
00046 
00047 void Box2Buffer(unsigned int c,unsigned int n,double *buffer,Tbox *b)
00048 {
00049   unsigned int k,i;
00050 
00051   /* Set the error code (from child to scheduler) */
00052   switch(c)
00053     {
00054     case EMPTY_BOX:
00055       buffer[0]=-1.0; /*and in the other direction, the first double is also used to send output messages to the scheduler*/
00056       break;
00057       
00058     case ERROR_IN_PROCESS:
00059       buffer[0]=-2.0; 
00060       break;
00061       
00062     case REDUCED_BOX_WITH_SOLUTION:
00063       buffer[0]=1.0;
00064       break;
00065 
00066     case REDUCED_BOX:
00067       buffer[0]=0.0;
00068       break;
00069 
00070     default:
00071       Error("Unknown reduceBox output in Box2Buffer");
00072       break;
00073     }
00074   
00075   /* Set 'n' (this is used to send info from scheduler to childs and back)*/
00076   buffer[1]=(double)n;
00077 
00078   buffer[2]=(double)b->level;
00079   buffer[3]=(double)b->n;
00080   k=4;
00081   for(i=0;i<b->n;i++)
00082     {
00083       buffer[k++]=LowerLimit(&(b->is[i]));
00084       buffer[k++]=UpperLimit(&(b->is[i]));
00085     }
00086 }
00087 
00088 void Buffer2Box(unsigned int *c,unsigned int *n,double *buffer,Tbox *b)
00089 {
00090   unsigned int i,k;
00091 
00092   if (buffer[0]<-1.5)
00093     *c=ERROR_IN_PROCESS;
00094   else
00095     {
00096       if (buffer[0]<-0.5)
00097         *c=EMPTY_BOX;
00098       else
00099         {
00100           if (buffer[0]>0.5)
00101             *c=REDUCED_BOX_WITH_SOLUTION;
00102           else
00103             *c=REDUCED_BOX;
00104         } 
00105     }
00106   
00107   *n=(unsigned int)(buffer[1]);
00108 
00109   b->level=(unsigned int)(buffer[2]);
00110 
00111   if ((unsigned int)(buffer[3])!=b->n)
00112     Error("Box and buffer have different size");
00113   
00114   k=4;
00115   for(i=0;i<b->n;i++)
00116     {
00117       NewInterval(buffer[k],buffer[k+1],&(b->is[i]));
00118       k+=2;
00119     }
00120 }
00121 
00122 /*
00123  * Copies b_in into b_out
00124  * b_out is supposed initiallized
00125  */
00126 void CopyBox(void *b_out,void *b_in)
00127 {
00128   ((Tbox *)b_out)->n=((Tbox *)b_in)->n;
00129   ((Tbox *)b_out)->level=((Tbox *)b_in)->level;
00130   NEW(((Tbox *)b_out)->is,((Tbox *)b_out)->n,Tinterval);
00131   if (((Tbox *)b_out)->n==0)
00132     ((Tbox *)b_out)->is=NULL;
00133   else
00134     memcpy(((Tbox *)b_out)->is,((Tbox *)b_in)->is,((Tbox *)b_out)->n*sizeof(Tinterval));
00135 }
00136 
00137 /*
00138  * Copies b_in into b_out for some selected variables
00139  * b_out is supposed initiallized
00140  */
00141 void CopyBoxSubset(boolean *used,void *b_out,void *b_in)
00142 {
00143   unsigned int i,n,k;
00144   
00145   if (used==NULL)
00146     n=((Tbox *)b_in)->n;
00147   else
00148     {
00149       n=0;
00150       for(i=0;i<((Tbox *)b_in)->n;i++)
00151         if (used[i]) n++;
00152     }
00153     
00154 
00155   ((Tbox *)b_out)->n=n;
00156   ((Tbox *)b_out)->level=((Tbox *)b_in)->level;
00157   NEW(((Tbox *)b_out)->is,n,Tinterval);
00158   k=0;
00159   for (i=0;i<((Tbox *)b_in)->n;i++)
00160     {
00161       if ((used==NULL)||(used[i]))
00162         {
00163           CopyInterval(&(((Tbox *)b_out)->is[k]),&(((Tbox *)b_in)->is[i]));
00164           k++;
00165         }
00166     }
00167 }
00168 
00169 void SetBoxIntervals(Tinterval *is,Tbox *b)
00170 {
00171   if (b->n>0)
00172     memcpy(b->is,is,b->n*sizeof(Tinterval));
00173 }
00174 
00175 void SetBoxInterval(unsigned int n,Tinterval *is,Tbox *b)
00176 {
00177   if (n<b->n)
00178     CopyInterval(&(b->is[n]),is);
00179   else
00180     Error("Index out of range in SetBoxInterval");
00181 }
00182 
00183 /*
00184  * Returns a pointer to the interval number 'n' of box 'b'
00185  */
00186 Tinterval *GetBoxInterval(unsigned int n, /*number of the interval to be obtained*/
00187                           Tbox *b         /*Box*/
00188                           )
00189 {
00190   if (n<b->n)
00191     return(&(b->is[n]));
00192   else
00193     Error("Index out of range in GetBoxInterval");
00194   return(NULL);
00195 }
00196 
00197 /*
00198  * Returns a pointer to the intervals stored in the box
00199 */
00200 Tinterval *GetBoxIntervals(Tbox *b)
00201 {
00202   return(b->is);
00203 }
00204 
00205 /*
00206  * bout will be the intersection of b1 and b2. If the boxes do not
00207  * intersect we return FALSE. We assumbe bout to be properly initialized
00208  */
00209 boolean BoxesIntersection(boolean *used,Tbox *b1,Tbox *b2,Tbox *bout)
00210 {
00211   unsigned int i;
00212   boolean boxes_intersect=TRUE;
00213 
00214   if (b1->n!=b2->n)
00215     Error("Input boxes with different size in BoxIntersection");
00216   
00217   for(i=0;((boxes_intersect)&&(i<b1->n));i++)
00218     {
00219       if ((used==NULL)||(used[i]))
00220         boxes_intersect = Intersection(&(b1->is[i]),&(b2->is[i]),&(bout->is[i]));
00221     }
00222 
00223   return(boxes_intersect);
00224 }
00225 
00226 /*
00227  * Returns a box that fully includes b_out and b.
00228  * Like in BoxesIntersection, we assume b_out to be properly initilized
00229  */
00230 void BoxUnion(boolean *used,Tbox *b1,Tbox *b2,Tbox *b_out)
00231 {
00232   unsigned int i;
00233 
00234   if (b1->n!=b2->n)
00235     Error("Input boxes with different size in BoxUnion");
00236 
00237   for(i=0;i<b1->n;i++)
00238     {
00239       if ((used==NULL)||(used[i]))
00240         Union(&(b1->is[i]),&(b2->is[i]),&(b_out->is[i]));
00241     }
00242 }
00243 
00244 
00245 /*
00246  * Returns TRUE if point v is in box b
00247  */
00248 boolean PointInBox(boolean *used,unsigned int n,double *v,Tbox *b)
00249 {
00250   unsigned int i;
00251   boolean in;
00252 
00253   if (n!=b->n)
00254     Error("Point and box of different dimensionality in PointInBox");
00255 
00256   in=TRUE;
00257   for(i=0;((in)&&(i<n));i++)
00258     {
00259       if ((used==NULL)||(used[i]))
00260         in=IsInside(v[i],&(b->is[i]));
00261     }
00262 
00263   return(in);
00264 }
00265 
00266 /*
00267   'used' has as many elements as ranges in 'b' but 'v' only
00268   has as many values as TRUE elements in 'used'
00269 */
00270 boolean PointInSubBox(boolean *used,Tvector *v,Tbox *b)
00271 {
00272   unsigned int i,k;
00273   boolean in;
00274 
00275   in=TRUE;
00276   k=0;
00277   for(i=0;((in)&&(i<b->n));i++)
00278     {
00279       if ((used==NULL)||(used[i]))
00280         {
00281           in=IsInside(*(double *)GetVectorElement(k,v),&(b->is[i]));
00282           k++;
00283         }
00284     }
00285 
00286   return(in);
00287 }
00288 
00289 /* Computes the distance of a point to the center of a sub-box.
00290    The ranges defining the sub-box are defined by "used" */
00291 double DistanceToSubBoxCenter(boolean *used,Tvector *v,Tbox *b)
00292 {
00293   unsigned int i,k;
00294   double d,d1;
00295 
00296   d=0.0;
00297   k=0;
00298   for(i=0;i<b->n;i++)
00299     {
00300       if ((used==NULL)||(used[i]))
00301         {
00302           d1=(*(double *)GetVectorElement(k,v))-IntervalCenter(&(b->is[i]));
00303           d+=d1*d1;
00304           k++;
00305         }
00306     }
00307 
00308   return(sqrt(d));
00309 }
00310 
00311 /*
00312  * Returns TRUE if the box is basically a point, i.e., if all
00313  * intervals are smaller than 'epsilon'
00314  */
00315 boolean IsPunctualBox(boolean *used,double epsilon,Tbox *b)
00316 {
00317   unsigned int i;
00318   boolean point;
00319 
00320   i=0;
00321   point=TRUE;
00322   while((i<b->n)&&(point))
00323     { 
00324       if ((used==NULL)||(used[i]))
00325         point=(IntervalSize(&(b->is[i]))<=epsilon);
00326       i++;
00327     }
00328 
00329   return(point);
00330 }
00331 
00332 /*
00333  * Returns TRUE if the b1 is fully included in b2
00334  */
00335 boolean BoxInclusion(boolean *used,Tbox *b1,Tbox *b2)
00336 {
00337   unsigned int i;
00338   boolean included;
00339 
00340   i=0;
00341   included=(b1->n==b2->n);
00342 
00343   while((i<b1->n)&&(included))
00344     { 
00345       if ((used==NULL)||(used[i]))
00346         included=IntervalInclusion(&(b1->is[i]),&(b2->is[i]));
00347       i++;
00348     }
00349 
00350   return(included); 
00351 }
00352 
00353 /*
00354  * Returns the size of box 'b'.
00355  */
00356 double GetBoxSize(boolean *used,Tbox *b          /*box to be measured*/
00357                   )
00358 {
00359   if (b->n==0)
00360     return(0);
00361   else
00362     return(IntervalSize(&(b->is[GetBoxMaxDim(used,b)])));
00363 }
00364 
00365 double GetBoxMaxSizeVarSet(Tvariable_set *vars,Tbox *b)
00366 {
00367   unsigned int i,n,k;
00368   double s,smax;
00369   
00370   smax=0.0;
00371   n=VariableSetSize(vars);
00372   for(i=0;i<n;i++)
00373     {
00374       k=GetVariableN(i,vars);
00375       s=IntervalSize(&(b->is[k]));
00376       if (s>smax)
00377         smax=s;
00378     }
00379 
00380   return(smax);
00381 }
00382 
00383 double GetBoxMinSizeVarSet(Tvariable_set *vars,Tbox *b)
00384 {
00385   unsigned int i,n,k;
00386   double s,smin;
00387   
00388   smin=0.0;
00389   n=VariableSetSize(vars);
00390   for(i=0;i<n;i++)
00391     {
00392       k=GetVariableN(i,vars);
00393       s=IntervalSize(&(b->is[k]));
00394       if ((i==0)||(s<smin))
00395         smin=s;
00396     }
00397 
00398   return(smin);
00399 }
00400 
00401 /*
00402  * Returns the length of the diagonal of box 'b'.
00403  */
00404 double GetBoxDiagonal(boolean *used,Tbox *b          /*box to be measured*/
00405                       )
00406 {
00407   unsigned int i;
00408   double d,d1;
00409 
00410   d=0.0;
00411   for(i=0;i<b->n;i++)
00412     {
00413       if ((used==NULL)||(used[i]))
00414         {
00415           d1=IntervalSize(&(b->is[i]));
00416           d=d+(d1*d1);
00417         }
00418     }
00419 
00420   return(sqrt(d));
00421 }
00422 
00423 /*
00424  * Returns the level (or depth) of a given box. The level corresponds
00425  * to the depth in the search three at which the box is stored 
00426 */
00427 unsigned int GetBoxLevel(Tbox *b)
00428 {
00429   return(b->level);
00430 }
00431 
00432 /*
00433  * Returns the distance between the center of two boxes
00434  */
00435 
00436 double GetBoxCenterDistance(boolean *used,Tbox *b1,Tbox *b2)
00437 {
00438   double d,c1,c2,c;
00439   unsigned int i;
00440 
00441   if (b1->n!=b2->n)
00442     Error("Can not compute distance between different size boxes");
00443 
00444   d=0;
00445   for(i=0;i<b1->n;i++)
00446     {      
00447       if ((used==NULL)||(used[i]))
00448         {
00449           c1=IntervalCenter(&(b1->is[i]));
00450           c2=IntervalCenter(&(b2->is[i]));
00451           c=c1-c2;
00452           d+=(c*c);
00453         }
00454     }
00455 
00456   return(sqrt(d));
00457 }
00458 
00459 
00460 /*
00461  * Returns the normalized volume of a box
00462  */
00463 double GetBoxVolume(boolean *used,Tbox *b)
00464 {
00465   double volume;
00466   unsigned int i;
00467 
00468   volume=1.0;
00469   for(i=0;i<b->n;i++)
00470     {
00471       if ((used==NULL)||(used[i]))
00472         volume*=(IntervalSize(&(b->is[i])));
00473     }
00474 
00475   if (b->n==0)
00476     return(0);
00477   else
00478     return(volume);
00479 }
00480 
00481 unsigned int GetBoxNIntervals(Tbox *b)
00482 {
00483   return(b->n);
00484 }
00485 
00486 /*
00487  * Returns the dimension along which the box has its maximal size
00488  */
00489 unsigned int GetBoxMaxDim(boolean *used,Tbox *b)
00490 {
00491   unsigned int i;
00492   unsigned int i_max;
00493   double s;
00494   double s_max;
00495 
00496   i_max=0;
00497   s_max=0.0;
00498   for(i=0;i<b->n;i++)
00499     {
00500       if ((used==NULL)||(used[i]))
00501         {
00502           s=IntervalSize(&(b->is[i]));
00503           if (s>s_max)
00504             {
00505               s_max=s;
00506               i_max=i;
00507             }
00508         }
00509     }
00510   if (b->n==0)
00511     return(NO_UINT);
00512   else
00513     return(i_max);
00514 }
00515 
00516 /*
00517  * Returns a dimension along which the box has to be splitted
00518  */
00519 unsigned int GetBoxSplitDim(boolean *used,Tbox *b)
00520 {  
00521 
00522   return(GetBoxMaxDim(used,b));
00523 }
00524 
00525 /*
00526  * Produces two symetrical boxes from 'b' ('b1' and 'b2') that result from
00527  * spliting 'b' along th 'n' dimension.
00528  */
00529 void SplitBox(unsigned int n, /*dimension along which the box is splited*/
00530               double r,       /*r in [0,1] where to split*/
00531               Tbox *b1,       /*first resulting box*/
00532               Tbox *b2,       /*second resulting box*/
00533               Tbox *b         /*original box*/
00534               )
00535 {
00536   double lp,up,cp;
00537 
00538   if ((r>1.0)||(r<0.0))
00539     Error("Wrong size to split a box");
00540   if (n<b->n)
00541     {  
00542       /*b1,b2 created here*/
00543 
00544       #if (_DEBUG>6)
00545         printf("Splitting box along dimension %u\n",n);
00546       #endif
00547 
00548       b1->n=b->n;
00549       b2->n=b->n;
00550 
00551       NEW(b1->is,b1->n,Tinterval);/* Lower box */
00552       NEW(b2->is,b2->n,Tinterval);/* Upper box */
00553  
00554       memcpy(b1->is,b->is,b1->n*sizeof(Tinterval)); /*the sub-boxes are the same as the original*/
00555       memcpy(b2->is,b->is,b1->n*sizeof(Tinterval)); /*but for the split dimension*/
00556 
00557       lp=LowerLimit(&(b->is[n])); /*lower point*/
00558       up=UpperLimit(&(b->is[n])); /*upper point*/
00559       
00560 /*
00561       if ((lp<0.0)&&(up>0.0)) 
00562         cp=0.0; 
00563       else 
00564         cp=lp+(up-lp)*r;  
00565 */
00566       if ((lp==-INF)&&(up==INF))
00567         cp=0.0;
00568       else
00569         cp=lp+(up-lp)*r; 
00570 
00571       NewInterval(lp,cp,&(b1->is[n]));
00572       NewInterval(cp,up,&(b2->is[n]));
00573       
00574       b1->level=b2->level=b->level+1; /* the boxes resulting from a split are of the next level of the search */
00575     }
00576   else
00577     Error("Index out of range in SplitBox");
00578 }
00579 
00580 
00581 /*
00582  * Scales a box taking into account the original ranges for the variables
00583  */
00584 void ScaleBox(double max_upper,Tbox *b)
00585 {
00586   unsigned int i;
00587 
00588   for(i=0;i<b->n;i++)
00589     IntervalScale(&(b->is[i]),max_upper,&(b->is[i]));
00590 }
00591 
00592 boolean CmpBoxDepthFirst(void *b1,void *b2,void *userData)
00593 {
00594   return(((Tbox *)b1)->level>((Tbox *)b2)->level);
00595 }
00596 
00597 boolean CmpBoxBreadthFirst(void *b1,void *b2,void *userData)
00598 {
00599   return(((Tbox *)b1)->level<((Tbox *)b2)->level);
00600 }
00601 
00602 /*
00603  * Prints box 'b' on file 'f'
00604  */
00605 void PrintBox(FILE *f,Tbox *b)
00606 { 
00607   unsigned int i;
00608 
00609   fprintf(f,"{ %u  ",b->n);
00610   for(i=0;i<b->n;i++)
00611     {
00612       PrintInterval(f,&(b->is[i]));
00613       fprintf(f," ");
00614     }
00615 
00616   fprintf(f,"}\n");
00617   
00618 }
00619 
00620 /*
00621  * Prints box 'b' on file 'f' for some selected variables.
00622  * This is basically used when printing solutions.
00623  */
00624 void PrintBoxSubset(FILE *f,boolean *used,char **varNames,Tbox *b)
00625 { 
00626   unsigned int i,n;
00627   
00628   if (used==NULL)
00629     n=b->n;
00630   else
00631     {
00632       n=0;
00633       for(i=0;i<b->n;i++)
00634         if (used[i]) n++;
00635     }
00636     
00637   fprintf(f,"{ %u  ",n);
00638   for(i=0;i<b->n;i++)
00639     {
00640       if ((used==NULL)||(used[i]))
00641         {
00642           if (varNames!=NULL)
00643             fprintf(f,"%s:",varNames[i]);
00644           PrintInterval(f,&(b->is[i]));
00645           fprintf(f," ");
00646         }
00647     }
00648 
00649   fprintf(f,"}\n");
00650   
00651 }
00652 
00653 /*
00654  * Reads a box from an opened file. Returns EOF if the file finishes before
00655  * the box could be read
00656 */
00657 int ReadBox(FILE *f,Tbox *b)
00658 {
00659   int token=~EOF;
00660   Tinterval *is;
00661   double i_min,i_max;
00662   unsigned int i;
00663   unsigned int n;
00664 
00665   do token=fgetc(f); while ((token!=EOF)&&(token!='{')); /* skip everything till the beginning of the box */
00666 
00667   if (token!=EOF)
00668     {
00669       fscanf(f,"%u",&n);
00670  
00671       NEW(is,n,Tinterval);
00672       
00673       /* Check whether we have a box with some intervals or an empty box */
00674       do token=fgetc(f); while ((token!=EOF)&&(token!='[')&&(token!='}'));
00675       if (token=='}')
00676         {
00677           /* We have an empty box (the old type of box files). Skip the box and advance until the
00678              first interval of the next box (or end of file) */
00679           do token=fgetc(f); while ((token!=EOF)&&(token!='['));
00680         }
00681       i=0;
00682       while ((token!=EOF)&&(token!='}')) //while the box is not closed (and there is something in the file)
00683         {
00684           if (token=='[') //if the interval exists
00685             {
00686               //read the extremes of the interval
00687               if (fscanf(f,"%lf",&i_min)!=EOF)
00688                 {
00689                   do token=fgetc(f); while ((token!=EOF)&&(token!=','));
00690                   if (fscanf(f,"%lf",&i_max)!=EOF)  
00691                     {               
00692                       //if there was no problem reading the extremes set up a new interval
00693                       NewInterval(i_min,i_max,&(is[i++]));
00694                       
00695                       if (i>n)
00696                         Error("Too many intervals in the box");
00697                     }
00698                   else
00699                     token=EOF;
00700                 }
00701             }
00702           //go to the begining of next interval (if it exists)
00703           do token=fgetc(f);  while ((token!=EOF)&&(token!='[')&&(token!='}'));
00704 
00705         }
00706 
00707       if (i<n)
00708         Error("Too few intervals in the box");
00709 
00710       if (token!=EOF)  //if the box was read correctly
00711         InitBox(n,is,b); /*and set the new ones*/
00712       free(is);
00713     }
00714   
00715   return(token);
00716 }
00717 
00718 /*Saves in binary format*/
00719 void SaveBox(FILE *f,Tbox *b)
00720 {
00721   fwrite(&(b->n),sizeof(unsigned int),1,f);
00722   fwrite(&(b->level),sizeof(unsigned int),1,f);
00723   if (b->n>0)
00724     fwrite(b->is,sizeof(Tinterval),b->n,f);
00725 }
00726 
00727 void LoadBox(FILE *f,Tbox *b)
00728 {
00729   fread(&(b->n),sizeof(unsigned int),1,f);
00730   fread(&(b->level),sizeof(unsigned int),1,f);
00731 
00732   if (b->n==0)
00733     b->is=NULL;
00734   else
00735     {
00736       NEW(b->is,b->n,Tinterval);
00737       fread(b->is,sizeof(Tinterval),b->n,f);
00738     }
00739 }
00740 
00741 /*
00742  * Deletes the internal data of a box
00743  */
00744 void DeleteBox(void *b)
00745 {
00746   free(((Tbox *)b)->is);
00747 }