world.c

Go to the documentation of this file.

#include "world.h"
#include "defines.h"
#include "error.h"
#include "varnames.h"
#include "random.h"
#include "list.h"

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

typedef struct {
  Tjoint *joint; 
  double sign; 
} TBranchStep;

typedef struct {
  Tvector steps; 
} Tbranch;

void CreateBranch(Tbranch *b);

void CopyBranch(Tbranch *b_dst,Tbranch *b_src);

void AddStepToBranch(double s,Tjoint *joint,Tbranch *b);

unsigned int nStepsBranch(Tbranch *b);

void GetBranchStep(unsigned int i,double *s,Tjoint **joint,Tbranch *b);

unsigned int GetBranchOrigin(Tbranch *b);

unsigned int GetBranchDestination(Tbranch *b);


void MergeBranches(Tbranch *b1,Tbranch *b2,Tbranch *b);

void DeleteBranch(Tbranch *b);

void CreateBranch(Tbranch *b)
{
  InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,INIT_NUM_JOINTS,&(b->steps)); 
}

void CopyBranch(Tbranch *b_dst,Tbranch *b_src)
{
  unsigned int i,n;
  double s;
  Tjoint *jo;

  CreateBranch(b_dst);
  n=nStepsBranch(b_src);
  for(i=0;i<n;i++)
    {
      GetBranchStep(i,&s,&jo,b_src);
      AddStepToBranch(s,jo,b_dst);
    }
}

void AddStepToBranch(double s,Tjoint *joint,Tbranch *b)
{
  TBranchStep *st;

  NEW(st,1,TBranchStep);

  st->joint=joint;
  st->sign=s;

  NewVectorElement(&st,&(b->steps));
}

unsigned int nStepsBranch(Tbranch *b)
{
  return(VectorSize(&(b->steps)));
}

void GetBranchStep(unsigned int i,double *s,Tjoint **joint,Tbranch *b)
{
  TBranchStep **st;

  st=(TBranchStep **)GetVectorElement(i,&(b->steps));
  if (st==NULL)
      Error("Wrong index in GetBranchStep");
  else
    {
     *s=(*st)->sign;
     *joint=(*st)->joint;
    }
}

unsigned int GetBranchOrigin(Tbranch *b)
{
  Tjoint *jo;
  double s;
  unsigned int k;

  GetBranchStep(0,&s,&jo,b);
  if (s>0) 
    k=JointFromID(jo);
  else
    k=JointToID(jo);

  return(k);
}

unsigned int GetBranchDestination(Tbranch *b)
{
  Tjoint *jo;
  double s;
  unsigned int k;

  GetBranchStep(nStepsBranch(b)-1,&s,&jo,b);
  if (s>0) 
    k=JointToID(jo);
  else
    k=JointFromID(jo);

  return(k);
}

void MergeBranches(Tbranch *b1,Tbranch *b2,Tbranch *b)
{
  /* We get two branches (b1,b2) representing two paths from
     the groundLink to the same link. 
     The output 'b' will be the closed branch resulting from
     eliminating from 'b1' and 'b2' the initial common path
     from the groudnLink to where the paths differ */

  signed int k,n1,n2,i,nm;
  Tjoint *jo1,*jo2;
  boolean equal;
  double s;

  n1=(signed int)nStepsBranch(b1);
  n2=(signed int)nStepsBranch(b2);
  nm=(n1<n2?n1:n2);
  k=0;
  equal=((n1>0)&&(n2>0));
  while ((k<nm)&&(equal))
    {
      GetBranchStep((unsigned int)k,&s,&jo1,b1);
      GetBranchStep((unsigned int)k,&s,&jo2,b2);
      if (jo1==jo2)
        k++;
      else
        equal=FALSE;
    }
  
  CreateBranch(b);

  for(i=k;i<n1;i++)
    {    
      GetBranchStep(i,&s,&jo1,b1);
      AddStepToBranch(s,jo1,b);
    }

  for(i=n2-1;i>=k;i--)
    {    
      GetBranchStep(i,&s,&jo2,b2);
      AddStepToBranch(-s,jo2,b);
    }
}

void DeleteBranch(Tbranch *b)
{ 
  unsigned int i,n;
  TBranchStep *st;

  n=nStepsBranch(b);
  for(i=0;i<n;i++)
    {    
      st=*(TBranchStep **)GetVectorElement(i,&(b->steps));
      free(st);
    }
  DeleteVector(&(b->steps));
}

/*********************************************************************/

typedef struct {
  Tbranch b; 
  unsigned int lID; 
} TKinTreeNode;

void InitKinTreeNode(unsigned int lID,TKinTreeNode *p,TKinTreeNode *n);

unsigned int GetKinTreeNodeLink(TKinTreeNode *n);

Tbranch *GetKinTreeNodeBranch(TKinTreeNode *n);

void DeleteKinTreeNode(TKinTreeNode *n);

void InitKinTreeNode(unsigned int lID,TKinTreeNode *p,TKinTreeNode *n)
{
  n->lID=lID;
  if (p==NULL)
    CreateBranch(&(n->b));
  else
    CopyBranch(&(n->b),&(p->b));
}

unsigned int GetKinTreeNodeLink(TKinTreeNode *n)
{
  return(n->lID);
}

Tbranch *GetKinTreeNodeBranch(TKinTreeNode *n)
{
  return(&(n->b));
}

void DeleteKinTreeNode(TKinTreeNode *n)
{
  DeleteBranch(&(n->b));
}

/*********************************************************************/


void Branch2Link(unsigned int f,unsigned int t,Tbranch *b,Tworld *w);

void GenerateKinTree(Tvector *bOut,Tworld *w);

void GenerateTransEquationsFromBranch(unsigned int eq_type,Tequation *eqs,
                                      Tbranch *b,Tworld *w);

void GenerateEquationsFromBranch(Tparameters *p,unsigned int eq_type,
                                 TCuikSystem *cs,Tbranch *b,Tworld *w);

boolean IsRevoluteBinaryLink(unsigned int nl,double ***p,Tworld *w);

void InitWorldKinCS(Tparameters *p,Tworld *w);

void InitWorldCS(Tworld *w);

void DeleteWorldCS(Tworld *w);


/************************************************************************/
void Branch2Link(unsigned int from,unsigned int to,Tbranch *b,Tworld *w)
{
  Tbranch *path; /*path to each link*/
  unsigned int *visited; /*visited links*/
  unsigned int *toExpand; /*Links to expand*/  
  unsigned int c; /*current link in expansion*/
  unsigned int i,j,t,f,nte,o,d;
  Tjoint *jo;
  boolean found;
  double s;

  NEW(path,w->nl,Tbranch);
  NEW(visited,w->nl,boolean);
  NEW(toExpand,w->nl,unsigned int);

  for(i=0;i<w->nl;i++)
    visited[i]=FALSE;

  c=0; /*current position in toExpand*/
  nte=1; /*number of elements in toExpand*/
  toExpand[c]=from; /*we start expanding form 'f'*/
  visited[from]=TRUE;
  CreateBranch(&(path[from])); /*Empty path to from 'f' to 'f' */
  found=(from==to);
  while((!found)&&(c<nte))
    {
      /*look for joints that end/start at current toExpand[c]*/
      for(j=0;((!found)&&(j<w->nj));j++)
        {
          jo=GetMechanismJoint(j,&(w->m));
          t=JointToID(jo);
          f=JointFromID(jo);
          if ((t==toExpand[c])||(f==toExpand[c]))
            {
              if (t==toExpand[c])
                {o=t;d=f;s=-1.0;}
              else
                {o=f;d=t;s=+1.0;}
                 
              if (!visited[d])
                {
                  found=(d==to);
                  toExpand[nte++]=d;
                  visited[d]=TRUE;
                  CopyBranch(&(path[d]),&(path[o]));
                  AddStepToBranch(s,jo,&(path[d]));
                }
            }
        }
      c++;
    }

  /*links can be disconnected from the ground link -> free links*/
  /*in this case we return an empty branch*/
  if (found)
    CopyBranch(b,&(path[to]));
  else
    CreateBranch(b);

  for(i=0;i<w->nl;i++)
    {
      if (visited[i])
        DeleteBranch(&(path[i]));
    }
  free(path);
  free(visited);
  free(toExpand);
}

void GenerateKinTree(Tvector *bOut,Tworld *w)
{
  boolean *usedJoint;
  boolean *openLink;
  TKinTreeNode **whereIsLink;
  TKinTreeNode *newNode;
  unsigned int i,k;
  unsigned int lID,t;
  boolean found;
  double s;
  Tbranch b2l;
  Tjoint newJoint,*jo;
  Tbranch *b;
  Tlink *ground;
  unsigned int nUsedJoints,lfID,ltID;

  /*First we ensure that the mechanism is fully connected*/
  for(i=1;i<w->nl;i++)
    {
      ground=GetMechanismLink(0,&(w->m));
      Branch2Link(0,i,&b2l,w);
      if (nStepsBranch(&b2l)==0)
        {
          NewFreeJoint(GetMechanismNJoints(&(w->m)),
                       0,ground,
                       i,GetMechanismLink(i,&(w->m)),&newJoint);
          AddJoint2Mechanism(&newJoint,&(w->m));
          DeleteJoint(&newJoint);
          /*The number of joints is larger now*/
          w->nj++;
        }
      DeleteBranch(&b2l);
    }

  /*Now we generate a expansion tree of the mechanism and we return one
    branch for each one of the tree leaves. 
    If there is a joint that connects the leave with one of the internal nodes
    of the tree then the returned joint is closed, otherwise it is open */
  InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,w->nl,bOut); 

  NEW(usedJoint,w->nj,boolean); /*TRUE if the joint has been already used in the tree construction*/
  for(i=0;i<w->nj;i++)
    usedJoint[i]=FALSE;

  NEW(openLink,w->nl,boolean); /*TRUE if link 'i' is in one of the leaves of the tree*/
  for(i=0;i<w->nl;i++)
    openLink[i]=FALSE;

  NEW(whereIsLink,w->nl,TKinTreeNode*); /*Path form the root to this link, if any*/
  for(i=0;i<w->nl;i++)
    whereIsLink[i]=NULL; /*initially no path to any link*/

  /*Start up the three from the groundLink*/
  NEW(whereIsLink[0],1,TKinTreeNode);
  InitKinTreeNode(0,NULL,whereIsLink[0]);/*We know how to get to the root node (using an empty path).*/
  openLink[0]=TRUE; /*the root node is one (the only one) of the leaves of the tree*/

  nUsedJoints=0;

  while(nUsedJoints<w->nj) /*all joints have to be added to the tree. This ensures all
                             links are also in the tree  since the mechanisms is fully
                             connected (see above)*/
    {
      /*Look for a non-used joint with one of the links already in the tree*/
      found=FALSE;
      k=0;
      while((!found)&&(k<w->nj))
        {
          if (!usedJoint[k])
            {
              jo=GetMechanismJoint(k,&(w->m));
              lfID=JointFromID(jo);
              ltID=JointToID(jo);

              if ((whereIsLink[lfID]!=NULL)||(whereIsLink[ltID]!=NULL))
                {
                  found=TRUE;
                  if (whereIsLink[lfID]!=NULL)
                    { 
                      lID=lfID; /*Father-link: Link already in the tree to which 
                                  a new branch is added*/
                      t=ltID; /*Child-Link: in general this is a link added to the
                                tree but it could be also a link already in the tree.
                                In this second case the joint creates a loop.*/
                      s=1; /*Orientation of the joint as added to the tree.*/
                    }
                  else
                    {
                      lID=ltID;
                      t=lfID;
                      s=-1;
                    }
                }
            }
          if (!found)
            k++;
        }
      if (found)
        {
          nUsedJoints++;
          usedJoint[k]=TRUE;
          openLink[lID]=FALSE; /*This could be already FALSE, but just in case ...*/

          /*We create a new path to patjh to link "t"*/
          NEW(newNode,1,TKinTreeNode);
          InitKinTreeNode(t,whereIsLink[lID],newNode);
          AddStepToBranch(s,jo,GetKinTreeNodeBranch(newNode));

          /*An now we check if the new path is the first path found to "t"
            or we already have another path (-> loop, merge paths!)*/
          if (whereIsLink[t]!=NULL)
            {
              /*We already have a path to "t". 
                The two alternative paths, conveniently merged
                form a loop */
              NEW(b,1,Tbranch);
              MergeBranches(GetKinTreeNodeBranch(newNode),
                            GetKinTreeNodeBranch(whereIsLink[t]),b);
              NewVectorElement(&b,bOut);

              DeleteKinTreeNode(newNode);
              free(newNode);

              openLink[t]=FALSE; /*If node "t" was a leave, we mark that
                                   as involved in a loop -> not open link any more.*/
            }
          else
            {
              /* the joint takes to a non-visited link*/
              whereIsLink[t]=newNode;
              openLink[t]=TRUE; /* the non-visited link is now one
                                   of the leaves of the three*/
            }
        }
    }

  for(i=0;i<w->nl;i++)
    {
      if (openLink[i])
        {
          NEW(b,1,Tbranch);
          CopyBranch(b,GetKinTreeNodeBranch(whereIsLink[i]));
          NewVectorElement(&b,bOut);
        }
      DeleteKinTreeNode(whereIsLink[i]);
      free(whereIsLink[i]);
    }

  free(usedJoint);
  free(openLink);
  free(whereIsLink);
}

void GenerateTransEquationsFromBranch(unsigned int eq_type,Tequation *eq,
                                      Tbranch *b,Tworld *w)
{
  /*A Loop is a closed branch: a path from one link to itself
    going through other links*/
  unsigned int n,i;
  double s;
  Tjoint *jo;
  
  for(i=0;i<3;i++)
    {
      InitEquation(&(eq[i]));
      SetEquationType(eq_type,&(eq[i]));
      SetEquationCmp(EQU,&(eq[i]));
    }

  n=nStepsBranch(b);
  for(i=0;i<n;i++)
    {
      /*Get current joint destination link (by treating the destitation we deal with
        all links since the first link is the ground link and does not require to be
        processed)*/
      GetBranchStep(i,&s,&jo,b);
      
      GenerateJointEquationsInBranch(s,&(w->kinCS),eq,jo);
    }
}

void GenerateEquationsFromBranch(Tparameters *p,unsigned int eq_type,
                                 TCuikSystem *cs,Tbranch *b,Tworld *w)
{
  unsigned int origin,destination,i;
  boolean closedBranch;
  Tequation eq[3];
  Tmonomial f;
  char *vname,*l1name;
  Tlink *l1;
  Tvariable var;
  Tinterval rangeInf;
  unsigned int vID;

  if (nStepsBranch(b)==0)
    {
      /* An empty branch is a branch from ground link to ground link and it
         ony appears when trying to give coordinates to the ground link ??*/
      origin=0;
      destination=0;
      closedBranch=FALSE;
    }
  else
    {
      origin=GetBranchOrigin(b);
      destination=GetBranchDestination(b);
      closedBranch=(origin==destination);
    }

  GenerateTransEquationsFromBranch(eq_type,eq,b,w);

  InitMonomial(&f);
  NewInterval(-w->maxCoord,w->maxCoord,&rangeInf);
  for(i=0;i<3;i++)
    {
      if (!closedBranch)
        {
          l1=GetMechanismLink(destination,&(w->m));
          l1name=GetLinkName(l1); 

          NEW(vname,strlen(l1name)+100,char);
          LINK_TRANS(vname,l1name,i); 

          NewVariable(SECONDARY_VAR,vname,&var);
          free(vname);
          SetVariableInterval(&rangeInf,&var);

          vID=AddVariable2CS(&var,cs);

          DeleteVariable(&var);

          AddCt2Monomial(-1.0,&f);
          AddVariable2Monomial(vID,1,&f);
          AddMonomial(&f,&(eq[i]));
          ResetMonomial(&f);
        }
          
      AddEquation2CS(p,&(eq[i]),cs);
      DeleteEquation(&(eq[i]));
    }
  DeleteInterval(&rangeInf);
  DeleteMonomial(&f);
}

boolean IsRevoluteBinaryLink(unsigned int nl,double ***p,Tworld *w)
{
  unsigned int i,j,nr,f,t,nnr;
  Tjoint *jo;

  i=0;
  nr=0;
  nnr=0;
  while ((i<w->nj)&&(nnr==0)&&(nr<3))
    {
      jo=GetMechanismJoint(i,&(w->m));
      f=JointFromID(jo);
      t=JointToID(jo);

      if ((f==nl)||(t==nl))
        {
          if (GetJointType(jo)==REV_JOINT)
            {
              if (nr<2)
                {
                  for(j=0;j<2;j++)
                    GetJointPoint(((f==nl)?0:1),j,p[nr][j],jo);
                }
              nr++; /*a new revolute joint on the link*/
            }
          else
            nnr++; /*a new non-revolute joint on the link*/
        }

      i++;
    }

  return((nnr==0)&&(nr==2));
}

void InitWorldKinCS(Tparameters *p,Tworld *w)
{
  unsigned int i,j;
  Tvector branches;
  unsigned int nBranches;
  Tbranch *b;
  Tjoint *jo;
  double ***points;

  NEW(points,2,double **);
  for(i=0;i<2;i++)
    {
      NEW(points[i],2,double *);
      for(j=0;j<2;j++)
        NEW(points[i][j],3,double);
    }

  if (w->nl==0)
    Error("Empty mechanism");
        
  /*In some cases it is advisable to re-define the reference frame for the
    links to get simpler equations*/
   for(i=0;i<w->nl;i++)
     {
       if (IsRevoluteBinaryLink(i,points,w))
         ChangeLinkReferenceFrame(points[0],points[1],GetMechanismLink(i,&(w->m)));
     }

  /* All links appear in the kinCS: we add the variables and equations 
     defining the rotation matrix for each link*/
  for(i=0;i<w->nl;i++)
    GenerateLinkRot(p,i,&(w->kinCS),GetMechanismLink(i,&(w->m)));

  /* Compute the paths from ground link to all other links (this ensures that
     links are fully connected adding as many free joints as necessary. */
  GenerateKinTree(&branches,w);
  nBranches=VectorSize(&branches);

  /* All joints appear in the kinCS: we add variables and equations 
     for the joint and joint limits */
 
  for(i=0;i<w->nj;i++)
    {
      jo=GetMechanismJoint(i,&(w->m));
      GenerateJointEquations(p,w->maxCoord,&(w->kinCS),jo);
      GenerateJointRangeEquations(p,&(w->kinCS),jo);
    }

  /* Generate the equations for the loops (and branches to terminal links) involved in
     the mechanism */
  for(i=0;i<nBranches;i++)
    {
      b=*(Tbranch **)GetVectorElement(i,&branches);
      GenerateEquationsFromBranch(p,SYSTEM_EQ,&(w->kinCS),b,w);
      DeleteBranch(b);
      free(b);
    }
  DeleteVector(&branches);    

  for(i=0;i<2;i++)
    {
      for(j=0;j<2;j++)
        free(points[i][j]);
      free(points[i]);
    }
  free(points);
}

void InitWorldCS(Tworld *w)
{
  InitCuikSystem(&(w->kinCS));
}

void DeleteWorldCS(Tworld *w)
{
  DeleteCuikSystem(&(w->kinCS));
}

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

void InitWorld(Tworld *w)
{
  InitMechanism(&(w->m));

  w->nl=0; 
  w->nj=0;
  w->nb=0;
  w->systemVars=NULL;

  w->refEqs=NULL;
  
  w->stage=EMPTY_WORLD;
}

unsigned int AddLink2World(Tlink *l,Tworld *w)
{
  unsigned int k;

  w->nl++;
  k=LinkNBodies(l);
  w->nb+=k;

  return(AddLink2Mechanism(l,&(w->m)));
}

unsigned int AddJoint2World(Tjoint *j,Tworld *w)
{
  w->nj++;

  return(AddJoint2Mechanism(j,&(w->m)));
}

unsigned int GetWorldDOF(Tworld *w)
{
  return(GetMechanismDOF(&(w->m)));
}

unsigned int GetWorldLinkID(char *linkName,Tworld *w)
{
  return(GetLinkID(linkName,&(w->m)));
}

Tlink *GetWorldLink(unsigned int linkID,Tworld *w)
{
  return(GetMechanismLink(linkID,&(w->m)));
}

unsigned int GetWorldNLinks(Tworld *w)
{
  return(w->nl);
}

unsigned int GetWorldNJoints(Tworld *w)
{
  return(w->nj);
}

unsigned int GetWorldNConvexBodiesInLinks(Tworld *w)
{
  return(w->nb);
}

unsigned int GetWorldSystemVars(boolean **sv,Tworld *w)
{
  if (w->stage<WORLD_WITH_EQUATIONS)
    Error("The equations and variables are not yet defined");

  return(GetCSSystemVars(sv,&(w->kinCS)));
}

void GenerateWorldEquationSystems(Tparameters *p,Tworld *w)
{  
  unsigned int i;
  Tbranch *b2l;
  
  if (w->stage<WORLD_WITH_GEOMETRY)
    Error("The geometry information is not yet defined");

  InitWorldCS(w);
  
  w->maxCoord=GetMechanismMaxCoordinate(&(w->m));

  /*Generate the kinematic equations: this includes link reference variables
    and equations, joint variables and equations, joint limit variables and
    equations and loop equations. Note that all links and joints are used in the kinCS 
    and thus, after generating it, we can assume that the variables associated
    to links and joints are all already defined. */
  InitWorldKinCS(p,w);


  /* We generate the equations defining the translation 
     from the ground link (ID=0) to all other links. These equations are the
     same as those generated by \ref GenerateEquationsFromBranch but for
     open branches we do not add the variables defining the X-Y-Z 
     of the link reference.
  */
  NEW(b2l,w->nl,Tbranch);
  NEW(w->refEqs,w->nl,Tequation *);
  for(i=0;i<w->nl;i++) 
    {
      Branch2Link(0,i,&(b2l[i]),w);
      NEW(w->refEqs[i],3,Tequation);
      GenerateTransEquationsFromBranch(COORD_EQ,w->refEqs[i],&(b2l[i]),w);
    }
  
  /*copy of the system variables to use later on*/
  GetCSSystemVars(&(w->systemVars),&(w->kinCS));

  /*The world definition is one stage further*/
  w->stage=WORLD_WITH_EQUATIONS;
}


void GetGlobalInitialBox(Tbox *b,Tworld *w)
{
  if (w->stage<WORLD_WITH_EQUATIONS)
    Error("The equations and variables are not yet defined");

  GenerateInitialBox(b,&(w->kinCS));
}

unsigned int MaxKinematicReduction(Tparameters *p,Tbox *b,double *reduction,Tworld *w)
{ 
  unsigned int s;
  double sizeIn,sizeOut;
  
  if (w->stage<WORLD_WITH_EQUATIONS)
    Error("The equations and variables are not yet defined");
  
  sizeIn=GetBoxSize(w->systemVars,b);

  #if ((!_USE_MPI)&&(_DEBUG==1)) 
    printf("            <s:%g,l:%u>(k)",sizeIn,GetBoxLevel(b));
  #endif

  s=MaxReduction(p,SYSTEM_VAR,b,&(w->kinCS));
  /*
    s can be EMPTY_BOX
             REDUCED_BOX
             REDUCED_BOX_WITH_SOLUTION
             ERROR_IN_PROCESS
  */
  
  if (s==EMPTY_BOX)
    {
      *reduction=0.0;
      #if ((!_USE_MPI)&&(_DEBUG==1))
        printf("e\n");
      #endif
    }
  else
    {
      sizeOut=GetBoxSize(w->systemVars,b);
      *reduction=sizeOut/sizeIn;

      #if ((!_USE_MPI)&&(_DEBUG==1))
        printf("[%g]<s:%g>\n",*reduction,sizeOut);
      #endif
    }
  #if ((!_USE_MPI)&&(_DEBUG==1))
    fflush(stdout);
  #endif

  return(s);
}

unsigned int SplitGlobalBox(Tparameters *p,Tbox *b,Tbox **b1,Tbox **b2,Tworld *w)
{
  unsigned int d;

  if (w->stage<WORLD_WITH_EQUATIONS)
    Error("The equations and variables are not yet defined");

  /*We split the box along its largest dimension (in the kinematic system!)*/
  d=ComputeSplitDim(p,b,&(w->kinCS));

  #if (_DEBUG>1)
    printf("        Spliting box along dimension %u\n",d);
  #endif

  NEW(*b1,1,Tbox);
  NEW(*b2,1,Tbox);

  SplitBox(d,CUT_POINT,*b1,*b2,b);

  return(d);
}

void PlotWorld(Tplot3d *pt,double axesLength,Tworld *w)
{
  if (w->stage<WORLD_WITH_GEOMETRY)
    Error("The equations and variables are not yet defined");

  PlotMechanism(pt,axesLength,&(w->m));
}

void MoveWorld(Tplot3d *pt,Tbox *b,Tworld *w)
{
  unsigned int i,k,n;
  double **r;
  double *v;

  if (w->stage<WORLD_WITH_EQUATIONS)
   Error("The equations and variables are not yet defined");


  n=GetCSNumVariables(&(w->kinCS));
  k=0;
  NEW(v,n,double);
  for(i=0;i<n;i++)
    {
      /*The input box only has values for the input variables*/
      if (w->systemVars[i])
        {
          v[i]=IntervalCenter(GetBoxInterval(k,b));
          k++;
        }
      else
        v[i]=0.0; /*Non system variables are set to 0. See below! */
    }

  /* The input box only has values for the system variables. The dummies variables
     generated for each link when formulating the problem need to have values (they 
     are used when moving links).
     Those dummy variables appear in the refEqs and, therefore, we have to re-generate the 
     solution BEFORE evaluating the refEqs equations.
  */
  RegenerateMechanismSolution(&(w->kinCS),v,&(w->m));

  NEW(r,w->nl,double *);
  for(i=0;i<w->nl;i++)
    {
      NEW(r[i],3,double);
      
      for(k=0;k<3;k++)
        r[i][k]=(EvaluateEquation(v,&(w->refEqs[i][k]))-GetEquationValue(&(w->refEqs[i][k])));
    }

  MoveMechanism(pt,&(w->kinCS),v,r,&(w->m));

  for(i=0;i<w->nl;i++)
    free(r[i]);
  free(r);

  free(v);
}

void AnimateWorld(char *pname,double axesLength,double frameDelay,Tlist *p,Tworld *w)
{
  Tplot3d p3d;
  boolean start;
  Titerator it;

  if (w->stage<WORLD_WITH_EQUATIONS)
    Error("The equations and variables are not yet defined");

  if (axesLength<0) axesLength=0;

  if (frameDelay<0) frameDelay=0;
  
  InitPlot3d(pname,FALSE,0,NULL,&p3d);
  
  Start3dBlock(&p3d);
  Delay3dObject(INITIAL_FRAME_DELAY,&p3d);

  PlotWorld(&p3d,axesLength,w);
  
  InitIterator(&it,p);
  First(&it);

  if (GetBoxNIntervals((Tbox *)GetCurrent(&it))<GetCSNumNonDummyVariables(&(w->kinCS)))
    Error("The input boxes does not have enough number of variables");

  start=TRUE;
  while(!EndOfList(&it))
    {
      MoveWorld(&p3d,(Tbox *)GetCurrent(&it),w);
      if (start) 
        {
          Close3dBlock(&p3d);
          start=FALSE;
        }
      Delay3dObject(FRAME_RATE+frameDelay,&p3d);

      Advance(&it);
    }
  Delay3dObject(FINAL_FRAME_DELAY,&p3d);
  ClosePlot3d(TRUE,0,0,0,&p3d); /*TRUE -> quit geomview after the animation*/
}

void PrintWorldCS(Tparameters *p,Tfilename *fname,Tworld *w)
{
  Tfilename fout,fparam;
  FILE *f;

  if (w->stage<WORLD_WITH_EQUATIONS)
    Error("The equations and variables are not yet defined");

  CreateFileName(GetFilePath(fname),GetFileName(fname),"_kin",CUIK_EXT,&fout);
  f=fopen(GetFileFullName(&fout),"w");
  if (!f) Error("Could not open file in PrintWorldCS");

  fprintf(f,"%% Kinematic subsystem\n");
  PrintCuikSystem(p,f,&(w->kinCS));
  fclose(f);
  DeleteFileName(&fout);

  /* Create a symbolic link to the main parameter file.
     If a the file exist, we remove it. */
  CreateFileName(GetFilePath(fname),GetFileName(fname),"_kin",PARAM_EXT,&fout);
  f=fopen(GetFileFullName(&fout),"r");
  if (f)
    {
      fclose(f);
      remove(GetFileFullName(&fout));
    }
  CreateFileName(GetFilePath(fname),GetFileName(fname),NULL,PARAM_EXT,&fparam);
  symlink(GetFileFullName(&fparam),GetFileFullName(&fout));
  DeleteFileName(&fout);
  DeleteFileName(&fparam);
}

void DeleteWorld(Tworld *w)
{
  if (w->stage>=WORLD_WITH_GEOMETRY)
    {
      DeleteMechanism(&(w->m));
    }
    
  if (w->stage>=WORLD_WITH_EQUATIONS)
    {
      if (w->systemVars!=NULL)
        {
          unsigned int i,k;

          for(i=0;i<w->nl;i++)
            {
              for(k=0;k<3;k++)
                DeleteEquation(&(w->refEqs[i][k]));
              free(w->refEqs[i]);
            }
          free(w->refEqs);

          /* if GenerateWorldEquationSystems was called */
          DeleteWorldCS(w);
          free(w->systemVars);
        }
    }
}