00001 #include "equation.h"
00002
00003 #include "defines.h"
00004
00005 #include <string.h>
00006 #include <math.h>
00007
00024 void ResetEquationMonomials(Tequation *eq);
00025
00033 void PurgeEquation(Tequation *eq);
00034
00035 void ResetEquationMonomials(Tequation *eq)
00036 {
00037 unsigned int i;
00038
00039 for(i=0;i<eq->nMonomials;i++)
00040 {
00041 DeleteMonomial(eq->monomials[i]);
00042 free(eq->monomials[i]);
00043 eq->monomials[i]=NULL;
00044 }
00045
00046 eq->nMonomials=0;
00047 }
00048
00049 void PurgeEquation(Tequation *eq)
00050 {
00051 Tequation eq1;
00052 unsigned int i;
00053
00054 InitEquation(&eq1);
00055
00056 eq1.cmp=eq->cmp;
00057 eq1.type=eq->type;
00058
00059 if (fabs(eq->value)<=ZERO)
00060 eq1.value=0.0;
00061 else
00062 eq1.value=eq->value;
00063 for(i=0;i<eq->nMonomials;i++)
00064 {
00065 if (fabs(GetMonomialCt(eq->monomials[i]))>ZERO)
00066 AddMonomial(eq->monomials[i],&eq1);
00067 }
00068 DeleteEquation(eq);
00069 CopyEquation(eq,&eq1);
00070 DeleteEquation(&eq1);
00071 }
00072
00073 void InitEquation(Tequation *eq)
00074 {
00075 unsigned int i;
00076
00077 eq->cmp=NOCMP;
00078 eq->type=NOTYPE_EQ;
00079 eq->order=0;
00080 eq->value=0.0;
00081
00082 eq->maxMonomials=INIT_NUM_MONOMIALS;
00083 eq->nMonomials=0;
00084 NEW(eq->monomials,eq->maxMonomials,Tmonomial *);
00085
00086 for(i=0;i<eq->maxMonomials;i++)
00087 eq->monomials[i]=NULL;
00088
00089 InitVarSet(&(eq->vars));
00090 }
00091
00092 void EquationFromLinearConstraint(TLinearConstraint *lc,Tequation *eq)
00093 {
00094 Tinterval error;
00095 double ct;
00096 unsigned int cmp;
00097 Tmonomial m;
00098 unsigned int i,n;
00099
00100 InitEquation(eq);
00101
00102 GetLinearConstraintError(&error,lc);
00103 if (IntervalSize(&error)==0)
00104 {
00105 ct=IntervalCenter(&error);
00106 cmp=EQU;
00107 }
00108 else
00109 {
00110 if (LowerLimit(&error)==-INF)
00111 {
00112 ct=UpperLimit(&error);
00113 cmp=LEQ;
00114 }
00115 else
00116 {
00117 if (UpperLimit(&error)==INF)
00118 {
00119 ct=LowerLimit(&error);
00120 cmp=GEQ;
00121 }
00122 else
00123 Error("The input linear constraint can not be converted into a single equation");
00124 }
00125 }
00126 eq->cmp=cmp;
00127 eq->value=ct;
00128
00129 n=GetNumTermsInLinearConstraint(lc);
00130 for(i=0;i<n;i++)
00131 {
00132 InitMonomial(&m);
00133 AddCt2Monomial(GetLinearConstraintCoefficient(i,lc),&m);
00134 AddVariable2Monomial(GetLinearConstraintVariable(i,lc),1,&m);
00135
00136 AddMonomial(&m,eq);
00137 DeleteMonomial(&m);
00138 }
00139 }
00140
00141
00142 void EquationFromLinearConstraintProduct(TLinearConstraint *lc1,TLinearConstraint *lc2,Tequation *eq)
00143 {
00144 Tinterval error1,error2;
00145 double ct1,ct2;
00146 Tmonomial m;
00147 unsigned int i,j,n1,n2;
00148
00149 InitEquation(eq);
00150
00151 GetLinearConstraintError(&error1,lc1);
00152 if (IntervalSize(&error1)==0)
00153 ct1=IntervalCenter(&error1);
00154 else
00155 Error("The input linear constraint can not be converted into an equation");
00156
00157 GetLinearConstraintError(&error2,lc2);
00158 if (IntervalSize(&error2)==0)
00159 ct2=IntervalCenter(&error2);
00160 else
00161 Error("The input linear constraint can not be converted into an equation");
00162
00163 n1=GetNumTermsInLinearConstraint(lc1);
00164 n2=GetNumTermsInLinearConstraint(lc2);
00165 for(i=0;i<n1;i++)
00166 {
00167 for(j=0;j<n2;j++)
00168 {
00169 InitMonomial(&m);
00170 AddCt2Monomial(GetLinearConstraintCoefficient(i,lc1),&m);
00171 AddVariable2Monomial(GetLinearConstraintVariable(i,lc1),1,&m);
00172
00173 AddCt2Monomial(GetLinearConstraintCoefficient(j,lc2),&m);
00174 AddVariable2Monomial(GetLinearConstraintVariable(j,lc2),1,&m);
00175
00176 AddMonomial(&m,eq);
00177 DeleteMonomial(&m);
00178 }
00179 }
00180
00181 for(i=0;i<n1;i++)
00182 {
00183 InitMonomial(&m);
00184 AddCt2Monomial(ct2,&m);
00185 AddCt2Monomial(GetLinearConstraintCoefficient(i,lc1),&m);
00186 AddVariable2Monomial(GetLinearConstraintVariable(i,lc1),1,&m);
00187 AddMonomial(&m,eq);
00188 DeleteMonomial(&m);
00189 }
00190
00191 for(j=0;j<n2;j++)
00192 {
00193 InitMonomial(&m);
00194 AddCt2Monomial(ct1,&m);
00195 AddCt2Monomial(GetLinearConstraintCoefficient(j,lc2),&m);
00196 AddVariable2Monomial(GetLinearConstraintVariable(j,lc2),1,&m);
00197 AddMonomial(&m,eq);
00198 DeleteMonomial(&m);
00199 }
00200 }
00201
00202 void CopyEquation(Tequation *eq_dst,Tequation *eq_orig)
00203 {
00204 unsigned int i;
00205
00206 eq_dst->cmp=eq_orig->cmp;
00207 eq_dst->type=eq_orig->type;
00208 eq_dst->value=eq_orig->value;
00209
00210 eq_dst->maxMonomials=eq_orig->maxMonomials;
00211 eq_dst->nMonomials=0;
00212 NEW(eq_dst->monomials,eq_dst->maxMonomials,Tmonomial*);
00213
00214 InitVarSet(&(eq_dst->vars));
00215 eq_dst->order=0;
00216
00217 for(i=0;i<eq_orig->nMonomials;i++)
00218 AddMonomial(eq_orig->monomials[i],eq_dst);
00219 }
00220
00221 void RewriteEquation(double epsilon,Tmapping *map,Tequation *eqOut,Tequation *eq)
00222 {
00223 unsigned int i,nv,o;
00224 double ct;
00225 unsigned int v,v1,v2;
00226 TLinearConstraint lc,lc1,lc2;
00227 Tvariable_set *vars;
00228 Tequation eqTmp;
00229
00230 InitEquation(eqOut);
00231 eqOut->cmp=eq->cmp;
00232 eqOut->type=eq->type;
00233 eqOut->value=eq->value;
00234
00235 for(i=0;i<eq->nMonomials;i++)
00236 {
00237 vars=GetMonomialVariables(eq->monomials[i]);
00238 nv=VariableSetSize(vars);
00239 switch (nv)
00240 {
00241 case 0:
00242 AddMonomial(eq->monomials[i],eqOut);
00243 break;
00244
00245 case 1:
00246 v=GetVariableN(0,vars);
00247 GetOriginalVarRelation(v,&lc,map);
00248 o=MonomialOrder(eq->monomials[i]);
00249
00250 if (o==1)
00251 EquationFromLinearConstraint(&lc,&eqTmp);
00252 else
00253 {
00254 if (o==2)
00255 EquationFromLinearConstraintProduct(&lc,&lc,&eqTmp);
00256 else
00257 Error("Cubic factor in a equation");
00258 }
00259 ct=GetMonomialCt(eq->monomials[i]);
00260 AccumulateEquations(&eqTmp,ct,eqOut);
00261
00262 DeleteLinearConstraint(&lc);
00263 DeleteEquation(&eqTmp);
00264 break;
00265
00266 case 2:
00267 v1=GetVariableN(0,vars);
00268 GetOriginalVarRelation(v1,&lc1,map);
00269
00270 v2=GetVariableN(1,vars);
00271 GetOriginalVarRelation(v2,&lc2,map);
00272
00273 EquationFromLinearConstraintProduct(&lc1,&lc2,&eqTmp);
00274 ct=GetMonomialCt(eq->monomials[i]);
00275 AccumulateEquations(&eqTmp,ct,eqOut);
00276
00277 DeleteLinearConstraint(&lc1);
00278 DeleteLinearConstraint(&lc2);
00279 DeleteEquation(&eqTmp);
00280 break;
00281
00282 default:
00283 Error("Only constant, linear or bilinear monomials are allowed");
00284 }
00285 }
00286 PurgeEquation(eqOut);
00287 }
00288
00289
00290 void AccumulateEquations(Tequation *eqn,double ct,Tequation *eq)
00291 {
00292 unsigned int i;
00293 Tmonomial fnew;
00294
00295 for(i=0;i<eqn->nMonomials;i++)
00296 {
00297 CopyMonomial(&fnew,eqn->monomials[i]);
00298 AddCt2Monomial(ct,&fnew);
00299 AddMonomial(&fnew,eq);
00300 DeleteMonomial(&fnew);
00301 }
00302 eq->value+=(ct*eqn->value);
00303 PurgeEquation(eq);
00304 }
00305
00306 void ResetEquation(Tequation *eq)
00307 {
00308 eq->cmp=NOCMP;
00309 eq->type=NOTYPE_EQ;
00310 eq->value=0.0;
00311 eq->order=0;
00312
00313 ResetEquationMonomials(eq);
00314
00315 ResetVarSet(&(eq->vars));
00316 }
00317
00318
00319
00320
00321
00322
00323
00324 unsigned int FixVariableInEquation(double epsilon,unsigned int nv,double b,Tequation *eq)
00325 {
00326 TLinearConstraint lc;
00327 unsigned int r;
00328
00329 InitLinearConstraint(&lc);
00330
00331 AddCt2LinearConstraint(b,&lc);
00332 r=ReplaceVariableInEquation(epsilon,nv,&lc,eq);
00333 DeleteLinearConstraint(&lc);
00334
00335 return(r);
00336 }
00337
00338
00339
00340
00341
00342
00343
00344 unsigned int ReplaceVariableInEquation(double epsilon,unsigned int nv,
00345 TLinearConstraint *lc,Tequation *eq)
00346 {
00347 unsigned int i,j,k,n;
00348 Tequation eqNew;
00349 Tmonomial m;
00350 unsigned int np;
00351 unsigned int p;
00352 Tvariable_set *vm;
00353 Tinterval error;
00354 double a,a1,ct;
00355 unsigned int ind,ind1;
00356
00357 InitEquation(&eqNew);
00358 SetEquationCmp(GetEquationCmp(eq),(&eqNew));
00359 SetEquationType(GetEquationType(eq),(&eqNew));
00360 SetEquationValue(GetEquationValue(eq),(&eqNew));
00361
00362 GetLinearConstraintError(&error,lc);
00363 if (IntervalSize(&error)>ZERO)
00364 Error("Only linear constraints with constant offset can be used in simplification");
00365 ct=-IntervalCenter(&error);
00366
00367 n=GetNumTermsInLinearConstraint(lc);
00368
00369 i=0;
00370 while(i<eq->nMonomials)
00371 {
00372 vm=GetMonomialVariables(eq->monomials[i]);
00373 np=GetPlaceinSet(nv,vm);
00374 if (np!=NO_UINT)
00375 {
00376 p=GetVariablePowerN(np,vm);
00377 if (p==1)
00378 {
00379 for(k=0;k<n;k++)
00380 {
00381 a=GetLinearConstraintCoefficient(k,lc);
00382 ind=GetLinearConstraintVariable(k,lc);
00383
00384 CopyMonomial(&m,eq->monomials[i]);
00385 AddVariable2Monomial(ind,1,&m);
00386 FixVariableInMonomial(nv,1,&m);
00387 AddCt2Monomial(a,&m);
00388 AddMonomial(&m,&eqNew);
00389 DeleteMonomial(&m);
00390 }
00391
00392 CopyMonomial(&m,eq->monomials[i]);
00393 FixVariableInMonomial(nv,1,&m);
00394 AddCt2Monomial(ct,&m);
00395 AddMonomial(&m,&eqNew);
00396 DeleteMonomial(&m);
00397 }
00398 else
00399 {
00400 if ((p>2)||(VariableSetSize(vm)>1))
00401 Error("Monomials should be lineal, bilineal or quadratic");
00402
00403
00404
00405
00406 for(k=0;k<n;k++)
00407 {
00408 a=GetLinearConstraintCoefficient(k,lc);
00409 ind=GetLinearConstraintVariable(k,lc);
00410
00411 for(j=0;j<n;j++)
00412 {
00413 a1=GetLinearConstraintCoefficient(j,lc);
00414 ind1=GetLinearConstraintVariable(j,lc);
00415
00416 CopyMonomial(&m,eq->monomials[i]);
00417 AddVariable2Monomial(ind,1,&m);
00418 AddVariable2Monomial(ind1,1,&m);
00419 FixVariableInMonomial(nv,1,&m);
00420 AddCt2Monomial(a*a1,&m);
00421 AddMonomial(&m,&eqNew);
00422 DeleteMonomial(&m);
00423 }
00424
00425 CopyMonomial(&m,eq->monomials[i]);
00426 AddVariable2Monomial(ind,1,&m);
00427 FixVariableInMonomial(nv,1,&m);
00428 AddCt2Monomial(2*a*ct,&m);
00429 AddMonomial(&m,&eqNew);
00430 DeleteMonomial(&m);
00431 }
00432
00433 CopyMonomial(&m,eq->monomials[i]);
00434 FixVariableInMonomial(nv,1,&m);
00435 AddCt2Monomial(ct*ct,&m);
00436 AddMonomial(&m,&eqNew);
00437 DeleteMonomial(&m);
00438 }
00439 }
00440 else
00441 {
00442 CopyMonomial(&m,eq->monomials[i]);
00443 ShiftVarIndexes(nv,GetMonomialVariables(&m));
00444 AddMonomial(&m,&eqNew);
00445 DeleteMonomial(&m);
00446 }
00447 i++;
00448 }
00449
00450 DeleteEquation(eq);
00451 CopyEquation(eq,&eqNew);
00452 DeleteEquation(&eqNew);
00453
00454 NormalizeEquation(eq);
00455
00456 if (eq->nMonomials>0)
00457 return(0);
00458 else
00459 {
00460 boolean hold=TRUE;
00461
00462 switch (eq->cmp)
00463 {
00464 case EQU:
00465 hold=(fabs(eq->value)<=epsilon);
00466 break;
00467 case LEQ:
00468 hold=(-epsilon<=eq->value);
00469 break;
00470 case GEQ:
00471 hold=( epsilon>=eq->value);
00472 break;
00473 }
00474
00475 if (!hold)
00476 {
00477 printf("Ct equation does not hold: %g\n",eq->value);
00478 return(2);
00479 }
00480 else
00481 return(1);
00482 }
00483 }
00484
00485 void CtScaleEquation(double ct,Tequation *eq)
00486 {
00487 if (eq->cmp==EQU)
00488 {
00489 unsigned int i;
00490
00491 if (fabs(ct)<ZERO)
00492 Error("Scaling an equation by 0");
00493
00494 for(i=0;i<eq->nMonomials;i++)
00495 AddCt2Monomial(ct,eq->monomials[i]);
00496 eq->value*=ct;
00497 }
00498 else
00499 Error("CtScaleEquation for non-equalities is not implemented yet");
00500 }
00501
00502 void VarScaleEquation(unsigned int v,Tequation *eq)
00503 {
00504 if (eq->cmp==EQU)
00505 {
00506 unsigned int i;
00507 Tmonomial m;
00508
00509 ResetVarSet(&(eq->vars));
00510 for(i=0;i<eq->nMonomials;i++)
00511 {
00512 AddVariable2Monomial(v,1,eq->monomials[i]);
00513 UnionVarSet(&(eq->vars),GetMonomialVariables(eq->monomials[i]),&(eq->vars));
00514 }
00515
00516 InitMonomial(&m);
00517 AddCt2Monomial(-eq->value,&m);
00518 AddVariable2Monomial(v,1,&m);
00519 AddMonomial(&m,eq);
00520 DeleteMonomial(&m);
00521
00522 eq->value=0;
00523 }
00524 else
00525 Error("VarScaleEquation for non-equalities is not implemented yet");
00526 }
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536 void NormalizeEquation(Tequation *eq)
00537 {
00538 boolean equal;
00539 unsigned int i;
00540 double s,s1;
00541
00542 PurgeEquation(eq);
00543 if ((eq->nMonomials>0)&&(eq->cmp==EQU))
00544 {
00545
00546 s=GetMonomialCt(eq->monomials[0]);
00547 s=(s<0?-1.0:1.0);
00548 for(i=0;i<eq->nMonomials;i++)
00549 AddCt2Monomial(s,eq->monomials[i]);
00550 eq->value*=s;
00551
00552
00553
00554 equal=TRUE;
00555 s=GetMonomialCt(eq->monomials[0]);
00556 for(i=1;((equal)&&(i<eq->nMonomials));i++)
00557 {
00558 s1=GetMonomialCt(eq->monomials[i]);
00559 equal=((fabs(s-s1)<=ZERO)||(fabs(s+s1)<=ZERO));
00560 }
00561
00562 if (equal)
00563 {
00564 if (fabs(eq->value-s)<=ZERO)
00565 eq->value=1;
00566 else
00567 {
00568 if (fabs(eq->value+s)<=ZERO)
00569 eq->value=-1;
00570 else
00571 eq->value/=s;
00572 }
00573 for(i=0;i<eq->nMonomials;i++)
00574 {
00575 s1=GetMonomialCt(eq->monomials[i]);
00576 if (s1>0)
00577 SetMonomialCt(+1,eq->monomials[i]);
00578 else
00579 SetMonomialCt(-1,eq->monomials[i]);
00580 }
00581 }
00582 }
00583 }
00584
00585 boolean IsSimplificable(unsigned int simpLevel,unsigned int nTerms,boolean *systemVars,
00586 unsigned int *v,TLinearConstraint *lc,
00587 Tequation *eq)
00588 {
00589 boolean s;
00590
00591 s=FALSE;
00592 *v=NO_UINT;
00593
00594 if ((eq->cmp==EQU)&&
00595 (simpLevel>0)&&
00596 (((simpLevel==1)&&(nTerms==0))||
00597 ((simpLevel==2)&&(nTerms<=MAX_TERMS_SIMP))||
00598 ((simpLevel==3)&&(nTerms<=MAX_TERMS_SIMP))
00599 ))
00600 {
00601 unsigned int o,nveq,i,j,l,r;
00602 double ct;
00603 boolean found;
00604
00605 o=GetEquationOrder(eq);
00606 nveq=VariableSetSize(&(eq->vars));
00607
00608 switch(o)
00609 {
00610 case 1:
00611 if (nveq==(nTerms+1))
00612 {
00613 if (simpLevel<3)
00614 {
00615
00616
00617 found=FALSE;
00618
00619
00620
00621
00622
00623 for(r=0;((!found)&&(r<2));r++)
00624 {
00625 i=0;
00626 while((!found)&&(i<eq->nMonomials))
00627 {
00628 l=GetVariableN(0,GetMonomialVariables(eq->monomials[i]));
00629
00630 if (((r==0)&&(!systemVars[l]))||
00631 ((r==1)&&(systemVars[l])))
00632 {
00633 ct=GetMonomialCt(eq->monomials[i]);
00634
00635 if ((ct==1)||(ct==-1))
00636 found=TRUE;
00637 else
00638 i++;
00639 }
00640 else
00641 i++;
00642 }
00643 }
00644 }
00645 else
00646 {
00647
00648
00649 unsigned int k;
00650 double d,d1,d2,ct1,dm;
00651
00652 found=FALSE;
00653
00654
00655
00656
00657
00658 for(r=0;((!found)&&(r<2));r++)
00659 {
00660 dm=INF;
00661 for(k=0;k<eq->nMonomials;k++)
00662 {
00663 l=GetVariableN(0,GetMonomialVariables(eq->monomials[k]));
00664
00665 if (((r==0)&&(!systemVars[l]))||
00666 ((r==1)&&(systemVars[l])))
00667 {
00668 ct1=GetMonomialCt(eq->monomials[k]);
00669 d1=fabs(ct1-1);
00670 d2=fabs(ct1+1);
00671 d=(d1<d2?d1:d2);
00672 if (d<dm)
00673 {
00674 found=TRUE;
00675 i=k;
00676 ct=ct1;
00677 dm=d;
00678 }
00679 }
00680 }
00681 }
00682 }
00683
00684 if (found)
00685 {
00686 InitLinearConstraint(lc);
00687 *v=GetVariableN(0,GetMonomialVariables(eq->monomials[i]));
00688 s=TRUE;
00689 for(j=0;j<eq->nMonomials;j++)
00690 {
00691 if (j!=i)
00692 {
00693 AddTerm2LinearConstraint(GetVariableN(0,GetMonomialVariables(eq->monomials[j])),
00694 -GetMonomialCt(eq->monomials[j]),
00695 lc
00696 );
00697 }
00698 }
00699 AddCt2LinearConstraint(eq->value,lc);
00700 if (ct!=1)
00701 {
00702 if (ct==-1)
00703 InvertLinearConstraint(lc);
00704 else
00705 ScaleLinearConstraint(1/ct,lc);
00706 }
00707 }
00708 }
00709 break;
00710 case 2:
00711 if ((nveq==1)&&(nTerms==0)&&(eq->value==0))
00712 {
00713
00714 s=TRUE;
00715
00716 InitLinearConstraint(lc);
00717 *v=GetVariableN(0,GetMonomialVariables(eq->monomials[0]));
00718 }
00719 break;
00720 }
00721 }
00722
00723 return(s);
00724 }
00725
00726
00727 void SetEquationType(unsigned int type,Tequation *eq)
00728 {
00729 eq->type=type;
00730 }
00731
00732 void SetEquationCmp(unsigned int cmp,Tequation *eq)
00733 {
00734 if ((cmp!=EQU)&&(cmp!=LEQ)&&(cmp!=GEQ))
00735 Error("Unknow equation cmp");
00736
00737 eq->cmp=cmp;
00738 }
00739
00740 void SetEquationValue(double v,Tequation *eq)
00741 {
00742 eq->value=v;
00743 }
00744
00745 boolean LinearEquation(Tequation *eq)
00746 {
00747 boolean l;
00748 unsigned int i;
00749
00750 l=TRUE;
00751 for(i=0;((l)&&(i<eq->nMonomials));i++)
00752 {
00753 l=((CtMonomial(eq->monomials[i]))||(LinearMonomial(eq->monomials[i])));
00754 }
00755 return(l);
00756 }
00757
00758 boolean BilinearEquation(Tequation *eq)
00759 {
00760
00761
00762
00763 boolean b;
00764 unsigned int i;
00765
00766 b=TRUE;
00767 for(i=0;((b)&&(i<eq->nMonomials));i++)
00768 {
00769 b=((CtMonomial(eq->monomials[i]))||
00770 (LinearMonomial(eq->monomials[i]))||
00771 (QuadraticMonomial(eq->monomials[i]))||
00772 (BilinearMonomial(eq->monomials[i])));
00773 }
00774 return(b);
00775 }
00776
00777 boolean CircleEquation(Tequation *eq)
00778 {
00779
00780
00781 boolean c=FALSE;
00782
00783 if ((eq->nMonomials==2)&&
00784 (GetMonomialCt(eq->monomials[0])==1.0)&&(QuadraticMonomial(eq->monomials[0]))&&
00785 (GetMonomialCt(eq->monomials[1])==1.0)&&(QuadraticMonomial(eq->monomials[1]))&&
00786 (eq->value>0.0))
00787 c=TRUE;
00788
00789 return(c);
00790 }
00791
00792 boolean SphereEquation(Tequation *eq)
00793 {
00794
00795 boolean s=FALSE;
00796
00797 if ((eq->nMonomials==3)&&(eq->cmp==EQU)&&(eq->value>0.0)&&
00798 (GetMonomialCt(eq->monomials[0])==1.0)&&(QuadraticMonomial(eq->monomials[0]))&&
00799 (GetMonomialCt(eq->monomials[1])==1.0)&&(QuadraticMonomial(eq->monomials[1]))&&
00800 (GetMonomialCt(eq->monomials[2])==1.0)&&(QuadraticMonomial(eq->monomials[2]))&&
00801 (eq->cmp==EQU))
00802 s=TRUE;
00803
00804 return(s);
00805 }
00806
00807 boolean SaddleEquation(Tequation *eq)
00808 {
00809
00810 boolean s=FALSE;
00811
00812 if ((eq->nMonomials==2)&&(eq->cmp==EQU)&&(eq->value==0.0)&&
00813 (GetMonomialCt(eq->monomials[0])==1.0)&&(BilinearMonomial(eq->monomials[0]))&&
00814 (LinearMonomial(eq->monomials[1]))&&
00815 (eq->cmp==EQU))
00816 s=TRUE;
00817
00818 return(s);
00819 }
00820
00821 boolean ParabolaEquation(Tequation *eq)
00822 {
00823
00824 boolean s=FALSE;
00825
00826 if ((eq->nMonomials==2)&&(eq->cmp==EQU)&&(eq->value==0.0)&&
00827 (GetMonomialCt(eq->monomials[0])==1.0)&&(QuadraticMonomial(eq->monomials[0]))&&
00828 (LinearMonomial(eq->monomials[1]))&&
00829 (eq->cmp==EQU))
00830 s=TRUE;
00831
00832 return(s);
00833 }
00834
00835 unsigned int GetEquationCmp(Tequation *eq)
00836 {
00837 return(eq->cmp);
00838 }
00839
00840 unsigned int GetEquationType(Tequation *eq)
00841 {
00842 return(eq->type);
00843 }
00844
00845 double GetEquationValue(Tequation *eq)
00846 {
00847 return(eq->value);
00848 }
00849
00850 unsigned int GetEquationOrder(Tequation *eq)
00851 {
00852 return(eq->order);
00853 }
00854
00855 Tvariable_set *GetEquationVariables(Tequation *eq)
00856 {
00857 return(&(eq->vars));
00858 }
00859
00860 unsigned int GetEquationNumVariables(Tequation *eq)
00861 {
00862 return(VariableSetSize(GetEquationVariables(eq)));
00863 }
00864
00865 unsigned int CmpEquations(Tequation *eq1,Tequation *eq2)
00866 {
00867
00868 unsigned int r;
00869
00870 if (eq1->type<eq2->type)
00871 r=2;
00872 else
00873 {
00874 if (eq2->type<eq1->type)
00875 r=1;
00876 else
00877 {
00878
00879 unsigned int o1,o2;
00880
00881 o1=GetEquationOrder(eq1);
00882 o2=GetEquationOrder(eq2);
00883
00884 if ((eq1->cmp>eq2->cmp)||
00885 ((eq1->cmp==eq2->cmp)&&(o1>o2))||
00886 ((eq1->cmp==eq2->cmp)&&(o1==o2)&&(eq1->nMonomials>eq2->nMonomials)))
00887 r=1;
00888 else
00889 {
00890 if ((eq2->cmp>eq1->cmp)||
00891 ((eq1->cmp==eq2->cmp)&&(o2>o1))||
00892 ((eq1->cmp==eq2->cmp)&&(o1==o2)&&(eq2->nMonomials>eq1->nMonomials)))
00893 r=2;
00894 else
00895 {
00896
00897 unsigned int i;
00898
00899 i=0;
00900 r=0;
00901 while ((i<eq1->nMonomials)&&(r==0))
00902 {
00903 r=CmpMonomial(eq1->monomials[i],eq2->monomials[i]);
00904 if (r==0)
00905 i++;
00906 }
00907
00908 if (r==0)
00909 {
00910
00911
00912 if (eq1->value>eq2->value)
00913 r=1;
00914 else
00915 {
00916 if (eq2->value>eq1->value)
00917 r=2;
00918 else
00919 r=0;
00920 }
00921 }
00922 }
00923 }
00924 }
00925 }
00926 if (r!=0)
00927 return(2);
00928 else
00929 return(r);
00930 }
00931
00932 void AddMonomial(Tmonomial* f,Tequation *eq)
00933 {
00934 if ((!EmptyMonomial(f))&&(fabs(GetMonomialCt(f))>ZERO))
00935 {
00936 if (CtMonomial(f))
00937 eq->value-=GetMonomialCt(f);
00938 else
00939 {
00940 unsigned int j;
00941
00942 j=FindMonomial(f,eq);
00943
00944 if (j!=NO_UINT)
00945 {
00946
00947
00948 SetMonomialCt(GetMonomialCt(eq->monomials[j])+GetMonomialCt(f),eq->monomials[j]);
00949
00950 if (CtMonomial(eq->monomials[j]))
00951 {
00952 unsigned int n;
00953
00954
00955
00956 if (fabs(GetMonomialCt(eq->monomials[j]))>ZERO)
00957 Error("Non Zero constant monomial");
00958
00959
00960
00961
00962
00963 DeleteMonomial(eq->monomials[j]);
00964 free(eq->monomials[j]);
00965
00966 for(n=j+1;n<eq->nMonomials;n++)
00967 eq->monomials[n-1]=eq->monomials[n];
00968 eq->nMonomials--;
00969 eq->monomials[eq->nMonomials]=NULL;
00970
00971
00972 ResetVarSet(&(eq->vars));
00973 for(n=0;n<eq->nMonomials;n++)
00974 UnionVarSet(&(eq->vars),GetMonomialVariables(eq->monomials[n]),&(eq->vars));
00975 }
00976 }
00977 else
00978 {
00979
00980 unsigned int o,j;
00981 Tmonomial *s;
00982
00983 if (eq->nMonomials==eq->maxMonomials)
00984 MEM_DUP(eq->monomials,eq->maxMonomials,Tmonomial*);
00985
00986 NEW(eq->monomials[eq->nMonomials],1,Tmonomial);
00987 CopyMonomial(eq->monomials[eq->nMonomials],f);
00988 eq->nMonomials++;
00989
00990 o=MonomialOrder(f);
00991 if (eq->order<o)
00992 eq->order=o;
00993
00994 UnionVarSet(&(eq->vars),GetMonomialVariables(f),&(eq->vars));
00995
00996 j=eq->nMonomials-1;
00997 while((j>0)&&(CmpMonomial(eq->monomials[j-1],eq->monomials[j])==1))
00998 {
00999 s=eq->monomials[j-1];
01000 eq->monomials[j-1]=eq->monomials[j];
01001 eq->monomials[j]=s;
01002
01003 j--;
01004 }
01005 }
01006 }
01007 }
01008 }
01009
01010 void GenerateParabolaEquation(unsigned int vx,unsigned int vy,Tequation *eq)
01011 {
01012 GenerateScaledParabolaEquation(1,vx,vy,eq);
01013 }
01014
01015 void GenerateScaledParabolaEquation(double s,
01016 unsigned int vx,unsigned int vy,Tequation *eq)
01017 {
01018 Tmonomial f;
01019
01020 if (s==0)
01021 Error("Defining a scaled parabola equation with scale equal to 0.");
01022
01023 InitEquation(eq);
01024 InitMonomial(&f);
01025
01026 AddVariable2Monomial(vx,2,&f);
01027 AddMonomial(&f,eq);
01028 ResetMonomial(&f);
01029
01030 AddVariable2Monomial(vy,1,&f);
01031 AddCt2Monomial(-s,&f);
01032 AddMonomial(&f,eq);
01033
01034 DeleteMonomial(&f);
01035
01036 SetEquationCmp(EQU,eq);
01037 SetEquationValue(0,eq);
01038 SetEquationType(DUMMY_EQ,eq);
01039 }
01040
01041 void GenerateSaddleEquation(unsigned int vx,unsigned int vy,unsigned int vz,
01042 Tequation *eq)
01043 {
01044 GenerateScaledSaddleEquation(1,vx,vy,vz,eq);
01045 }
01046
01047 void GenerateScaledSaddleEquation(double s,
01048 unsigned int vx,unsigned int vy,unsigned int vz,
01049 Tequation *eq)
01050 {
01051 if (s==0)
01052 Error("Defining a scaled saddle equation with scale equal to 0.");
01053
01054 if (vx==vy)
01055 GenerateScaledParabolaEquation(s,vx,vz,eq);
01056 else
01057 {
01058 Tmonomial f;
01059
01060 InitEquation(eq);
01061 InitMonomial(&f);
01062
01063 AddVariable2Monomial(vx,1,&f);
01064 AddVariable2Monomial(vy,1,&f);
01065 AddMonomial(&f,eq);
01066 ResetMonomial(&f);
01067
01068 AddVariable2Monomial(vz,1,&f);
01069 AddCt2Monomial(-s,&f);
01070 AddMonomial(&f,eq);
01071
01072 DeleteMonomial(&f);
01073
01074 SetEquationCmp(EQU,eq);
01075 SetEquationValue(0,eq);
01076 SetEquationType(DUMMY_EQ,eq);
01077 }
01078 }
01079
01080 void GenerateNormEquation(unsigned int vx,unsigned int vy,unsigned int vz,
01081 double n,
01082 Tequation *eq)
01083 {
01084 unsigned int v[3];
01085
01086 v[0]=vx;
01087 v[1]=vy;
01088 v[2]=vz;
01089 GenerateGeneralNormEquation(3,v,n,eq);
01090 }
01091
01092 void GenerateGeneralNormEquation(unsigned int nv,unsigned int *v,double n,Tequation *eq)
01093 {
01094 Tmonomial f;
01095 unsigned int i;
01096
01097 InitEquation(eq);
01098
01099 InitMonomial(&f);
01100 for(i=0;i<nv;i++)
01101 {
01102 AddVariable2Monomial(v[i],2,&f);
01103 AddMonomial(&f,eq);
01104 ResetMonomial(&f);
01105 }
01106 DeleteMonomial(&f);
01107
01108 SetEquationCmp(EQU,eq);
01109 SetEquationValue(n,eq);
01110 SetEquationType(SYSTEM_EQ,eq);
01111 }
01112
01113 void GenerateCrossProductEquations(unsigned int v1x,unsigned int v1y,unsigned int v1z,
01114 unsigned int v2x,unsigned int v2y,unsigned int v2z,
01115 unsigned int v3x,unsigned int v3y,unsigned int v3z,
01116 unsigned int vs,
01117 double s,
01118 Tequation *eq)
01119 {
01120
01121 Tmonomial f;
01122 unsigned int i;
01123
01124 for(i=0;i<3;i++)
01125 InitEquation(&(eq[i]));
01126
01127 InitMonomial(&f);
01128
01129 AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1y,1,&f);AddVariable2Monomial(v2z,1,&f);
01130 AddMonomial(&f,&(eq[0]));ResetMonomial(&f);
01131 AddCt2Monomial(-1.0,&f);AddVariable2Monomial(v1z,1,&f);AddVariable2Monomial(v2y,1,&f);
01132 AddMonomial(&f,&(eq[0]));ResetMonomial(&f);
01133 AddCt2Monomial(-1,&f);
01134 if (vs==NO_UINT)
01135 AddCt2Monomial(s,&f);
01136 else
01137 AddVariable2Monomial(vs,1,&f);
01138 AddVariable2Monomial(v3x,1,&f);
01139 AddMonomial(&f,&(eq[0]));ResetMonomial(&f);
01140
01141 AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1z,1,&f);AddVariable2Monomial(v2x,1,&f);
01142 AddMonomial(&f,&(eq[1]));ResetMonomial(&f);
01143 AddCt2Monomial(-1.0,&f);AddVariable2Monomial(v1x,1,&f);AddVariable2Monomial(v2z,1,&f);
01144 AddMonomial(&f,&(eq[1]));ResetMonomial(&f);
01145 AddCt2Monomial(-1,&f);
01146 if (vs==NO_UINT)
01147 AddCt2Monomial(s,&f);
01148 else
01149 AddVariable2Monomial(vs,1,&f);
01150 AddVariable2Monomial(v3y,1,&f);
01151 AddMonomial(&f,&(eq[1]));ResetMonomial(&f);
01152
01153 AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1x,1,&f);AddVariable2Monomial(v2y,1,&f);
01154 AddMonomial(&f,&(eq[2]));ResetMonomial(&f);
01155 AddCt2Monomial(-1.0,&f);AddVariable2Monomial(v1y,1,&f);AddVariable2Monomial(v2x,1,&f);
01156 AddMonomial(&f,&(eq[2]));ResetMonomial(&f);
01157 AddCt2Monomial(-1,&f);
01158 if (vs==NO_UINT)
01159 AddCt2Monomial(s,&f);
01160 else
01161 AddVariable2Monomial(vs,1,&f);
01162 AddVariable2Monomial(v3z,1,&f);
01163 AddMonomial(&f,&(eq[2]));
01164
01165 DeleteMonomial(&f);
01166
01167 for(i=0;i<3;i++)
01168 {
01169 SetEquationCmp(EQU,&(eq[i]));
01170 SetEquationValue(0.0,&(eq[i]));
01171 SetEquationType(SYSTEM_EQ,&(eq[i]));
01172 }
01173 }
01174
01175 void GenerateDotProductEquation(unsigned int v1x,unsigned int v1y,unsigned int v1z,
01176 unsigned int v2x,unsigned int v2y,unsigned int v2z,
01177 unsigned int vc,
01178 double c,
01179 Tequation *eq)
01180 {
01181 Tmonomial f;
01182
01183 InitEquation(eq);
01184
01185 InitMonomial(&f);
01186
01187 AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1x,1,&f);AddVariable2Monomial(v2x,1,&f);
01188 AddMonomial(&f,eq);ResetMonomial(&f);
01189 AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1y,1,&f);AddVariable2Monomial(v2y,1,&f);
01190 AddMonomial(&f,eq);ResetMonomial(&f);
01191 AddCt2Monomial(+1.0,&f);AddVariable2Monomial(v1z,1,&f);AddVariable2Monomial(v2z,1,&f);
01192 AddMonomial(&f,eq);ResetMonomial(&f);
01193
01194 if (vc!=NO_UINT)
01195 {
01196 AddCt2Monomial(-1.0,&f);AddVariable2Monomial(vc,1,&f);
01197 AddMonomial(&f,eq);ResetMonomial(&f);
01198 SetEquationValue(0.0,eq);
01199 }
01200 else
01201 SetEquationValue(c,eq);
01202
01203 DeleteMonomial(&f);
01204
01205 SetEquationCmp(EQU,eq);
01206 SetEquationType(SYSTEM_EQ,eq);
01207
01208 }
01209
01210 unsigned int FindMonomial(Tmonomial *f,Tequation *eq)
01211 {
01212 unsigned int j;
01213 boolean found;
01214 Tvariable_set *vf;
01215
01216 vf=GetMonomialVariables(f);
01217
01218 found=FALSE;
01219 j=0;
01220 while((j<eq->nMonomials)&&(!found))
01221 {
01222 if (CmpVarSet(vf,GetMonomialVariables(eq->monomials[j]))==0)
01223 found=TRUE;
01224 else
01225 j++;
01226 }
01227 if (found)
01228 return(j);
01229 else
01230 return(NO_UINT);
01231 }
01232
01233 Tmonomial *GetMonomial(unsigned int i,Tequation *eq)
01234 {
01235 if (i<eq->nMonomials)
01236 return(eq->monomials[i]);
01237 else
01238 return(NULL);
01239 }
01240
01241 unsigned int GetNumMonomials(Tequation *eq)
01242 {
01243 return(eq->nMonomials);
01244 }
01245
01246 void LinearEquation2LinearConstraint(TLinearConstraint *lc,Tequation *eq)
01247 {
01248 InitLinearConstraint(lc);
01249 if (GetEquationOrder(eq)<2)
01250 {
01251 unsigned int i,v;
01252 double ct;
01253
01254 for(i=0;i<eq->nMonomials;i++)
01255 {
01256 v=GetVariableN(0,GetMonomialVariables(eq->monomials[i]));
01257 ct=GetMonomialCt(eq->monomials[i]);
01258 AddTerm2LinearConstraint(v,ct,lc);
01259 }
01260 AddCt2LinearConstraint(-eq->value,lc);
01261 }
01262 }
01263
01264 double EvaluateEquation(double *varValues,Tequation *eq)
01265 {
01266 double v;
01267 unsigned int i;
01268
01269 if (eq->cmp==NOCMP)
01270 Error("Evaluation of an undefined equation");
01271
01272 v=0.0;
01273
01274 for(i=0;i<eq->nMonomials;i++)
01275 v+=EvaluateMonomial(varValues,eq->monomials[i]);
01276
01277 return(v);
01278 }
01279
01280 void EvaluateEquationInt(Tinterval *varValues,Tinterval *i_out,Tequation *eq)
01281 {
01282 unsigned int i;
01283 Tinterval i_aux;
01284
01285 if (eq->cmp==NOCMP)
01286 Error("Evaluation of an undefined equation");
01287
01288 NewInterval(0,0,i_out);
01289 for(i=0;i<eq->nMonomials;i++)
01290 {
01291 EvaluateMonomialInt(varValues,&i_aux,eq->monomials[i]);
01292 IntervalAdd(i_out,&i_aux,i_out);
01293 }
01294 }
01295
01296 void DeriveEquation(unsigned int nv,Tequation *d,Tequation *eq)
01297 {
01298 Tmonomial f;
01299 unsigned int i;
01300
01301 d->cmp=EQU;
01302 d->type=DERIVED_EQ;
01303 d->order=0;
01304 d->value=0;
01305
01306 d->maxMonomials=eq->maxMonomials;
01307 d->nMonomials=0;
01308 NEW(d->monomials,d->maxMonomials,Tmonomial*);
01309
01310 for(i=0;i<d->maxMonomials;i++)
01311 d->monomials[i]=NULL;
01312 InitVarSet(&(d->vars));
01313
01314 for(i=0;i<eq->nMonomials;i++)
01315 {
01316 DeriveMonomial(nv,&f,eq->monomials[i]);
01317 AddMonomial(&f,d);
01318 DeleteMonomial(&f);
01319 }
01320 }
01321
01322 void PrintMonomials(FILE *f,char **varNames,Tequation *eq)
01323 {
01324 unsigned int i;
01325
01326 if (eq->cmp!=EQU)
01327 PrintEquation(f,varNames,eq);
01328 else
01329 {
01330 for(i=0;i<eq->nMonomials;i++)
01331 PrintMonomial(f,(i==0),varNames,eq->monomials[i]);
01332
01333 if (fabs(eq->value)>ZERO)
01334 fprintf(f,"%+.12g",-eq->value);
01335 fprintf(f,"\n");
01336 }
01337 }
01338
01339 void PrintEquation(FILE *f,char **varNames,Tequation *eq)
01340 {
01341 unsigned int i;
01342
01343 fprintf(f," ");
01344 for(i=0;i<eq->nMonomials;i++)
01345 PrintMonomial(f,(i==0),varNames,eq->monomials[i]);
01346
01347 switch(eq->cmp)
01348 {
01349 case EQU:
01350 fprintf(f," = %.12g;\n",eq->value);
01351 break;
01352 case GEQ:
01353 fprintf(f," >= %.12g;\n",eq->value);
01354 break;
01355 case LEQ:
01356 fprintf(f," <= %.12g;\n",eq->value);
01357 break;
01358 case NOCMP:
01359 fprintf(f,"??\n");
01360 Error("Unkown equation type in PrintEquation");
01361 break;
01362 }
01363 }
01364
01365 void DeleteEquation(Tequation *eq)
01366 {
01367 ResetEquationMonomials(eq);
01368 free(eq->monomials);
01369
01370 DeleteVarSet(&(eq->vars));
01371 }