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

The CuikSuite Project

equations.c

Go to the documentation of this file.
00001 #include "equations.h"
00002 
00003 #include "defines.h"
00004 #include "error.h"
00005 #include "geom.h"
00006 
00007 #include <math.h>
00008 
00027 void SetEquationInfo(Tequation *e,TequationInfo *ei);
00028 
00040 Tequation *GetOriginalEquation(TequationInfo *ei);
00041 
00051 void CopyEquationInfo(TequationInfo *ei_dst,TequationInfo *ei_src);
00052 
00067 void GetFirstOrderApproximationToEquation(Tbox *b,double *c,
00068                                           TLinearConstraint *lc,
00069                                           TequationInfo *ei);
00085 void ErrorDueToVariable(unsigned int m,double *c,Tinterval *is,double *ev,TequationInfo *ei);
00086 
00101 boolean  LinearizeSaddleEquation(double epsilon,
00102                                  Tbox *b,
00103                                  TSimplex *lp,TequationInfo *ei);
00104 
00105 
00121 boolean LinearizeParabolaEquation(double epsilon,
00122                                   Tbox *b,
00123                                   TSimplex *lp,TequationInfo *ei);
00124 
00141 boolean LinearizeCircleEquation(double epsilon,
00142                                 Tbox *b,
00143                                 TSimplex *lp,TequationInfo *ei);
00144 
00161 boolean LinearizeSphereEquation(double epsilon,
00162                                 Tbox *b,
00163                                 TSimplex *lp,TequationInfo *ei);
00164 
00182 boolean LinearizeGeneralEquationInt(unsigned int cmp,
00183                                     double epsilon,
00184                                     Tbox *b,
00185                                     double *c,
00186                                     TSimplex *lp,
00187                                     TequationInfo *ei);
00203 boolean LinearizeGeneralEquation(double epsilon,
00204                                  Tbox *b,
00205                                  TSimplex *lp,TequationInfo *ei);
00206 
00215 void DeleteEquationInfo(TequationInfo *ei);
00216 
00217 
00218 /*******************************************************************************/
00219 
00220 void SetEquationInfo(Tequation *eq,TequationInfo *ei)
00221 {
00222   unsigned int i,j,nf;
00223   Tvariable_set *vars,*varsf;
00224   Tequation dd;
00225   Tmonomial *f;
00226   unsigned int s;
00227   Tinterval bounds;
00228   double v;
00229 
00230 
00231   if (LinearEquation(eq))
00232     ei->EqType=LINEAR_EQUATION;
00233   else
00234     {
00235       if (ParabolaEquation(eq))
00236         ei->EqType=PARABOLA_EQUATION;
00237       else
00238         {
00239           if (SaddleEquation(eq))
00240             ei->EqType=SADDLE_EQUATION;
00241           else
00242             {
00243               if (CircleEquation(eq))
00244                 ei->EqType=CIRCLE_EQUATION;
00245               else
00246                 {
00247                   if (SphereEquation(eq))
00248                     ei->EqType=SPHERE_EQUATION;
00249                   else
00250                     ei->EqType=GENERAL_EQUATION;
00251                 }
00252             }
00253         }
00254     }
00255 
00256   if (GetNumMonomials(eq)==0)
00257     Error("Only linear, quadratic, and bilinear equations are considered");
00258   else
00259     {
00260       NEW(ei->equation,1,Tequation);
00261       CopyEquation(ei->equation,eq);
00262 
00263       vars=GetEquationVariables(eq);
00264       ei->n=VariableSetSize(vars);
00265 
00266       if (ei->EqType==LINEAR_EQUATION)
00267         {
00268           /* Define a LinearConstraint from the Linear equation.
00269              The linear constraint is the structure to be directly added to the simplex */
00270 
00271           NEW(ei->lc,1,TLinearConstraint);
00272           InitLinearConstraint(ei->lc);
00273           v=GetEquationValue(eq);
00274           NewInterval(v,v,&bounds);
00275           SimplexExpandBounds(GetEquationCmp(eq),&bounds);
00276           SetLinearConstraintError(&bounds,ei->lc);
00277 
00278           nf=GetNumMonomials(eq);
00279           for(i=0;i<nf;i++)
00280             {
00281               f=GetMonomial(i,eq);
00282               varsf=GetMonomialVariables(f);
00283               s=VariableSetSize(varsf);
00284               if (s!=1) /* The ct terms of the equation are all in the right-hand part */
00285                 Error("Non-linear equation disguised as linear (SetEquationInfo)");
00286 
00287               AddTerm2LinearConstraint(GetVariableN(0,varsf),GetMonomialCt(f),ei->lc);
00288             }
00289 
00290           ei->Jacobian=NULL;
00291           ei->Hessian=NULL;
00292         }
00293       else
00294         {
00295           /*Compute the Jacobain/Hessian to be used in the linear approximation*/
00296               
00297           NEW(ei->Jacobian,ei->n,Tequation *);
00298           for(i=0;i<ei->n;i++)
00299             {
00300               NEW(ei->Jacobian[i],1,Tequation);
00301               DeriveEquation(GetVariableN(i,vars),ei->Jacobian[i],eq);
00302             }
00303               
00304           NEW(ei->Hessian,ei->n,double *);
00305           for(i=0;i<ei->n;i++)
00306             NEW(ei->Hessian[i],ei->n,double);
00307               
00308           for(i=0;i<ei->n;i++)
00309             {
00310               for(j=i;j<ei->n;j++)
00311                 {
00312                   DeriveEquation(GetVariableN(j,vars),&dd,ei->Jacobian[i]);
00313                       
00314                   nf=GetNumMonomials(&dd);
00315                       
00316                   if (nf==0)
00317                     {
00318                       ei->Hessian[i][j]=ei->Hessian[j][i]=-GetEquationValue(&dd);
00319                     }
00320                   else
00321                     Error("Non-quadratic equation in SetEquationInfo");
00322                       
00323                   DeleteEquation(&dd);
00324                 }
00325             }
00326               
00327           ei->lc=NULL;  
00328         }
00329     }
00330 }
00331 
00332 Tequation *GetOriginalEquation(TequationInfo *ei)
00333 {
00334   return(ei->equation);
00335 }
00336 
00337 void CopyEquationInfo(TequationInfo *ei_dst,TequationInfo *ei_src)
00338 {
00339   unsigned int i,j;
00340 
00341   NEW(ei_dst->equation,1,Tequation);
00342   CopyEquation(ei_dst->equation,ei_src->equation);
00343 
00344   ei_dst->n=ei_src->n;
00345 
00346   ei_dst->EqType=ei_src->EqType;
00347 
00348   if (ei_dst->EqType==LINEAR_EQUATION)
00349     {
00350       NEW(ei_dst->lc,1,TLinearConstraint);
00351       CopyLinearConstraint(ei_dst->lc,ei_src->lc);
00352 
00353       ei_dst->Jacobian=NULL;
00354       ei_dst->Hessian=NULL;
00355     }
00356   else
00357     {
00358       NEW(ei_dst->Jacobian,ei_dst->n,Tequation *);
00359       for(i=0;i<ei_dst->n;i++)
00360         {
00361           NEW(ei_dst->Jacobian[i],1,Tequation);
00362           CopyEquation(ei_dst->Jacobian[i],ei_src->Jacobian[i]);
00363         }
00364       
00365       NEW(ei_dst->Hessian,ei_dst->n,double *);
00366       for(i=0;i<ei_dst->n;i++)
00367         {
00368           NEW(ei_dst->Hessian[i],ei_dst->n,double);
00369           
00370           for(j=0;j<ei_dst->n;j++)
00371             ei_dst->Hessian[i][j]=ei_src->Hessian[i][j];
00372         }
00373       
00374       ei_dst->lc=NULL; 
00375     }
00376 }
00377 
00378 
00379 void GetFirstOrderApproximationToEquation(Tbox *b,double *c,
00380                                           TLinearConstraint *lc,
00381                                           TequationInfo *ei)
00382 {
00383   double d;
00384   Tinterval a;
00385   unsigned int i,j,k;
00386   Tvariable_set *vars;
00387   Tinterval *is;
00388   unsigned int m;
00389   Tinterval *cInt,error,dfc,o;
00390   double v;
00391 
00392   m=GetBoxNIntervals(b);
00393   is=GetBoxIntervals(b);
00394  
00395   /* f(x) = f(c) + df(c) (x-c) + E2 */
00396   /*     with E2 a quadratic error term */
00397   /* f(x) = df(c) x + f(c) - df(c) c + E2 */
00398   /*     f(c) and df(c) are evaluated UP and DOWN
00399          to avoid missing solutions */
00400   /* f(x) = [df(c)_l df(c)_u] x + [f(c)_l f(c)_u] - [df(c)_l df(c)_u] c + E2 */
00401   /* f(x) = df(c)_l x + [0 df(c)_u-df(c)_l] x + [f(c)_l f(c)_u] - [df(c)_l df(c)_u] c + E2  */
00402   /*     We accumulate all the intervals into one error term */
00403   /*     Error = [0 df(c)_u-df(c)_l] x + [f(c)_l f(c)_u] - [df(c)_l df(c)_u] c + E2  */
00404   /*     Error = [0 df(c)_u-df(c)_l] x + [f(c)_l f(c)_u] + [-df(c)_u -df(c)_l] c + E2  */
00405   /* f(x) = df(c)_l x +  Error */
00406   
00407 
00408   /* To take into account floating point errors, we perform interval evaluation of the
00409      equations but on punctual intervals (defined from the given points).  */
00410   
00411   vars=GetEquationVariables(ei->equation);
00412 
00413   NEW(cInt,m,Tinterval);
00414   for(i=0;i<ei->n;i++)
00415     {
00416       k=GetVariableN(i,vars);
00417       NewInterval(c[k],c[k],&(cInt[k]));
00418     }
00419 
00420   EvaluateEquationInt(cInt,&error,ei->equation); 
00421   v=GetEquationValue(ei->equation);
00422   
00423   /* The coefficients and value of the equations can be affected by some noise
00424      due to floating point roundings when operating them. We add a small
00425      range (1e-11) to compensate for those possible errors. */
00426   NewInterval(-v-ZERO,-v+ZERO,&o);
00427   
00428   IntervalAdd(&error,&o,&error);      
00429 
00430   InitLinearConstraint(lc);
00431 
00432   for(i=0;i<ei->n;i++)
00433     {
00434       k=GetVariableN(i,vars);
00435 
00436       EvaluateEquationInt(cInt,&dfc,ei->Jacobian[i]); 
00437       v=GetEquationValue(ei->Jacobian[i]);
00438       /* The coefficients and value of the equations can be affected by some noise
00439          due to floating point roundings when operating them. We add a small
00440          range (1e-11) to compensate for those possible errors. */
00441       NewInterval(-v-ZERO,-v+ZERO,&o);
00442 
00443       IntervalAdd(&dfc,&o,&dfc);
00444 
00445       d=LowerLimit(&dfc);
00446 
00447       IntervalOffset(&dfc,-d,&a);
00448       IntervalProduct(&a,&(is[k]),&a);
00449       IntervalAdd(&error,&a,&error);
00450 
00451       IntervalInvert(&dfc,&a);
00452       IntervalScale(&a,c[k],&a);
00453       IntervalAdd(&error,&a,&error);
00454 
00455       AddTerm2LinearConstraint(k,d,lc);
00456     }
00457   
00458   /* Now we bound the error using interval arithmetics */
00459   /*   Error= 0.5 * \sum_{i,j} r[i] H_{i,j} r[j] */
00460 
00461   /* The interval-based bound is exact as far as the Hessian is ct (i.e., we only
00462      have quadratic functions) and if there are not repeated variables in the
00463      error expression ( 0.5 * \sum_{i,j} r[i] H_{i,j} r[j]). This is the case
00464      of the circle, vector product and scalar product that appear in our problems.
00465      In other cases we could over-estimate the error (this delays the convergence,
00466      but does not avoid it)
00467   */
00468 
00469   /* We re-use the cInt vector previously allocated. This is not a problem since cInt
00470      is not used any more and the size of cInt (num var in the problem) is larger than
00471      ei->n (number of variables in the current equation) */
00472   for(i=0;i<ei->n;i++)
00473     {
00474       k=GetVariableN(i,vars);
00475       IntervalOffset(&(is[k]),-c[k],&(cInt[i])); 
00476     }
00477 
00478   for(i=0;i<ei->n;i++)
00479     {
00480       for(j=0;j<ei->n;j++)
00481         {
00482           if (ei->Hessian[i][j]!=0.0)
00483             {
00484               if (i==j)
00485                 IntervalPow(&(cInt[i]),2,&a);
00486               else
00487                 IntervalProduct(&(cInt[i]),&(cInt[j]),&a);
00488               
00489               /* The coefficients and value of the equations can be affected by some noise
00490                  due to floating point roundings when operating them. We add a small
00491                  range (1e-11) to compensate for those possible errors. */
00492               NewInterval((0.5*ei->Hessian[i][j])-ZERO,(0.5*ei->Hessian[i][j])+ZERO,&o);
00493               IntervalProduct(&a,&o,&a);
00494 
00495               IntervalAdd(&error,&a,&error);
00496             }
00497         }
00498     }
00499   
00500   free(cInt);
00501   
00502   /*  If one of the input ranges infinite, we override the computed error
00503       (probably have overflow....) */
00504   if (GetBoxMaxSizeVarSet(vars,b)<INF)
00505     IntervalInvert(&error,&error);
00506   else
00507     NewInterval(-INF,INF,&error);
00508 
00509   SetLinearConstraintError(&error,lc);
00510 }
00511 
00512 void ErrorDueToVariable(unsigned int m,double *c,Tinterval *is,
00513                         double *ev,TequationInfo *ei)
00514 {
00515 
00516   if (ei->EqType!=LINEAR_EQUATION)
00517     {
00518       Tvariable_set *vars;
00519       unsigned int i,k;
00520       Tinterval error;
00521       Tinterval *r,a;
00522       unsigned int j;
00523       /*
00524         Error= 1/2 * \sum_{i,j} r[i] * H_{i,j} * r[j] -> 
00525         Error= \sum{i} Error(i) 
00526         with
00527         Error(i)= 1/2 * r[i] * \sum_{j}  H_{i,j}* r[j]
00528 
00529         The output of this function is ev[i]=Error(i) (defined as above)
00530       */
00531 
00532       vars=GetEquationVariables(ei->equation);
00533   
00534       NEW(r,ei->n,Tinterval);
00535 
00536       for(i=0;i<ei->n;i++)
00537         {
00538           k=GetVariableN(i,vars);
00539           IntervalOffset(&(is[k]),-c[k],&(r[i])); 
00540         }
00541 
00542       for(i=0;i<ei->n;i++)
00543         {
00544           NewInterval(0,0,&error);
00545           for(j=0;j<ei->n;j++)
00546             {
00547               if (ei->Hessian[i][j]!=0.0)
00548                 {
00549                   if (i==j)
00550                     IntervalPow(&(r[i]),2,&a);
00551                   else
00552                     IntervalProduct(&(r[i]),&(r[j]),&a);
00553                   
00554                   IntervalScale(&a,ei->Hessian[i][j],&a);
00555                   IntervalAdd(&error,&a,&error);
00556                 }
00557             }
00558           
00559           ev[i]=IntervalSize(&error)/2.0;
00560         }
00561 
00562       free(r);  
00563     }
00564 
00565 }
00566 
00567 void DeleteEquationInfo(TequationInfo *ei)
00568 {
00569   unsigned int i;
00570 
00571   DeleteEquation(ei->equation);
00572   free(ei->equation);
00573 
00574   if (ei->EqType==LINEAR_EQUATION)
00575     {
00576       DeleteLinearConstraint(ei->lc);
00577       free(ei->lc);
00578     }
00579   else
00580     {
00581       for(i=0;i<ei->n;i++)
00582         {
00583           DeleteEquation(ei->Jacobian[i]);
00584           free(ei->Jacobian[i]);
00585         }
00586       free(ei->Jacobian);
00587       
00588       for(i=0;i<ei->n;i++)
00589         free(ei->Hessian[i]);
00590       
00591       free(ei->Hessian);
00592     }
00593 }
00594 
00595 void InitEquations(Tequations *eqs)
00596 {
00597   unsigned int i;
00598 
00599   eqs->n=0;
00600   eqs->s=0;
00601   eqs->c=0;
00602   eqs->d=0;
00603   eqs->e=0;
00604   eqs->max=INIT_NUM_EQUATIONS;
00605   NEW(eqs->equation,eqs->max,TequationInfo *);
00606   
00607   for(i=0;i<eqs->max;i++)
00608     eqs->equation[i]=NULL;
00609 }
00610 
00611 void CopyEquations(Tequations *eqs_dst,Tequations *eqs_src)
00612 {
00613   unsigned int i;
00614 
00615   eqs_dst->n=eqs_src->n;
00616   eqs_dst->s=eqs_src->s;
00617   eqs_dst->c=eqs_src->c;
00618   eqs_dst->d=eqs_src->d;
00619   eqs_dst->e=eqs_src->e;
00620 
00621   eqs_dst->max=eqs_src->max;
00622 
00623   NEW(eqs_dst->equation,eqs_dst->max,TequationInfo *);
00624 
00625   for(i=0;i<eqs_src->max;i++)
00626     {
00627       if (eqs_src->equation[i]!=NULL)
00628         {
00629           NEW(eqs_dst->equation[i],1,TequationInfo);
00630           CopyEquationInfo(eqs_dst->equation[i],eqs_src->equation[i]);
00631         }
00632       else
00633         eqs_dst->equation[i]=NULL;
00634     }
00635 }
00636 
00637 boolean UsedVarInNonDummyEquations(unsigned int nv,Tequations *eqs)
00638 {
00639   Tequation *eq;
00640   boolean found;
00641   unsigned int i;
00642   
00643   i=0;
00644   found=FALSE;
00645   while((i<eqs->n)&&(!found))
00646     {
00647       eq=GetEquation(i,eqs);
00648       if (GetEquationType(eq)!=DUMMY_EQ)
00649         found=VarIncluded(nv,GetEquationVariables(eq));
00650 
00651       if (!found) i++;
00652     }
00653 
00654   return(found);
00655 }
00656 
00657 boolean UsedVarInEquations(unsigned int nv,Tequations *eqs)
00658 {
00659   boolean found;
00660   unsigned int i;
00661   
00662   i=0;
00663   found=FALSE;
00664   while((i<eqs->n)&&(!found))
00665     {
00666       found=VarIncluded(nv,GetEquationVariables(GetEquation(i,eqs)));
00667       if (!found) i++;
00668     }
00669 
00670   return(found);
00671 }
00672 
00673 void RemoveEquationsWithVar(double epsilon,unsigned int nv,Tequations *eqs)
00674 {
00675   Tequation *eq;
00676   Tequations newEqs;
00677   unsigned int j;
00678 
00679   InitEquations(&newEqs);
00680   for(j=0;j<eqs->n;j++)
00681     {
00682       
00683       eq=GetEquation(j,eqs);
00684       if (!VarIncluded(nv,GetEquationVariables(eq)))
00685         {
00686           /*The equations with the given variables are not included in the new set*/
00687           /*The given variables is to be removed from the problem (it is not used
00688             any more!). Removing a variable causes a shift in all the variable
00689             indexes above the removed one. This effect can be easily simulated
00690             by fixing the variable-to-be-removed for the equations to be kept in 
00691             the new set. */
00692           FixVariableInEquation(epsilon,nv,1,eq);
00693           AddEquation(eq,&newEqs);
00694         }
00695     }
00696 
00697   DeleteEquations(eqs);
00698   CopyEquations(eqs,&newEqs);
00699   DeleteEquations(&newEqs);
00700 }
00701 
00702 boolean ReplaceVariableInEquations(double epsilon,
00703                                    unsigned int nv,TLinearConstraint *lc,
00704                                    Tequations *eqs)
00705 {
00706   boolean hold;
00707   unsigned int j,r;
00708   Tequations newEqs;
00709   Tequation *eqn;
00710   
00711   hold=TRUE;
00712   InitEquations(&newEqs);
00713   for(j=0;((hold)&&(j<eqs->n));j++)
00714     {
00715       eqn=GetEquation(j,eqs);
00716 
00717       r=ReplaceVariableInEquation(epsilon,nv,lc,eqn);
00718 
00719       if (r==2)
00720         hold=FALSE; /*the equation became ct and does not hold -> the whole system fails*/
00721       else
00722         {
00723           if (r==0)
00724             AddEquation(eqn,&newEqs);
00725 
00726           /*if r==1, a ct equation that holds -> no need to add it to the new set*/
00727         }
00728     }
00729 
00730   DeleteEquations(eqs);
00731   CopyEquations(eqs,&newEqs);
00732   DeleteEquations(&newEqs);
00733 
00734   return(hold);
00735 }
00736 
00737 boolean GaussianElimination(Tequations *eqs)
00738 {
00739   unsigned int i,j,k,neq,neqn,meq;
00740   Tequation *eqi,*eqj,eqMin,eqNew;
00741   Tequations newEqs;
00742   unsigned int nfi,nfj,r;
00743   double ct;
00744   boolean changes;
00745   Tmonomial *fi,*fj;
00746 
00747   /*We try to get the simplest possible set of equations
00748     Simple equation = as less number of monomials as possible*/
00749 
00750   changes=FALSE;
00751 
00752   neq=NEquations(eqs);
00753   InitEquations(&newEqs);
00754   for(i=0;i<neq;i++)
00755     {
00756       eqi=GetEquation(i,eqs);
00757 
00758       if ((GetEquationCmp(eqi)!=EQU)||(GetEquationType(eqi)==DUMMY_EQ))
00759         AddEquation(eqi,&newEqs);
00760       else
00761         {
00762           nfi=GetNumMonomials(eqi);
00763 
00764           CopyEquation(&eqMin,eqi);
00765           
00766           /* We try to simplify eqi linearly operating it with the
00767              already simplified equations and the equations still to be
00768              simplified. */
00769           neqn=NEquations(&newEqs);
00770           meq=neqn+(neq-i-1);
00771           for(j=0;j<meq;j++)
00772             {
00773               if (j<neqn)
00774                 eqj=GetEquation(j,&newEqs);
00775               else
00776                 eqj=GetEquation(i+1+(j-neqn),eqs);
00777                 
00778               if ((GetEquationCmp(eqj)==EQU)&&(GetEquationType(eqj)!=DUMMY_EQ))
00779                 {
00780                   nfj=GetNumMonomials(eqj);
00781 
00782                   for(k=0;k<nfi;k++)
00783                     {
00784                       fi=GetMonomial(k,eqi);
00785                       r=FindMonomial(fi,eqj);
00786 
00787                       if (r!=NO_UINT) 
00788                         {
00789                           /*If the monomial of eqi is also in eqj*/
00790                           fj=GetMonomial(r,eqj);
00791                           ct=-GetMonomialCt(fi)/GetMonomialCt(fj);
00792 
00793                           CopyEquation(&eqNew,eqi);
00794                           AccumulateEquations(eqj,ct,&eqNew);
00795                           
00796                           /*If the equation after operation has less monomials than
00797                             the minimal equation we have up to now -> we have a new
00798                             minimal */
00799                           /*
00800                           if ((GetNumMonomials(&eqNew)<GetNumMonomials(&eqMin))||
00801                               ((GetNumMonomials(&eqNew)==GetNumMonomials(&eqMin))&&
00802                               (fabs(GetEquationValue(&eqNew))<fabs(GetEquationValue(&eqMin)))))*/
00803                           if ((GetNumMonomials(&eqNew)<GetNumMonomials(&eqMin))||
00804                               ((GetNumMonomials(&eqNew)==GetNumMonomials(&eqMin))&&
00805                                (GetEquationNumVariables(&eqNew)<GetEquationNumVariables(&eqMin))))
00806                             {
00807                               DeleteEquation(&eqMin);
00808                               CopyEquation(&eqMin,&eqNew);
00809                               changes=TRUE;
00810                             }
00811                           DeleteEquation(&eqNew);
00812                         }
00813                     }
00814                 }
00815             }
00816             
00817           AddEquation(&eqMin,&newEqs); 
00818           DeleteEquation(&eqMin); 
00819         }
00820     }
00821 
00822   DeleteEquations(eqs);
00823   CopyEquations(eqs,&newEqs);
00824   DeleteEquations(&newEqs);
00825 
00826   return(changes);
00827 }
00828 
00829 unsigned int NEquations(Tequations *eqs)
00830 {
00831   return(eqs->n);
00832 }
00833 
00834 unsigned int NSystemEquations(Tequations *eqs)
00835 {
00836   return(eqs->s);
00837 }
00838 
00839 unsigned int NCoordEquations(Tequations *eqs)
00840 {
00841   return(eqs->c);
00842 }
00843 
00844 unsigned int NDummyEquations(Tequations *eqs)
00845 {
00846   return(eqs->d);
00847 }
00848 
00849 unsigned int NEqualityEquations(Tequations *eqs)
00850 {
00851   return(eqs->e);
00852 }
00853 
00854 boolean IsSystemEquation(unsigned int i,Tequations *eqs)
00855 {
00856   return((i<eqs->n)&&
00857          (GetEquationType(GetOriginalEquation(eqs->equation[i]))==SYSTEM_EQ));
00858 }
00859 
00860 boolean IsCoordEquation(unsigned int i,Tequations *eqs)
00861 {
00862   return((i<eqs->n)&&
00863          (GetEquationType(GetOriginalEquation(eqs->equation[i]))==COORD_EQ));
00864 }
00865 
00866 boolean IsDummyEquation(unsigned int i,Tequations *eqs)
00867 {
00868   return((i<eqs->n)&&
00869          (GetEquationType(GetOriginalEquation(eqs->equation[i]))==DUMMY_EQ));
00870 }
00871 
00872 unsigned int GetEquationTypeN(unsigned int i,Tequations *eqs)
00873 {
00874   if (eqs->equation[i]==NULL)
00875     return(NOTYPE_EQ);
00876   else
00877     return(GetEquationType(GetOriginalEquation(eqs->equation[i])));
00878 }
00879 
00880 boolean HasEquation(Tequation *equation,Tequations *eqs)
00881 {
00882   boolean found;
00883   unsigned int i;
00884   
00885   i=0;
00886   found=FALSE;
00887   while((i<eqs->n)&&(!found))
00888     {
00889       found=(CmpEquations(equation,GetOriginalEquation(eqs->equation[i]))==0);
00890       if (!found) i++;
00891     }
00892   return(found);
00893 }
00894 
00895 unsigned int CropEquation(unsigned int ne,
00896                           double epsilon,double rho,
00897                           Tbox *b,
00898                           Tequations *eqs)
00899 {
00900   unsigned int status;
00901   TequationInfo *ei;
00902   unsigned int cmp;
00903   Tequation *eq;
00904   Tvariable_set *vars;
00905   
00906   double val,alpha;
00907   Tvariable_set *varsf;;
00908   Tinterval x_new,y_new,z_new;
00909   unsigned int nx,ny,nz; 
00910   unsigned int m,i,k;
00911   Tinterval *is,bounds;
00912 
00913   ei=eqs->equation[ne];
00914 
00915   eq=GetOriginalEquation(ei);
00916 
00917   status=NOT_REDUCED_BOX;
00918 
00919   cmp=GetEquationCmp(eq);
00920 
00921   #if (_DEBUG>6)
00922     printf("\n       Croping Equation: %u\n",ne);
00923   #endif
00924 
00925   if (cmp==EQU)
00926     {
00927       m=GetBoxNIntervals(b);
00928       is=GetBoxIntervals(b);
00929 
00930       vars=GetEquationVariables(eq);
00931 
00932       val=GetEquationValue(eq);
00933 
00934       #if (_DEBUG>6)
00935         printf("         Eq: ");
00936         PrintEquation(stdout,NULL,eq);
00937         printf("         Input ranges: ");
00938         for(i=0;i<ei->n;i++)
00939           {
00940             k=GetVariableN(i,vars);
00941             printf(" v[%u]=",k);PrintInterval(stdout,&(is[k]));printf("  ");
00942           }
00943         printf("\n");
00944       #endif
00945 
00946             
00947       /* Special equations cropped with special functions that get tighter bounds
00948          (they use non-linear functions) */
00949       switch(ei->EqType)
00950         {
00951         case LINEAR_EQUATION:
00952           status=CropLinearConstraint(epsilon,b,ei->lc);
00953           break;
00954 
00955         case PARABOLA_EQUATION:
00956           /* x^2 - \alpha y = 0 */
00957           nx=GetVariableN(0,GetMonomialVariables(GetMonomial(0,eq)));
00958           ny=GetVariableN(0,GetMonomialVariables(GetMonomial(1,eq)));
00959           alpha=GetMonomialCt(GetMonomial(1,eq));
00960 
00961           if (((IntervalSize(&(is[nx]))>=epsilon)||
00962                (IntervalSize(&(is[ny]))>=epsilon))&&
00963               (IntervalSize(&(is[nx]))<INF))
00964             {
00965               if (RectangleParabolaClipping(&(is[nx]),-alpha,&(is[ny]),&x_new,&y_new))
00966                 { 
00967                   status=REDUCED_BOX;
00968                   CopyInterval(&(is[nx]),&x_new);
00969                   CopyInterval(&(is[ny]),&y_new);
00970                 }
00971               else
00972                 status=EMPTY_BOX;
00973             }
00974           break;
00975 
00976         case SADDLE_EQUATION:
00977           /* x*y - \alpha z = 0 */
00978           varsf=GetMonomialVariables(GetMonomial(0,eq));
00979           alpha=GetMonomialCt(GetMonomial(1,eq));
00980 
00981           nx=GetVariableN(0,varsf);
00982           ny=GetVariableN(1,varsf);
00983           nz=GetVariableN(0,GetMonomialVariables(GetMonomial(1,eq)));
00984       
00985           if (((IntervalSize(&(is[nx]))>=epsilon)||
00986                (IntervalSize(&(is[ny]))>=epsilon)||
00987                (IntervalSize(&(is[nz]))>=epsilon))&&
00988               (IntervalSize(&(is[nx]))<INF)&&
00989               (IntervalSize(&(is[ny]))<INF))
00990             {
00991               if (BoxSaddleClipping(&(is[nx]),&(is[ny]),-alpha,&(is[nz]),
00992                                     &x_new,&y_new,&z_new))
00993                 { 
00994                   status=REDUCED_BOX;
00995                   CopyInterval(&(is[nx]),&x_new);
00996                   CopyInterval(&(is[ny]),&y_new);
00997                   CopyInterval(&(is[nz]),&z_new);
00998                 }
00999               else
01000                 status=EMPTY_BOX;
01001             }
01002           break;
01003 
01004         case CIRCLE_EQUATION:
01005           /* x^2 + y^2 = val */
01006           nx=GetVariableN(0,vars);
01007           ny=GetVariableN(1,vars);
01008 
01009           if ((IntervalSize(&(is[nx]))>=epsilon)||
01010               (IntervalSize(&(is[ny]))>=epsilon))
01011             {
01012               if (RectangleCircleClipping(val,
01013                                           &(is[nx]),&(is[ny]),
01014                                           &x_new,&y_new))
01015                 { 
01016                   status=REDUCED_BOX;
01017                   CopyInterval(&(is[nx]),&x_new);
01018                   CopyInterval(&(is[ny]),&y_new);
01019                 }
01020               else
01021                 status=EMPTY_BOX;
01022             }
01023           break;
01024 
01025         case SPHERE_EQUATION:
01026           /* x^2 + y^2 + z^2 = val */
01027           nx=GetVariableN(0,vars);
01028           ny=GetVariableN(1,vars);
01029           nz=GetVariableN(2,vars);
01030       
01031           if ((IntervalSize(&(is[nx]))>=epsilon)||
01032               (IntervalSize(&(is[ny]))>=epsilon)||
01033               (IntervalSize(&(is[nz]))>=epsilon))
01034             {
01035               if (BoxSphereClipping(val,
01036                                     &(is[nx]),&(is[ny]),&(is[nz]),
01037                                     &x_new,&y_new,&z_new))
01038                 { 
01039                   status=REDUCED_BOX;
01040                   CopyInterval(&(is[nx]),&x_new);
01041                   CopyInterval(&(is[ny]),&y_new);
01042                   CopyInterval(&(is[nz]),&z_new);
01043                 }
01044               else
01045                 status=EMPTY_BOX;
01046             }
01047           break;
01048 
01049         case GENERAL_EQUATION:
01050           /* When simplifying systems it is quite common to introduce equations
01051              of type x^2=ct or x*y=ct. We treat them as special cases and taking into
01052              accound the non-linealities -> stronger crop */
01053           if ((GetNumMonomials(eq)==1)&&
01054               (GetMonomialCt(GetMonomial(0,eq))==1))
01055             {
01056               Tinterval r;
01057 
01058               if (VariableSetSize(vars)>2)
01059                 Error("Three variables in a single monomial?");
01060                 
01061 
01062               if (VariableSetSize(vars)==1) /*only one variable in the equation*/
01063                 {
01064                   /* Only one variable included in the equation-> x^2=ct
01065                      (the linal case x=ct is treated above */
01066                   /* ax^2=ct is transformed to x^2+y^2=ct with y=[0,0] */
01067                   nx=GetVariableN(0,vars);
01068 
01069                   NewInterval(-ZERO,+ZERO,&r);
01070 
01071                   if (IntervalSize(&(is[nx]))>=epsilon)
01072                     {
01073                       if (RectangleCircleClipping(val,
01074                                                   &(is[nx]),&r,
01075                                                   &x_new,&y_new))
01076                         { 
01077                           status=REDUCED_BOX;
01078                           CopyInterval(&(is[nx]),&x_new);
01079                         }
01080                       else
01081                         status=EMPTY_BOX;
01082                     }
01083                 }
01084               else
01085                 {
01086                   /* xy=ct is transformed to xy-z=0 with z=ct*/
01087                   nx=GetVariableN(0,vars);
01088                   ny=GetVariableN(1,vars);
01089                   NewInterval(val-ZERO,val+ZERO,&r);
01090 
01091                   if ((IntervalSize(&(is[nx]))>=epsilon)||
01092                       (IntervalSize(&(is[ny]))>=epsilon))
01093                     {
01094                       if (BoxSaddleClipping(&(is[nx]),&(is[ny]),1,&r,
01095                                             &x_new,&y_new,&z_new))
01096                         { 
01097                           status=REDUCED_BOX;
01098                           CopyInterval(&(is[nx]),&x_new);
01099                           CopyInterval(&(is[ny]),&y_new);
01100                         }
01101                       else
01102                         status=EMPTY_BOX;
01103                     }
01104                 }
01105             }
01106           else
01107             {
01108               double *c; /*central point*/
01109               TLinearConstraint lc;
01110 
01111               NEW(c,m,double); 
01112 
01113               do {
01114                 /*possible improvement -> use a random point inside the ranges
01115                   this will cut increase the crop */
01116                 for(i=0;i<ei->n;i++)
01117                   {
01118                     k=GetVariableN(i,vars);
01119                     c[k]=IntervalCenter(&(is[k]));
01120                   }
01121 
01122                 GetFirstOrderApproximationToEquation(b,c,&lc,ei);
01123 
01124                 GetLinearConstraintError(&bounds,&lc);
01125                 SimplexExpandBounds(cmp,&bounds);
01126                 SetLinearConstraintError(&bounds,&lc);          
01127 
01128                 status=CropLinearConstraint(epsilon,b,&lc);
01129                 DeleteLinearConstraint(&lc);
01130              
01131               } while(status==REDUCED_BOX);
01132 
01133               free(c);
01134             }
01135           break;
01136           
01137         default:
01138           Error("Unknown equation type in CropEquation");
01139         }
01140     }
01141   #if (_DEBUG>6)
01142   else
01143     printf("           Inequalities are not cropped\n");
01144     
01145   printf("         Output ranges: ");
01146   for(i=0;i<ei->n;i++)
01147     {
01148       k=GetVariableN(i,vars);
01149       printf(" v[%u]=",k);PrintInterval(stdout,&(is[k]));printf("  ");
01150     }
01151   printf("\n         ");
01152   #endif     
01153  
01154   return(status);
01155 }
01156 
01157 void AddEquation(Tequation *equation,Tequations *eqs)
01158 {
01159   if ((GetEquationType(equation)==NOTYPE_EQ)||(GetEquationCmp(equation)==NOCMP))
01160     Error("Adding an equation with unknown type of cmp");
01161 
01162   NormalizeEquation(equation);
01163 
01164   if ((GetNumMonomials(equation)>0)&&(!HasEquation(equation,eqs)))
01165     {
01166       unsigned int t,j;
01167       TequationInfo *s;
01168 
01169       if (eqs->n==eqs->max)
01170         {
01171           unsigned int i,k;
01172 
01173           k=eqs->max;
01174           MEM_DUP(eqs->equation,eqs->max,TequationInfo *);
01175 
01176           for(i=k;i<eqs->max;i++)
01177             eqs->equation[i]=NULL;
01178         }
01179 
01180       NEW(eqs->equation[eqs->n],1,TequationInfo);
01181       SetEquationInfo(equation,eqs->equation[eqs->n]);
01182 
01183       t=GetEquationType(equation);
01184       if (t==SYSTEM_EQ) eqs->s++;
01185       if (t==COORD_EQ) eqs->c++;
01186       if (t==DUMMY_EQ) eqs->d++;
01187       if (GetEquationCmp(equation)==EQU) eqs->e++;
01188 
01189       eqs->n++;
01190 
01191       j=eqs->n-1;
01192       while((j>0)&&(CmpEquations(GetOriginalEquation(eqs->equation[j-1]),
01193                                  GetOriginalEquation(eqs->equation[j]))==1))
01194         {
01195           s=eqs->equation[j-1];
01196           eqs->equation[j-1]=eqs->equation[j];
01197           eqs->equation[j]=s;
01198 
01199           j--;
01200         }
01201     }
01202 }
01203 
01204 Tequation *GetEquation(unsigned int n,Tequations *eqs)
01205 {
01206   if (n<eqs->n)
01207     return(GetOriginalEquation(eqs->equation[n]));
01208   else
01209     return(NULL);
01210 }
01211 
01212 boolean LinearizeSaddleEquation(double epsilon,Tbox *b,
01213                                 TSimplex *lp,TequationInfo *ei)
01214 { 
01215   Tvariable_set *varsf;
01216   unsigned int nx,ny,nz;
01217   double x_min,x_max;
01218   double y_min,y_max;
01219   double *c;
01220   boolean full;
01221   Tequation *eq;
01222   Tinterval *is;
01223   unsigned int m;
01224 
01225   m=GetBoxNIntervals(b);
01226   is=GetBoxIntervals(b);
01227   eq=GetOriginalEquation(ei);
01228 
01229   /* x*y - z =0  */
01230   varsf=GetMonomialVariables(GetMonomial(0,eq));
01231   
01232   nx=GetVariableN(0,varsf);
01233   ny=GetVariableN(1,varsf);
01234   nz=GetVariableN(0,GetMonomialVariables(GetMonomial(1,eq)));
01235 
01236   x_min=LowerLimit(&(is[nx]));
01237   x_max=UpperLimit(&(is[nx]));
01238 
01239   y_min=LowerLimit(&(is[ny]));
01240   y_max=UpperLimit(&(is[ny]));
01241 
01242   NEW(c,m,double); 
01243   
01244   {
01245     double px[4]={x_min,x_min,x_max,x_max};
01246     double py[4]={y_min,y_max,y_min,y_max};
01247     unsigned int cmp[4]={LEQ,GEQ,GEQ,LEQ};
01248     
01249     unsigned int i; 
01250     
01251     full=TRUE;
01252     for(i=0;((full)&&(i<4));i++)
01253       {
01254         c[nx]=px[i];
01255         c[ny]=py[i];
01256         c[nz]=0;
01257         
01258         full=LinearizeGeneralEquationInt(cmp[i],epsilon,b,c,lp,ei);
01259       }
01260   }
01261 
01262   free(c);
01263   
01264   return(full);
01265 }
01266 
01267 boolean LinearizeParabolaEquation(double epsilon,
01268                                   Tbox *b,
01269                                   TSimplex *lp,TequationInfo *ei)
01270 {
01271   unsigned int nx,ny;
01272   double x_min,x_max,x_c;
01273   double y_min,y_max;
01274   double *c;
01275   boolean full;
01276   Tequation *eq;
01277   Tinterval *is;
01278   unsigned int m;
01279 
01280   m=GetBoxNIntervals(b);
01281   is=GetBoxIntervals(b);
01282   eq=GetOriginalEquation(ei);
01283 
01284   /* x^2 - y =0  */
01285   nx=GetVariableN(0,GetMonomialVariables(GetMonomial(0,eq)));
01286   ny=GetVariableN(0,GetMonomialVariables(GetMonomial(1,eq)));
01287 
01288   x_min=LowerLimit(&(is[nx]));
01289   x_max=UpperLimit(&(is[nx]));
01290   x_c=IntervalCenter(&(is[nx]));
01291 
01292   y_min=LowerLimit(&(is[ny]));
01293   y_max=UpperLimit(&(is[ny]));
01294 
01295   NEW(c,m,double); 
01296   
01297   {
01298     double px[3]={x_c,x_min,x_max};
01299     unsigned int cmp[3]={EQU,LEQ,LEQ};
01300     unsigned int i;
01301 
01302     full=TRUE;
01303     for(i=0;((full)&&(i<3));i++)
01304       {
01305         c[nx]=px[i];
01306         c[ny]=0;  /*The value of 'y' is not used in the linealization*/
01307         
01308         full=LinearizeGeneralEquationInt(cmp[i],epsilon,b,c,lp,ei);
01309       }
01310   }
01311 
01312   free(c);
01313 
01314   return(full);
01315 }
01316 
01317 boolean LinearizeCircleEquation(double epsilon,
01318                                 Tbox *b,
01319                                 TSimplex *lp,TequationInfo *ei)
01320 {
01321   Tequation *eq;
01322   Tvariable_set *vars;
01323   unsigned int nx,ny;
01324   double x_min,x_max,x_c;
01325   double y_min,y_max,y_c;
01326   double *c;
01327   double r2; 
01328   /*At most 5 linearization points are selected (one
01329     for each box corner and one from the center of the
01330     interval x,y)*/
01331   double cx[5];
01332   double cy[5];
01333   unsigned int cmp[5];
01334   unsigned int i,nc; 
01335   boolean full;
01336   Tinterval *is;
01337   unsigned int m;
01338 
01339   m=GetBoxNIntervals(b);
01340   is=GetBoxIntervals(b);
01341   eq=GetOriginalEquation(ei);
01342 
01343   vars=GetEquationVariables(eq);
01344 
01345   r2=GetEquationValue(eq);
01346 
01347   nx=GetVariableN(0,vars);
01348   ny=GetVariableN(1,vars);
01349 
01350   x_min=LowerLimit(&(is[nx]));
01351   x_max=UpperLimit(&(is[nx]));
01352   x_c=IntervalCenter(&(is[nx]));
01353 
01354   y_min=LowerLimit(&(is[ny]));
01355   y_max=UpperLimit(&(is[ny]));
01356   y_c=IntervalCenter(&(is[ny]));
01357 
01358   NEW(c,m,double); /* space for the linealization point */
01359   
01360   /* for large input ranges (both in x and y) we add linealization constraints
01361      from the corners of the box*/
01362   nc=0;
01363   {
01364     /* Check which of the 4 box corners are out of the circle.
01365        For the external points we add a linealization point
01366        selected on the circle: intersection of the circle and a 
01367        line from the origin to the corner (a normalization of
01368        point (x,y) to norm sqrt(r2))
01369     */
01370     double px[4]={x_min,x_min,x_max,x_max};
01371     double py[4]={y_min,y_max,y_min,y_max};
01372     double norm;
01373     
01374     for(i=0;i<4;i++)
01375       {
01376         ROUNDDOWN;
01377         norm=px[i]*px[i]+py[i]*py[i];
01378         ROUNDNEAR;
01379         if (norm>(r2+epsilon)) /*corner (clearly) out of the circle*/
01380           {
01381             norm=sqrt(r2/norm);
01382             cx[nc]=px[i]*norm;
01383             cy[nc]=py[i]*norm; 
01384             cmp[nc]=LEQ;
01385             nc++;
01386           }
01387       }
01388   }
01389   
01390   /* A point selected from the center of x range is added always
01391      'y' is selected so that it is on the circle, if possible.
01392      If not, y is just eh center of the y range
01393   */
01394   cx[nc]=x_c;
01395   cy[nc]=y_c;
01396   //cmp[nc]=EQU;
01397   cmp[nc]=GetEquationCmp(eq);
01398   nc++;
01399 
01400   full=TRUE;
01401   for(i=0;((full)&&(i<nc));i++)
01402     {
01403       c[nx]=cx[i];
01404       c[ny]=cy[i];
01405 
01406       full=LinearizeGeneralEquationInt(cmp[i],epsilon,b,c,lp,ei);
01407     }
01408 
01409   free(c);
01410 
01411   return(full);
01412 }
01413 
01414 boolean LinearizeSphereEquation(double epsilon,
01415                                 Tbox *b,
01416                                 TSimplex *lp,TequationInfo *ei)
01417 {
01418   Tequation *eq;
01419   Tvariable_set *vars;
01420   unsigned int nx,ny,nz;
01421   double x_min,x_max,x_c;
01422   double y_min,y_max,y_c;
01423   double z_min,z_max,z_c;
01424   double *c;
01425   double r2; 
01426   /*At most 9 linearization points are selected (one
01427     for each box corner and one from the center of the
01428     interval x,y,z)*/
01429   double cx[9];
01430   double cy[9];
01431   double cz[9];
01432   unsigned int cmp[9];
01433   unsigned int i,nc; 
01434   boolean full;
01435   Tinterval *is;
01436   unsigned int m;
01437 
01438   m=GetBoxNIntervals(b);
01439   is=GetBoxIntervals(b);
01440   eq=GetOriginalEquation(ei);
01441 
01442   vars=GetEquationVariables(eq);
01443 
01444   r2=GetEquationValue(eq);
01445 
01446   nx=GetVariableN(0,vars);
01447   ny=GetVariableN(1,vars);
01448   nz=GetVariableN(2,vars);
01449 
01450   x_min=LowerLimit(&(is[nx]));
01451   x_max=UpperLimit(&(is[nx]));
01452   x_c=IntervalCenter(&(is[nx]));
01453 
01454   y_min=LowerLimit(&(is[ny]));
01455   y_max=UpperLimit(&(is[ny]));
01456   y_c=IntervalCenter(&(is[ny]));
01457 
01458   z_min=LowerLimit(&(is[nz]));
01459   z_max=UpperLimit(&(is[nz]));
01460   z_c=IntervalCenter(&(is[nz]));
01461 
01462   NEW(c,m,double); /* space for the linealization point */
01463   
01464   /* for large input ranges (both in x and y) we add linealization constraints
01465      from the corners of the box*/
01466   nc=0;
01467 
01468   {
01469     /* Check which of the 8 box corners are out of the sphere.
01470        For the external points we add a linealization point
01471        selected on the sphere: intersection of the sphere and a 
01472        line from the origin to the corner (a normalization of
01473        point (x,y,z) to norm sqrt(r2))
01474     */
01475     double px[8]={x_min,x_min,x_min,x_min,x_max,x_max,x_max,x_max};
01476     double py[8]={y_min,y_min,y_max,y_max,y_min,y_min,y_max,y_max};
01477     double pz[8]={z_min,z_max,z_min,z_max,z_min,z_max,z_min,z_max};
01478     double norm;
01479     
01480     for(i=0;i<8;i++)
01481       {
01482         ROUNDDOWN;
01483         norm=px[i]*px[i]+py[i]*py[i]+pz[i]*pz[i];
01484         ROUNDNEAR;
01485         if (norm>(r2+epsilon)) /*corner (clearly) out of the sphere*/
01486           {
01487             norm=sqrt(r2/norm);
01488             cx[nc]=px[i]*norm;
01489             cy[nc]=py[i]*norm;
01490             cz[nc]=pz[i]*norm; 
01491             cmp[nc]=LEQ;
01492             nc++;
01493           }
01494       }
01495   }
01496   
01497   /* A point selected from the center of x range is added always
01498      'y' is selected so that it is on the circle, if possible.
01499      If not, y is just eh center of the y range
01500   */
01501   cx[nc]=x_c;
01502   cy[nc]=y_c;
01503   cz[nc]=z_c; 
01504   cmp[nc]=EQU;
01505   nc++;
01506 
01507   full=TRUE;
01508   for(i=0;((full)&&(i<nc));i++)
01509     {
01510       c[nx]=cx[i];
01511       c[ny]=cy[i];
01512       c[nz]=cz[i];
01513 
01514       full=LinearizeGeneralEquationInt(cmp[i],epsilon,b,c,lp,ei);
01515     }
01516 
01517   free(c);
01518 
01519   return(full);
01520 }
01521 
01522 boolean LinearizeGeneralEquationInt(unsigned int cmp,
01523                                     double epsilon,
01524                                     Tbox *b,double *c,
01525                                     TSimplex *lp,TequationInfo *ei)
01526 {  
01527   Tvariable_set *vars;
01528   TLinearConstraint lc;
01529   boolean full=TRUE;
01530 
01531   vars=GetEquationVariables(GetOriginalEquation(ei));
01532 
01533   if (GetBoxMaxSizeVarSet(vars,b)<INF)
01534     {
01535       GetFirstOrderApproximationToEquation(b,c,&lc,ei);
01536 
01537       full=SimplexAddNewConstraint(epsilon,&lc,cmp,GetBoxIntervals(b),lp);
01538 
01539       DeleteLinearConstraint(&lc);
01540     }
01541 
01542   return(full);
01543 }
01544 
01545 boolean LinearizeGeneralEquation(double epsilon,
01546                                  Tbox *b,
01547                                  TSimplex *lp,TequationInfo *ei)
01548 {
01549   Tequation *eq;
01550   unsigned int i,k;
01551   Tvariable_set *vars;
01552   double *c;
01553   boolean full;
01554   Tinterval *is;
01555   unsigned int m;
01556   double l,u;
01557 
01558   m=GetBoxNIntervals(b);
01559   is=GetBoxIntervals(b);
01560 
01561   eq=GetOriginalEquation(ei);
01562 
01563   vars=GetEquationVariables(eq);
01564 
01565   NEW(c,m,double); 
01566   for(i=0;i<ei->n;i++)
01567     {
01568       k=GetVariableN(i,vars);
01569       l=LowerLimit(&(is[k]));
01570       u=UpperLimit(&(is[k]));
01571       if ((l==-INF)||(u==INF))
01572         c[k]=0.0;
01573       else
01574         c[k]=IntervalCenter(&(is[k]));
01575     }
01576 
01577   full=LinearizeGeneralEquationInt(GetEquationCmp(eq),epsilon,b,c,lp,ei);
01578 
01579   free(c);
01580 
01581   return(full);
01582 }
01583 
01584 boolean AddEquation2Simplex(unsigned int ne, /*eq identifier*/
01585                             double lr2tm_q,double lr2tm_s,
01586                             double epsilon,
01587                             double rho,
01588                             Tbox *b,
01589                             Tvariables *vs,
01590                             TSimplex *lp,
01591                             Tequations *eqs)
01592 {
01593   boolean full;
01594 
01595   full=TRUE;
01596 
01597   if (ne<eqs->n)/* No equation -> nothing done */
01598     {
01599       TequationInfo *ei;
01600       Tequation *eq;
01601       Tinterval r;
01602       double val;
01603       double s;
01604       boolean ctEq;
01605 
01606       ei=eqs->equation[ne];
01607       eq=GetOriginalEquation(ei);
01608 
01609       #if (_DEBUG>4)
01610         printf("     Adding equation %u of type %u to the simplex \n",ne,ei->EqType);
01611         printf("       ");
01612         PrintEquation(stdout,NULL,eq);
01613       #endif
01614 
01615       if (CropEquation(ne,epsilon,rho,b,eqs)==EMPTY_BOX)
01616         {
01617           #if (((!_USE_MPI)&&(_DEBUG==1))||(_DEBUG>4))
01618             fprintf(stderr,"C");
01619           #endif
01620           full=FALSE;
01621         }
01622       else
01623         {
01624           /*There is no need to add constant equations to the simplex.
01625             Thus, we evaluate the equations and if the result is almost
01626             constant we just check if the equation holds or not.
01627             If not, the system has no solution.
01628             The same applies to inequalities that already hold
01629           */
01630 
01631           EvaluateEquationInt(GetBoxIntervals(b),&r,eq);
01632           val=GetEquationValue(eq);
01633           IntervalOffset(&r,-val,&r);
01634       
01635           #if (_DEBUG>4)
01636             printf("         EvalIntEq:v");
01637             PrintInterval(stdout,&r);
01638             if (IntervalSize(&r)<INF)
01639               printf(" -> %g\n",IntervalSize(&r));
01640             else
01641               printf(" -> +inf\n");
01642           #endif
01643 
01644           ctEq=FALSE;
01645 
01646           switch(GetEquationCmp(eq))
01647             {
01648             case EQU:   
01649               if (IntervalSize(&r)<=(10*epsilon))
01650                 {
01651                   /* We have an (almost) constant equation -> just check whether
01652                      it holds or not but do not add it to the simplex */
01653                   /* We allow for some numerical inestabilities: numerical values
01654                      below epsilon are converted to zero when defining the linear
01655                      constraints and, consequently, we have to admit epsilon-like
01656                      errors
01657                   */
01658                   ctEq=TRUE;
01659                   full=((IsInside(0.0,&r))||
01660                         (fabs(LowerLimit(&r))<10*epsilon)||
01661                         (fabs(UpperLimit(&r))<10*epsilon));
01662 
01663                 }
01664               break;
01665 
01666             case LEQ:
01667               if (UpperLimit(&r)<0.0)
01668                 {
01669                   ctEq=TRUE;
01670                   full=TRUE;
01671                 }
01672               else
01673                 {
01674                   if (LowerLimit(&r)>0.0)
01675                     {
01676                       ctEq=TRUE;
01677                       full=FALSE;
01678                     }
01679                 }
01680               break;
01681             case GEQ:
01682               if (UpperLimit(&r)<0.0)
01683                 {
01684                   ctEq=TRUE;
01685                   full=FALSE;
01686                 }
01687               else
01688                 {
01689                   if (LowerLimit(&r)>0.0)
01690                     {
01691                       ctEq=TRUE;
01692                       full=TRUE;
01693                     }
01694                 }
01695               break;
01696             }
01697           
01698           #if (_DEBUG>4)
01699             if (ctEq)
01700               {
01701                 printf("     Constant Equation\n");
01702                 printf("       Eq eval:[%g %g] -> ",LowerLimit(&r),UpperLimit(&r));
01703                 if (full) 
01704                   printf("ct full equation\n\n");
01705                 else
01706                   printf("ct empty equation\n\n");
01707               }
01708           #endif
01709 
01710           if (!ctEq)
01711             {
01712               /* The equation is not constant, we proceed to add it into
01713                  the simplex tableau, linearizing if needed.*/
01714 
01715               s=GetBoxMinSizeVarSet(GetEquationVariables(eq),b);
01716 
01717               /* Use a particular linear relaxation depending on the
01718                  equation type */
01719               switch (ei->EqType)
01720                 {
01721                 case LINEAR_EQUATION:
01722                   full=SimplexAddNewConstraint(epsilon,
01723                                                ei->lc,
01724                                                GetEquationCmp(eq),
01725                                                GetBoxIntervals(b),lp);
01726                   break;
01727                   
01728                 case SADDLE_EQUATION:
01729                   if (s<lr2tm_s)
01730                     full=LinearizeGeneralEquation(epsilon,b,lp,eqs->equation[ne]);
01731                   else
01732                     full=LinearizeSaddleEquation(epsilon,b,lp,eqs->equation[ne]);
01733                   break;
01734                   
01735                 case PARABOLA_EQUATION:
01736                   if (s<lr2tm_q)
01737                     full=LinearizeGeneralEquation(epsilon,b,lp,eqs->equation[ne]);
01738                   else
01739                     full=LinearizeParabolaEquation(epsilon,b,lp,eqs->equation[ne]);
01740                   break;
01741                   
01742                 case CIRCLE_EQUATION:
01743                   if (s<lr2tm_q)
01744                     full=LinearizeGeneralEquation(epsilon,b,lp,eqs->equation[ne]);
01745                   else
01746                     full=LinearizeCircleEquation(epsilon,b,lp,eqs->equation[ne]);
01747                   break;
01748                       
01749                 case SPHERE_EQUATION:
01750                   if (s<lr2tm_q)
01751                     full=LinearizeGeneralEquation(epsilon,b,lp,eqs->equation[ne]);
01752                   else
01753                     full=LinearizeSphereEquation(epsilon,b,lp,eqs->equation[ne]);
01754                   break;
01755                   
01756                 case GENERAL_EQUATION:
01757                   full=LinearizeGeneralEquation(epsilon,b,lp,eqs->equation[ne]);
01758                   break;
01759                   
01760                 default:
01761                   Error("Unknown equation type in AddEquation2Simplex");
01762                 }
01763 
01764             } 
01765           #if ((!_USE_MPI)&&(_DEBUG==1))
01766             if (!full)
01767               fprintf(stderr,"A");
01768           #endif     
01769         }
01770     }
01771 
01772   return(full);
01773 }
01774 
01775 void UpdateSplitWeight(unsigned int ne,double *splitDim,
01776                        Tbox *b,Tequations *eqs)
01777 {
01778   unsigned int i,k;
01779   TequationInfo *ei;
01780   Tvariable_set *vars;
01781   double *c,*ev;
01782   Tinterval *is;
01783   unsigned int m;
01784 
01785   if (ne<eqs->n)
01786     {
01787       m=GetBoxNIntervals(b);
01788       is=GetBoxIntervals(b);
01789 
01790       ei=eqs->equation[ne];
01791  
01792       if (ei->EqType!=LINEAR_EQUATION)
01793         {
01794           vars=GetEquationVariables(ei->equation);
01795 
01796           NEW(c,m,double);
01797           NEW(ev,m,double); 
01798 
01799           for(i=0;i<ei->n;i++)
01800             {
01801               k=GetVariableN(i,vars);
01802               c[k]=IntervalCenter(&(is[k]));
01803             }
01804 
01805           ErrorDueToVariable(m,c,is,ev,ei);
01806 
01807           for(i=0;i<ei->n;i++)
01808             {
01809               k=GetVariableN(i,vars);
01810               splitDim[k]+=ev[i];
01811             }
01812 
01813           free(c);
01814           free(ev);
01815         }
01816     }
01817 }
01818 
01819 void PrintEquations(FILE *f,char **varNames,Tequations *eqs)
01820 {
01821   unsigned int i;
01822   
01823   if (eqs->s>0)
01824     {
01825       fprintf(f,"\n[SYSTEM EQS]\n\n");
01826       for(i=0;i<eqs->n;i++)
01827         {
01828           if (IsSystemEquation(i,eqs))
01829             PrintEquation(f,varNames,GetOriginalEquation(eqs->equation[i]));
01830         }
01831     }
01832 
01833   if (eqs->c>0)
01834     {
01835       fprintf(f,"\n[COORD EQS]\n\n");
01836       for(i=0;i<eqs->n;i++)
01837         {
01838           if (IsCoordEquation(i,eqs))
01839             PrintEquation(f,varNames,GetOriginalEquation(eqs->equation[i]));
01840         }
01841     }
01842 
01843   if (eqs->d>0)
01844     {
01845       fprintf(f,"\n[DUMMY EQS]\n\n");
01846       for(i=0;i<eqs->n;i++)
01847         {
01848           if (IsDummyEquation(i,eqs))
01849             PrintEquation(f,varNames,GetOriginalEquation(eqs->equation[i]));
01850         }
01851     }
01852 }
01853 
01854 void DeleteEquations(Tequations *eqs)
01855 {
01856   unsigned int i;
01857 
01858   for(i=0;i<eqs->n;i++)
01859     {    
01860       if (eqs->equation[i]!=NULL)
01861         {
01862           DeleteEquationInfo(eqs->equation[i]);
01863           free(eqs->equation[i]);
01864           eqs->equation[i]=NULL;
01865         }
01866     }
01867   eqs->n=0;
01868   eqs->s=0; 
01869   eqs->c=0; 
01870   eqs->d=0; 
01871   eqs->e=0; 
01872 
01873   free(eqs->equation);
01874   eqs->equation=NULL;
01875 }