link.c

Go to the documentation of this file.

#include "link.h"
#include "polyhedron.h"
#include "varnames.h"

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

void CacheRotVars(unsigned int r,TCuikSystem *cs,Tlink *l);

void CacheRotVarsFLinks(TCuikSystem *cs,Tlink *l);

void CacheRotVarsLinks(TCuikSystem *cs,Tlink *l);

void CacheRotVarsQLinks(TCuikSystem *cs,Tlink *l);

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

void InitLink(char *name,Tlink *l)
{
  unsigned int namel,i;

  namel=strlen(name);
  if (namel==0)
    l->name=NULL;
  else
    {
      NEW(l->name,namel+1,char);
      strcpy(l->name,name);
    }

  InitVector(sizeof(double *),CopyVoidPtr,DeleteVoidPtr,INIT_NUM_SHAPES,&(l->bodies));

  l->s=1;
  l->c=0;
  HTransformIdentity(&(l->R));
  HTransformIdentity(&(l->iR));
  
  l->allSpheres=TRUE;

  for(i=0;i<3;i++)
    l->axisID[i]=NO_UINT;

  /*sum of the maximum coordinate value for all bodies in the link*/
  l->maxCoord=0.0;

  l->csvID=NULL;
  l->nvID=0;
  l->vID=NULL;
}

void CopyLink(Tlink *l_dst,Tlink *l_src)
{
  unsigned int i,n;

  if (l_src==NULL)
    l_dst->name=NULL;
  else
    {
      NEW(l_dst->name,strlen(l_src->name)+1,char);
      strcpy(l_dst->name,l_src->name);
    }

  InitVector(sizeof(double *),CopyVoidPtr,DeleteVoidPtr,INIT_NUM_SHAPES,&(l_dst->bodies));

  l_dst->allSpheres=TRUE;

  n=LinkNBodies(l_src);
  for(i=0;i<n;i++)
    AddBody2Link(GetLinkBody(i,l_src),l_dst);
  
  l_dst->c=l_src->c;
  l_dst->s=l_src->s;
  HTransformCopy(&(l_dst->R),&(l_src->R));
  HTransformCopy(&(l_dst->iR),&(l_src->iR));

  for(i=0;i<3;i++)
    l_dst->axisID[i]=l_src->axisID[i];

  l_dst->maxCoord=l_src->maxCoord;
  
  if (l_src->csvID==NULL)
    {
      l_dst->csvID=NULL;
      l_dst->nvID=0;
      l_dst->vID=NULL;
    }
  else
    {
      l_dst->csvID=l_src->csvID;
      l_dst->nvID=l_src->nvID;
      NEW(l_dst->vID,l_dst->nvID,unsigned int);
      memcpy(l_dst->vID,l_src->vID,l_dst->nvID*sizeof(unsigned int));
    }
}

void AddBody2Link(Tpolyhedron *b,Tlink *l)
{
  Tpolyhedron *bint;

  NEW(bint,1,Tpolyhedron);
  CopyPolyhedron(bint,b);

  NewVectorElement(&bint,&(l->bodies));

  l->allSpheres=((l->allSpheres)&&
                 ((GetPolyhedronType(bint)==SPHERE)||
                  (GetPolyhedronStatus(bint)==DECOR_SHAPE)));

  l->maxCoord+=GetPolyhedronMaxCoordinate(bint);
}

void ChangeLinkReferenceFrame(unsigned int r,double **p1,double **p2,Tlink *l)
{
  if ((r!=REP_QLINKS)&&(r!=REP_JOINTS))
    {
      /* This function has no effect when using quaternions.  */
      double v[3][3],n1,n2;
      unsigned int i,j;
      double s,c,d,ac;

      /* Define the vectors from the points */
      n1=0;
      n2=0;
      for(i=0;i<3;i++)
        {
          v[0][i]=p1[1][i]-p1[0][i];
          v[1][i]=p2[1][i]-p2[0][i];
          n1+=v[0][i]*v[0][i];
          n2+=v[1][i]*v[1][i];
        }
      /* Normalize the vectors */
      n1=sqrt(n1);
      n2=sqrt(n2);
      for(i=0;i<3;i++)
        {
          v[0][i]/=n1;
          v[1][i]/=n2;
        }

      /*compute the cos/sin between the two vectors*/
      /*The dot product of the two vectors is the cosinus
        between the vectors*/
      c=0; 
      for(i=0;i<3;i++)
        c+=v[0][i]*v[1][i];
      ac=fabs(c);
      if (ac<0.9) /*Ensure the two vectors are (by far) not aligned*/
        {
          /*The cross product of the two vectors is a vector
            orthogonal to the two vectors with norm the sinus
            of the angle between the vectors*/
          v[2][0]=v[0][1]*v[1][2]-v[0][2]*v[1][1];
          v[2][1]=v[0][2]*v[1][0]-v[0][0]*v[1][2];
          v[2][2]=v[0][0]*v[1][1]-v[0][1]*v[1][0];
        
          s=0;
          for(i=0;i<3;i++)
            s+=v[2][i]*v[2][i];
          s=sqrt(s);
          for(i=0;i<3;i++)
            v[2][i]/=s;
       
          l->c=c;
          l->s=s;

          /* Compute the inverse of the matrix with vector 'v' as colums
             (i.e., the inverse of the basis change).
             Rows of R are computed taking into account that 
             R * [v1 v2 v3] = Id 
             R=[w1;w2;w3]
             From construction w3=v3
             and w1=\alpha v1 + \beta v2
                 w2=\gamma v1 + \delta v2
             where \alpa, \beta, \gamma, \delta are easy to find
             taking into account that w1*v1\tr=1 w1*v2\tr=0,... 
          */
          d=(1/(s*s));
          for(j=0;j<3;j++)
            HTransformSetElement(0,j,d*(v[0][j]-c*v[1][j]),&(l->R));
          for(j=0;j<3;j++)
            HTransformSetElement(1,j,d*(v[1][j]-c*v[0][j]),&(l->R));
          for(j=0;j<3;j++)
            HTransformSetElement(2,j,v[2][j],&(l->R));
          
          /* iR: The inverse of R: v1 v2 v3 in colums */
           for(i=0;i<3;i++)
             {
               for(j=0;j<3;j++)
                 HTransformSetElement(j,i,v[i][j],&(l->iR));
             }
        }
    }
}

unsigned int LinkNBodies(Tlink *l)
{
  return(VectorSize(&(l->bodies)));
}

unsigned int LinkNAtoms(Tlink *l)
{
  unsigned int i,m,n;
  Tpolyhedron *bd;
      
  n=0;
  m=LinkNBodies(l);
        
  if ((m>0)&&(VisibleLink(l)))
    {
      for(i=0;i<m;i++)
        {
          bd=GetLinkBody(i,l);
          if (GetPolyhedronType(bd)==SPHERE) 
            n++;
        }
    }
  return(n);
}

Tpolyhedron *GetLinkBody(unsigned int i,Tlink *l)
{
  Tpolyhedron **b;

  b=(Tpolyhedron **)GetVectorElement(i,&(l->bodies));
  if (b==NULL)
    return(NULL);
  else
    return(*b);
}

unsigned int GetLinkBodyStatus(unsigned int i,Tlink *l)
{
  Tpolyhedron **b;

  b=(Tpolyhedron **)GetVectorElement(i,&(l->bodies));
  if (b==NULL)
    return(NO_UINT);
  else
    return(GetPolyhedronStatus(*b));
}

char *GetLinkName(Tlink *l)
{
  return(l->name);
}

boolean IsLinkAllSpheres(Tlink *l)
{
  return(l->allSpheres);
}

void SetPoseVars(Tparameters *p,boolean *vars,TCuikSystem *cs,Tlink *l)
{
  SetTransVars(vars,cs,l);
  SetRotVars(p,vars,cs,l);
}

void SetTransVars(boolean *vars,TCuikSystem *cs,Tlink *l)
{
  char *vname;
  unsigned int k,id;

  NEW(vname,strlen(l->name)+100,char);
  for(k=0;k<3;k++)
    {
      LINK_TRANS(vname,l->name,k);
      id=GetCSVariableID(vname,cs);
      if (id!=NO_UINT) /*Ground links has not variables*/
        vars[id]=TRUE;
    }
  free(vname);
}


void CacheRotVars(unsigned int r,TCuikSystem *cs,Tlink *l)
{
  switch(r)
    {
    case REP_LINKS:
      CacheRotVarsLinks(cs,l);
      break;
    case REP_FLINKS:
      CacheRotVarsFLinks(cs,l);
      break;
    case REP_QLINKS:
      CacheRotVarsQLinks(cs,l);
      break;
    case REP_JOINTS:
      break;
    default:
      Error("Undefined representation type in CacheRotVars");
    }
}

void CacheRotVarsFLinks(TCuikSystem *cs,Tlink *l)
{
  if (cs!=l->csvID)
    {
      char *vname;
      unsigned int i,j,k;

      l->nvID=9;
      if (l->csvID==NULL)
        NEW(l->vID,l->nvID,unsigned int);
      
      NEW(vname,strlen(l->name)+100,char);
      k=0;
      for(i=0;i<3;i++) /*vector*/
        {
          for(j=0;j<3;j++) /*component*/
            {
              LINK_ROT(vname,l->name,i,j);
              /* for the ground link all vID will be NO_UINT. Use at your own
                 risk. */
              l->vID[k]=GetCSVariableID(vname,cs);
              k++;
            }
        }
      free(vname);

      l->csvID=cs;
    }
}

void CacheRotVarsLinks(TCuikSystem *cs,Tlink *l)
{
  if (cs!=l->csvID)
    {
      char *vname;
      unsigned int i,j,k;

      l->nvID=12;
      if (l->csvID==NULL)
        NEW(l->vID,l->nvID,unsigned int);
      
      NEW(vname,strlen(l->name)+100,char);
      k=0;
      for(i=0;i<4;i++) /*vector*/
        {
          for(j=0;j<3;j++) /*component*/
            {
              LINK_ROT2(vname,l->name,i,j);
              /* for the ground link all vID will be NO_UINT. Use at your own*/
              l->vID[k]=GetCSVariableID(vname,cs);
              k++;
            }
        }
      free(vname);
 
      l->csvID=cs; 
    }
}

void CacheRotVarsQLinks(TCuikSystem *cs,Tlink *l) 
{
  if (cs!=l->csvID)
    {
      char *vname; 
      unsigned int i,j,k; 

      l->nvID=14; 
      if (l->csvID==NULL) 
        NEW(l->vID,l->nvID,unsigned int); 
      
      NEW(vname,strlen(l->name)+100,char); 
      k=0; 
      for(j=0;j<4;j++)
        {
          LINK_ROT3_Q(vname,l->name,j);  
          l->vID[k]=GetCSVariableID(vname,cs); 
          k++;
        }

      for(i=0;i<4;i++)  
        {
          for(j=i;j<3;j++) 
            {
              LINK_ROT3_E(vname,l->name,i,j);
              /* for the ground link all vID will be NO_UINT. Use at your own */
              l->vID[k]=GetCSVariableID(vname,cs); 
              k++;
            }
        } 

      free(vname);

      l->csvID=cs;
    }
}

void SetRotVars(Tparameters *p,boolean *vars,TCuikSystem *cs,Tlink *l)
{  
  unsigned int r;

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

  if (r!=REP_JOINTS)
    {
      unsigned int i,id,n=0;
  
      CacheRotVars(r,cs,l); 

      switch(r)
        {
        case REP_LINKS:
          n=6;
          break;
        case REP_FLINKS:
          n=9;
          break;
        case REP_QLINKS:
          n=4;
          break;
        default:
          Error("Undefined representation type in SetRotVars");
        }

      for(i=0;i<n;i++) /*system vars = 3 vectors*/
        {
          id=l->vID[i];
          if (id!=NO_UINT) /*Ground links has not variables*/
            vars[id]=TRUE;
        }
    }
}

void GetLinkPoseSimpVars(Tparameters *p,boolean *sv,
                         TCuikSystem *cs,Tlink *l)
{
  unsigned int r;

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

  if (r!=REP_JOINTS)
    {
      unsigned int i,id,n=0;
      char *v;
  
      CacheRotVars(r,cs,l); 

      switch(r)
        {
        case REP_LINKS:
          n=6;
          break;
        case REP_FLINKS:
          n=9;
          break;
        case REP_QLINKS:
          n=4;
          break;
        default:
          Error("Undefined representation type in SetRotVars");
        }

      for(i=0;i<n;i++) /*system vars = 3 vectors*/
        {
          id=l->vID[i];
          if (id!=NO_UINT) /*Ground links has not variables*/
            {
              v=GetCSVariableName(id,cs);
              if (IsSystemVarInSimpCS(p,v,cs))
                sv[id]=TRUE;
            }
        }
    }
}

void GenerateLinkRot(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  switch(r)
    {
    case REP_LINKS:
      GenerateLinkRotLinks(p,lID,cs,l);
      break;
    case REP_FLINKS:
      GenerateLinkRotFLinks(p,lID,cs,l);
      break;
    case REP_QLINKS:
      GenerateLinkRotQLinks(p,lID,cs,l);
      break;
    case REP_JOINTS:
      break;
    default:
      Error("Undefined representation type in GenerateLinkRot");
    }
}

void GenerateLinkRotFLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
{
  if (!IsGroundLink(lID))
    {
      char *vname;

      /*If the cuiksytem already has the link variables (and thus the corresponding equations)
        there is nothing to be done. */
      
      NEW(vname,strlen(l->name)+100,char);

      LINK_ROT(vname,l->name,0,0);

      if (GetCSVariableID(vname,cs)==NO_UINT)
        {
          unsigned int i,j;
          Tinterval range;
          Tequation eqn;
          Tequation eq[3];
          unsigned int linkVars[9]; 
          Tvariable var;

          NewInterval(-1.0,1.0,&range);
          for(i=0;i<3;i++) /*vector*/
            {
              for(j=0;j<3;j++) /*component*/
                {
                  LINK_ROT(vname,l->name,i,j);
                  NewVariable(SYSTEM_VAR,vname,&var);
                  SetVariableInterval(&range,&var);
                  linkVars[i*3+j]=AddVariable2CS(&var,cs);        
                  DeleteVariable(&var);
                }
            }

          for(i=0;i<2+ROT_REDUNDANCY;i++)
            {
              GenerateNormEquation(linkVars[3*i+0],linkVars[3*i+1],linkVars[3*i+2],1.0,&eqn);
              AddEquation2CS(p,&eqn,cs);
              DeleteEquation(&eqn);
            } 

          /*ONLY ONE OF THE THREE FOLLOWING BLOCKS IS REQUIRED, THE OTHER TWO ARE REDUNDANT. */
           #if (ROT_REDUNDANCY==1)
          /*********************************************************************/

          GenerateDotProductEquation(linkVars[0],linkVars[1],linkVars[2], /* X */
                                     linkVars[3],linkVars[4],linkVars[5], /* Y */
                                     NO_UINT,l->c,&eqn);
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);


          GenerateCrossProductEquations(linkVars[0],linkVars[1],linkVars[2], /* X */
                                        linkVars[3],linkVars[4],linkVars[5], /* Y */
                                        linkVars[6],linkVars[7],linkVars[8], /* Z */
                                        NO_UINT,l->s,eq);
          for(i=0;i<3;i++)
            {
              AddEquation2CS(p,&(eq[i]),cs);
              DeleteEquation(&(eq[i]));
            }
          /*********************************************************************/
          #endif
         
          /*********************************************************************/
          
            GenerateDotProductEquation(linkVars[6],linkVars[7],linkVars[8], /* Z */
                                       linkVars[0],linkVars[1],linkVars[2], /* X */
                                       NO_UINT,0,&eqn);
            AddEquation2CS(p,&eqn,cs);
            DeleteEquation(&eqn);
            if (l->s==1) /*This second redundant equation is only used for orthogonal X-Y vectors */
              { 
                GenerateCrossProductEquations(linkVars[6],linkVars[7],linkVars[8], /* Z */
                                              linkVars[0],linkVars[1],linkVars[2], /* X */
                                              linkVars[3],linkVars[4],linkVars[5], /* Y */
                                              NO_UINT,1.0,eq);
                for(i=0;i<3;i++)
                  {
                    AddEquation2CS(p,&(eq[i]),cs);
                    DeleteEquation(&(eq[i]));
                  }
              }
          /*********************************************************************/
          

          #if (ROT_REDUNDANCY==1) /*Redundancy is only used for orthogonal X-Y vectors */
          /*********************************************************************/
            GenerateDotProductEquation(linkVars[3],linkVars[4],linkVars[5], /* Y */
                                       linkVars[6],linkVars[7],linkVars[8], /* Z */
                                       NO_UINT,0,&eqn);
            AddEquation2CS(p,&eqn,cs);
            DeleteEquation(&eqn);
            
            
            if (l->s==1) /*This second redundant equation is only used for orthogonal X-Y vectors */
              { 
              GenerateCrossProductEquations(linkVars[3],linkVars[4],linkVars[5], /* Y */
                                            linkVars[6],linkVars[7],linkVars[8], /* Z */
                                            linkVars[0],linkVars[1],linkVars[2], /* X */
                                            NO_UINT,1.0,eq);
              for(i=0;i<3;i++)
                {
                  AddEquation2CS(p,&(eq[i]),cs);
                  DeleteEquation(&(eq[i]));
                }
            }
          /*********************************************************************/
          #endif
        }

      free(vname);
    }
}

void GenerateLinkRotLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
{
  if (!IsGroundLink(lID))
    {
      char *vname;

      /*If the cuiksytem already has the link variables (and thus the corresponding equations)
        there is nothing to be done. */

      NEW(vname,strlen(l->name)+100,char);
  
      LINK_ROT2(vname,l->name,0,0);

      if (GetCSVariableID(vname,cs)==NO_UINT)
        {
          unsigned int i,j;
          Tinterval range;
          Tequation eqn;
          unsigned int linkVars[12]; 
          Tvariable var;

          NewInterval(-1.0,1.0,&range);
          /* vectors u,v,w,wp */
          for(i=0;i<4;i++) /*vector*/
            {
              for(j=0;j<3;j++) /*component*/
                {
                  LINK_ROT2(vname,l->name,i,j);
                  NewVariable((i<2?SYSTEM_VAR:DUMMY_VAR),vname,&var);
                  SetVariableInterval(&range,&var);
                  linkVars[i*3+j]=AddVariable2CS(&var,cs);        
                  DeleteVariable(&var);
                }
            }

          GenerateScaledSaddleEquation(l->s,linkVars[1],linkVars[5],linkVars[6],&eqn); 
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);

          GenerateScaledSaddleEquation(l->s,linkVars[2],linkVars[3],linkVars[7],&eqn); 
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);

          GenerateScaledSaddleEquation(l->s,linkVars[0],linkVars[4],linkVars[8],&eqn); 
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);

          GenerateScaledSaddleEquation(l->s,linkVars[2],linkVars[4],linkVars[9],&eqn); 
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);

          GenerateScaledSaddleEquation(l->s,linkVars[0],linkVars[5],linkVars[10],&eqn); 
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);

          GenerateScaledSaddleEquation(l->s,linkVars[1],linkVars[3],linkVars[11],&eqn); 
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);

          for(i=0;i<2;i++)
            {
              GenerateNormEquation(linkVars[3*i+0],linkVars[3*i+1],linkVars[3*i+2],1.0,&eqn);
              AddEquation2CS(p,&eqn,cs);
              DeleteEquation(&eqn);
            }
          
          GenerateDotProductEquation(linkVars[0],linkVars[1],linkVars[2], /* X */
                                     linkVars[3],linkVars[4],linkVars[5], /* Y */
                                     NO_UINT,l->c,&eqn);
          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);
        }

      free(vname);
    }
}

void GenerateLinkRotQLinks(Tparameters *p,unsigned int lID,TCuikSystem *cs,Tlink *l)
{
  if (!IsGroundLink(lID))
    {
      char *vname;

      /*If the cuiksytem already has the link variables (and thus the corresponding equations)
        there is nothing to be done. */

      NEW(vname,strlen(l->name)+100,char);
      
      LINK_ROT3_E(vname,l->name,0,0);

      if (GetCSVariableID(vname,cs)==NO_UINT)
        {
          unsigned int i,j,n;
          Tinterval range,range2;
          Tequation eqn;
          unsigned int linkVars[10]; 
          unsigned int qVars[4];
          Tvariable var;
          Tmonomial f;

          NewInterval(-1.0,1.0,&range);
          NewInterval(0.0,1.0,&range2);

          n=0;
          for(i=0;i<4;i++)
            { 
              for(j=i;j<4;j++)
                {
                  LINK_ROT3_E(vname,l->name,i,j);
                  NewVariable(DUMMY_VAR,vname,&var);
                  if (i==j)
                    SetVariableInterval(&range2,&var);
                  else
                    SetVariableInterval(&range,&var);
                  linkVars[n]=AddVariable2CS(&var,cs);
                  n++;
                  DeleteVariable(&var);
                }
            }
          
          for(j=0;j<4;j++)
            {
              LINK_ROT3_Q(vname,l->name,j);
              NewVariable(SYSTEM_VAR,vname,&var);
              if (j==0)
                SetVariableInterval(&range2,&var);
              else
                SetVariableInterval(&range,&var);
              qVars[j]=AddVariable2CS(&var,cs);   
              DeleteVariable(&var);
            }

          /* Add the relation between the E variables (dummies) and the
            Q variables (system) */
          n=0;
          for(i=0;i<4;i++)
            { 
              for(j=i;j<4;j++)
                {
                  /* When i=j (-> qVars[i]==qVars[j]) a parabola equation is generated! */
                  GenerateSaddleEquation(qVars[i],qVars[j],linkVars[n],&eqn);
                  AddEquation2CS(p,&eqn,cs);
                  DeleteEquation(&eqn);
                  n++;
                }
            }

          /* norm of the Q (system) variables is 1 (we use the already introduced
             dummy e_i variables) */
          InitEquation(&eqn);
          InitMonomial(&f);
          
          AddVariable2Monomial(NFUN,linkVars[0],1,&f);
          AddMonomial(&f,&eqn);ResetMonomial(&f);
          AddVariable2Monomial(NFUN,linkVars[4],1,&f);
          AddMonomial(&f,&eqn);ResetMonomial(&f);
          AddVariable2Monomial(NFUN,linkVars[7],1,&f);
          AddMonomial(&f,&eqn);ResetMonomial(&f);
          AddVariable2Monomial(NFUN,linkVars[9],1,&f);
          AddMonomial(&f,&eqn);

          DeleteMonomial(&f);

          SetEquationCmp(EQU,&eqn);
          SetEquationValue(1,&eqn);
          SetEquationType(SYSTEM_EQ,&eqn);

          AddEquation2CS(p,&eqn,cs);
          DeleteEquation(&eqn);
                
        }
      free(vname);
    }
}

void ApplyLinkRot(Tparameters *pr,
                  double sf,unsigned int sv,double *p,Tequation *eq,
                  TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
  switch(r)
    {
    case REP_LINKS:
      ApplyLinkRotLinks(sf,sv,p,eq,cs,groundLink,l);
      break;
    case REP_FLINKS:
      ApplyLinkRotFLinks(sf,sv,p,eq,cs,groundLink,l);
      break;
    case REP_QLINKS:
      ApplyLinkRotQLinks(sf,sv,p,eq,cs,groundLink,l);
      break;
    case REP_JOINTS:
      break;
    default:
      Error("Undefined representation type in ApplyLinkRot");
    }
}

void ApplyLinkRotFLinks(double sf,unsigned int sv,double *p,Tequation *eq,
                    TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  /*
  sf*sv*R*T(px py px) where T is defined from the linkVars (check that hasVars==TRUE) 
  */
  
  Tmonomial f;
  unsigned int i;

  InitMonomial(&f);
  if (groundLink)
    {
      for(i=0;i<3;i++)
        { 
          if (sv!=NO_UINT)
            AddVariable2Monomial(NFUN,sv,1,&f);
          AddCt2Monomial(sf*p[i],&f);
          
          AddMonomial(&f,&(eq[i]));
          ResetMonomial(&f);
        }
    }
  else
    {
      unsigned int j;
      unsigned int linkVars[9]; 
      char *vname;
      double pInt[3];

      HTransformApply(p,pInt,&(l->R));

      NEW(vname,strlen(l->name)+100,char);

      for(i=0;i<3;i++)
        {
          for(j=0;j<3;j++)
            {
              LINK_ROT(vname,l->name,i,j);
              linkVars[i*3+j]=GetCSVariableID(vname,cs);
              if (linkVars[i*3+j]==NO_UINT)
                Error("Undefined reference variable in ApplyLinkRotFM");
            }
        }
  
      for(i=0;i<3;i++)
        {
          for(j=0;j<3;j++)
            {
              if (fabs(pInt[j])>ZERO)
                {
                  AddVariable2Monomial(NFUN,linkVars[j*3+i],1,&f);
                  if (sv!=NO_UINT)
                    AddVariable2Monomial(NFUN,sv,1,&f);
                  AddCt2Monomial(sf*pInt[j],&f);
                  
                  AddMonomial(&f,&(eq[i]));
                  ResetMonomial(&f);
                }
            }
        }
      free(vname);
    }
  DeleteMonomial(&f);
}

void ApplyLinkRotLinks(double sf,unsigned int sv,double *p,Tequation *eq,
                    TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  /*
  sf*sv*R*T(px py px) where T is defined from the linkVars (check that hasVars==TRUE) 
  */
  
  Tmonomial f;

  InitMonomial(&f);
  if (groundLink)
    {
      unsigned int i;

      for(i=0;i<3;i++)
        {
          if (sv!=NO_UINT)
            AddVariable2Monomial(NFUN,sv,1,&f);
          AddCt2Monomial(sf*p[i],&f);
          
          AddMonomial(&f,&(eq[i]));
          ResetMonomial(&f);
        }
    }
  else
    {
      unsigned int i,j,n;
      unsigned int linkVars[12]; 
      char *vname;
      double pInt[3];

      HTransformApply(p,pInt,&(l->R));

      NEW(vname,strlen(l->name)+100,char);
      
      for(i=0;i<4;i++) /*vector*/
        {
          for(j=0;j<3;j++) /*component*/
            {
              LINK_ROT2(vname,l->name,i,j);
              n=i*3+j;
              linkVars[n]=GetCSVariableID(vname,cs);
              if (linkVars[n]==NO_UINT)
                Error("Undefined reference variable in ApplyLinkRotPM");
            }
        }

      for(i=0;i<3;i++) /*row*/
        {
          for(j=0;j<4;j++) /*column*/
            {
              if (fabs(pInt[(j<2?j:2)])>ZERO)
                {
                  AddVariable2Monomial(NFUN,linkVars[j*3+i],1,&f);
                  if (sv!=NO_UINT)
                    AddVariable2Monomial(NFUN,sv,1,&f);
                  AddCt2Monomial((j==3?-1:1)*sf*pInt[(j<2?j:2)],&f);
                  
                  AddMonomial(&f,&(eq[i]));
                  ResetMonomial(&f);
                }
            }
        }

      free(vname);
    }
  DeleteMonomial(&f);  
}

void ApplyLinkRotQLinks(double sf,unsigned int sv,double *p,Tequation *eq,
                   TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  /*
  sf*sv*R*T(px py px) where T is defined from the linkVars (check that hasVars==TRUE) 
  (R=Id for quaternions -> not used)
  */
  
  Tmonomial f;
  unsigned int i;

  /*Note that quaternions asume an orthonormal rotation matrix (i.e., we do not allow
    the change of the link reference)*/

  InitMonomial(&f);
  if (groundLink)
    {
      unsigned int j;
      
      double id[3][3]={{1.0,0.0,0.0},{0.0,1.0,0.0},{0.0,0.0,1.0}};
      /*
      double id[3][3]={{0.82559761618954,-0.52917109941051,-0.19587374426100}, 
                       {0.20084362075732, 0.59999081140825,-0.77438547650815}, 
                       {0.52730486072408, 0.59999081140825, 0.60163162324002}}; 
      */
      double ct;

      for(i=0;i<3;i++)
        { 
          ct=0.0;
          for(j=0;j<3;j++)
            ct+=id[i][j]*p[j];

          if (sv!=NO_UINT)
            AddVariable2Monomial(NFUN,sv,1,&f);
          AddCt2Monomial(sf*ct,&f);
          
          AddMonomial(&f,&(eq[i]));
          ResetMonomial(&f);
        }
    }
  else
    {
      unsigned int i,j,n;
      unsigned int linkVars[10]; 
      char *vname;
      unsigned int map[2][3][3]={
        {{4,1,2},
         {1,0,5},
         {2,5,0}},
        {{7,8,6},
         {8,7,3},
         {6,3,4}}};
      double sg[2][3][3]={
        {{-1.0,+1.0,+1.0},
         {+1.0,-1.0,+1.0},
         {+1.0,+1.0,-1.0}},
        {{-1.0,-1.0,+1.0},
         {+1.0,-1.0,-1.0},
         {-1.0,+1.0,-1.0}}};

      NEW(vname,strlen(l->name)+100,char);

      n=0;
      for(i=0;i<4;i++)
        { 
          for(j=i;j<4;j++)
            {
              LINK_ROT3_E(vname,l->name,i,j);
              linkVars[n]=GetCSVariableID(vname,cs);
              if (linkVars[n]==NO_UINT)
                Error("Undefined reference variable in ApplyLinkRot");
              n++;
            }
        }

      /* Diagonal elements */
      for(i=0;i<3;i++)
        {
           if (sv!=NO_UINT)
             AddVariable2Monomial(NFUN,sv,1,&f);
           AddCt2Monomial(sf*p[i],&f);

          AddMonomial(&f,&(eq[i]));
          ResetMonomial(&f);
        }

      for(n=0;n<2;n++)
        {
          for(i=0;i<3;i++) /*row*/
            {
              for(j=0;j<3;j++) /*column*/
                {
                  if (fabs(p[j])>ZERO)
                    {
                      AddVariable2Monomial(NFUN,linkVars[map[n][i][j]],1,&f);
                      if (sv!=NO_UINT)
                        AddVariable2Monomial(NFUN,sv,1,&f);
                      AddCt2Monomial(2.0*sg[n][i][j]*sf*p[j],&f);
                      
                      AddMonomial(&f,&(eq[i]));
                      ResetMonomial(&f);
                    }
                }
            }
        }

      free(vname);
    }
  DeleteMonomial(&f);  
}

void ApplyLinkRotVar(Tparameters *pr,double sf,unsigned int *vID,Tequation *eq,
                     TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
  if (r!=REP_JOINTS)
    {
      Tequation u[3],v[3],w[3];
      unsigned int i,j;
      double ct[3];

      for(i=0;i<3;i++)
        {
          InitEquation(&(u[i]));
          InitEquation(&(v[i]));
          InitEquation(&(w[i]));
        }
      /* We have to compute M*R*[vID]
           M=matrix given by the link variables (=M*iR) 
           R=Basis change
           vID=given variable vector 
         This is computed as (M*R)*[vID] 
         First [u v w]=M*R  
            u=M*R_1  (first column or R)
            v=M*R_2  (second column or R)
            w=M*R_3  (third column or R)
         and then [u v w]*[vID] -> linear combination of u,v,w*/
      for(i=0;i<3;i++)
        {
          for(j=0;j<3;j++)
            ct[j]=l->R[j][i];
          ApplyLinkRot(pr,sf,NO_UINT,ct,(i==0?u:(i==1?v:w)),cs,groundLink,l);
        }
      for(i=0;i<3;i++)
        {
          VarAccumulateEquations(&(u[i]),vID[0],&(eq[i]));
          VarAccumulateEquations(&(v[i]),vID[1],&(eq[i]));
          VarAccumulateEquations(&(w[i]),vID[2],&(eq[i]));
        }
      for(i=0;i<3;i++)
        {
          DeleteEquation(&(u[i]));
          DeleteEquation(&(v[i]));
          DeleteEquation(&(w[i]));
        }
    }  
}

void RegenerateLinkSolution(Tparameters *p,TCuikSystem *cs,double *sol,
                            boolean groundLink,Tlink *l)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  switch(r)
    {
    case REP_LINKS:
      RegenerateLinkSolutionLinks(cs,sol,groundLink,l);
      break;
    case REP_FLINKS:
      /* When using 9 variables all variables used to represent the
         rotation of the link are system variables and, thus, no
         dummy has to be computed.*/
      break;
    case REP_QLINKS:
      RegenerateLinkSolutionQLinks(cs,sol,groundLink,l);
      break;
    case REP_JOINTS:
      break;
    default:
      Error("Undefined representation type in RegenerateLinkSolution");
    }
}

void RegenerateLinkSolutionLinks(TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    { 
      CacheRotVarsLinks(cs,l);
      
      /* compute w and wp from u and v (recall that u X v = s*(w - wp) )*/
      sol[l->vID[6]]=sol[l->vID[1]]*sol[l->vID[5]]/l->s;
      sol[l->vID[7]]=sol[l->vID[2]]*sol[l->vID[3]]/l->s;
      sol[l->vID[8]]=sol[l->vID[0]]*sol[l->vID[4]]/l->s;

      sol[l->vID[9]] =sol[l->vID[2]]*sol[l->vID[4]]/l->s;
      sol[l->vID[10]]=sol[l->vID[0]]*sol[l->vID[5]]/l->s;
      sol[l->vID[11]]=sol[l->vID[1]]*sol[l->vID[3]]/l->s;
    }
}

void RegenerateLinkSolutionQLinks(TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      unsigned int i,j,n;
      double qVars[4];

      CacheRotVarsQLinks(cs,l);

      for(j=0;j<4;j++)
        qVars[j]=sol[l->vID[j]];

      n=4;
      for(i=0;i<4;i++)
        {
          for(j=i;j<4;j++)
            {
              sol[l->vID[n]]=qVars[i]*qVars[j];
              n++;
            }
        }
    }
}

void RegenerateLinkBox(Tparameters *p,TCuikSystem *cs,Tbox *b,
                       boolean groundLink,Tlink *l)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  switch(r)
    {
    case REP_LINKS:
      RegenerateLinkBoxLinks(cs,b,groundLink,l);
      break;
    case REP_FLINKS:
      /* When using 9 variables all variables used to represent the
         rotation of the link are system variables and, thus, no
         dummy has to be computed.*/
      break;
    case REP_QLINKS:
      RegenerateLinkBoxQLinks(cs,b,groundLink,l);
      break;
    case REP_JOINTS:
      break;
    default:
      Error("Undefined representation type in RegenerateLinkBox");
    }
}

void RegenerateLinkBoxLinks(TCuikSystem *cs,Tbox *b,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      Tinterval r,*is;

      CacheRotVarsLinks(cs,l);

      is=GetBoxIntervals(b);

      /* compute w and wp from u and v (recall that u X v = s*(w - wp)) */
      IntervalProduct(&(is[l->vID[1]]),&(is[l->vID[5]]),&r);
      IntervalScale(&r,1/l->s,&r);
      SetBoxInterval(l->vID[6],&r,b);

      IntervalProduct(&(is[l->vID[2]]),&(is[l->vID[3]]),&r);
      IntervalScale(&r,1/l->s,&r);
      SetBoxInterval(l->vID[7],&r,b);

      IntervalProduct(&(is[l->vID[0]]),&(is[l->vID[4]]),&r);
      IntervalScale(&r,1/l->s,&r);
      SetBoxInterval(l->vID[8],&r,b);

      IntervalProduct(&(is[l->vID[2]]),&(is[l->vID[4]]),&r);
      IntervalScale(&r,1/l->s,&r);
      SetBoxInterval(l->vID[9],&r,b);

      IntervalProduct(&(is[l->vID[0]]),&(is[l->vID[5]]),&r);
      IntervalScale(&r,1/l->s,&r);
      SetBoxInterval(l->vID[10],&r,b);

      IntervalProduct(&(is[l->vID[1]]),&(is[l->vID[3]]),&r);
      IntervalScale(&r,1/l->s,&r);
      SetBoxInterval(l->vID[11],&r,b);
    }
}

void RegenerateLinkBoxQLinks(TCuikSystem *cs,Tbox *b,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      unsigned int i,j,n;
      Tinterval qVars[4];
      Tinterval r;
 
      CacheRotVarsQLinks(cs,l);

      for(j=0;j<4;j++)
        CopyInterval(&(qVars[j]),GetBoxInterval(l->vID[j],b));

      n=4;
      for(i=0;i<4;i++)
        {
          for(j=i;j<4;j++)
            {
              IntervalProduct(&(qVars[i]),&(qVars[j]),&r);
              SetBoxInterval(l->vID[n],&r,b);
              n++;
            }
        }
    }
}

void GetTransform2Link(Tparameters *p,TCuikSystem *cs,double *sol,
                       boolean groundLink,double *trans,THTransform *t,Tlink *l)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  switch(r)
    {
    case REP_LINKS:
      GetTransform2LinkLinks(cs,sol,groundLink,trans,t,l);
      break;
    case REP_FLINKS:
      GetTransform2LinkFLinks(cs,sol,groundLink,trans,t,l);
      break;
    case REP_QLINKS:
      GetTransform2LinkQLinks(cs,sol,groundLink,trans,t,l);
      break;
    case REP_JOINTS:
      break;
    default:
      Error("Undefined representation type in GetTransform2Link");
    }
}

void GetTransform2LinkFLinks(TCuikSystem *cs,double *sol,boolean groundLink,
                         double *r,THTransform *t,Tlink *l)
{
  if (groundLink)
    HTransformIdentity(t);
  else
    {
      unsigned int i,j;

      CacheRotVarsFLinks(cs,l);

      /* Define the homogeneous transform using the rotation and the translation.*/
      HTransformIdentity(t);
      for(i=0;i<3;i++)
        {
          HTransformSetElement(i,AXIS_H,r[i],t);
          for(j=0;j<3;j++)
            HTransformSetElement(i,j,sol[l->vID[i+j*3]],t);
        }

      /* The link variables representing the link rotation encode
             M*iR
         M = orthonormal rotation matrix
         iR= basis change to obtain a more numerically safe basis
         The orthonormal matrix is obtained by multiplying
             (M*iR)*R

         See ChangeLinkReferenceFrame
      */
      HTransformProduct(t,&(l->R),t);
      /* This ensures that the transformation is actually rigid 
         (fixes numerical errors and the error of taking the center
         of the solution box as a proper solution) */
      HTransformOrthonormalize(t,t);
    }
}

void GetTransform2LinkLinks(TCuikSystem *cs,double *sol,boolean groundLink,
                         double *r,THTransform *t,Tlink *l)
{
  if (groundLink)
    HTransformIdentity(t);
  else
    {
      unsigned int i,j;

      CacheRotVarsLinks(cs,l);

      /* Define the homogeneous transform using the rotation and the translation.*/
      HTransformIdentity(t);
      for(i=0;i<3;i++) /* component */
        {
          HTransformSetElement(i,AXIS_H,r[i],t);
          for(j=0;j<3;j++) /* vector */
            { 
              if (j<2)
                HTransformSetElement(i,j,sol[l->vID[i+j*3]],t);
              else
                HTransformSetElement(i,j,(sol[l->vID[i+j*3]]-sol[l->vID[i+(j+1)*3]]),t);
            }
        }

      /* The link variables representing the link rotation encode
             M*iR
         M = orthonormal rotation matrix
         iR= basis change to obtain a more numerically safe basis
         The orthonormal matrix is obtained by multiplying
             (M*iR)*R

         See ChangeLinkReferenceFrame
      */
      HTransformProduct(t,&(l->R),t);  
      /* This ensures that the transformation is actually rigid
         (fixes numerical errors and the error of taking the center
         of the solution box as a proper solution) */   
      HTransformOrthonormalize(t,t);
    }
}


void GetTransform2LinkQLinks(TCuikSystem *cs,double *sol,boolean groundLink,
                       double *r,THTransform *t,Tlink *l)
{
  if (groundLink)
    HTransformIdentity(t);
  else
    {
      unsigned int i,j,n;
      double e[4][4]; /* only the upper half is used */

      n=4;
      for(i=0;i<4;i++)
        {
          for(j=i;j<4;j++)
            {
              e[i][j]=2*sol[l->vID[n]];
              n++;
            }
        }

      /* Define the homogeneous transform using the rotation and the translation.*/
      HTransformIdentity(t);
     
      HTransformSetElement(0,0,1-e[1][1]-e[2][2],t);
      HTransformSetElement(0,1,  e[0][1]-e[2][3],t);
      HTransformSetElement(0,2,  e[0][2]+e[1][3],t);
      HTransformSetElement(0,3,r[0],t);

      HTransformSetElement(1,0,  e[0][1]+e[2][3],t);
      HTransformSetElement(1,1,1-e[0][0]-e[2][2],t);
      HTransformSetElement(1,2,  e[1][2]-e[0][3],t);
      HTransformSetElement(1,3,r[1],t);

      HTransformSetElement(2,0,  e[0][2]-e[1][3],t);
      HTransformSetElement(2,1,  e[1][2]+e[0][3],t);
      HTransformSetElement(2,2,1-e[0][0]-e[1][1],t);
      HTransformSetElement(2,3,r[2],t);
      
      /* No basis change (R) is used in Quaternion representation */

      /* This ensures that the transformation is actually rigid */
      HTransformOrthonormalize(t,t);
    }
}

void GenerateLinkSolution(Tparameters *p,THTransform *t,TCuikSystem *cs,
                          double *sol,boolean groundLink,Tlink *l)
{
  unsigned int r;

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  switch(r)
    {
    case REP_LINKS:
      GenerateLinkSolutionLinks(t,cs,sol,groundLink,l);
      break;
    case REP_FLINKS:
      GenerateLinkSolutionFLinks(t,cs,sol,groundLink,l);
      break;
    case REP_QLINKS:
      GenerateLinkSolutionQLinks(t,cs,sol,groundLink,l);
      break;
    case REP_JOINTS:
      break;
    default:
      Error("Undefined representation type in GenerateLinkSolution");
    }
}

void GenerateLinkSolutionFLinks(THTransform *t,TCuikSystem *cs,
                          double *sol,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      unsigned int i,j,n;
      THTransform it;
      
      /* The link variables encode (M*iR) and the given 't' is only M */
      HTransformProduct(t,&(l->iR),&it);

      CacheRotVarsFLinks(cs,l);
      
      n=0;
      for(i=0;i<3;i++) /*vector*/
        { 
          for(j=0;j<3;j++) /*component*/
            {
              sol[l->vID[n]]=HTransformGetElement(j,i,&it);
              n++;
            }
        }
      HTransformDelete(&it);
    }
}

void GenerateLinkSolutionLinks(THTransform *t,TCuikSystem *cs,
                          double *sol,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      unsigned int i,j,n;
      THTransform it;
      
      /* The link variables encode (M*iR) and the given 't' is only M */
      HTransformProduct(t,&(l->iR),&it);

      CacheRotVarsLinks(cs,l);
      
      n=0;
      for(i=0;i<2;i++) /*vector*/
        { 
          for(j=0;j<3;j++) /*componet*/
            {
              sol[l->vID[n]]=HTransformGetElement(j,i,&it);
              n++;
            }
        }
      HTransformDelete(&it);

      RegenerateLinkSolutionLinks(cs,sol,groundLink,l);
    }
}

void GenerateLinkSolutionQLinks(THTransform *t,TCuikSystem *cs,
                           double *sol,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      double r,s,q;

      CacheRotVarsQLinks(cs,l);
      
      /* Recover the 4 quaternion elements from the transformation
         matrix*/ 
      q=HTransformGetElement(0,0,t)+
        HTransformGetElement(1,1,t)+
        HTransformGetElement(2,2,t); 
      r=sqrt(1+q);
      s=0.5/r;

      sol[l->vID[0]]=0.5*r;
      sol[l->vID[1]]=(HTransformGetElement(2,1,t)-HTransformGetElement(1,2,t))*s;
      sol[l->vID[2]]=(HTransformGetElement(0,2,t)-HTransformGetElement(2,0,t))*s;
      sol[l->vID[3]]=(HTransformGetElement(1,0,t)-HTransformGetElement(0,1,t))*s;
        
      RegenerateLinkSolutionQLinks(cs,sol,groundLink,l);
    }
}

boolean VisibleLink(Tlink *l)
{
  /* A visible link is a link with at least one visible body */
  unsigned int i,n; 
  boolean found;

  found=FALSE;

  n=LinkNBodies(l);
  
  for(i=0;((i<n)&&(!found));i++)
    found=(GetPolyhedronStatus(GetLinkBody(i,l))!=HIDDEN_SHAPE);

  return(found);
}

double GetLinkMaxCoordinate(Tlink *l)
{
  return(l->maxCoord);
}

void PlotLink(Tplot3d *pt,double axesLength,Tlink *l)
{
  unsigned int n;

  n=LinkNBodies(l);

  if ((n>0)&&(VisibleLink(l)))
    {
      unsigned int i;

      for(i=0;i<n;i++)
        PlotPolyhedron(pt,GetLinkBody(i,l));

      if (axesLength>0)
        {
          double points[3][2]={{0,0},{0,0},{0,0}};
          Tcolor axesColor;

          for(i=0;i<3;i++)
            {
              NewColor((i==0?1:0),(i==1?1:0),(i==2?1:0),&axesColor);
              l->axisID[i]=StartNew3dObject(&axesColor,pt);
              DeleteColor(&axesColor);

              points[i][1]=axesLength;
              PlotVect3d(2,points[0],points[1],points[2],pt);
              points[i][1]=0;

              Close3dObject(pt);
            }
        }
      else
        {
          for(i=0;i<3;i++)
            l->axisID[i]=NO_UINT;
        }
    }
}

void LinkPrintAtoms(FILE *f,THTransform *tl,Tlink *l)
{
  unsigned int i,m;
  Tpolyhedron *bd;

  m=LinkNBodies(l);

  if ((m>0)&&(VisibleLink(l)))
    {
      for(i=0;i<m;i++)
        {
          bd=GetLinkBody(i,l);
          PolyhedronPrintCenter(f,tl,bd);
        }
    }
}

void LinkStoreAtoms(FILE *f,THTransform *tl,Tlink *l)
{
  unsigned int i,m;
  Tpolyhedron *bd;

  m=LinkNBodies(l);

  if ((m>0)&&(VisibleLink(l)))
    {
      for(i=0;i<m;i++)
        {
          bd=GetLinkBody(i,l);
          PolyhedronPrintCenterAndCenter(f,tl,bd);
        }
    }
}

void MoveLinkFromTransform(Tplot3d *pt,THTransform *t,Tlink *l)
{
  unsigned int m;

  m=LinkNBodies(l);

  if ((m>0)&&(VisibleLink(l)))
    {
      /* Systems not included in any loop are not in the system -> can
         not be moved (can move freely)*/

      unsigned int i;
      Tpolyhedron *bd;
      
      for(i=0;i<3;i++)
        {
          if (l->axisID[i]!=NO_UINT)
            Move3dObject(l->axisID[i],t,pt);
        }

      /*Apply the same transform to all the bodies in the link*/ 
      for(i=0;i<m;i++)
        {
          bd=GetLinkBody(i,l);
          MovePolyhedron(pt,t,bd);
        }
    }
}

void PrintLink(FILE *f,char *path,char *prefix,Tlink *l)
{
  unsigned int i;
  unsigned int n;
  Tpolyhedron *b;
  char *label;
  
  if (prefix==NULL)
    label=l->name;
  else
    {
      unsigned int ln,lp;

      ln=strlen(l->name);
      lp=strlen(prefix);
      NEW(label,ln+2+lp,char);
      sprintf(label,"%s_%s",prefix,l->name);
      label[lp+ln+1]=0;
    }

  fprintf(f,"  %s ",l->name);
  n=LinkNBodies(l);
  if (n>0)
    fprintf(f,": ");
  for(i=0;i<n;i++)
    {
      b=GetLinkBody(i,l);
      PrintPolyhedron(f,path,label,i,b);
    }
  fprintf(f,"\n");

  if (prefix!=NULL)
    free(label);
}

void DeleteLink(Tlink *l)
{
  unsigned int i,n;
  Tpolyhedron *b;

  if (l->name!=NULL)
    free(l->name);
     
  /*Bodies are not deleted since mechanisms take care of them
    (here we only have IDs of the bodies)*/

  n=LinkNBodies(l);  
  for(i=0;i<n;i++)
    {
      b=GetLinkBody(i,l);
      DeletePolyhedron(b);
      free(b);
    }
  DeleteVector(&(l->bodies));

  HTransformDelete(&(l->R));
  HTransformDelete(&(l->iR));

  if (l->csvID!=NULL)
    {
      free(l->vID);
      l->vID=NULL;
      l->csvID=NULL;
      l->nvID=0;
    }
}