equation.c

Go to the documentation of this file.

#include "equation.h"

#include "defines.h"

#include <string.h>
#include <math.h>

void ResetEquationMonomials(Tequation *eq);

void PurgeEquation(Tequation *eq);

void ResetEquationMonomials(Tequation *eq)
{
  unsigned int i;

  for(i=0;i<eq->nMonomials;i++)
    {
      DeleteMonomial(eq->monomials[i]);
      free(eq->monomials[i]);
      eq->monomials[i]=NULL;
    }

  eq->nMonomials=0;
}

void PurgeEquation(Tequation *eq)
{
  Tequation eq1;
  unsigned int i;
  
  InitEquation(&eq1);

  eq1.cmp=eq->cmp;
  eq1.type=eq->type;

  if (fabs(eq->value)<=ZERO)
    eq1.value=0.0;
  else
    eq1.value=eq->value;  
  for(i=0;i<eq->nMonomials;i++)
    {
      if (fabs(GetMonomialCt(eq->monomials[i]))>ZERO)
        AddMonomial(eq->monomials[i],&eq1);
    }
  DeleteEquation(eq);
  CopyEquation(eq,&eq1);
  DeleteEquation(&eq1);
}

void InitEquation(Tequation *eq)
{
  unsigned int i;

  eq->cmp=NOCMP;
  eq->type=NOTYPE_EQ;
  eq->order=0;
  eq->value=0.0;

  eq->maxMonomials=INIT_NUM_MONOMIALS;
  eq->nMonomials=0;
  NEW(eq->monomials,eq->maxMonomials,Tmonomial *);

  for(i=0;i<eq->maxMonomials;i++)
    eq->monomials[i]=NULL;

  InitVarSet(&(eq->vars));
}

void EquationFromLinearConstraint(TLinearConstraint *lc,Tequation *eq)
{
  Tinterval error;
  double ct;
  unsigned int cmp;
  Tmonomial m;
  unsigned int i,n;

  InitEquation(eq);

  GetLinearConstraintError(&error,lc);
  if (IntervalSize(&error)==0)
    {
      ct=IntervalCenter(&error);
      cmp=EQU;
    }
  else
    {
      if (LowerLimit(&error)==-INF)
        {
          ct=UpperLimit(&error);
          cmp=LEQ;
        }
      else
        {
          if (UpperLimit(&error)==INF)
            {
              ct=LowerLimit(&error);
              cmp=GEQ;
            }
          else
            Error("The input linear constraint can not be converted into a single equation");
        }
    }
  eq->cmp=cmp;
  eq->value=ct;

  n=GetNumTermsInLinearConstraint(lc);
  for(i=0;i<n;i++)
    {
      InitMonomial(&m);
      AddCt2Monomial(GetLinearConstraintCoefficient(i,lc),&m);
      AddVariable2Monomial(GetLinearConstraintVariable(i,lc),1,&m);
      
      AddMonomial(&m,eq);
      DeleteMonomial(&m);
    }
}


void EquationFromLinearConstraintProduct(TLinearConstraint *lc1,TLinearConstraint *lc2,Tequation *eq)
{
  Tinterval error1,error2;
  double ct1,ct2;
  Tmonomial m;
  unsigned int i,j,n1,n2;

  InitEquation(eq);

  GetLinearConstraintError(&error1,lc1);
  if (IntervalSize(&error1)==0)
    ct1=IntervalCenter(&error1);
  else
    Error("The input linear constraint can not be converted into an equation");

  GetLinearConstraintError(&error2,lc2);
  if (IntervalSize(&error2)==0)
    ct2=IntervalCenter(&error2);
  else
    Error("The input linear constraint can not be converted into an equation");
    
  n1=GetNumTermsInLinearConstraint(lc1);
  n2=GetNumTermsInLinearConstraint(lc2);
  for(i=0;i<n1;i++)
    {
      for(j=0;j<n2;j++) 
        {
          InitMonomial(&m);
          AddCt2Monomial(GetLinearConstraintCoefficient(i,lc1),&m);
          AddVariable2Monomial(GetLinearConstraintVariable(i,lc1),1,&m);

          AddCt2Monomial(GetLinearConstraintCoefficient(j,lc2),&m);
          AddVariable2Monomial(GetLinearConstraintVariable(j,lc2),1,&m);
          
          AddMonomial(&m,eq);
          DeleteMonomial(&m);
        }
    }

  for(i=0;i<n1;i++)
    {
      InitMonomial(&m);
      AddCt2Monomial(ct2,&m);
      AddCt2Monomial(GetLinearConstraintCoefficient(i,lc1),&m);
      AddVariable2Monomial(GetLinearConstraintVariable(i,lc1),1,&m);
      AddMonomial(&m,eq);
      DeleteMonomial(&m);
    }

  for(j=0;j<n2;j++) 
    {
      InitMonomial(&m);
      AddCt2Monomial(ct1,&m);
      AddCt2Monomial(GetLinearConstraintCoefficient(j,lc2),&m);
      AddVariable2Monomial(GetLinearConstraintVariable(j,lc2),1,&m);
      AddMonomial(&m,eq);
      DeleteMonomial(&m);
    }
}

void CopyEquation(Tequation *eq_dst,Tequation *eq_orig)
{
  unsigned int i;

  eq_dst->cmp=eq_orig->cmp;
  eq_dst->type=eq_orig->type;
  eq_dst->value=eq_orig->value;
  
  eq_dst->maxMonomials=eq_orig->maxMonomials;
  eq_dst->nMonomials=0;
  NEW(eq_dst->monomials,eq_dst->maxMonomials,Tmonomial*);

  InitVarSet(&(eq_dst->vars));
  eq_dst->order=0;

  for(i=0;i<eq_orig->nMonomials;i++)
    AddMonomial(eq_orig->monomials[i],eq_dst);
}

void RewriteEquation(double epsilon,Tmapping *map,Tequation *eqOut,Tequation *eq)
{
  unsigned int i,nv,o;
  double ct;
  unsigned int v,v1,v2;
  TLinearConstraint lc,lc1,lc2;
  Tvariable_set *vars;
  Tequation eqTmp;

  InitEquation(eqOut);
  eqOut->cmp=eq->cmp;
  eqOut->type=eq->type;
  eqOut->value=eq->value;
  
  for(i=0;i<eq->nMonomials;i++)
    {
      vars=GetMonomialVariables(eq->monomials[i]);
      nv=VariableSetSize(vars);
      switch (nv)
        {
        case 0:
          AddMonomial(eq->monomials[i],eqOut);
          break;

        case 1:
          v=GetVariableN(0,vars);
          GetOriginalVarRelation(v,&lc,map);
          o=MonomialOrder(eq->monomials[i]);

          if (o==1)
            EquationFromLinearConstraint(&lc,&eqTmp);
          else
            {
              if (o==2)
                EquationFromLinearConstraintProduct(&lc,&lc,&eqTmp);
              else
                Error("Cubic factor in a equation");
            }
          ct=GetMonomialCt(eq->monomials[i]);
          AccumulateEquations(&eqTmp,ct,eqOut);

          DeleteLinearConstraint(&lc);
          DeleteEquation(&eqTmp);
          break;

        case 2:
          v1=GetVariableN(0,vars);
          GetOriginalVarRelation(v1,&lc1,map);

          v2=GetVariableN(1,vars);
          GetOriginalVarRelation(v2,&lc2,map);

          EquationFromLinearConstraintProduct(&lc1,&lc2,&eqTmp);
          ct=GetMonomialCt(eq->monomials[i]);
          AccumulateEquations(&eqTmp,ct,eqOut);

          DeleteLinearConstraint(&lc1);
          DeleteLinearConstraint(&lc2);
          DeleteEquation(&eqTmp);
          break;

        default:
          Error("Only constant, linear or bilinear monomials are allowed");
        }
    }
  PurgeEquation(eqOut);
}


void AccumulateEquations(Tequation *eqn,double ct,Tequation *eq)
{
  unsigned int i;
  Tmonomial fnew;

  for(i=0;i<eqn->nMonomials;i++)
    {
      CopyMonomial(&fnew,eqn->monomials[i]);
      AddCt2Monomial(ct,&fnew);
      AddMonomial(&fnew,eq);
      DeleteMonomial(&fnew);
    }
  eq->value+=(ct*eqn->value);
  PurgeEquation(eq);
}

void ResetEquation(Tequation *eq)
{
  eq->cmp=NOCMP;
  eq->type=NOTYPE_EQ;
  eq->value=0.0;
  eq->order=0;

  ResetEquationMonomials(eq);

  ResetVarSet(&(eq->vars));
}

/*
    Returns
      0: normal case 
      1: the equation becomes constant and it holds
      2  the equation becomes constant and it does not hold
*/
unsigned int FixVariableInEquation(double epsilon,unsigned int nv,double b,Tequation *eq)
{ 
  TLinearConstraint lc;
  unsigned int r;

  InitLinearConstraint(&lc);

  AddCt2LinearConstraint(b,&lc);
  r=ReplaceVariableInEquation(epsilon,nv,&lc,eq);
  DeleteLinearConstraint(&lc);
  
  return(r);
}

/*
    Returns
      0: normal case 
      1: the equation becomes constant and it holds
      2  the equation becomes constant and it does not hold
*/
unsigned int ReplaceVariableInEquation(double epsilon,unsigned int nv,
                                       TLinearConstraint *lc,Tequation *eq)
{ 
  unsigned int i,j,k,n;
  Tequation eqNew;
  Tmonomial m;
  unsigned int np;
  unsigned int p;
  Tvariable_set *vm;
  Tinterval error;
  double a,a1,ct;
  unsigned int ind,ind1;

  InitEquation(&eqNew);
  SetEquationCmp(GetEquationCmp(eq),(&eqNew));
  SetEquationType(GetEquationType(eq),(&eqNew));
  SetEquationValue(GetEquationValue(eq),(&eqNew));

  GetLinearConstraintError(&error,lc);
  if (IntervalSize(&error)>ZERO)
    Error("Only linear constraints with constant offset can be used in simplification");
  ct=-IntervalCenter(&error);

  n=GetNumTermsInLinearConstraint(lc);

  i=0;
  while(i<eq->nMonomials)
    { 
      vm=GetMonomialVariables(eq->monomials[i]);
      np=GetPlaceinSet(nv,vm);
      if (np!=NO_UINT) /*The variable is in the monomial */
        {
          p=GetVariablePowerN(np,vm);
          if (p==1)
            {
              for(k=0;k<n;k++)
                {
                  a=GetLinearConstraintCoefficient(k,lc);
                  ind=GetLinearConstraintVariable(k,lc);

                  CopyMonomial(&m,eq->monomials[i]);
                  AddVariable2Monomial(ind,1,&m);
                  FixVariableInMonomial(nv,1,&m);
                  AddCt2Monomial(a,&m);
                  AddMonomial(&m,&eqNew);
                  DeleteMonomial(&m);
                }

              CopyMonomial(&m,eq->monomials[i]);
              FixVariableInMonomial(nv,1,&m);
              AddCt2Monomial(ct,&m);
              AddMonomial(&m,&eqNew);
              DeleteMonomial(&m);
            }
          else
            {
              if ((p>2)||(VariableSetSize(vm)>1))
                Error("Monomials should be lineal, bilineal or quadratic");
              /* here we have a monomial of the for c*x^2 and we need to
                 apply a subtitution x=a*y+b that gives
                     c*(a*y+b)^2=c*(a^2*y^2+2*a*y*b+b^2)=c*(a*y)^2+c*(2*a*b*y)+c*(b^2)
              */
              for(k=0;k<n;k++)
                {
                  a=GetLinearConstraintCoefficient(k,lc);
                  ind=GetLinearConstraintVariable(k,lc);

                  for(j=0;j<n;j++)
                    {
                      a1=GetLinearConstraintCoefficient(j,lc);
                      ind1=GetLinearConstraintVariable(j,lc);

                      CopyMonomial(&m,eq->monomials[i]);
                      AddVariable2Monomial(ind,1,&m);
                      AddVariable2Monomial(ind1,1,&m);
                      FixVariableInMonomial(nv,1,&m);
                      AddCt2Monomial(a*a1,&m);
                      AddMonomial(&m,&eqNew);
                      DeleteMonomial(&m);
                    }

                  CopyMonomial(&m,eq->monomials[i]);
                  AddVariable2Monomial(ind,1,&m);
                  FixVariableInMonomial(nv,1,&m);
                  AddCt2Monomial(2*a*ct,&m);
                  AddMonomial(&m,&eqNew);
                  DeleteMonomial(&m);
                }

              CopyMonomial(&m,eq->monomials[i]);
              FixVariableInMonomial(nv,1,&m);
              AddCt2Monomial(ct*ct,&m);
              AddMonomial(&m,&eqNew);
              DeleteMonomial(&m);
            }
        }
      else
        {
          CopyMonomial(&m,eq->monomials[i]);
          ShiftVarIndexes(nv,GetMonomialVariables(&m));
          AddMonomial(&m,&eqNew);
          DeleteMonomial(&m);
        }
      i++;
    }

  DeleteEquation(eq);
  CopyEquation(eq,&eqNew);
  DeleteEquation(&eqNew);

  NormalizeEquation(eq);

  if (eq->nMonomials>0)
    return(0);
  else
    {
      boolean hold=TRUE;

      switch (eq->cmp)
        {
        case EQU:
          hold=(fabs(eq->value)<=epsilon);
          break;
        case LEQ:
          hold=(-epsilon<=eq->value);
          break;
        case GEQ:
          hold=( epsilon>=eq->value);
          break;
        }

      if (!hold)
        {         
          printf("Ct equation does not hold: %g\n",eq->value);
          return(2);
        }
      else
        return(1);
    }
}

void CtScaleEquation(double ct,Tequation *eq)
{
  if (eq->cmp==EQU)
    {
      unsigned int i;
      
      if (fabs(ct)<ZERO)
        Error("Scaling an equation by 0");
      
      for(i=0;i<eq->nMonomials;i++)
        AddCt2Monomial(ct,eq->monomials[i]);
      eq->value*=ct;
    }
   else
     Error("CtScaleEquation for non-equalities is not implemented yet");
}

void VarScaleEquation(unsigned int v,Tequation *eq)
{
   if (eq->cmp==EQU)
    {
      unsigned int i;
      Tmonomial m;
      
      ResetVarSet(&(eq->vars));
      for(i=0;i<eq->nMonomials;i++)
        {
          AddVariable2Monomial(v,1,eq->monomials[i]);
          UnionVarSet(&(eq->vars),GetMonomialVariables(eq->monomials[i]),&(eq->vars));
        }
      
      InitMonomial(&m);
      AddCt2Monomial(-eq->value,&m);
      AddVariable2Monomial(v,1,&m);
      AddMonomial(&m,eq); 
      DeleteMonomial(&m);
      
      eq->value=0;
    } 
   else
     Error("VarScaleEquation for non-equalities is not implemented yet");
}

/*
  Simple form of normalization to avoid numerical inestabilities
  If all monomials have the same ct (up to ZERO) we make them
  all equal to one. 
  This helps in many cases when we replace variables to avoid
  transforming a circle equation into a scaled circle equation
  (that is not identified as a circle!)
*/
void NormalizeEquation(Tequation *eq)
{
  boolean equal;
  unsigned int i;
  double s,s1;

  PurgeEquation(eq);
  if ((eq->nMonomials>0)&&(eq->cmp==EQU))
    {
      /*first monomial is set to possitive*/
      s=GetMonomialCt(eq->monomials[0]);
      s=(s<0?-1.0:1.0); 
      for(i=0;i<eq->nMonomials;i++)
        AddCt2Monomial(s,eq->monomials[i]);
      eq->value*=s;

      /* if all constants in the momomials are "almost" the same
         we set all them to 1 (or -1)*/
      equal=TRUE;
      s=GetMonomialCt(eq->monomials[0]); /* s is possitive !! */
      for(i=1;((equal)&&(i<eq->nMonomials));i++)
        {
          s1=GetMonomialCt(eq->monomials[i]);
          equal=((fabs(s-s1)<=ZERO)||(fabs(s+s1)<=ZERO));
        }

      if (equal)
        {
          if (fabs(eq->value-s)<=ZERO)
            eq->value=1;
          else
            {
              if (fabs(eq->value+s)<=ZERO)
                eq->value=-1;
              else
                eq->value/=s;
            }
          for(i=0;i<eq->nMonomials;i++)
            {
              s1=GetMonomialCt(eq->monomials[i]);
              if (s1>0)
                SetMonomialCt(+1,eq->monomials[i]);
              else
                SetMonomialCt(-1,eq->monomials[i]);
            }
        }
    }
}

boolean IsSimplificable(unsigned int simpLevel,unsigned int nTerms,boolean *systemVars,
                        unsigned int *v,TLinearConstraint *lc,
                        Tequation *eq)
{
  boolean s;

  s=FALSE;
  *v=NO_UINT;

  if ((eq->cmp==EQU)&&
      (simpLevel>0)&&
      (((simpLevel==1)&&(nTerms==0))||
       ((simpLevel==2)&&(nTerms<=MAX_TERMS_SIMP))||
       ((simpLevel==3)&&(nTerms<=MAX_TERMS_SIMP))
       ))
    {
      unsigned int o,nveq,i,j,l,r;
      double ct;
      boolean found;
            
      o=GetEquationOrder(eq);
      nveq=VariableSetSize(&(eq->vars));

      switch(o)
        {
        case 1:
          if (nveq==(nTerms+1))
            {
              if (simpLevel<3) /*simpLevel=1,2 -> nTerms = 0,1 -> nveq=1,2*/
                {
                  /* x=ct */
                  /* x + a * y = ct */
                  found=FALSE;
                  /* two loops, in the first one we try to find a non-system variable
                     we can put in function of other (including system) variables.
                     If this is not possible, we try, as a last resort, to put remove
                     system variables (to put them in function of other variables, possibly
                     non system ones) */
                  for(r=0;((!found)&&(r<2));r++)
                    {
                      i=0;
                      while((!found)&&(i<eq->nMonomials))
                        {
                          l=GetVariableN(0,GetMonomialVariables(eq->monomials[i]));

                          if (((r==0)&&(!systemVars[l]))|| 
                              ((r==1)&&(systemVars[l])))
                            {
                              ct=GetMonomialCt(eq->monomials[i]);
                              
                              if ((ct==1)||(ct==-1))
                                found=TRUE;
                              else
                                i++;
                            }
                          else
                            i++;
                        }
                    }
                }
              else
                {
                  /*simpLevel=3 -> nTerms=2*/
                  /*General case: a*x+b*y=ct*/
                  unsigned int k;
                  double d,d1,d2,ct1,dm;

                  found=FALSE;
                  /* two loops, in the first one we try to find a non-system variable
                     we can put in function of other (including system) variables.
                     If this is not possible, we try, as a last resort, to put remove
                     system variables (to put them in function of other variables, possibly
                     non system ones) */
                  for(r=0;((!found)&&(r<2));r++)
                    {
                      dm=INF;
                      for(k=0;k<eq->nMonomials;k++)
                        {
                          l=GetVariableN(0,GetMonomialVariables(eq->monomials[k]));

                          if (((r==0)&&(!systemVars[l]))||
                              ((r==1)&&(systemVars[l])))
                            {
                              ct1=GetMonomialCt(eq->monomials[k]);
                              d1=fabs(ct1-1);
                              d2=fabs(ct1+1);
                              d=(d1<d2?d1:d2);
                              if (d<dm)
                                {
                                  found=TRUE;
                                  i=k;
                                  ct=ct1;
                                  dm=d;
                                }
                            }
                        }
                    }
                }

              if (found)
                {
                  InitLinearConstraint(lc);
                  *v=GetVariableN(0,GetMonomialVariables(eq->monomials[i]));
                  s=TRUE;
                  for(j=0;j<eq->nMonomials;j++)
                    {
                      if (j!=i)
                        {
                          AddTerm2LinearConstraint(GetVariableN(0,GetMonomialVariables(eq->monomials[j])),
                                                   -GetMonomialCt(eq->monomials[j]),
                                                   lc
                                                   );
                        }
                    }
                  AddCt2LinearConstraint(eq->value,lc);
                  if (ct!=1)
                    {
                      if (ct==-1)
                        InvertLinearConstraint(lc);
                      else
                        ScaleLinearConstraint(1/ct,lc);   
                    }
                }
            }     
          break;
        case 2:
          if ((nveq==1)&&(nTerms==0)&&(eq->value==0))
            {
              /* a x^2 =0  -> x=0 */
              s=TRUE;
              
              InitLinearConstraint(lc);
              *v=GetVariableN(0,GetMonomialVariables(eq->monomials[0]));
            }
          break;
        }
    }

  return(s);
}


void SetEquationType(unsigned int type,Tequation *eq)
{
  eq->type=type;
}

void SetEquationCmp(unsigned int cmp,Tequation *eq)
{
  if ((cmp!=EQU)&&(cmp!=LEQ)&&(cmp!=GEQ))
    Error("Unknow equation cmp");

  eq->cmp=cmp;
}

void SetEquationValue(double v,Tequation *eq)
{
  eq->value=v;
}

boolean LinearEquation(Tequation *eq)
{
  boolean l;
  unsigned int i;

  l=TRUE;
  for(i=0;((l)&&(i<eq->nMonomials));i++)
    {
      l=((CtMonomial(eq->monomials[i]))||(LinearMonomial(eq->monomials[i])));
    }
  return(l);
}

boolean BilinearEquation(Tequation *eq)
{  
  /* A polynomial equation with monomial of order at most two (quadratic or involving
     at most two variables) */

  boolean b;
  unsigned int i;

  b=TRUE;
  for(i=0;((b)&&(i<eq->nMonomials));i++)
    {
      b=((CtMonomial(eq->monomials[i]))||
         (LinearMonomial(eq->monomials[i]))||
         (QuadraticMonomial(eq->monomials[i]))||
         (BilinearMonomial(eq->monomials[i])));
    }
  return(b);
}

boolean CircleEquation(Tequation *eq)
{
  /* x^2 + y^2 = r^2 */

  boolean c=FALSE;

  if ((eq->nMonomials==2)&&
      (GetMonomialCt(eq->monomials[0])==1.0)&&(QuadraticMonomial(eq->monomials[0]))&&
      (GetMonomialCt(eq->monomials[1])==1.0)&&(QuadraticMonomial(eq->monomials[1]))&&
      (eq->value>0.0))
    c=TRUE;
    
  return(c);
}

boolean SphereEquation(Tequation *eq)
{
  /* x^2 + y^2 + z^2 = r^2 */
  boolean s=FALSE;

  if ((eq->nMonomials==3)&&(eq->cmp==EQU)&&(eq->value>0.0)&&
      (GetMonomialCt(eq->monomials[0])==1.0)&&(QuadraticMonomial(eq->monomials[0]))&&
      (GetMonomialCt(eq->monomials[1])==1.0)&&(QuadraticMonomial(eq->monomials[1]))&&
      (GetMonomialCt(eq->monomials[2])==1.0)&&(QuadraticMonomial(eq->monomials[2]))&&
      (eq->cmp==EQU))
    s=TRUE;

  return(s);
}

boolean SaddleEquation(Tequation *eq)
{  
  /* x*y + \alpha*z = 0  -> z = (-1/\alpha)*x*y   */
  boolean s=FALSE;
  
  if ((eq->nMonomials==2)&&(eq->cmp==EQU)&&(eq->value==0.0)&&
      (GetMonomialCt(eq->monomials[0])==1.0)&&(BilinearMonomial(eq->monomials[0]))&&
      (LinearMonomial(eq->monomials[1]))&&
      (eq->cmp==EQU))
    s=TRUE;

  return(s);
}

boolean ParabolaEquation(Tequation *eq)
{  
  /* x^2 + \alpha*y = 0 -> y = (-1/alpha)*x^2 */
  boolean s=FALSE;
  
  if ((eq->nMonomials==2)&&(eq->cmp==EQU)&&(eq->value==0.0)&&
      (GetMonomialCt(eq->monomials[0])==1.0)&&(QuadraticMonomial(eq->monomials[0]))&&
      (LinearMonomial(eq->monomials[1]))&&
      (eq->cmp==EQU))
    s=TRUE;

  return(s);
}

unsigned int GetEquationCmp(Tequation *eq)
{
  return(eq->cmp);
}

unsigned int GetEquationType(Tequation *eq)
{
  return(eq->type);
}

double GetEquationValue(Tequation *eq)
{
  return(eq->value);
}

unsigned int GetEquationOrder(Tequation *eq)
{
  return(eq->order);
}

Tvariable_set *GetEquationVariables(Tequation *eq)
{
  return(&(eq->vars));
}

unsigned int GetEquationNumVariables(Tequation *eq)
{
  return(VariableSetSize(GetEquationVariables(eq)));
}

unsigned int CmpEquations(Tequation *eq1,Tequation *eq2)
{  

  unsigned int r;

  if (eq1->type<eq2->type)
    r=2;
  else
    {
      if (eq2->type<eq1->type)
        r=1;
      else
        {
          /* Here both equations are of the same type */
          unsigned int o1,o2;

          o1=GetEquationOrder(eq1);
          o2=GetEquationOrder(eq2);

          if ((eq1->cmp>eq2->cmp)||
              ((eq1->cmp==eq2->cmp)&&(o1>o2))||
              ((eq1->cmp==eq2->cmp)&&(o1==o2)&&(eq1->nMonomials>eq2->nMonomials)))
            r=1;
          else
            {
              if ((eq2->cmp>eq1->cmp)||
                  ((eq1->cmp==eq2->cmp)&&(o2>o1))||
                  ((eq1->cmp==eq2->cmp)&&(o1==o2)&&(eq2->nMonomials>eq1->nMonomials)))
                r=2;
              else
                {
                  /*the two equations have the same cmp, order, and number of monomials*/
                  unsigned int i;
          
                  i=0;
                  r=0;
                  while ((i<eq1->nMonomials)&&(r==0))
                    {
                      r=CmpMonomial(eq1->monomials[i],eq2->monomials[i]);
                      if (r==0)
                        i++;
                    }

                  if (r==0)
                    {
                      /*the 2 equations have the same order and the same
                        set of monomials. We compare the value*/
                      if (eq1->value>eq2->value)
                        r=1;
                      else
                        {
                          if (eq2->value>eq1->value)
                            r=2;
                          else
                            r=0;
                        }
                    }
                }
            }
        }
    }
  if (r!=0)
    return(2);
  else
    return(r);
}

void AddMonomial(Tmonomial* f,Tequation *eq)
{
  if ((!EmptyMonomial(f))&&(fabs(GetMonomialCt(f))>ZERO))
    {
      if (CtMonomial(f))
        eq->value-=GetMonomialCt(f);
      else
        {
          unsigned int j;

          j=FindMonomial(f,eq);

          if (j!=NO_UINT)
            {
              /* The equation already has a monomial with the same equations as the
                 one we are adding -> just add the constants*/
              SetMonomialCt(GetMonomialCt(eq->monomials[j])+GetMonomialCt(f),eq->monomials[j]);

              if (CtMonomial(eq->monomials[j]))
                {
                  unsigned int n;

                  /* A non-constant monomial can only become constant if it
                     becomes 0 */
                  if (fabs(GetMonomialCt(eq->monomials[j]))>ZERO)
                    Error("Non Zero constant monomial");
                  
                  /*If the monomial becomes ct (i.e., 0) we have to compact the
                    monomials and recompute the varset for the equation*/

                  /*Remove the Zeroed monomial from the equation*/
                  DeleteMonomial(eq->monomials[j]);
                  free(eq->monomials[j]);

                  for(n=j+1;n<eq->nMonomials;n++)
                    eq->monomials[n-1]=eq->monomials[n];
                  eq->nMonomials--;
                  eq->monomials[eq->nMonomials]=NULL;

                  /*Recompute the variables in the equation*/
                  ResetVarSet(&(eq->vars));
                  for(n=0;n<eq->nMonomials;n++)
                    UnionVarSet(&(eq->vars),GetMonomialVariables(eq->monomials[n]),&(eq->vars));
                }
            }
          else
            {
              /* A new monomial -> add it to the equation */
              unsigned int o,j;
              Tmonomial *s;

              if (eq->nMonomials==eq->maxMonomials)
                MEM_DUP(eq->monomials,eq->maxMonomials,Tmonomial*);

              NEW(eq->monomials[eq->nMonomials],1,Tmonomial);
              CopyMonomial(eq->monomials[eq->nMonomials],f);
              eq->nMonomials++;
      
              o=MonomialOrder(f);
              if (eq->order<o)
                eq->order=o;
              
              UnionVarSet(&(eq->vars),GetMonomialVariables(f),&(eq->vars));

              j=eq->nMonomials-1;
              while((j>0)&&(CmpMonomial(eq->monomials[j-1],eq->monomials[j])==1))
                {
                  s=eq->monomials[j-1];
                  eq->monomials[j-1]=eq->monomials[j];
                  eq->monomials[j]=s;

                  j--;
                }
            }
        }
    }
}

void GenerateParabolaEquation(unsigned int vx,unsigned int vy,Tequation *eq)
{
  GenerateScaledParabolaEquation(1,vx,vy,eq);
}

void GenerateScaledParabolaEquation(double s,
                                    unsigned int vx,unsigned int vy,Tequation *eq)
{
  Tmonomial f;

  if (s==0)
    Error("Defining a scaled parabola equation with scale equal to 0.");

  InitEquation(eq);
  InitMonomial(&f); 

  AddVariable2Monomial(vx,2,&f);
  AddMonomial(&f,eq);
  ResetMonomial(&f);

  AddVariable2Monomial(vy,1,&f);
  AddCt2Monomial(-s,&f);
  AddMonomial(&f,eq);
  
  DeleteMonomial(&f);

  SetEquationCmp(EQU,eq);
  SetEquationValue(0,eq);
  SetEquationType(DUMMY_EQ,eq);
}

void GenerateSaddleEquation(unsigned int vx,unsigned int vy,unsigned int vz,
                            Tequation *eq)
{
  GenerateScaledSaddleEquation(1,vx,vy,vz,eq);
}

void GenerateScaledSaddleEquation(double s,
                                  unsigned int vx,unsigned int vy,unsigned int vz,
                                  Tequation *eq)
{
  if (s==0)
    Error("Defining a scaled saddle equation with scale equal to 0.");

  if (vx==vy)
    GenerateScaledParabolaEquation(s,vx,vz,eq);
  else
    {
      Tmonomial f;

      InitEquation(eq);
      InitMonomial(&f); 
      
      AddVariable2Monomial(vx,1,&f);
      AddVariable2Monomial(vy,1,&f);
      AddMonomial(&f,eq);
      ResetMonomial(&f);
      
      AddVariable2Monomial(vz,1,&f);
      AddCt2Monomial(-s,&f);
      AddMonomial(&f,eq);
      
      DeleteMonomial(&f);
      
      SetEquationCmp(EQU,eq); 
      SetEquationValue(0,eq);
      SetEquationType(DUMMY_EQ,eq);
    }
}

void GenerateNormEquation(unsigned int vx,unsigned int vy,unsigned int vz,
                          double n,
                          Tequation *eq)
{
  unsigned int v[3];

  v[0]=vx;
  v[1]=vy;
  v[2]=vz;
  GenerateGeneralNormEquation(3,v,n,eq);
}

void GenerateGeneralNormEquation(unsigned int nv,unsigned int *v,double n,Tequation *eq)
{
  Tmonomial f;
  unsigned int i;

  InitEquation(eq);

  InitMonomial(&f);
  for(i=0;i<nv;i++)
    {
      AddVariable2Monomial(v[i],2,&f);
      AddMonomial(&f,eq);
      ResetMonomial(&f);
    }
  DeleteMonomial(&f);

  SetEquationCmp(EQU,eq);
  SetEquationValue(n,eq);
  SetEquationType(SYSTEM_EQ,eq);
}

void GenerateCrossProductEquations(unsigned int v1x,unsigned int v1y,unsigned int v1z,
                                   unsigned int v2x,unsigned int v2y,unsigned int v2z,
                                   unsigned int v3x,unsigned int v3y,unsigned int v3z,
                                   unsigned int vs,
                                   double s,
                                   Tequation *eq)
{
  
  Tmonomial f;
  unsigned int i;

  for(i=0;i<3;i++)
    InitEquation(&(eq[i]));
  
  InitMonomial(&f);

  AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1y,1,&f);AddVariable2Monomial(v2z,1,&f);
  AddMonomial(&f,&(eq[0]));ResetMonomial(&f);
  AddCt2Monomial(-1.0,&f);AddVariable2Monomial(v1z,1,&f);AddVariable2Monomial(v2y,1,&f);
  AddMonomial(&f,&(eq[0]));ResetMonomial(&f);
  AddCt2Monomial(-1,&f);
  if (vs==NO_UINT)
    AddCt2Monomial(s,&f);
  else
    AddVariable2Monomial(vs,1,&f);
  AddVariable2Monomial(v3x,1,&f);
  AddMonomial(&f,&(eq[0]));ResetMonomial(&f);

  AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1z,1,&f);AddVariable2Monomial(v2x,1,&f);
  AddMonomial(&f,&(eq[1]));ResetMonomial(&f);
  AddCt2Monomial(-1.0,&f);AddVariable2Monomial(v1x,1,&f);AddVariable2Monomial(v2z,1,&f);
  AddMonomial(&f,&(eq[1]));ResetMonomial(&f);
  AddCt2Monomial(-1,&f);
  if (vs==NO_UINT)
    AddCt2Monomial(s,&f);
  else
    AddVariable2Monomial(vs,1,&f);
  AddVariable2Monomial(v3y,1,&f);
  AddMonomial(&f,&(eq[1]));ResetMonomial(&f);

  AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1x,1,&f);AddVariable2Monomial(v2y,1,&f);
  AddMonomial(&f,&(eq[2]));ResetMonomial(&f);
  AddCt2Monomial(-1.0,&f);AddVariable2Monomial(v1y,1,&f);AddVariable2Monomial(v2x,1,&f);
  AddMonomial(&f,&(eq[2]));ResetMonomial(&f);
  AddCt2Monomial(-1,&f);
  if (vs==NO_UINT)
    AddCt2Monomial(s,&f);
  else
    AddVariable2Monomial(vs,1,&f);
  AddVariable2Monomial(v3z,1,&f);
  AddMonomial(&f,&(eq[2]));

  DeleteMonomial(&f);

  for(i=0;i<3;i++)
    {
      SetEquationCmp(EQU,&(eq[i]));
      SetEquationValue(0.0,&(eq[i]));
      SetEquationType(SYSTEM_EQ,&(eq[i]));
    }
}

void GenerateDotProductEquation(unsigned int v1x,unsigned int v1y,unsigned int v1z,
                                unsigned int v2x,unsigned int v2y,unsigned int v2z,
                                unsigned int vc,
                                double c,
                                Tequation *eq)
{
  Tmonomial f;  
  
  InitEquation(eq);

  InitMonomial(&f); 
  
  AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1x,1,&f);AddVariable2Monomial(v2x,1,&f);
  AddMonomial(&f,eq);ResetMonomial(&f);
  AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1y,1,&f);AddVariable2Monomial(v2y,1,&f);
  AddMonomial(&f,eq);ResetMonomial(&f);
  AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1z,1,&f);AddVariable2Monomial(v2z,1,&f);
  AddMonomial(&f,eq);ResetMonomial(&f);

  if (vc!=NO_UINT)
    {
      AddCt2Monomial(-1.0,&f);AddVariable2Monomial(vc,1,&f);
      AddMonomial(&f,eq);ResetMonomial(&f);
      SetEquationValue(0.0,eq);
    }
  else
    SetEquationValue(c,eq);

  DeleteMonomial(&f); 
    
  SetEquationCmp(EQU,eq);
  SetEquationType(SYSTEM_EQ,eq);
 
}

unsigned int FindMonomial(Tmonomial *f,Tequation *eq)
{
  unsigned int j;
  boolean found;
  Tvariable_set *vf;

  vf=GetMonomialVariables(f);

  found=FALSE;
  j=0;
  while((j<eq->nMonomials)&&(!found))
    {
      if (CmpVarSet(vf,GetMonomialVariables(eq->monomials[j]))==0)
        found=TRUE;
      else
        j++;
    }
  if (found)
    return(j);
  else
    return(NO_UINT);
}

Tmonomial *GetMonomial(unsigned int i,Tequation *eq)
{
  if (i<eq->nMonomials)
    return(eq->monomials[i]);
  else
    return(NULL);
}

unsigned int GetNumMonomials(Tequation *eq)
{
  return(eq->nMonomials);
}

void LinearEquation2LinearConstraint(TLinearConstraint *lc,Tequation *eq)
{
  InitLinearConstraint(lc);
  if (GetEquationOrder(eq)<2)
    {
      unsigned int i,v;
      double ct;

      for(i=0;i<eq->nMonomials;i++)
        {
          v=GetVariableN(0,GetMonomialVariables(eq->monomials[i]));
          ct=GetMonomialCt(eq->monomials[i]);
          AddTerm2LinearConstraint(v,ct,lc);
        }
      AddCt2LinearConstraint(-eq->value,lc);
    }
}

double EvaluateEquation(double *varValues,Tequation *eq)
{
  double v;
  unsigned int i;

  if (eq->cmp==NOCMP)
    Error("Evaluation of an undefined equation");

  v=0.0;

  for(i=0;i<eq->nMonomials;i++)
    v+=EvaluateMonomial(varValues,eq->monomials[i]);

  return(v);
}

void EvaluateEquationInt(Tinterval *varValues,Tinterval *i_out,Tequation *eq)
{
  unsigned int i;
  Tinterval i_aux;

  if (eq->cmp==NOCMP)
    Error("Evaluation of an undefined equation");

  NewInterval(0,0,i_out);
  for(i=0;i<eq->nMonomials;i++)
    {
      EvaluateMonomialInt(varValues,&i_aux,eq->monomials[i]);
      IntervalAdd(i_out,&i_aux,i_out);
    }
}

void DeriveEquation(unsigned int nv,Tequation *d,Tequation *eq)
{
  Tmonomial f;
  unsigned int i;

  d->cmp=EQU;
  d->type=DERIVED_EQ;
  d->order=0;
  d->value=0;
  
  d->maxMonomials=eq->maxMonomials;
  d->nMonomials=0;
  NEW(d->monomials,d->maxMonomials,Tmonomial*);

  for(i=0;i<d->maxMonomials;i++)
    d->monomials[i]=NULL;
  InitVarSet(&(d->vars));

  for(i=0;i<eq->nMonomials;i++)
    {
      DeriveMonomial(nv,&f,eq->monomials[i]);
      AddMonomial(&f,d);
      DeleteMonomial(&f);
    }
}

void PrintMonomials(FILE *f,char **varNames,Tequation *eq)
{
  unsigned int i;

  if (eq->cmp!=EQU)
    PrintEquation(f,varNames,eq);
  else
    {
      for(i=0;i<eq->nMonomials;i++)
        PrintMonomial(f,(i==0),varNames,eq->monomials[i]);

      if (fabs(eq->value)>ZERO)
        fprintf(f,"%+.12g",-eq->value);
      fprintf(f,"\n");
    }
}

void PrintEquation(FILE *f,char **varNames,Tequation *eq)
{
  unsigned int i;

  fprintf(f,"   ");
  for(i=0;i<eq->nMonomials;i++)
    PrintMonomial(f,(i==0),varNames,eq->monomials[i]);

  switch(eq->cmp)
    {
    case EQU:
      fprintf(f," = %.12g;\n",eq->value);
      break;
    case GEQ:
      fprintf(f," >= %.12g;\n",eq->value);
      break;
    case LEQ:
      fprintf(f," <= %.12g;\n",eq->value);
      break;
    case NOCMP:
      fprintf(f,"??\n");
      Error("Unkown equation type in PrintEquation");
      break;
    }
}

void DeleteEquation(Tequation *eq)
{  
  ResetEquationMonomials(eq);
  free(eq->monomials);
  
  DeleteVarSet(&(eq->vars));
}