00001 #include "world.h"
00002 #include "defines.h"
00003 #include "error.h"
00004 #include "varnames.h"
00005 #include "random.h"
00006 #include "list.h"
00007
00008 #include <math.h>
00009 #include <string.h>
00010 #include <unistd.h>
00011
00028 typedef struct {
00029 Tjoint *joint;
00030 double sign;
00034 } TBranchStep;
00035
00052 typedef struct {
00053 Tvector steps;
00054 } Tbranch;
00055
00063 void CreateBranch(Tbranch *b);
00064
00073 void CopyBranch(Tbranch *b_dst,Tbranch *b_src);
00074
00085 void AddStepToBranch(double s,Tjoint *joint,Tbranch *b);
00086
00096 unsigned int nStepsBranch(Tbranch *b);
00097
00111 void GetBranchStep(unsigned int i,double *s,Tjoint **joint,Tbranch *b);
00112
00122 unsigned int GetBranchOrigin(Tbranch *b);
00123
00133 unsigned int GetBranchDestination(Tbranch *b);
00134
00135
00149 void MergeBranches(Tbranch *b1,Tbranch *b2,Tbranch *b);
00150
00158 void DeleteBranch(Tbranch *b);
00159
00160 void CreateBranch(Tbranch *b)
00161 {
00162 InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,INIT_NUM_JOINTS,&(b->steps));
00163 }
00164
00165 void CopyBranch(Tbranch *b_dst,Tbranch *b_src)
00166 {
00167 unsigned int i,n;
00168 double s;
00169 Tjoint *jo;
00170
00171 CreateBranch(b_dst);
00172 n=nStepsBranch(b_src);
00173 for(i=0;i<n;i++)
00174 {
00175 GetBranchStep(i,&s,&jo,b_src);
00176 AddStepToBranch(s,jo,b_dst);
00177 }
00178 }
00179
00180 void AddStepToBranch(double s,Tjoint *joint,Tbranch *b)
00181 {
00182 TBranchStep *st;
00183
00184 NEW(st,1,TBranchStep);
00185
00186 st->joint=joint;
00187 st->sign=s;
00188
00189 NewVectorElement(&st,&(b->steps));
00190 }
00191
00192 unsigned int nStepsBranch(Tbranch *b)
00193 {
00194 return(VectorSize(&(b->steps)));
00195 }
00196
00197 void GetBranchStep(unsigned int i,double *s,Tjoint **joint,Tbranch *b)
00198 {
00199 TBranchStep **st;
00200
00201 st=(TBranchStep **)GetVectorElement(i,&(b->steps));
00202 if (st==NULL)
00203 Error("Wrong index in GetBranchStep");
00204 else
00205 {
00206 *s=(*st)->sign;
00207 *joint=(*st)->joint;
00208 }
00209 }
00210
00211 unsigned int GetBranchOrigin(Tbranch *b)
00212 {
00213 Tjoint *jo;
00214 double s;
00215 unsigned int k;
00216
00217 GetBranchStep(0,&s,&jo,b);
00218 if (s>0)
00219 k=JointFromID(jo);
00220 else
00221 k=JointToID(jo);
00222
00223 return(k);
00224 }
00225
00226 unsigned int GetBranchDestination(Tbranch *b)
00227 {
00228 Tjoint *jo;
00229 double s;
00230 unsigned int k;
00231
00232 GetBranchStep(nStepsBranch(b)-1,&s,&jo,b);
00233 if (s>0)
00234 k=JointToID(jo);
00235 else
00236 k=JointFromID(jo);
00237
00238 return(k);
00239 }
00240
00241 void MergeBranches(Tbranch *b1,Tbranch *b2,Tbranch *b)
00242 {
00243
00244
00245
00246
00247
00248
00249 signed int k,n1,n2,i,nm;
00250 Tjoint *jo1,*jo2;
00251 boolean equal;
00252 double s;
00253
00254 n1=(signed int)nStepsBranch(b1);
00255 n2=(signed int)nStepsBranch(b2);
00256 nm=(n1<n2?n1:n2);
00257 k=0;
00258 equal=((n1>0)&&(n2>0));
00259 while ((k<nm)&&(equal))
00260 {
00261 GetBranchStep((unsigned int)k,&s,&jo1,b1);
00262 GetBranchStep((unsigned int)k,&s,&jo2,b2);
00263 if (jo1==jo2)
00264 k++;
00265 else
00266 equal=FALSE;
00267 }
00268
00269 CreateBranch(b);
00270
00271 for(i=k;i<n1;i++)
00272 {
00273 GetBranchStep(i,&s,&jo1,b1);
00274 AddStepToBranch(s,jo1,b);
00275 }
00276
00277 for(i=n2-1;i>=k;i--)
00278 {
00279 GetBranchStep(i,&s,&jo2,b2);
00280 AddStepToBranch(-s,jo2,b);
00281 }
00282 }
00283
00284 void DeleteBranch(Tbranch *b)
00285 {
00286 unsigned int i,n;
00287 TBranchStep *st;
00288
00289 n=nStepsBranch(b);
00290 for(i=0;i<n;i++)
00291 {
00292 st=*(TBranchStep **)GetVectorElement(i,&(b->steps));
00293 free(st);
00294 }
00295 DeleteVector(&(b->steps));
00296 }
00297
00298
00299
00307 typedef struct {
00308 Tbranch b;
00310 unsigned int lID;
00311 } TKinTreeNode;
00312
00322 void InitKinTreeNode(unsigned int lID,TKinTreeNode *p,TKinTreeNode *n);
00323
00333 unsigned int GetKinTreeNodeLink(TKinTreeNode *n);
00334
00344 Tbranch *GetKinTreeNodeBranch(TKinTreeNode *n);
00345
00353 void DeleteKinTreeNode(TKinTreeNode *n);
00354
00355 void InitKinTreeNode(unsigned int lID,TKinTreeNode *p,TKinTreeNode *n)
00356 {
00357 n->lID=lID;
00358 if (p==NULL)
00359 CreateBranch(&(n->b));
00360 else
00361 CopyBranch(&(n->b),&(p->b));
00362 }
00363
00364 unsigned int GetKinTreeNodeLink(TKinTreeNode *n)
00365 {
00366 return(n->lID);
00367 }
00368
00369 Tbranch *GetKinTreeNodeBranch(TKinTreeNode *n)
00370 {
00371 return(&(n->b));
00372 }
00373
00374 void DeleteKinTreeNode(TKinTreeNode *n)
00375 {
00376 DeleteBranch(&(n->b));
00377 }
00378
00379
00380
00381
00398 void Branch2Link(unsigned int f,unsigned int t,Tbranch *b,Tworld *w);
00399
00423 void GenerateKinTree(Tvector *bOut,Tworld *w);
00424
00443 void GenerateTransEquationsFromBranch(unsigned int eq_type,Tequation *eqs,
00444 Tbranch *b,Tworld *w);
00445
00468 void GenerateEquationsFromBranch(Tparameters *p,unsigned int eq_type,
00469 TCuikSystem *cs,Tbranch *b,Tworld *w);
00470
00491 boolean IsRevoluteBinaryLink(unsigned int nl,double ***p,Tworld *w);
00492
00502 void InitWorldKinCS(Tparameters *p,Tworld *w);
00503
00518 void InitWorldCS(Tworld *w);
00519
00528 void DeleteWorldCS(Tworld *w);
00529
00530
00531
00532 void Branch2Link(unsigned int from,unsigned int to,Tbranch *b,Tworld *w)
00533 {
00534 Tbranch *path;
00535 unsigned int *visited;
00536 unsigned int *toExpand;
00537 unsigned int c;
00538 unsigned int i,j,t,f,nte,o,d;
00539 Tjoint *jo;
00540 boolean found;
00541 double s;
00542
00543 NEW(path,w->nl,Tbranch);
00544 NEW(visited,w->nl,boolean);
00545 NEW(toExpand,w->nl,unsigned int);
00546
00547 for(i=0;i<w->nl;i++)
00548 visited[i]=FALSE;
00549
00550 c=0;
00551 nte=1;
00552 toExpand[c]=from;
00553 visited[from]=TRUE;
00554 CreateBranch(&(path[from]));
00555 found=(from==to);
00556 while((!found)&&(c<nte))
00557 {
00558
00559 for(j=0;((!found)&&(j<w->nj));j++)
00560 {
00561 jo=GetMechanismJoint(j,&(w->m));
00562 t=JointToID(jo);
00563 f=JointFromID(jo);
00564 if ((t==toExpand[c])||(f==toExpand[c]))
00565 {
00566 if (t==toExpand[c])
00567 {o=t;d=f;s=-1.0;}
00568 else
00569 {o=f;d=t;s=+1.0;}
00570
00571 if (!visited[d])
00572 {
00573 found=(d==to);
00574 toExpand[nte++]=d;
00575 visited[d]=TRUE;
00576 CopyBranch(&(path[d]),&(path[o]));
00577 AddStepToBranch(s,jo,&(path[d]));
00578 }
00579 }
00580 }
00581 c++;
00582 }
00583
00584
00585
00586 if (found)
00587 CopyBranch(b,&(path[to]));
00588 else
00589 CreateBranch(b);
00590
00591 for(i=0;i<w->nl;i++)
00592 {
00593 if (visited[i])
00594 DeleteBranch(&(path[i]));
00595 }
00596 free(path);
00597 free(visited);
00598 free(toExpand);
00599 }
00600
00601 void GenerateKinTree(Tvector *bOut,Tworld *w)
00602 {
00603 boolean *usedJoint;
00604 boolean *openLink;
00605 TKinTreeNode **whereIsLink;
00606 TKinTreeNode *newNode;
00607 unsigned int i,k;
00608 unsigned int lID,t;
00609 boolean found;
00610 double s;
00611 Tbranch b2l;
00612 Tjoint newJoint,*jo;
00613 Tbranch *b;
00614 Tlink *ground;
00615 unsigned int nUsedJoints,lfID,ltID;
00616
00617
00618 for(i=1;i<w->nl;i++)
00619 {
00620 ground=GetMechanismLink(0,&(w->m));
00621 Branch2Link(0,i,&b2l,w);
00622 if (nStepsBranch(&b2l)==0)
00623 {
00624 NewFreeJoint(GetMechanismNJoints(&(w->m)),
00625 0,ground,
00626 i,GetMechanismLink(i,&(w->m)),&newJoint);
00627 AddJoint2Mechanism(&newJoint,&(w->m));
00628 DeleteJoint(&newJoint);
00629
00630 w->nj++;
00631 }
00632 DeleteBranch(&b2l);
00633 }
00634
00635
00636
00637
00638
00639 InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,w->nl,bOut);
00640
00641 NEW(usedJoint,w->nj,boolean);
00642 for(i=0;i<w->nj;i++)
00643 usedJoint[i]=FALSE;
00644
00645 NEW(openLink,w->nl,boolean);
00646 for(i=0;i<w->nl;i++)
00647 openLink[i]=FALSE;
00648
00649 NEW(whereIsLink,w->nl,TKinTreeNode*);
00650 for(i=0;i<w->nl;i++)
00651 whereIsLink[i]=NULL;
00652
00653
00654 NEW(whereIsLink[0],1,TKinTreeNode);
00655 InitKinTreeNode(0,NULL,whereIsLink[0]);
00656 openLink[0]=TRUE;
00657
00658 nUsedJoints=0;
00659
00660 while(nUsedJoints<w->nj)
00661
00662
00663 {
00664
00665 found=FALSE;
00666 k=0;
00667 while((!found)&&(k<w->nj))
00668 {
00669 if (!usedJoint[k])
00670 {
00671 jo=GetMechanismJoint(k,&(w->m));
00672 lfID=JointFromID(jo);
00673 ltID=JointToID(jo);
00674
00675 if ((whereIsLink[lfID]!=NULL)||(whereIsLink[ltID]!=NULL))
00676 {
00677 found=TRUE;
00678 if (whereIsLink[lfID]!=NULL)
00679 {
00680 lID=lfID;
00681
00682 t=ltID;
00683
00684
00685 s=1;
00686 }
00687 else
00688 {
00689 lID=ltID;
00690 t=lfID;
00691 s=-1;
00692 }
00693 }
00694 }
00695 if (!found)
00696 k++;
00697 }
00698 if (found)
00699 {
00700 nUsedJoints++;
00701 usedJoint[k]=TRUE;
00702 openLink[lID]=FALSE;
00703
00704
00705 NEW(newNode,1,TKinTreeNode);
00706 InitKinTreeNode(t,whereIsLink[lID],newNode);
00707 AddStepToBranch(s,jo,GetKinTreeNodeBranch(newNode));
00708
00709
00710
00711 if (whereIsLink[t]!=NULL)
00712 {
00713
00714
00715
00716 NEW(b,1,Tbranch);
00717 MergeBranches(GetKinTreeNodeBranch(newNode),
00718 GetKinTreeNodeBranch(whereIsLink[t]),b);
00719 NewVectorElement(&b,bOut);
00720
00721 DeleteKinTreeNode(newNode);
00722 free(newNode);
00723
00724 openLink[t]=FALSE;
00725
00726 }
00727 else
00728 {
00729
00730 whereIsLink[t]=newNode;
00731 openLink[t]=TRUE;
00732
00733 }
00734 }
00735 }
00736
00737 for(i=0;i<w->nl;i++)
00738 {
00739 if (openLink[i])
00740 {
00741 NEW(b,1,Tbranch);
00742 CopyBranch(b,GetKinTreeNodeBranch(whereIsLink[i]));
00743 NewVectorElement(&b,bOut);
00744 }
00745 DeleteKinTreeNode(whereIsLink[i]);
00746 free(whereIsLink[i]);
00747 }
00748
00749 free(usedJoint);
00750 free(openLink);
00751 free(whereIsLink);
00752 }
00753
00754 void GenerateTransEquationsFromBranch(unsigned int eq_type,Tequation *eq,
00755 Tbranch *b,Tworld *w)
00756 {
00757
00758
00759 unsigned int n,i;
00760 double s;
00761 Tjoint *jo;
00762
00763 for(i=0;i<3;i++)
00764 {
00765 InitEquation(&(eq[i]));
00766 SetEquationType(eq_type,&(eq[i]));
00767 SetEquationCmp(EQU,&(eq[i]));
00768 }
00769
00770 n=nStepsBranch(b);
00771 for(i=0;i<n;i++)
00772 {
00773
00774
00775
00776 GetBranchStep(i,&s,&jo,b);
00777
00778 GenerateJointEquationsInBranch(s,&(w->kinCS),eq,jo);
00779 }
00780 }
00781
00782 void GenerateEquationsFromBranch(Tparameters *p,unsigned int eq_type,
00783 TCuikSystem *cs,Tbranch *b,Tworld *w)
00784 {
00785 unsigned int origin,destination,i;
00786 boolean closedBranch;
00787 Tequation eq[3];
00788 Tmonomial f;
00789 char *vname,*l1name;
00790 Tlink *l1;
00791 Tvariable var;
00792 Tinterval rangeInf;
00793 unsigned int vID;
00794
00795 if (nStepsBranch(b)==0)
00796 {
00797
00798
00799 origin=0;
00800 destination=0;
00801 closedBranch=FALSE;
00802 }
00803 else
00804 {
00805 origin=GetBranchOrigin(b);
00806 destination=GetBranchDestination(b);
00807 closedBranch=(origin==destination);
00808 }
00809
00810 GenerateTransEquationsFromBranch(eq_type,eq,b,w);
00811
00812 InitMonomial(&f);
00813 NewInterval(-w->maxCoord,w->maxCoord,&rangeInf);
00814 for(i=0;i<3;i++)
00815 {
00816 if (!closedBranch)
00817 {
00818 l1=GetMechanismLink(destination,&(w->m));
00819 l1name=GetLinkName(l1);
00820
00821 NEW(vname,strlen(l1name)+100,char);
00822 LINK_TRANS(vname,l1name,i);
00823
00824 NewVariable(SECONDARY_VAR,vname,&var);
00825 free(vname);
00826 SetVariableInterval(&rangeInf,&var);
00827
00828 vID=AddVariable2CS(&var,cs);
00829
00830 DeleteVariable(&var);
00831
00832 AddCt2Monomial(-1.0,&f);
00833 AddVariable2Monomial(vID,1,&f);
00834 AddMonomial(&f,&(eq[i]));
00835 ResetMonomial(&f);
00836 }
00837
00838 AddEquation2CS(p,&(eq[i]),cs);
00839 DeleteEquation(&(eq[i]));
00840 }
00841 DeleteInterval(&rangeInf);
00842 DeleteMonomial(&f);
00843 }
00844
00845 boolean IsRevoluteBinaryLink(unsigned int nl,double ***p,Tworld *w)
00846 {
00847 unsigned int i,j,nr,f,t,nnr;
00848 Tjoint *jo;
00849
00850 i=0;
00851 nr=0;
00852 nnr=0;
00853 while ((i<w->nj)&&(nnr==0)&&(nr<3))
00854 {
00855 jo=GetMechanismJoint(i,&(w->m));
00856 f=JointFromID(jo);
00857 t=JointToID(jo);
00858
00859 if ((f==nl)||(t==nl))
00860 {
00861 if (GetJointType(jo)==REV_JOINT)
00862 {
00863 if (nr<2)
00864 {
00865 for(j=0;j<2;j++)
00866 GetJointPoint(((f==nl)?0:1),j,p[nr][j],jo);
00867 }
00868 nr++;
00869 }
00870 else
00871 nnr++;
00872 }
00873
00874 i++;
00875 }
00876
00877 return((nnr==0)&&(nr==2));
00878 }
00879
00880 void InitWorldKinCS(Tparameters *p,Tworld *w)
00881 {
00882 unsigned int i,j;
00883 Tvector branches;
00884 unsigned int nBranches;
00885 Tbranch *b;
00886 Tjoint *jo;
00887 double ***points;
00888
00889 NEW(points,2,double **);
00890 for(i=0;i<2;i++)
00891 {
00892 NEW(points[i],2,double *);
00893 for(j=0;j<2;j++)
00894 NEW(points[i][j],3,double);
00895 }
00896
00897 if (w->nl==0)
00898 Error("Empty mechanism");
00899
00900
00901
00902 for(i=0;i<w->nl;i++)
00903 {
00904 if (IsRevoluteBinaryLink(i,points,w))
00905 ChangeLinkReferenceFrame(points[0],points[1],GetMechanismLink(i,&(w->m)));
00906 }
00907
00908
00909
00910 for(i=0;i<w->nl;i++)
00911 GenerateLinkRot(p,i,&(w->kinCS),GetMechanismLink(i,&(w->m)));
00912
00913
00914
00915 GenerateKinTree(&branches,w);
00916 nBranches=VectorSize(&branches);
00917
00918
00919
00920
00921 for(i=0;i<w->nj;i++)
00922 {
00923 jo=GetMechanismJoint(i,&(w->m));
00924 GenerateJointEquations(p,w->maxCoord,&(w->kinCS),jo);
00925 GenerateJointRangeEquations(p,&(w->kinCS),jo);
00926 }
00927
00928
00929
00930 for(i=0;i<nBranches;i++)
00931 {
00932 b=*(Tbranch **)GetVectorElement(i,&branches);
00933 GenerateEquationsFromBranch(p,SYSTEM_EQ,&(w->kinCS),b,w);
00934 DeleteBranch(b);
00935 free(b);
00936 }
00937 DeleteVector(&branches);
00938
00939 for(i=0;i<2;i++)
00940 {
00941 for(j=0;j<2;j++)
00942 free(points[i][j]);
00943 free(points[i]);
00944 }
00945 free(points);
00946 }
00947
00948 void InitWorldCS(Tworld *w)
00949 {
00950 InitCuikSystem(&(w->kinCS));
00951 }
00952
00953 void DeleteWorldCS(Tworld *w)
00954 {
00955 DeleteCuikSystem(&(w->kinCS));
00956 }
00957
00958
00959
00960
00961
00962
00963
00964 void InitWorld(Tworld *w)
00965 {
00966 InitMechanism(&(w->m));
00967
00968 w->nl=0;
00969 w->nj=0;
00970 w->nb=0;
00971 w->systemVars=NULL;
00972
00973 w->refEqs=NULL;
00974
00975 w->stage=EMPTY_WORLD;
00976 }
00977
00978 unsigned int AddLink2World(Tlink *l,Tworld *w)
00979 {
00980 unsigned int k;
00981
00982 w->nl++;
00983 k=LinkNBodies(l);
00984 w->nb+=k;
00985
00986 return(AddLink2Mechanism(l,&(w->m)));
00987 }
00988
00989 unsigned int AddJoint2World(Tjoint *j,Tworld *w)
00990 {
00991 w->nj++;
00992
00993 return(AddJoint2Mechanism(j,&(w->m)));
00994 }
00995
00996 unsigned int GetWorldDOF(Tworld *w)
00997 {
00998 return(GetMechanismDOF(&(w->m)));
00999 }
01000
01001 unsigned int GetWorldLinkID(char *linkName,Tworld *w)
01002 {
01003 return(GetLinkID(linkName,&(w->m)));
01004 }
01005
01006 Tlink *GetWorldLink(unsigned int linkID,Tworld *w)
01007 {
01008 return(GetMechanismLink(linkID,&(w->m)));
01009 }
01010
01011 unsigned int GetWorldNLinks(Tworld *w)
01012 {
01013 return(w->nl);
01014 }
01015
01016 unsigned int GetWorldNJoints(Tworld *w)
01017 {
01018 return(w->nj);
01019 }
01020
01021 unsigned int GetWorldNConvexBodiesInLinks(Tworld *w)
01022 {
01023 return(w->nb);
01024 }
01025
01026 unsigned int GetWorldSystemVars(boolean **sv,Tworld *w)
01027 {
01028 if (w->stage<WORLD_WITH_EQUATIONS)
01029 Error("The equations and variables are not yet defined");
01030
01031 return(GetCSSystemVars(sv,&(w->kinCS)));
01032 }
01033
01034 void GenerateWorldEquationSystems(Tparameters *p,Tworld *w)
01035 {
01036 unsigned int i;
01037 Tbranch *b2l;
01038
01039 if (w->stage<WORLD_WITH_GEOMETRY)
01040 Error("The geometry information is not yet defined");
01041
01042 InitWorldCS(w);
01043
01044 w->maxCoord=GetMechanismMaxCoordinate(&(w->m));
01045
01046
01047
01048
01049
01050
01051 InitWorldKinCS(p,w);
01052
01053
01054
01055
01056
01057
01058
01059
01060 NEW(b2l,w->nl,Tbranch);
01061 NEW(w->refEqs,w->nl,Tequation *);
01062 for(i=0;i<w->nl;i++)
01063 {
01064 Branch2Link(0,i,&(b2l[i]),w);
01065 NEW(w->refEqs[i],3,Tequation);
01066 GenerateTransEquationsFromBranch(COORD_EQ,w->refEqs[i],&(b2l[i]),w);
01067 }
01068
01069
01070 GetCSSystemVars(&(w->systemVars),&(w->kinCS));
01071
01072
01073 w->stage=WORLD_WITH_EQUATIONS;
01074 }
01075
01076
01077 void GetGlobalInitialBox(Tbox *b,Tworld *w)
01078 {
01079 if (w->stage<WORLD_WITH_EQUATIONS)
01080 Error("The equations and variables are not yet defined");
01081
01082 GenerateInitialBox(b,&(w->kinCS));
01083 }
01084
01085 unsigned int MaxKinematicReduction(Tparameters *p,Tbox *b,double *reduction,Tworld *w)
01086 {
01087 unsigned int s;
01088 double sizeIn,sizeOut;
01089
01090 if (w->stage<WORLD_WITH_EQUATIONS)
01091 Error("The equations and variables are not yet defined");
01092
01093 sizeIn=GetBoxSize(w->systemVars,b);
01094
01095 #if ((!_USE_MPI)&&(_DEBUG==1))
01096 printf(" <s:%g,l:%u>(k)",sizeIn,GetBoxLevel(b));
01097 #endif
01098
01099 s=MaxReduction(p,SYSTEM_VAR,b,&(w->kinCS));
01100
01101
01102
01103
01104
01105
01106
01107 if (s==EMPTY_BOX)
01108 {
01109 *reduction=0.0;
01110 #if ((!_USE_MPI)&&(_DEBUG==1))
01111 printf("e\n");
01112 #endif
01113 }
01114 else
01115 {
01116 sizeOut=GetBoxSize(w->systemVars,b);
01117 *reduction=sizeOut/sizeIn;
01118
01119 #if ((!_USE_MPI)&&(_DEBUG==1))
01120 printf("[%g]<s:%g>\n",*reduction,sizeOut);
01121 #endif
01122 }
01123 #if ((!_USE_MPI)&&(_DEBUG==1))
01124 fflush(stdout);
01125 #endif
01126
01127 return(s);
01128 }
01129
01130 unsigned int SplitGlobalBox(Tparameters *p,Tbox *b,Tbox **b1,Tbox **b2,Tworld *w)
01131 {
01132 unsigned int d;
01133
01134 if (w->stage<WORLD_WITH_EQUATIONS)
01135 Error("The equations and variables are not yet defined");
01136
01137
01138 d=ComputeSplitDim(p,b,&(w->kinCS));
01139
01140 #if (_DEBUG>1)
01141 printf(" Spliting box along dimension %u\n",d);
01142 #endif
01143
01144 NEW(*b1,1,Tbox);
01145 NEW(*b2,1,Tbox);
01146
01147 SplitBox(d,CUT_POINT,*b1,*b2,b);
01148
01149 return(d);
01150 }
01151
01152 void PlotWorld(Tplot3d *pt,double axesLength,Tworld *w)
01153 {
01154 if (w->stage<WORLD_WITH_GEOMETRY)
01155 Error("The equations and variables are not yet defined");
01156
01157 PlotMechanism(pt,axesLength,&(w->m));
01158 }
01159
01160 void MoveWorld(Tplot3d *pt,Tbox *b,Tworld *w)
01161 {
01162 unsigned int i,k,n;
01163 double **r;
01164 double *v;
01165
01166 if (w->stage<WORLD_WITH_EQUATIONS)
01167 Error("The equations and variables are not yet defined");
01168
01169
01170 n=GetCSNumVariables(&(w->kinCS));
01171 k=0;
01172 NEW(v,n,double);
01173 for(i=0;i<n;i++)
01174 {
01175
01176 if (w->systemVars[i])
01177 {
01178 v[i]=IntervalCenter(GetBoxInterval(k,b));
01179 k++;
01180 }
01181 else
01182 v[i]=0.0;
01183 }
01184
01185
01186
01187
01188
01189
01190
01191 RegenerateMechanismSolution(&(w->kinCS),v,&(w->m));
01192
01193 NEW(r,w->nl,double *);
01194 for(i=0;i<w->nl;i++)
01195 {
01196 NEW(r[i],3,double);
01197
01198 for(k=0;k<3;k++)
01199 r[i][k]=(EvaluateEquation(v,&(w->refEqs[i][k]))-GetEquationValue(&(w->refEqs[i][k])));
01200 }
01201
01202 MoveMechanism(pt,&(w->kinCS),v,r,&(w->m));
01203
01204 for(i=0;i<w->nl;i++)
01205 free(r[i]);
01206 free(r);
01207
01208 free(v);
01209 }
01210
01211 void AnimateWorld(char *pname,double axesLength,double frameDelay,Tlist *p,Tworld *w)
01212 {
01213 Tplot3d p3d;
01214 boolean start;
01215 Titerator it;
01216
01217 if (w->stage<WORLD_WITH_EQUATIONS)
01218 Error("The equations and variables are not yet defined");
01219
01220 if (axesLength<0) axesLength=0;
01221
01222 if (frameDelay<0) frameDelay=0;
01223
01224 InitPlot3d(pname,FALSE,0,NULL,&p3d);
01225
01226 Start3dBlock(&p3d);
01227 Delay3dObject(INITIAL_FRAME_DELAY,&p3d);
01228
01229 PlotWorld(&p3d,axesLength,w);
01230
01231 InitIterator(&it,p);
01232 First(&it);
01233
01234 if (GetBoxNIntervals((Tbox *)GetCurrent(&it))<GetCSNumNonDummyVariables(&(w->kinCS)))
01235 Error("The input boxes does not have enough number of variables");
01236
01237 start=TRUE;
01238 while(!EndOfList(&it))
01239 {
01240 MoveWorld(&p3d,(Tbox *)GetCurrent(&it),w);
01241 if (start)
01242 {
01243 Close3dBlock(&p3d);
01244 start=FALSE;
01245 }
01246 Delay3dObject(FRAME_RATE+frameDelay,&p3d);
01247
01248 Advance(&it);
01249 }
01250 Delay3dObject(FINAL_FRAME_DELAY,&p3d);
01251 ClosePlot3d(TRUE,0,0,0,&p3d);
01252 }
01253
01254 void PrintWorldCS(Tparameters *p,Tfilename *fname,Tworld *w)
01255 {
01256 Tfilename fout,fparam;
01257 FILE *f;
01258
01259 if (w->stage<WORLD_WITH_EQUATIONS)
01260 Error("The equations and variables are not yet defined");
01261
01262 CreateFileName(GetFilePath(fname),GetFileName(fname),"_kin",CUIK_EXT,&fout);
01263 f=fopen(GetFileFullName(&fout),"w");
01264 if (!f) Error("Could not open file in PrintWorldCS");
01265
01266 fprintf(f,"%% Kinematic subsystem\n");
01267 PrintCuikSystem(p,f,&(w->kinCS));
01268 fclose(f);
01269 DeleteFileName(&fout);
01270
01271
01272
01273 CreateFileName(GetFilePath(fname),GetFileName(fname),"_kin",PARAM_EXT,&fout);
01274 f=fopen(GetFileFullName(&fout),"r");
01275 if (f)
01276 {
01277 fclose(f);
01278 remove(GetFileFullName(&fout));
01279 }
01280 CreateFileName(GetFilePath(fname),GetFileName(fname),NULL,PARAM_EXT,&fparam);
01281 symlink(GetFileFullName(&fparam),GetFileFullName(&fout));
01282 DeleteFileName(&fout);
01283 DeleteFileName(&fparam);
01284 }
01285
01286 void DeleteWorld(Tworld *w)
01287 {
01288 if (w->stage>=WORLD_WITH_GEOMETRY)
01289 {
01290 DeleteMechanism(&(w->m));
01291 }
01292
01293 if (w->stage>=WORLD_WITH_EQUATIONS)
01294 {
01295 if (w->systemVars!=NULL)
01296 {
01297 unsigned int i,k;
01298
01299 for(i=0;i<w->nl;i++)
01300 {
01301 for(k=0;k<3;k++)
01302 DeleteEquation(&(w->refEqs[i][k]));
01303 free(w->refEqs[i]);
01304 }
01305 free(w->refEqs);
01306
01307
01308 DeleteWorldCS(w);
01309 free(w->systemVars);
01310 }
01311 }
01312 }