link.c

Go to the documentation of this file.

#include "link.h"
#include "cpolyhedron.h"
#include "varnames.h"

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

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

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

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

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

  l_dst->maxCoord=l_src->maxCoord;
}

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

  NEW(bint,1,Tcpolyhedron);
  CopyCPolyhedron(bint,b);

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

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

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

    /* 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];
    
    if (fabs(c)<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) */
        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));
      }
  #endif
}

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

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

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

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

#if (ROT_REP==0)
void GenerateLinkRot(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. */
          /*********************************************************************/

          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]));
            }
          /*********************************************************************/

          #if (ROT_REDUNDANCY==1)
          /*********************************************************************/
          
            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]));
                  }
              }
          /*********************************************************************/
          #endif

          #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 ApplyLinkRot(double sf,unsigned int sv,double *p,Tequation *eq,
                  TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  /*
  sf*sv*R(px py px) where R 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(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 ApplyLinkRotVar");
            }
        }
  
      for(i=0;i<3;i++)
        {
          for(j=0;j<3;j++)
            {
              if (fabs(pInt[j])>ZERO)
                {
                  AddVariable2Monomial(linkVars[j*3+i],1,&f);
                  if (sv!=NO_UINT)
                    AddVariable2Monomial(sv,1,&f);
                  AddCt2Monomial(sf*pInt[j],&f);
                  
                  AddMonomial(&f,&(eq[i]));
                  ResetMonomial(&f);
                }
            }
        }
      free(vname);
    }
  DeleteMonomial(&f);
}

void RegenerateLinkSolution(TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
{
  /* 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.*/
}

void GetTransform2Link(TCuikSystem *cs,double *sol,boolean groundLink,
                       double *r,THTransform *t,Tlink *l)
{
  if (groundLink)
    HTransformIdentity(t);
  else
    {
      unsigned int i,j,n;
      unsigned int linkVars[9]; 
      char *vname;

      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);
              n=(j*3)+i;
              linkVars[n]=GetCSVariableID(vname,cs);
              if (linkVars[n]==NO_UINT)
                Error("Undefined reference variable in ApplyLinkRotVar");
            }
        }

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

      /* Note that in the general case the reference frame for a link
         used to generate the equations (and, thus, the solutions)
         is not orthonormal, i.e., it is not a homogeneous transform.

         The use of matrix l->R makes it orthonormal since it recorves the
         reference frame used to define the bodies in global coordiantes.

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

      free(vname);
    }
}

#endif


#if (ROT_REP==1)
void GenerateLinkRot(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 ApplyLinkRot(double sf,unsigned int sv,double *p,Tequation *eq,
                  TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  /*
  sf*sv*R(px py px) where R 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(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 ApplyLinkRotVar");
            }
        }

      for(i=0;i<3;i++) /*row*/
        {
          for(j=0;j<4;j++) /*column*/
            {
              if (fabs(pInt[(j<2?j:2)])>ZERO)
                {
                  AddVariable2Monomial(linkVars[j*3+i],1,&f);
                  if (sv!=NO_UINT)
                    AddVariable2Monomial(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 RegenerateLinkSolution(TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      unsigned int i,j,n,vID[6];
      char *vname;
      double v[2][3];

      NEW(vname,strlen(l->name)+100,char);
  
      for(i=0;i<2;i++) /*vector (u,v)*/
        {
          for(j=0;j<3;j++) /*component (x,y,z)*/
            {
              LINK_ROT2(vname,l->name,i,j);
              n=GetCSVariableID(vname,cs);
              if (n==NO_UINT)
                Error("Undefined reference variable in RegenerateLinkSolution");

              v[i][j]=sol[n];      
            }
        }      

      n=0;
      for(i=2;i<4;i++) /*vector (w,wp)*/
        {
          for(j=0;j<3;j++) /*component (x,y,z)*/
            {
              LINK_ROT2(vname,l->name,i,j);
              vID[n]=GetCSVariableID(vname,cs);
              if (vID[n]==NO_UINT)
                Error("Undefined reference variable in RegenerateLinkSolution");
              n++;
            }
        }      

      /* compute w and wp from u and v (recall that u X v = w - wp)*/
      sol[vID[0]]=l->s*v[0][1]*v[1][2];
      sol[vID[1]]=l->s*v[0][2]*v[1][0];
      sol[vID[2]]=l->s*v[0][0]*v[1][1];

      sol[vID[3]]=l->s*v[0][2]*v[1][1];
      sol[vID[4]]=l->s*v[0][0]*v[1][2];
      sol[vID[5]]=l->s*v[0][1]*v[1][0];
      
      free(vname);
    }
}

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

      /*In general this function has been used before reaching this point but
        just in case....*/
      RegenerateLinkSolution(cs,sol,FALSE,l);

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

      HTransformIdentity(t);
      for(i=0;i<3;i++) /*vector (u,v,w) = column*/
        {
          HTransformSetElement(i,AXIS_H,r[i],t);

          for(j=0;j<3;j++) /*component = row*/
            {
              LINK_ROT2(vname,l->name,i,j);
              vID=GetCSVariableID(vname,cs);
              if (vID==NO_UINT)
                Error("Undefined reference variable in ApplyLinkRotVar");

              if (i<2) /*The values of u,v can be directly set into the transform*/
                HTransformSetElement(j,i,sol[vID],t);
              else
                {
                  /* when considering w, we have to also use vector wp since
                     the third column of the rotation matrix is  w-wp*/
                  LINK_ROT2(vname,l->name,3,j); /*4th vector is wp*/
                  vID2=GetCSVariableID(vname,cs);
                  if (vID2==NO_UINT)
                    Error("Undefined reference variable in ApplyLinkRotVar");

                  HTransformSetElement(j,i,sol[vID]-sol[vID2],t);
                }
            }
        }

      free(vname);

      /* Note that in the general case the reference frame for a link
         used to generate the equations (and, thus, the solutions)
         is not orthonormal, i.e., it is not a homogeneous transform.

         The use of matrix l->R makes it orthonormal since it recorves the
         reference frame used to define the bodies in global coordiantes.

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

#if (ROT_REP==2)
void GenerateLinkRot(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(linkVars[0],1,&f);
          AddMonomial(&f,&eqn);ResetMonomial(&f);
          AddVariable2Monomial(linkVars[4],1,&f);
          AddMonomial(&f,&eqn);ResetMonomial(&f);
          AddVariable2Monomial(linkVars[7],1,&f);
          AddMonomial(&f,&eqn);ResetMonomial(&f);
          AddVariable2Monomial(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(double sf,unsigned int sv,double *p,Tequation *eq,
                  TCuikSystem *cs,boolean groundLink,Tlink *l)
{
  /*
  sf*sv*R(px py px) where R is defined from the linkVars (check that hasVars==TRUE) 
  */
  
  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(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(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(pInt[j])>ZERO)
                    {
                      AddVariable2Monomial(linkVars[map[n][i][j]],1,&f);
                      if (sv!=NO_UINT)
                        AddVariable2Monomial(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 RegenerateLinkSolution(TCuikSystem *cs,double *sol,boolean groundLink,Tlink *l)
{
  if (!groundLink)
    {
      unsigned int i,j,n;
      double qVars[4];
      char *vname;
 
      NEW(vname,strlen(l->name)+100,char);

      for(j=0;j<4;j++)
        {
          LINK_ROT3_Q(vname,l->name,j);
          n=GetCSVariableID(vname,cs); 
          if (n==NO_UINT)
            Error("Undefined reference variable in RegenerateLinkSolution");
          qVars[j]=sol[n];
        }

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

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

      /*In general this function has been used before reaching this point but
        just in case....*/
      RegenerateLinkSolution(cs,sol,FALSE,l);

      NEW(vname,strlen(l->name)+100,char);
  
      for(i=0;i<4;i++)
        {
          for(j=i;j<4;j++)
            {
              LINK_ROT3_E(vname,l->name,i,j);
              n=GetCSVariableID(vname,cs); 
              if (n==NO_UINT)
                Error("Undefined reference variable in GetTransform2Link");
              e[i][j]=2*sol[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);
      
      /* This ensures that the transformation is actually rigid */
      HTransformOrthonormalize(t,t);

      free(vname);
    }
}
#endif

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

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

  n=LinkNBodies(l);

  if (n>0)
    {
      unsigned int i;

      for(i=0;i<n;i++)
        PlotCPolyhedron(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 MoveLink(Tplot3d *pt,TCuikSystem *cs,double *sol,
              double *r,Tlink *l)
{
  unsigned int m;
  m=LinkNBodies(l);

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

      unsigned int i;
      THTransform t;
      Tcpolyhedron *bd;
          
      /* First we compute the transform to apply to all the bodies of the link */
      GetTransform2Link(cs,sol,FALSE,r,&t,l);
      
      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);

          MoveCPolyhedron(pt,&t,bd);
        }
      
      HTransformDelete(&t);
    }
}

void DeleteLink(Tlink *l)
{
  unsigned int i,n;
  Tcpolyhedron *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);
      DeleteCPolyhedron(b);
      free(b);
    }
  DeleteVector(&(l->bodies));

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