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

The CuikSuite Project

csmapping.c

Go to the documentation of this file.
00001 #include "csmapping.h"
00002 #include "defines.h"
00003 
00004 #include <math.h>
00005 
00014 void InitMapping(Tvariables *o,Tvariables *s,Tmapping *m)
00015 {
00016   unsigned int i,vID;
00017 
00018   NEW(m->original,1,Tvariables);
00019   CopyVariables(m->original,o);
00020   NEW(m->simple,1,Tvariables);
00021   CopyVariables(m->simple,s);
00022 
00023   m->sizeO=NVariables(o); /* The number of variables is cached */
00024   m->sizeS=NVariables(s);
00025 
00026   /*
00027     Some of the variables in the simple system are included in the original one.
00028     This is a one to one relation and, thus, it can be straighforwardly defined.
00029     However this is of little use (only used in GetVarIDInOriginal)
00030   */
00031   NEW(m->orig,m->sizeS,unsigned int);
00032 
00033   for(i=0;i<m->sizeS;i++)
00034     {
00035       /* this can be NO_UINT if the variable in the simplified system is not
00036          in the original one (ex. dummies!!)*/
00037       m->orig[i]=GetVariableID(GetVariableName(GetVariable(i,s)),o);
00038     }
00039 
00040   /* 
00041      The variables in the original system can be not linked to any variable in the
00042      simple system or be linearly related with a variable in the simple system.
00043      Observe that identity (copy) is a form of linear relation (with ct 1 and offest 0).
00044      Only this last relation (identity) can be automatically detected and set.
00045      Other type of relations have to be set by upper levels (using 'SetOriginalVarRelation').
00046   */
00047   NEW(m->related,m->sizeO,boolean);
00048   NEW(m->lc,m->sizeO,TLinearConstraint);
00049 
00050   for(i=0;i<m->sizeO;i++)
00051     {
00052       vID=GetVariableID(GetVariableName(GetVariable(i,o)),s);
00053 
00054       m->related[i]=(vID!=NO_UINT);
00055       InitLinearConstraint(&(m->lc[i]));
00056       if (m->related[i])
00057         {
00058           /* Var 'i' in the original system is var vID in the simplified one */
00059           AddTerm2LinearConstraint(vID,1,&(m->lc[i]));
00060         }
00061     }
00062 }
00063 
00064 void CopyMapping(Tmapping *m_dst,Tmapping *m_src)
00065 {
00066   unsigned int i;
00067 
00068   if (m_src->original==NULL)
00069     m_dst->original=NULL;
00070   else
00071     {
00072       NEW(m_dst->original,1,Tvariables);
00073       CopyVariables(m_dst->original,m_src->original);
00074     }
00075 
00076   if (m_src->simple==NULL)
00077     m_dst->simple=NULL;
00078   else
00079     {
00080       NEW(m_src->simple,1,Tvariables);
00081       CopyVariables(m_dst->simple,m_src->simple);
00082     }
00083 
00084   m_dst->sizeO=m_src->sizeO;
00085   m_dst->sizeS=m_src->sizeS;
00086 
00087   NEW(m_dst->orig,m_dst->sizeS,unsigned int);
00088   for(i=0;i<m_src->sizeS;i++)
00089     m_dst->orig[i]=m_src->orig[i];
00090 
00091   NEW(m_dst->related,m_dst->sizeO,boolean);
00092   NEW(m_dst->lc,m_dst->sizeO,TLinearConstraint);
00093   for(i=0;i<m_src->sizeO;i++)
00094     {
00095       m_dst->related[i]=m_src->related[i];
00096       CopyLinearConstraint(&(m_dst->lc[i]),&(m_src->lc[i]));
00097     }
00098 }
00099 
00100 void SetOriginalVarRelation(unsigned int nvo,TLinearConstraint *lc,Tmapping *m)
00101 {
00102   if (nvo<m->sizeO)
00103     {
00104       m->related[nvo]=TRUE;
00105       CopyLinearConstraint(&(m->lc[nvo]),lc);
00106     }
00107   else
00108     Error("Var index out of range in SetOriginalVarRelation");
00109 }
00110 
00111 void GetOriginalVarRelation(unsigned int nvo,TLinearConstraint *lc,Tmapping *m)
00112 {
00113   if (m->related[nvo])
00114     CopyLinearConstraint(lc,&(m->lc[nvo]));
00115   else
00116     InitLinearConstraint(lc);
00117 }
00118 
00119 unsigned int GetSizeOriginal(Tmapping *m)
00120 {
00121   return(m->sizeO);
00122 }
00123 
00124 unsigned int GetSizeSimple(Tmapping *m)
00125 {
00126   return(m->sizeS);
00127 }
00128 
00129 unsigned int GetVarIDInOriginal(unsigned int v,Tmapping *m)
00130 {
00131   if (v<m->sizeS)
00132     return(m->orig[v]);
00133   else
00134     return(NO_UINT);
00135 }
00136 
00137 void SimpleFromOriginal(Tbox *o,Tbox *s,Tmapping *m)
00138 {
00139   /* The same variable in the system */
00140   /* All ranges in the simple box change (recall that the
00141      simple system is a strict subset of the original system) */
00142 
00143   /*When applying this mapping the information in 'o' is considered the
00144     most up to date and, thus, the one in 's' is discarted*/
00145 
00146   /*When accumulating (intersecting) the several constraints that define the 
00147     range for the variables in the simple system we can discover that there
00148     is no solution (empty intersection)*/
00149   unsigned int i;
00150 
00151   BoxFromVariables(s,m->simple);
00152   for(i=0;i<m->sizeS;i++)
00153     {
00154       if (m->orig[i]!=NO_UINT)
00155         CopyInterval(GetBoxInterval(i,s),GetBoxInterval(m->orig[i],o));
00156     }
00157 }
00158 
00159 void UpdateOriginalFromSimple(Tbox *s,Tbox *o,Tmapping *m)
00160 {
00161   unsigned int i;
00162   Tinterval r,e,*is,*io;
00163 
00164   is=GetBoxIntervals(s);
00165   for(i=0;i<m->sizeO;i++)
00166     {
00167       if (m->related[i])
00168         {
00169           EvaluateLinearConstraintInt(is,&r,&(m->lc[i]));
00170           GetLinearConstraintError(&e,&(m->lc[i]));
00171           IntervalInvert(&e,&e);
00172           IntervalAdd(&e,&r,&r);
00173           io=GetBoxInterval(i,o);
00174           if (!Intersection(io,&r,io))
00175             CopyInterval(io,&r);
00176         }
00177     }
00178 }
00179 
00180 /* Since all variables in simple are also in original, the only information
00181    relevant to be printed in how the variables in the original system
00182    depend on those in the simple one
00183 */
00184 void PrintMapping(FILE *f,Tmapping *m)
00185 {
00186   unsigned int i;
00187   char *n;
00188   char **varNamesO,**varNamesS;
00189 
00190   if (m->original==NULL)
00191     varNamesO=NULL;
00192   else
00193     {
00194       NEW(varNamesO,m->sizeO,char *);
00195       GetVariableNames(varNamesO,m->original);
00196     }
00197 
00198   if (m->simple==NULL)
00199     varNamesS=NULL;
00200   else
00201     {
00202       NEW(varNamesS,m->sizeS,char *);
00203       GetVariableNames(varNamesS,m->simple);
00204     }
00205 
00206   for(i=0;i<m->sizeO;i++)
00207     {
00208       fprintf(f,"%%   ");
00209       if (varNamesO==NULL)
00210         fprintf(f,"v_%u",i);
00211       else
00212         {
00213           n=varNamesO[i];
00214           PRINT_VARIABLE_NAME(f,n);
00215         }
00216       fprintf(f,"=");
00217       if (m->related[i])
00218         PrintLinearConstraint(f,FALSE,varNamesS,&(m->lc[i]));
00219       else
00220         {
00221           //PRINT_VARIABLE_NAME(f,n);
00222           fprintf(f,"No in simplification\n");
00223         }
00224     } 
00225   if (varNamesO!=NULL)
00226     free(varNamesO);
00227   
00228   if (varNamesS!=NULL)
00229     free(varNamesS);
00230 }
00231 
00232 void LoadMapping(FILE *f,Tmapping *m)
00233 {
00234   unsigned int i;
00235 
00236   m->original=NULL;
00237   m->simple=NULL;
00238 
00239   fscanf(f,"%u",&(m->sizeS));
00240   NEW(m->orig,m->sizeS,unsigned int);
00241   for(i=0;i<m->sizeS;i++)
00242     fscanf(f,"%u ",&(m->orig[i]));
00243   
00244   fscanf(f,"%u",&(m->sizeO));
00245   NEW(m->related,m->sizeO,boolean);
00246   NEW(m->lc,m->sizeO,TLinearConstraint);
00247   for(i=0;i<m->sizeO;i++)
00248     {
00249       fprintf(f,"%u",m->related[i]);
00250       if (m->related[i])
00251         LoadLinearConstraint(f,&(m->lc[i]));
00252       else
00253         InitLinearConstraint(&(m->lc[i]));
00254     }
00255 }
00256 
00257 void SaveMapping(FILE *f,Tmapping *m)
00258 {
00259   unsigned int i;
00260 
00261   fprintf(f,"%u\n",m->sizeS);
00262   for(i=0;i<m->sizeS;i++)
00263     fprintf(f,"%u ",m->orig[i]);
00264   fprintf(f,"\n");
00265 
00266   fprintf(f,"%u\n",m->sizeO);
00267   for(i=0;i<m->sizeO;i++)
00268     {
00269       fprintf(f,"%u ",m->related[i]);
00270       if (m->related[i])
00271         SaveLinearConstraint(f,&(m->lc[i]));
00272       fprintf(f,"\n");
00273     } 
00274 }
00275 
00276 void DeleteMapping(Tmapping *m)
00277 {
00278   unsigned int i;
00279 
00280   if (m->original!=NULL)
00281     {
00282       DeleteVariables(m->original);
00283       free(m->original);
00284     }
00285 
00286   if (m->simple!=NULL)
00287     {
00288       DeleteVariables(m->simple);
00289       free(m->simple);
00290     }
00291 
00292   free(m->orig);
00293   for(i=0;i<m->sizeO;i++)
00294     DeleteLinearConstraint(&(m->lc[i]));
00295   free(m->related);
00296 }
00297