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 "shtransform.h"

#include <math.h>
#include <string.h>
#include <ctype.h>
#include <xlocale.h>
#include <time.h>
#include <unistd.h>
#ifdef _OPENMP
  #include <omp.h>
#endif

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 GetTransformFromBranch(THTransform *tj,THTransform *t,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 GetTransformFromBranch(THTransform *tj,THTransform *t,Tbranch *b)
{
  unsigned int i,n;
  TBranchStep *st;
  THTransform ti;

  HTransformIdentity(t);
  n=nStepsBranch(b);
  for(i=0;i<n;i++)
    {    
      st=*(TBranchStep **)GetVectorElement(i,&(b->steps));
      if (st->sign>0)
        HTransformProduct(t,&(tj[GetJointID(st->joint)]),t);
      else
        {
          HTransformInverse(&(tj[GetJointID(st->joint)]),&ti);
          HTransformProduct(t,&ti,t);
        }
    }
}

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));
}


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

unsigned int Branches2Links(unsigned int from,unsigned int *links,unsigned int *jointTo,
                            boolean *isLeaf,Tbranch *b,Tworld *w);

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

void GenerateMEquationFromBranch(Tparameters *p,Tbranch *b,TMequation *meq,Tworld *w);

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

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


void DeleteWorldCollisionInfo(Tworld *w);

void WorldInitDOFInfo(Tworld *w);

void WorldDeleteDOFInfo(Tworld *w);

void GetLinkTransformsFromDOF(double *dof,THTransform **tl,Tworld *w);

void GetLinkTransformsFromSolution(Tparameters *p,double *sol,
                                   THTransform **tl,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 DeleteWorldCD(Tworld *w);

/************************************************************************/
unsigned int Branches2Links(unsigned int from,unsigned int *links,unsigned int *jointTo,
                            boolean *isLeaf,Tbranch *b,Tworld *w)
{
  boolean *visited; /* visited links */
  unsigned int c; /* current link in expansion (index in 'links' array) */
  unsigned int i,j,t,f,nte,o,d;
  Tjoint *jo;
  double s;
  boolean found;

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

  c=0; /* current position in 'links' */
  links[c]=from; /* we start expanding form node 'from' */
  visited[from]=TRUE;
  nte=1; /* number of elements in 'links' */
  jointTo[from]=NO_UINT; /* node 'from' is the root (no joint lead to it). */
  isLeaf[from]=TRUE; /* initially 'from' is a leaf link */

  /* Initialize the branches to the links */
  for(i=0;i<w->nl;i++)
    CreateBranch(&(b[i])); 
 
  while(c<nte)
    {
      /* look all the joints that end/start at current links[c] */
      found=FALSE;
      for(j=0;((!found)&&(j<w->nj));j++)
        {
          jo=GetMechanismJoint(j,&(w->m));
          t=JointToID(jo);
          f=JointFromID(jo);
          /* It is probably a good idea to try to avoid "inverse" joints
             as much as possible */
          if ((t==links[c])||(f==links[c]))
            {
              if (t==links[c])
                {o=t;d=f;s=-1.0;}
              else
                {o=f;d=t;s=+1.0;}
                 
              if (!visited[d])
                {                 
                  /* Depth-first search produces longer equations */
                  /*found=TRUE;*/ /* comment this to get a Breadth-first search */

                  isLeaf[o]=FALSE; /* Now link 'o' has a child */
                  links[nte++]=d;  /* Add link 'd' to the list of nodes to expand */
                  visited[d]=TRUE; /* link 'd' is already reached */
                  jointTo[d]=j;
                  isLeaf[d]=TRUE; /* initially link 'd' is a leaf */ 
                  CopyBranch(&(b[d]),&(b[o]));
                  AddStepToBranch(s,jo,&(b[d]));
                }
            }
        }
      c++;
    }

  free(visited);
  
  return(nte);
}

void GenerateKinTree(Tvector *bOut,Tworld *w)
{
  boolean *usedJoint;
  Tbranch *b,*b1;
  unsigned int i,n,j,lfID,ltID;
  Tjoint newJoint,*jo;
  Tlink *ground;

  /*First we ensure that the mechanism is fully connected*/
  ground=GetMechanismLink(0,&(w->m));
  
  NEW(w->branch2Link,w->nl,Tbranch);
  NEW(w->sortedLinks,w->nl,unsigned int);
  NEW(w->jointTo,w->nl,unsigned int);
  NEW(w->openLink,w->nl,boolean);
  n=Branches2Links(0,w->sortedLinks,w->jointTo,w->openLink,w->branch2Link,w);
  if (n!=w->nl)
    {
      /* we need to reach all links (link 0 is root!) */
      for(i=1;i<w->nl;i++)
        {
          if (nStepsBranch(&(w->branch2Link[i]))==0)
            {
              NewFreeJoint(w->nj,
                           0,ground,
                           i,GetMechanismLink(i,&(w->m)),&newJoint);
              j=AddJoint2Mechanism(&newJoint,&(w->m));
              DeleteJoint(&newJoint);

              /* define the tree info for this link */
              AddStepToBranch(1.0,GetMechanismJoint(j,&(w->m)),&(w->branch2Link[i]));
              w->sortedLinks[n++]=i;
              w->jointTo[i]=j;
              w->openLink[0]=FALSE; /* just in case :) */
              w->openLink[i]=TRUE;


              /*The number of joints is larger now*/
              w->nj++;
            }
        }
    }

  NEW(usedJoint,w->nj,boolean);
  for(i=0;i<w->nj;i++)
    usedJoint[i]=FALSE;
  for(i=1;i<w->nl;i++) /* no joint takes to link 0! */
    usedJoint[w->jointTo[i]]=TRUE;

  /* Initialize the vector with the pointers to the loop (or not loop) equations. */
  InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,w->nl,bOut); 

  /* Here, the non-used joints form a basis of cycles */
  for(i=0;i<w->nj;i++)
    {
      if (!usedJoint[i])
        {
          jo=GetMechanismJoint(i,&(w->m));
          lfID=JointFromID(jo);
          ltID=JointToID(jo);
          
          /* Add the non-used joint to one of the branches to close the loop */
          NEW(b1,1,Tbranch);
          CopyBranch(b1,&(w->branch2Link[lfID]));
          AddStepToBranch(1,jo,b1);

          NEW(b,1,Tbranch);
          MergeBranches(b1,&(w->branch2Link[ltID]),b);
          NewVectorElement(&b,bOut);

          DeleteBranch(b1);
          free(b1);

          /* Just in case, the two links are not open any more */
          w->openLink[lfID]=FALSE;
          w->openLink[ltID]=FALSE;
        }
    }

  /* Open links define open branches */
  for(i=0;i<w->nl;i++)
    {
      if (w->openLink[i])
        {
          NEW(b,1,Tbranch);
          CopyBranch(b,&(w->branch2Link[i]));
          NewVectorElement(&b,bOut);
        }
    }

  /* Release memory */
  free(usedJoint);
}

void GenerateMEquationFromBranch(Tparameters *p,Tbranch *b,TMequation *meq,Tworld *w)
{
  unsigned int i,n;
  double s;
  Tjoint *jo;
  TTransSeq ts;

  /*  Id = Id */
  InitMEquation(meq);

  /* Now add the variable terms, if any */
  n=nStepsBranch(b);
  for(i=0;i<n;i++)
    {
      GetBranchStep(i,&s,&jo,b);
      GetJointTransSeq(p,&(w->kinCS),&ts,jo);      
      AddTransSeq2MEquation(s,&ts,meq);
      DeleteTransSeq(&ts);
    }
  SimplifyMEquation(meq);
}

void GenerateTransEquationsFromBranch(Tparameters *p,unsigned int eq_type,
                                      Tequation *eq,Tbranch *b,Tworld *w)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));

  if (r==REP_JOINTS)
    Error("GenerateTransEquationsFromBranch should not be used in JOINT-based representations");
  else
    {
      /*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(p,s,&(w->kinCS),eq,jo);
        }
    }
}

boolean 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;
  unsigned int r;

  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);
    }

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));

  if (r==REP_JOINTS)
    {
      if (closedBranch)
        {
          TMequation meq;

          GenerateMEquationFromBranch(p,b,&meq,w);
          AddMatrixEquation2CS(p,&meq,cs);
          DeleteMEquation(&meq);
        }
    }
  else
    {
      GenerateTransEquationsFromBranch(p,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(SYSTEM_VAR,vname,&var);
              free(vname);
              SetVariableInterval(&rangeInf,&var);

              vID=AddVariable2CS(&var,cs);

              DeleteVariable(&var);

              AddCt2Monomial(-1.0,&f);
              AddVariable2Monomial(NFUN,vID,1,&f);
              AddMonomial(&f,&(eq[i]));
              ResetMonomial(&f);

              /* For open loops, the equations only give the coordinates of the
                 end of the loop -> they are coordinating equations.*/
              SetEquationType(COORD_EQ,&(eq[i]));
            }
          
          AddEquation2CS(p,&(eq[i]),cs);
          DeleteEquation(&(eq[i]));
        }
      DeleteInterval(&rangeInf);
      DeleteMonomial(&f);

    }
  return(!closedBranch);
}


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;
  unsigned int r;

  if (w->nl==0)
    Error("Empty mechanism");

  if (w->nl==1)
    Error("A world with just one link makes no sense");
  else
    {
      r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
        
      /*In some cases it is advisable to re-define the reference frame for the
        links to get simpler equations*/
      if ((r!=REP_JOINTS)&&(AllRevolute(&(w->m))))
        {
          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);
            }

          for(i=0;i<w->nl;i++)
            {
              if (IsRevoluteBinaryLink(i,points,w))
                ChangeLinkReferenceFrame(r,points[0],points[1],
                                         GetMechanismLink(i,&(w->m)));
            }

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

      /* 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. IMPORTANT: the GenerateKinTree
         function can geneterate new joints !!! */
      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);
    }
}

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

void DeleteWorldCS(Tworld *w)
{
  unsigned int i;
  
  for(i=0;i<w->nl;i++)
    DeleteBranch(&(w->branch2Link[i]));
  free(w->branch2Link);

  DeleteEquations(&(w->refEqs));
  DeleteJacobian(&(w->refEqsJ));

  free(w->openLink);
  free(w->sortedLinks);
  free(w->jointTo);
  
  DeleteCuikSystem(&(w->kinCS));

  free(w->systemVars);

  WorldDeleteDOFInfo(w);
}


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


void InitWorldCD(Tparameters *pr,unsigned int mt,Tworld *w)
{ 
  if (w->stage!=WORLD_DEFINED)
    Error("Complete the workd definition before using the InitWorldCD function");

  if (w->wcd!=NULL)
    {
      DeleteCD(w->wcd);
      free(w->wcd);
      w->wcd=NULL;
    }

  /* The collision detection engine is only initialized if there is any
     collision to detect. */
  if (AnyCollision(w))
    {
      unsigned int e,i;

      e=(unsigned int)(GetParameter(CT_CD_ENGINE,pr));

      if (mt==0)
        Error("Wrong maximum number of threads in InitWorldCD");

      w->nwcd=mt;
      NEW(w->wcd,w->nwcd,TworldCD);
      
      /* when mt>0 is because OpenMP is available and will be used -> initialize in parallel */
      #pragma omp parallel for private(i) if (mt>1)
      for(i=0;i<mt;i++)
        InitCD(e,(mt>1),&(w->m),&(w->e),w->checkCollisionsLL,w->checkCollisionsLO,&(w->wcd[i]));
    }
}

boolean WorldCanCollide(Tworld *w)
{
  return(w->wcd!=NULL);
}

boolean WorldContinuousCD(Tworld *w)
{
  if (w->wcd==NULL)
    return(FALSE);
  else
    /* we query the first collision detecion structure, even if we have many. */
    return(GetCDEngine(w->wcd)==C_FCL);
}

boolean MoveAndCheckCDFromTransforms(boolean all,unsigned int tID,
                                     THTransform *tl,THTransform *tlPrev,Tworld *w)
{
  if (w->wcd!=NULL)
    {
      /* If sequential force it, even if tID is different from 0. */
      if (w->nwcd==0)
        return(CheckCollision(all,tl,tlPrev,&(w->wcd[0])));
      else
        return(CheckCollision(all,tl,tlPrev,&(w->wcd[tID])));
    }
  else
    return(FALSE); /* no collison to detect */
}


boolean MoveAndCheckCD(Tparameters *p,boolean all,unsigned int tID,
                       double *sol,double *solPrev,Tworld *w)
{
  if (w->wcd!=NULL)
    {
      THTransform *tl,*tlPrev=NULL;
      boolean c;

      GetLinkTransformsFromSolution(p,sol,&tl,w);
      if ((solPrev!=NULL)&&(GetCDEngine(w->wcd)==C_FCL)) 
        GetLinkTransformsFromSolution(p,sol,&tlPrev,w);

      c=MoveAndCheckCDFromTransforms(all,tID,tl,tlPrev,w);
      
      DeleteLinkTransforms(tl,w);
      if (tlPrev!=NULL)
        DeleteLinkTransforms(tlPrev,w);
      
      return(c);
    }
  else
    return(FALSE); /* no collison to detect */
}

void DeleteWorldCD(Tworld *w)
{
  if (w->wcd!=NULL)
    {
      unsigned int i;
      
      for(i=0;i<w->nwcd;i++)
        DeleteCD(&(w->wcd[i]));

      free(w->wcd);
      w->wcd=NULL;
    }
}


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

void GetLinkTransformsFromDOF(double *dof,THTransform **tl,Tworld *w)
{
  unsigned int i,l,parent,jID;
  THTransform *tj,t;
  Tjoint *j;

  NEW(tj,w->nj,THTransform);
  for(i=0;i<w->nj;i++)
    {
      j=GetMechanismJoint(i,&(w->m));
      GetJointTransform(&(dof[w->joint2dof[i]]),&(tj[i]),j);
    }

  NEW(*tl,w->nl,THTransform);
#if (0)
  /* The old, slower way to obtain the transform for each link.  */
  for(i=0;i<w->nl;i++)
    GetTransformFromBranch(tj,&((*tl)[i]),&(w->branch2Link[i]));
#else
  /* The new way takes advantage of the kinematic tree structure
     to reduce the number of matrix products (does not repeate 
     compuations). */
  HTransformIdentity(&((*tl)[0]));
  for(i=1;i<w->nl;i++)
    {
      l=w->sortedLinks[i]; /* next link whose pose can be easily computed */
      jID=w->jointTo[l]; /* joint to use to compute such pose */
      j=GetMechanismJoint(jID,&(w->m));
      if (l==JointToID(j))
        {
          parent=JointFromID(j); 
          HTransformProduct(&((*tl)[parent]),&(tj[jID]),&((*tl)[l]));
        }
      else
        {
          parent=JointToID(j); 
          HTransformInverse(&(tj[jID]),&t);
          HTransformProduct(&((*tl)[parent]),&t,&((*tl)[l]));
        }
    }
#endif

  for(i=0;i<w->nj;i++)
    HTransformDelete(&(tj[i]));
  free(tj);
}

void GetLinkTransformsFromSolutionPoint(Tparameters *p,boolean simp,double *sol,
                                        THTransform **tl,Tworld *w)
{
  unsigned int r;

  if (w->stage!=WORLD_DEFINED)
   Error("The equations and variables are not yet defined");

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  if (r==REP_JOINTS)
    {
      /* In the JOINT-based representation all variables are system variables */
      /* and the simplified point is the same as the original */
      GetLinkTransformsFromDOF(sol,tl,w);
    }
  else
    {
      double *v;
      
      if (simp)
        RegenerateOriginalPoint(p,sol,&v,&(w->kinCS));
      else
        {
          unsigned int n,i,k;
  
          /* The input only has values for the input variables. Get the values for the
             non-system variables first. */

          n=GetCSNumVariables(&(w->kinCS));
          k=0;
          NEW(v,n,double);
          for(i=0;i<n;i++)
            {
              if (w->systemVars[i])
                {
                  v[i]=sol[k];
                  k++;
                }
              else
                v[i]=0.0; /*Non system variables are set to 0. See below! */
            }
        }
      RegenerateMechanismSolution(p,&(w->kinCS),v,&(w->m));

      GetLinkTransformsFromSolution(p,v,tl,w);

      free(v);
    }
}

unsigned int GetSolutionPointFromLinkTransforms(Tparameters *p,THTransform *tl,double **sol,Tworld *w)
{
  unsigned int rep,n;
  
  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));

  n=GetCSNumSystemVariables(&(w->kinCS));
  NEW(*sol,n,double);

  if (rep==REP_JOINTS)
    GetMechanismDOFsFromTransforms(p,tl,*sol,&(w->m));
  else
    {
      unsigned int i,nv,k;
      double *dof,*sample;
      Tbox b;

      /* Get the dof from the transforms */
      NEW(dof,w->ndof,double);
      GetMechanismDOFsFromTransforms(p,tl,dof,&(w->m));
      
      /* Get the sample from the dof */
      nv=WorldDOF2Sol(p,dof,&sample,&b,w);

      /* and keep only the system variables */
      k=0;
      for(i=0;i<nv;i++)
        {
          if (w->systemVars[i])
            {
              (*sol)[k]=sample[i];
              k++;
            }
        }

      /* Release used memory */
      DeleteBox(&b);
      free(sample);
      free(dof);
    }

  return(GetCSNumSystemVariables(&(w->kinCS)));
}


void GetLinkTransformsFromSolution(Tparameters *p,double *sol,
                                   THTransform **tl,Tworld *w)
{
  unsigned int rep;

  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));

  if (rep==REP_JOINTS)
    GetLinkTransformsFromDOF(sol,tl,w);
  else
    {  
      unsigned int i;
      Tlink *l;
      double *r;

      NEW(*tl,w->nl,THTransform);

      NEW(r,3*w->nl,double);
      EvaluateEqualityEquations(FALSE,sol,r,&(w->refEqs));
      
      for(i=0;i<w->nl;i++)
        {
          l=GetMechanismLink(i,&(w->m)); 
          GetTransform2Link(p,&(w->kinCS),sol,IsGroundLink(i),
                            &(r[3*i]),&((*tl)[i]),l);
        }
      free(r);
    }
}

void DeleteLinkTransforms(THTransform *tl,Tworld *w)
{
  unsigned int i;

  for(i=0;i<w->nl;i++)
    HTransformDelete(&(tl[i]));

  free(tl);
}



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

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

  w->nl=0; 
  w->nj=0;
  w->no=0;
  w->nb=0;
  w->np=0;
  w->nvars=0;
  w->nsvars=0;
  w->systemVars=NULL;
  w->checkCollisionsLL=NULL;
  w->checkCollisionsLO=NULL;
  
  w->endEffector=NO_UINT;

  w->branch2Link=NULL;

  w->stage=WORLD_IN_DEFINITION;

  w->wcd=NULL;
}

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

  if (w->stage!=WORLD_IN_DEFINITION)
    Error("Links can only be added during the world definition");

  if (endEffector)
    w->endEffector=w->nl;

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

  /* define a new row and column in he LL table */

  /* expand the previous rows, if any */
  for(j=0;j<w->nl-1;j++)
    {
      MEM_EXPAND(w->checkCollisionsLL[j],w->nl,boolean); /* enlarge */
      w->checkCollisionsLL[j][w->nl-1]=FALSE;
    }
  
  /* create a new row */
  if (w->checkCollisionsLL==NULL)
    { NEW(w->checkCollisionsLL,w->nl,boolean*); } /* create */
  else
    { MEM_EXPAND(w->checkCollisionsLL,w->nl,boolean*); } /* enlarge */

  /* allocate and initialize the new row */
  NEW(w->checkCollisionsLL[w->nl-1],w->nl,boolean);
  for(j=0;j<w->nl;j++)
    w->checkCollisionsLL[w->nl-1][j]=FALSE;

  if (w->no>0)
    {
      /* define a new row in he LO table */
      if (w->checkCollisionsLO==NULL)
        { NEW(w->checkCollisionsLO,w->nl,boolean*); } /* create */
      else
        { MEM_EXPAND(w->checkCollisionsLO,w->nl,boolean*); } /* enlarge */
      NEW(w->checkCollisionsLO[w->nl-1],w->no,boolean);
      for(j=0;j<w->no;j++)
        w->checkCollisionsLO[w->nl-1][j]=FALSE;
    }

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

unsigned int AddJoint2World(Tjoint *j,Tworld *w)
{
  if (w->stage!=WORLD_IN_DEFINITION)
    Error("Joints can only be added during the world definition");

  w->nj++;

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

void AddObstacle2World(char *name,Tpolyhedron *o,Tworld *w)
{
  unsigned int j;

  if (w->stage!=WORLD_IN_DEFINITION)
    Error("Obstacles can only be added during the world definition");

  w->no++;
  w->np++;

  if (w->nl>0)
    {
      /* the first obstacle, we create the LO table */
      if (w->no==1)
        NEW(w->checkCollisionsLO,w->nl,boolean*);

      /* Add a column to the LO table */
      for(j=0;j<w->nl;j++)
        {
          if (w->no==1)
            { NEW(w->checkCollisionsLO[j],w->no,boolean); /*create*/}
          else
            { MEM_EXPAND(w->checkCollisionsLO[j],w->no,boolean); /* enlarge */}
          w->checkCollisionsLO[j][w->no-1]=FALSE;
        }
      
      /* initialize the new row */
      for(j=0;j<w->no;j++)
        w->checkCollisionsLO[w->nl-1][j]=FALSE;
    }

  AddShape2Environment(name,o,&(w->e));
}

unsigned int GetWorldMobility(Tworld *w)
{
  return(GetMechanismMobility(&(w->m)));
}

boolean IsMechanismInWorldAllSpheres(Tworld *w)
{
  return(IsMechanismAllSpheres(&(w->m)));
}

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

unsigned int GetWorldObstacleID(char *obsName,Tworld *w)
{
  return(GetObstacleID(obsName,&(w->e)));
}

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

Tjoint *GetWorldJoint(unsigned int jointID,Tworld *w)
{
  return(GetMechanismJoint(jointID,&(w->m)));
}

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

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

unsigned int GetWorldNObstacles(Tworld *w)
{
  return(w->no);
}

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

unsigned int GetWorldNConvexBodies(Tworld *w)
{
  return(w->np);
}

void DeleteWorldCollisionInfo(Tworld *w)
{
  unsigned int i;

  if (w->checkCollisionsLL!=NULL)
    {
      for(i=0;i<w->nl;i++)
        free(w->checkCollisionsLL[i]);
      free(w->checkCollisionsLL);
    }

  if (w->checkCollisionsLO!=NULL)
    {
      for(i=0;i<w->nl;i++)
        free(w->checkCollisionsLO[i]);
      free(w->checkCollisionsLO);
    }
}

void WorldInitDOFInfo(Tworld *w)
{
  unsigned int i,k,l,n,p;
  Tjoint *j;

  if (w->stage!=WORLD_DEFINED)
    Error("The DOF info is to be defined at the end of the world definition");

  w->ndof=0;
  for(i=0;i<w->nj;i++)
    {
       j=GetMechanismJoint(i,&(w->m));
       w->ndof+=GetJointDOF(j);
    }

  NEW(w->joint2dof,w->nj,unsigned int);
  NEW(w->dof2joint,w->ndof,unsigned int);
  NEW(w->dof2range,w->ndof,unsigned int);

  n=0;
  for(i=0;i<w->nj;i++)
    {
      j=GetMechanismJoint(i,&(w->m));
      p=CoupledWith(j);
      if (p==NO_UINT)
        {
          /* Note that if Num_DOF==0 (FIX_JOINT) nothing is done 
             and the dof2joint and joint2dof mappings are not
             actually used. */

          w->joint2dof[i]=n;

          k=GetJointDOF(j);
          for(l=0;l<k;l++)
            {
              w->dof2joint[n]=i;
              w->dof2range[n]=l;
              n++;
            }
        }
      else
        {
          if (p>=i)
            Error("Joints can only be coupled with preceeding joints");
          /* We will re-use the values from the original joint. */
          w->joint2dof[i]=w->joint2dof[p];
        }
    }
}


void WorldDeleteDOFInfo(Tworld *w)
{
  free(w->joint2dof);
  free(w->dof2joint);
  free(w->dof2range);
}

unsigned int GetWorldNDOF(Tworld *w)
{
  return(w->ndof);
}

void GetWorldRangeDOF(unsigned int ndof,Tinterval *r,Tworld *w)
{
  Tjoint *j;

  if (ndof>=w->ndof)
    Error("DOF out of range in GetWorldRangeDOF");

  j=GetMechanismJoint(w->dof2joint[ndof],&(w->m));
  GetJointRangeN(w->dof2range[ndof],w->maxCoord,r,j);
}

void GetWorldJointLabel(unsigned int ndof,char **string,Tworld *w)
{
  if (w->dof2joint==NULL)
    Error("Uninitialized dof info in GetWorkdJointLabel");
  if (w->dof2range[ndof]==0)
    GetJointName(string,GetMechanismJoint(w->dof2joint[ndof],&(w->m)));
  else
    *string=NULL;
}

void GetWorldDOFLabel(unsigned int ndof,char **string,Tworld *w)
{
  Tjoint *j;

  if (w->dof2joint==NULL)
    Error("Uninitialized dof info in GetWorkdDOFLabel");
  
  j=GetMechanismJoint(w->dof2joint[ndof],&(w->m));
  if (GetJointDOF(j)==1)
    GetJointName(string,j);
  else
    {
      char *jointName;

      GetJointName(&jointName,GetMechanismJoint(w->dof2joint[ndof],&(w->m)));
      NEW(*string,strlen(jointName)+10,char);

      sprintf(*string,"%s %u",jointName,w->dof2range[ndof]+1);
      free(jointName);
    }
}

inline boolean IsWorldPolynomial(Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  return(IsCSPolynomial(&(w->kinCS)));
}

void CheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w)
{
  unsigned int i,j;

  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  CheckSelfCollisions(fl,w);
  
  if (w->no>fo)
    {
      /* All link-obstacles are checked for collision except
         the ground link with any obstacle */
      for(i=fl;i<w->nl;i++)
        {
          for(j=fo;j<w->no;j++)
            w->checkCollisionsLO[i][j]=((!IsGroundLink(i))&&
                                        (GetObstacleShapeStatus(j,&(w->e))!=DECOR_SHAPE));
        }
    }
}

void NoCheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w)
{
  unsigned int i,j;

  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  NoCheckSelfCollisions(fl,w);
  
  if (w->no>fo)
    {
      for(i=fl;i<w->nl;i++)
        {
          for(j=fo;j<w->no;j++)
            w->checkCollisionsLO[i][j]=FALSE;
        }
    }
}

void CheckSelfCollisions(unsigned int fl,Tworld *w)
{
  unsigned int i,j;

  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  /* Collisions of a link with itself are not checked */
  for(i=fl;i<w->nl;i++)
    {
      for(j=fl;j<w->nl;j++)
        w->checkCollisionsLL[i][j]=(i!=j);
    }
}

void NoCheckSelfCollisions(unsigned int fl,Tworld *w)
{
  unsigned int i,j;

  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  for(i=fl;i<w->nl;i++)
    {
      for(j=fl;j<w->nl;j++)
        w->checkCollisionsLL[i][j]=FALSE;
    }
}

void CheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w)
{
  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  if ((a<w->nl)&&(b<w->nl)&&(a!=b))
    w->checkCollisionsLL[a][b]=w->checkCollisionsLL[b][a]=TRUE;
}

void NoCheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w)
{
  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  if ((a<w->nl)&&(b<w->nl))
    w->checkCollisionsLL[a][b]=w->checkCollisionsLL[b][a]=FALSE;
}

void CheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w)
{ 
  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  if ((a<w->nl)&&(!IsGroundLink(a))&&(b<w->no))
    w->checkCollisionsLO[a][b]=TRUE;
}

void NoCheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w)
{
  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The geometry information must be added while defining the world");

  if ((a<w->nl)&&(b<w->no))
    w->checkCollisionsLO[a][b]=FALSE;
}

boolean AnyCollision(Tworld *w)
{
  unsigned int i;
  boolean any;

  any=FALSE;
  for(i=0;((i<w->nl)&&(!any));i++)
    any=LinkCanCollide(i,w->nl,w->no,w->checkCollisionsLL,w->checkCollisionsLO);

  return(any);
}

void PrintCollisions(FILE *f,Tworld *w)
{
  if (AnyCollision(w))
    {
      unsigned int i,j;
      char *l;

      fprintf(f,"[COLLISIONS]\n\n");
      fprintf(f,"  do not check: all\n");
      for(i=0;i<w->nl;i++)
        {
          l=GetLinkName(GetMechanismLink(i,&(w->m)));
          for(j=i+1;j<w->nl;j++)
            {
              if ((w->checkCollisionsLL[i][j])||(w->checkCollisionsLL[j][i]))
                fprintf(f,"  check: %s,%s\n",l,GetLinkName(GetMechanismLink(j,&(w->m))));
            }
        }
      for(i=0;i<w->nl;i++)
        {
          l=GetLinkName(GetMechanismLink(i,&(w->m)));
          for(j=0;j<w->no;j++)
            if (w->checkCollisionsLO[i][j])
              fprintf(f,"  check: %s,%s\n",l,GetObstacleName(j,&(w->e)));
        }
      fprintf(f,"\n");
    }
}

boolean NewtonInWorld(Tparameters *p,double *v,Tbox *b_sol,Tworld *w)
{
  double *vA;
  Tbox b_solA;
  boolean converged;
  unsigned int i,k,nv,nvs;
  boolean *systemVarsKin;

  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  nv=GetCSSystemVars(&systemVarsKin,&(w->kinCS));
  NEW(vA,nv,double);

  converged=CuikNewton(p,vA,&b_solA,&(w->kinCS));

  nvs=0;
  for(i=0;i<nv;i++)
    {
      if (systemVarsKin[i])
        nvs++;
    }

  InitBox(nvs,NULL,b_sol);

  k=0;
  for(i=0;i<nv;i++)
    {
      if (systemVarsKin[i])
        {
          v[k]=vA[i];
          SetBoxInterval(k,GetBoxInterval(i,&b_solA),b_sol);
          k++;
        }
    }
  DeleteBox(&b_solA);
  free(vA);
  free(systemVarsKin);

  #if (_DEBUG>2)
    printf("Newton generated box:");
    PrintBox(stdout,b_sol);
    fflush(stdout);
  #endif

  return(converged);
}

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

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

inline unsigned int GetWorldVarTopology(unsigned int vID,Tworld *w)
{
  return(GetCSVarTopology(vID,&(w->kinCS)));
}

unsigned int GetWorldSimpVariableMask(Tparameters *p,boolean **sv,Tworld *w)
{
  unsigned int i,n;

  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  n=GetCSNumVariables(&(w->kinCS));

  NEW((*sv),n,boolean);
  for(i=0;i<n;i++)
    (*sv)[i]=FALSE;

  for(i=0;i<w->nl;i++)
    GetLinkPoseSimpVars(p,*sv,&(w->kinCS),GetMechanismLink(i,&(w->m)));

  return(n);
}

inline void GetWorldVarNames(char **vn,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  return(GetCSVariableNames(vn,&(w->kinCS)));
}

void GenerateWorldEquations(Tparameters *p,Tworld *w)
{  
  unsigned int i,rep;
  Tvariables vs;
  
  if (w->stage!=WORLD_IN_DEFINITION)
    Error("The equations are alrady defined");

  w->stage=WORLD_DEFINED;

  InitWorldCS(w);

  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));

  w->maxCoord=GetMechanismMaxCoordinate(&(w->m))+
              GetEnvironmentMaxCoordinate(&(w->e));

  /*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. 
    This already initializes the branch2link, the sortedLinks and the jointTo
    arrays.
  */
  InitWorldKinCS(p,w);


  /* We generate the equations defining the translation 
     from the ground link 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.
  */

  InitEquations(&(w->refEqs));
  if (rep==REP_JOINTS)
    {
      TMequation ref;
      
      for(i=0;i<w->nl;i++) 
        {
          GenerateMEquationFromBranch(p,&(w->branch2Link[i]),&ref,w);
          AddMatrixEquation(&ref,&(w->refEqs));
          DeleteMEquation(&ref);
        }
    }
  else
    {
      Tequation ref[3];
      unsigned int j;

      for(i=0;i<w->nl;i++) 
        {
          GenerateTransEquationsFromBranch(p,COORD_EQ,ref,&(w->branch2Link[i]),w);
          for(j=0;j<3;j++)
            {
              AddEquationNoSimp(&(ref[j]),&(w->refEqs));
              DeleteEquation(&(ref[j]));
            }
        }
    }
  GetCSVariables(&vs,&(w->kinCS));
  InitJacobian(&vs,&(w->refEqs),&(w->refEqsJ));
  DeleteVariables(&vs);

  /*copy of the system variables to use later on*/
  w->nvars=GetCSSystemVars(&(w->systemVars),&(w->kinCS));
  w->nsvars=0;
  for(i=0;i<w->nvars;i++)
    {
      if (w->systemVars[i])
        w->nsvars++;
    }

  /* Define the dof information */
  WorldInitDOFInfo(w);
}

void GenerateWorldSingularityEquations(Tparameters *p,char *ln,TCuikSystem *cs,Tworld *w)
{
  unsigned int i,rep;

  if (w->stage!=WORLD_DEFINED)
    GenerateWorldEquations(p,w); /* Generate kin equations only */
  
  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  if (rep==REP_JOINTS)
    Error("GenerateWorldSingularityEquations is not defined for DOF-based representations");

  CopyCuikSystem(cs,&(w->kinCS));

  for(i=0;i<w->nj;i++)
    GenerateJointRangeSingularityEquations(p,cs,GetMechanismJoint(i,&(w->m)));

  if (ln!=NULL)
    {
      unsigned int lk;

      lk=GetMechanismLinkID(ln,&(w->m));
      if (lk!=NO_UINT)
        {
          Tequation eq;
          char *vname;
          Tlink *l;
          char *lname;
          Tinterval rangeInf;
          Tvariable var;
          Tmonomial f;
          unsigned int vID;
          
          l=GetMechanismLink(lk,&(w->m));
          lname=GetLinkName(l); 
          NEW(vname,strlen(lname)+100,char);
          NewInterval(-w->maxCoord,w->maxCoord,&rangeInf);
          InitMonomial(&f);

          for(i=0;i<3;i++)
            {
              CopyEquation(&eq,GetEquation(lk*3+i,&(w->refEqs)));

              LINK_TRANS(vname,lname,i); 

              NewVariable(CARTESIAN_VAR,vname,&var);
              SetVariableInterval(&rangeInf,&var);
              vID=AddVariable2CS(&var,cs);
              DeleteVariable(&var);

              AddCt2Monomial(-1.0,&f);
              AddVariable2Monomial(NFUN,vID,1,&f);
              AddMonomial(&f,&eq);
              ResetMonomial(&f);

              SetEquationType(COORD_EQ,&eq);

              AddEquation2CS(p,&eq,cs);

              DeleteEquation(&eq);
            }
          free(vname);
          DeleteMonomial(&f);
        }
    }
}
void GenerateWorldTWSEquations(Tparameters *p,char *ln,TCuikSystem *cs,Tworld *w)
{
  unsigned int i,nv,vID;
  boolean *selectedVars;
  char *vname;

  GenerateWorldSingularityEquations(p,ln,cs,w);

  nv=GetCSNumVariables(cs);
  NEW(selectedVars,nv,boolean);

  /*in principle all variables are to be used in the Jacobian*/
  for(i=0;i<nv;i++)
    selectedVars[i]=TRUE; 

  /*but we exclude those for translation*/
  NEW(vname,strlen(ln)+100,char);
  for(i=0;i<3;i++)
    {
      LINK_TRANS(vname,ln,i);

      vID=GetCSVariableID(vname,cs);
      if (vID==NO_UINT)
        Error("Translation variable is not included in the cuiksystem (in GenerateWorldTWSEquations)");
      selectedVars[vID]=FALSE;
    }
 
  AddSimplifiedJacobianEquations(p,selectedVars,cs);

  free(selectedVars);
  free(vname);
}

inline void GetWorldJacobian(TJacobian *J,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in GetWorldKinJacobian");

  GetCSJacobian(J,&(w->kinCS));
}

inline void GetWorldSimpJacobian(Tparameters *p,TJacobian *J,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in GetWorldKinJacobian");

  GetSimpCSJacobian(p,J,&(w->kinCS));
}

inline unsigned int GetWorldSimpTopology(Tparameters *p,
                                         unsigned int **t,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in GetWorldSimpTopology"); 
  
  return(GetSimpCSTopology(p,t,&(w->kinCS)));
}

inline unsigned int WorldSimpCuikNewton(Tparameters *p,double *pt,Tworld *w)
{
  return(CuikNewtonSimp(p,pt,&(w->kinCS)));
}

inline void EvaluateWorldJacobian(double *pt,double ***J,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in EvaluateWorldKinJacobian");
  
  EvaluateCSJacobian(pt,J,&(w->kinCS));
}

inline void WorldEvaluateEquations(double *pt,double *r,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in EvaluateWorldKinJacobian");
  
  EvaluateCSEquations(pt,r,&(w->kinCS));
}

inline boolean WorldInequalitiesHold(double *pt,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in WorldInequalitiesHold");

  return(InequalitiesHoldOnPoint(pt,&(w->kinCS)));
}

inline boolean WorldSimpInequalitiesHold(Tparameters *p,double *pt,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in WorldInequalitiesHold");

  return(SimpInequalitiesHoldOnPoint(p,pt,&(w->kinCS)));
}

inline double WorldErrorInSimpInequalities(Tparameters *p,double *pt,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in WorldErrorInSimpInequalities");

  return(ErrorInSimpInequalitiesOnPoint(p,pt,&(w->kinCS)));
}

inline void WorldEvaluateSimpEquations(Tparameters *p,double *pt,double *r,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in WorldEvaluateSimpEquations");
  
  EvaluateSimpCSEquations(p,pt,r,&(w->kinCS));
}

inline void WorldEvaluateSubSetSimpEquations(Tparameters *p,boolean *se,double *pt,double *r,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in WorldEvaluateSubSetSimpEquations");
  
  EvaluateSubSetSimpCSEquations(p,se,pt,r,&(w->kinCS));
}

double WorldErrorInEquations(double *pt,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in WorldErrorInEquations");
  
  return(ErrorInCSEquations(pt,&(w->kinCS)));
}

inline double WorldErrorInSimpEquations(Tparameters *p,double *pt,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in WorldErrorInSimpEquations");
  
  return(ErrorInSimpCSEquations(p,pt,&(w->kinCS)));
}

double WorldErrorFromDOFs(Tparameters *p,double *dof,Tworld *w)
{
  double e,*v;
  Tbox b;

  WorldDOF2Sol(p,dof,&v,&b,w);
  DeleteBox(&b);
  e=WorldErrorInEquations(v,w);
  free(v);

  return(e);
}

inline double EvaluateWorldCost(Tparameters *p,boolean simp,double *pt,void *w)
{
  if (((Tworld *)w)->stage!=WORLD_DEFINED)
    Error("World without equations in EvaluateWorldCost");
  
  return(EvaluateCSCost(p,simp,pt,(void *)&(((Tworld *)w)->kinCS)));
}

unsigned int GetWorldNumVariables(Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in EvaluateWorldKinJacobian");
  
  return(GetCSNumVariables(&(w->kinCS)));
}

inline unsigned int GetWorldNumSystemVariables(Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in EvaluateWorldKinJacobian");
  
  return(GetCSNumSystemVariables(&(w->kinCS)));
}

inline unsigned int RegenerateWorldOriginalPoint(Tparameters *p,double *s,double **o,Tworld *w)
{
  unsigned int nv,r;

  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in EvaluateWorldKinJacobian");

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));

  /* for JOINTS representations -> no simplification -> nothing to reconstruct */
  if (r==REP_JOINTS)
    {
      nv=GetCSNumVariables(&(w->kinCS));
      NEW(*o,nv,double);
      memcpy(*o,s,nv*sizeof(double));
    }
  else
    {
      /* Revover the original system variables and some of the dummies  */
      nv=RegenerateOriginalPoint(p,s,o,&(w->kinCS));

      /* Some dummy variables that only appear in dummy equations are 
         not present in the simplified system and their value can not be
         determined using 'RegenerateOriginalPoint'. We have to use the
         semantic of the variables to recover their value. */
      RegenerateMechanismSolution(p,&(w->kinCS),*o,&(w->m));
    }
  return(nv);
}

inline unsigned int WorldGenerateSimplifiedPoint(Tparameters *p,double *o,double **s,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("World without equations in EvaluateWorldKinJacobian");

  return(GenerateSimplifiedPoint(p,o,s,&(w->kinCS)));
}

inline void GetWorldInitialBox(Tbox *b,Tworld *w)
{
   if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

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

inline void GetWorldSimpInitialBox(Tparameters *p,Tbox *b,Tworld *w)
{
  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  GenerateSimpInitialBox(p,b,&(w->kinCS)); 
}

unsigned int MaxWorldReduction(Tparameters *p,Tbox *b,double *reduction,Tworld *w)
{ 
  unsigned int s;
  
  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");
  
  #if ((!_USE_MPI)&&(_DEBUG==1)) 
  {
    double sizeIn;
    
    sizeIn=GetBoxSize(w->systemVars,b);
    printf("            Kin <s:%g,l:%u>-> ",sizeIn,GetBoxLevel(b));
  }
  #endif

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

        sizeOut=GetBoxSize(w->systemVars,b);
        printf("%g (s:%g)\n",*reduction,sizeOut);
      }
    fflush(stdout);
  #endif

  return(s);
}

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

  /* We init the collision detection to be able to print
     collision information when moving the world */
  if (w->wcd==NULL)
    InitWorldCD(pr,1,w);

  Start3dBlock(pt);
    PlotEnvironment(pt,&(w->e));
    PlotMechanism(pt,axesLength,&(w->m));
  Close3dBlock(pt);
}

unsigned int RegenerateWorldSolutionPoint(Tparameters*pr,double *p,double **v,Tworld *w)
{
  unsigned int i,n,k;

  if (w->stage!=WORLD_DEFINED)
   Error("The equations and variables are not yet defined");

  n=GetCSNumVariables(&(w->kinCS));
  NEW(*v,n,double);

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

  RegenerateMechanismSolution(pr,&(w->kinCS),*v,&(w->m));
  
  return(n);
}

void WorldPrintAtoms(Tparameters *pr,FILE *f,double *pt,Tworld *w)
{  
  THTransform *tl;

  if (GetEnvironmentNObstacles(&(w->e)))
    Warning("The bodies in the environment would not be considered");

  GetLinkTransformsFromSolutionPoint(pr,FALSE,pt,&tl,w);
 
  MechanismPrintAtoms(f,tl,&(w->m));

  DeleteLinkTransforms(tl,w);
}

void WorldStoreRigidGroups(Tparameters *pr,FILE *f,double *pt,Tworld *w)
{  
  THTransform *tl;

  if (GetEnvironmentNObstacles(&(w->e)))
    Warning("The bodies in the environment would not be considered");

  GetLinkTransformsFromSolutionPoint(pr,FALSE,pt,&tl,w);

  MechanismStoreRigidAtoms(f,tl,&(w->m));

  DeleteLinkTransforms(tl,w);
}

void WorldAtomJacobian(Tparameters *pr,double *sol,unsigned int *nr,unsigned int *nc,double ***J,Tworld *w)
{
  double *a;
  unsigned int ta,*na,nb;
  unsigned int i,j,k,r;
  Tlink *l;
  Tpolyhedron *b;
  
  if (!IsMechanismAllSpheres(&(w->m)))
    Error("The world is not sphere-only (WorldAtomJacobian)");

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
  if (r!=REP_JOINTS)
    Error("WorldAtomJacobian is only defined for JOINT-based representations");

  /* Count the number of atoms (total and per link) */
  NEW(na,w->nl,unsigned int); /* number of atoms per link */
  ta=0;
  for(i=0;i<w->nl;i++)
    {
      l=GetMechanismLink(i,&(w->m));
      nb=LinkNBodies(l);
      na[i]=0;
      for(j=0;j<nb;j++)
        {
          b=GetLinkBody(j,l);
          if (GetPolyhedronType(b)==SPHERE)
            na[i]++;
        }
      ta+=na[i];
    }

  /* Get the coordinates for each atom */
  NEW(a,3*ta,double); /* coordinates for each atom */
  k=0;
  for(i=0;i<w->nl;i++)
    {
      l=GetMechanismLink(i,&(w->m));
      nb=LinkNBodies(l);
      for(j=0;j<nb;j++)
        {
          b=GetLinkBody(j,l);
          if (GetPolyhedronType(b)==SPHERE)
            {
              GetPolyhedronCenter(&(a[k]),b);
              k+=3;
            }
        }
    }
  
  EvaluateJacobianXVectors(sol,ta,w->nl,na,a,nr,nc,J,&(w->refEqsJ));

  free(a);
}

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

  if (w->stage!=WORLD_DEFINED)
   Error("The equations and variables are not yet defined");

  if (GetCSNumSystemVariables(&(w->kinCS))<GetBoxNIntervals(b))
    Warning("System-solution dimension missmatch in MoveWorld (using only the first value in the solution)");

  if (GetCSNumSystemVariables(&(w->kinCS))>GetBoxNIntervals(b))
    Error("Not enough values in the solution (using a different REPRESENTATION parameter?)");

  n=GetCSNumVariables(&(w->kinCS));
  NEW(v,n,double);

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
  if (r==REP_JOINTS)
    {
      /* In dof repre. all variables are system variables */
      GetBoxCenter(NULL,v,b);

      MoveWorldDOF(pr,pt,v,w);
    }
  else
    {
      unsigned int i,k;
      THTransform *tl;

      k=0;
      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(pr,&(w->kinCS),v,&(w->m));

      GetLinkTransformsFromSolution(pr,v,&tl,w);

      if (MoveAndCheckCDFromTransforms(TRUE,0,tl,NULL,w)) 
        fprintf(stderr,"  Configuration is in collision\n"); 
      else 
        fprintf(stderr,"  Configuration is NOT in collision\n"); 
   
      fprintf(stderr,"  Configuration error: %g\n",WorldErrorInEquations(v,w));

      MoveMechanismFromTransforms(pr,pt,tl,&(w->m));

      DeleteLinkTransforms(tl,w);
    }
  free(v);
      
}

void PrintWorldAxes(Tparameters *pr,FILE *f,Tbox *b,Tworld *w)
{
  unsigned int i,k,n;
  double *r;
  double *v;
  Tjoint *j;
  unsigned int rep;

  if (w->stage!=WORLD_DEFINED)
   Error("The equations and variables are not yet defined");

  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
  if (rep==REP_JOINTS)
    Error("PrintWorldAxes is not defined for JOINT-based representations");

  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(pr,&(w->kinCS),v,&(w->m));

  NEW(r,w->nl*3,double);
  EvaluateEqualityEquations(FALSE,v,r,&(w->refEqs));

  for(i=0;i<w->nj;i++)
    {
      j=GetMechanismJoint(i,&(w->m));
      PrintJointAxes(pr,f,&(w->kinCS),v,r,j);
    }
  fprintf(f,"\n");

  free(r);

  free(v);
}

void PrintWorldCollisionInfo(FILE *f,char *fname,Tworld *w)
{
  if (w->wcd!=NULL)
    StoreCollisionInfo(f,fname,w->endEffector,&(w->m),w->wcd);
}

void MoveWorldDOF(Tparameters *pr,Tplot3d *pt,double *dof,Tworld *w)
{
  THTransform *tl;
  double *sample;
  Tbox bsample;

  if (w->stage!=WORLD_DEFINED)
   Error("The equations and variables are not yet defined");

  GetLinkTransformsFromDOF(dof,&tl,w);
  
  if (MoveAndCheckCDFromTransforms(TRUE,0,tl,NULL,w)) 
    {
      fprintf(stderr,"  Configuration is in collision\n"); 
      PrintCollisionInfo(tl,&(w->m),w->wcd);
    }
  else 
    fprintf(stderr,"  Configuration is NOT in collision\n"); 

  WorldDOF2Sol(pr,dof,&sample,&bsample,w);
  fprintf(stderr,"  Configuration error: %g\n",WorldErrorInEquations(sample,w));
  free(sample);
  DeleteBox(&bsample);

  MoveMechanismFromTransforms(pr,pt,tl,&(w->m));

  DeleteLinkTransforms(tl,w);
}

unsigned int WorldDOF2Sol(Tparameters *p,double *dof,double **sol,Tbox *b,Tworld *w)
{
  unsigned int r,i,n;
  Tjoint *j;

  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  /* Ensure that the input dofs are in correct range */
  for(i=0;i<w->ndof;i++)
    {
      j=GetMechanismJoint(w->dof2joint[i],&(w->m));
      if (GetJointRangeTopology(w->dof2range[i],j)==TOPOLOGY_S)
        PI2PI(dof[i]);
    }
  
  n=GetCSNumVariables(&(w->kinCS));
  NEW(*sol,n,double);
  for(i=0;i<n;i++)
    (*sol)[i]=INF;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));

  if (r==REP_JOINTS)
    /* in dof all variables are system variables */
    memcpy(*sol,dof,n*sizeof(double));
  else
    {
      unsigned int k,l1,l2;
      THTransform *tl;
      Tjoint *j;
      Tlink *l;

      GetLinkTransformsFromDOF(dof,&tl,w);
      for(i=0;i<w->nl;i++)
        {
          l=GetMechanismLink(i,&(w->m));
          GenerateLinkSolution(p,&(tl[i]),&(w->kinCS),*sol,IsGroundLink(i),l);
          if (w->openLink[i])
            {
              /* Some links are at the end of an open chain of links (i.e., the
                 mechanism is not fully composed by closed chains). In this case
                 the system includes variables to represent the end position (x,y,z)
                 of those links. Here we define the value for these variables from
                 the translation part of the corresponding link transform. */
              char *vname,*ln;
              unsigned int vID;

              ln=GetLinkName(l); 
              NEW(vname,strlen(ln)+100,char);

              for(k=0;k<3;k++)
                {
                  LINK_TRANS(vname,ln,k);
 
                  vID=GetCSVariableID(vname,&(w->kinCS));
                  if (vID==NO_UINT)
                    Error("Undefined reference variable in WorldDOF2Sol");

                  (*sol)[vID]=HTransformGetElement(k,AXIS_H,&(tl[i]));
                }
              free(vname);
            }
        }

      for(i=0;i<w->nj;i++)
        { 
          j=GetMechanismJoint(i,&(w->m));
          l1=JointFromID(j);
          l2=JointToID(j);
          GenerateJointSolution(p,&(dof[w->joint2dof[i]]),
                                &(tl[l1]),&(tl[l2]),&(w->kinCS),*sol,j);
        }
  
      DeleteLinkTransforms(tl,w);
    }

  InitBoxFromPoint(n,*sol,b);

  return(n);
}

void WorldSample2DOF(Tparameters *p,double *sample,double *dof,Tworld *w)
{
  unsigned int r ;

  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  
  if (r==REP_JOINTS)
    {
      /* The samples are already in JOINTS form */
      memcpy(dof,sample,sizeof(double)*w->ndof);
    }
  else
    {
      unsigned int i,n,k;
      THTransform *tl;
      double *v;

      /* Fully reconstruct the solution point (obtain non-system variables from
         system ones) */
      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]=sample[k];
              k++;
            }
          else
            v[i]=0.0; /*Non system variables are set to 0. See below! */
        }

      RegenerateMechanismSolution(p,&(w->kinCS),v,&(w->m));
      
      /* Get a transform giving the absolute pose for each link using 
         the full solution point. */
      GetLinkTransformsFromSolution(p,v,&tl,w);
      
      free(v);
      
      /* Obtain the joint variables from the link absolute poses */
      GetMechanismDOFsFromTransforms(p,tl,dof,&(w->m));
      
      DeleteLinkTransforms(tl,w);
    }
}

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

  if (w->stage!=WORLD_DEFINED)
    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(pr,&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(pr,&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;
  FILE *f;

  if (w->stage!=WORLD_DEFINED)
    Error("The equations and variables are not yet defined");

  CreateFileName(GetFilePath(fname),GetFileName(fname),NULL,CUIK_EXT,&fout);
  f=fopen(GetFileFullName(&fout),"w");
  if (!f) Error("Could not open file in PrintWorldCS");
  #if (_DEBUG>0)
    printf("Generating cuik file              : %s\n",GetFileFullName(&fout));
  #endif

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

void PrintWorld(char *fname,int argc,char **arg,Tworld *w)
{
  Tfilename fworld;
  FILE *f;
  time_t t;
  char hostname[50];

  CreateFileName(NULL,fname,NULL,WORLD_EXT,&fworld);
  fprintf(stderr,"Writing world to                  : %s\n",GetFileFullName(&fworld));
 
  f=fopen(GetFileFullName(&fworld),"w");
  if (!f)
    Error("Could not open the file to store the world");

  /* Do not document the link/joint/obstacle definitions. */
  fprintf(f,"/** \\cond */\n\n");

  PrintMechanism(f,GetFilePath(&fworld),GetFileName(&fworld),&(w->m));

  PrintEnvironment(f,GetFilePath(&fworld),&(w->e));

  /* Print the collision information */
  PrintCollisions(f,w);

  /* Autognerated documentation to avoid errors when generating the documentation. */
  fprintf(f,"/** \\endcond */\n");
  fprintf(f,"/**\n");
  fprintf(f,"   \\file %s.%s\n\n",GetFileName(&fworld),GetFileExtension(&fworld));
  fprintf(f,"   \\brief Automatically generated world file.\n\n");
  if ((argc>0)&&(arg!=NULL))
    {
      unsigned int i;

      fprintf(f,"   Automatically generated world file with the command:\n\n      - ");
      for(i=0;i<(unsigned int)argc;i++)
        fprintf(f,"%s ",arg[i]);
      fprintf(f,"\n      .\n\n");
    }
  else
    fprintf(f,"   Automatically generated world file.\n");
  t=time(NULL);
  gethostname(hostname,50);
  fprintf(f,"   Created on: %s   Executed at: %s\n",ctime(&t),hostname);
  fprintf(f,"*/\n");

  fclose(f);

  DeleteFileName(&fworld);
}

void DeleteWorld(Tworld *w)
{
  DeleteEnvironment(&(w->e));
  DeleteMechanism(&(w->m));
  DeleteWorldCollisionInfo(w);
  DeleteWorldCD(w);

  /* if GenerateWorldEquationSystems was called */
  if (w->stage==WORLD_DEFINED)
    DeleteWorldCS(w);
}