joint.c

Go to the documentation of this file.

#include "joint.h"

#include "error.h"
#include "defines.h"
#include "geom.h"
#include "varnames.h"

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

void NewFreeJoint(unsigned int id,
                  unsigned int linkID1,Tlink *link1,
                  unsigned int linkID2,Tlink *link2,
                  Tjoint *j)
{
  unsigned int i,k;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=FREE_JOINT;
  
  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;
  
  for(i=0;i<6;i++)
    {
      for(k=0;k<3;k++)
        j->points[i][k]=0.0;
    }

  for(i=0;i<3;i++)
    {
      for(k=0;k<3;k++)
        j->normals[i][k]=0.0;
    }

  j->hasLimits=FALSE;
  j->avoidLimits=FALSE;
  j->avoidLimitsWeight=0.0;
  NewInterval(-INF,INF,&(j->range));
  NewInterval(-INF,INF,&(j->range2));

  j->obj3d=NO_UINT;
  j->rad=0;
  j->length=0;
  NewColor(0,0,0,&(j->color));

  HTransformIdentity(&(j->trs));
}

void NewFixJoint(unsigned int id,
                 unsigned int linkID1,Tlink *link1,
                 unsigned int linkID2,Tlink *link2,
                 THTransform *t,
                 Tjoint *j)
{
  unsigned int i,k;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=FIX_JOINT;
  
  HTransformCopy(&(j->trs),t);

  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;
  
  /*
    The translation form link1 to link2 is used in loops.
    We store it in points[0] to speed up the access 
    (see GenerateJointEquationsInBranch)
   */
  j->points[0][0]=HTransformGetElement(0,3,t);
  j->points[0][1]=HTransformGetElement(1,3,t);
  j->points[0][2]=HTransformGetElement(2,3,t);

  for(i=1;i<4;i++)
    {
      for(k=0;k<3;k++)
        j->points[i][k]=0.0;
    }

  for(i=0;i<2;i++)
    {
      for(k=0;k<3;k++)
        j->normals[i][k]=0.0;
    }


  j->hasLimits=FALSE;
  j->avoidLimits=FALSE;
  j->avoidLimitsWeight=0.0;
  NewInterval(-INF,INF,&(j->range));
  NewInterval(-INF,INF,&(j->range2));

  j->obj3d=NO_UINT;
  j->rad=0;
  j->length=0;
  NewColor(0,0,0,&(j->color));
}

void NewRevoluteJoint(unsigned int id,
                      unsigned int linkID1,Tlink *link1,
                      unsigned int linkID2,Tlink *link2,
                      double **points,
                      boolean hasLimits,Tinterval *range,double **rPoints,
                      boolean avoidLimits,double avoidLimitsWeight,
                      Tjoint *j)
{
  unsigned int i,k;
  double nr;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=REV_JOINT;

  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;

  for(i=0;i<4;i++)
    {
      for(k=0;k<3;k++)
        j->points[i][k]=points[i][k];
    }

  /* compute the normalized vectors along the rotation axis */
  for(i=0;i<2;i++)
    {
      nr=0.0;
      for(k=0;k<3;k++)
        {
          j->normals[i][k]=points[2*i+1][k]-points[2*i][k];
          nr+=(j->normals[i][k]*j->normals[i][k]);
        }
      nr=sqrt(nr);
      for(k=0;k<3;k++)
        j->normals[i][k]/=nr;
    }

  j->hasLimits=hasLimits;

  if (j->hasLimits)
    {
      double c;
      double center;
      THTransform t,rx,a;
      double zero[3]={0,0,0};

      for(i=0;i<2;i++)
        {
          /*normalize the reference vectors*/
          nr=0.0;
          for(k=0;k<3;k++)
            {
              j->vrange[i][k]=rPoints[i][k]-points[2*i][k];
              nr+=(j->vrange[i][k]*j->vrange[i][k]);
            }
          nr=sqrt(nr);
          for(k=0;k<3;k++)
            j->vrange[i][k]/=nr;

          /*Test that the rotation vectors are orthogonal to the rotation axes*/
          c=0; /*cosinus between the two vectors*/
          for(k=0;k<3;k++)
            c+=(j->vrange[i][k]*j->normals[i][k]);

          if (fabs(c)>ZERO)
            Error("Rotation axes and rotation reference vectors are not orthonormal (Revolute Joint)");
        }

      /*Copy of the range for the rotation angle*/
      CopyInterval(&(j->range),range);

      /*We offset the range so that it is zero-centered*/
      center=IntervalCenter(&(j->range));
      IntervalOffset(&(j->range),-center,&(j->range));

      /* Since we moved the rotation range to make it zero-centered, we 
         have to rotate the reference vectors accordingly. 
         In this way, the angle between the reference vectors is changed in the same
         way as the offset applied to the range interval. */
      /* The rotation is around the vector defining the revolute joint for
         the current link. This rotation is defined by rotating the axis vector
         to the X axis, rotating around X and then rotating the vector back
         to the its original position.*/

      HTransformX2Vect(1,1,zero,j->normals[0],&t); /* this rotates X to the
                                                      revolute axis vector*/
      HTransformRx(center,&rx); /*rotation around X*/
      
      HTransformInverse(&t,&a); /*this rotates the axis vector to X*/
      HTransformProduct(&rx,&a,&a);
      HTransformProduct(&t,&a,&a);
      
      HTransformApply(j->vrange[0],j->vrange[0],&a); 
      
      HTransformDelete(&a);
      HTransformDelete(&rx);
      HTransformDelete(&t);

      j->avoidLimits=avoidLimits;
      j->avoidLimitsWeight=avoidLimitsWeight;
    }
  else
    {
      /*Set some empty information for the range (not used)*/
      NewInterval(-INF,INF,&(j->range));
      for(i=0;i<2;i++)
        {
          for(k=0;k<3;k++)
            j->vrange[i][k]=0.0;
        }
      j->avoidLimits=FALSE;
      j->avoidLimitsWeight=0;
    }

  NewInterval(-INF,INF,&(j->range2));

  j->obj3d=NO_UINT;
  j->rad=0;
  j->length=0;
  NewColor(0,0,0,&(j->color));  
  HTransformIdentity(&(j->trs));
}


void NewUniversalJoint(unsigned int id,
                       unsigned int linkID1,Tlink *link1,
                       unsigned int linkID2,Tlink *link2,
                       double **points,
                       boolean hasLimits,
                       Tinterval*range1,Tinterval *range2,double **rPoints,
                       boolean avoidLimits,double avoidLimitsWeight,
                       Tjoint *j)
{
  unsigned int i,k;
  double nr;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=UNV_JOINT;

  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;

  for(i=0;i<4;i++)
    {
      for(k=0;k<3;k++)
        j->points[i][k]=points[i][k];
    }

  /* compute the normalized vectors along the rotation axis */
  for(i=0;i<2;i++)
    {
      nr=0.0;
      for(k=0;k<3;k++)
        {
          j->normals[i][k]=points[2*i+1][k]-points[2*i][k];
          nr+=(j->normals[i][k]*j->normals[i][k]);
        }
      nr=sqrt(nr);
      for(k=0;k<3;k++)
        j->normals[i][k]/=nr;
    }

  j->hasLimits=hasLimits;

  if (j->hasLimits)
    {
      double c;
      double center;
      THTransform t,rx,a;
      double zero[3]={0,0,0};
      Tinterval *rd,*rs; 

      for(i=0;i<2;i++)
        {
          /*normalize the reference vectors*/
          nr=0.0;
          for(k=0;k<3;k++)
            {
              j->vrange[i][k]=rPoints[i][k]-points[2*i][k];
              nr+=(j->vrange[i][k]*j->vrange[i][k]);
            }
          nr=sqrt(nr);
          for(k=0;k<3;k++)
            j->vrange[i][k]/=nr;

          /*Test that the rotation vectors are orthogonal to the rotation axes*/
          c=0; /*cosinus between the two vectors*/
          for(k=0;k<3;k++)
            c+=(j->vrange[i][k]*j->normals[i][k]);

          if (fabs(c)>ZERO)
            Error("Rotation axes and rotation reference vectors are not orthonormal (Universal Joint)");

          
        }

      /*Copy of the ranges for the 2 rotation angles*/
      /*and we offset them so that they are zero-centered*/
      for(k=0;k<3;k++)
        {
          if (k==0)
            {
              rs=range1;
              rd=&(j->range);
            }
          else
            {
              rs=range2;
              rd=&(j->range2);
            }

          CopyInterval(rd,rs);
          /*We offset the ranges so that they are zero-centered*/
          center=IntervalCenter(rd);
          IntervalOffset(rd,-center,rd);
          
          /* Since we moved the rotation range to make it zero-centered, we 
             have to rotate ONE of the the reference vectors accordingly. 
             In this way, the angle between the reference vectors is changed in the same
             way as the offset applied to the range interval. */
          /* The rotation is around the vector defining the revolute joint for
             the current link. This rotation is defined by rotating the axis vector
             to the X axis, rotating around X and then rotating the vector back
             to the its original position.*/
          HTransformX2Vect(1,1,zero,j->normals[k],&t); /*this rotates X to to the
                                                 revolute axis vector*/
          HTransformRx(center,&rx); /*rotation around X*/
          
          HTransformInverse(&t,&a); /*this rotates the axis vector to X*/
          HTransformProduct(&rx,&a,&a);
          HTransformProduct(&t,&a,&a);
          
          HTransformApply(j->vrange[k],j->vrange[k],&a);
      
          HTransformDelete(&a);
          HTransformDelete(&rx);
          HTransformDelete(&t);
        }
      j->avoidLimits=avoidLimits;
      j->avoidLimitsWeight=avoidLimitsWeight;
    }
  else
    {
      /*Set some empty information for the range (not used)*/
      NewInterval(-INF,INF,&(j->range));
      NewInterval(-INF,INF,&(j->range2));
      for(i=0;i<2;i++)
        {
          for(k=0;k<3;k++)
            j->vrange[i][k]=0.0;
        }
      j->avoidLimits=FALSE;
      j->avoidLimitsWeight=0.0;
    }

  j->obj3d=NO_UINT;
  j->rad=0;
  j->length=0;
  NewColor(0,0,0,&(j->color));  
  HTransformIdentity(&(j->trs));
}

void NewSphericalJoint(unsigned int id,
                       unsigned int linkID1,Tlink *link1,
                       unsigned int linkID2,Tlink *link2,
                       double **points,
                       boolean hasLimits,double range,double **rPoints,
                       boolean avoidLimits,double avoidLimitsWeight,
                       Tjoint *j)
{
  unsigned int i,k;
  double nr;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=SPH_JOINT;

  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;

  /*for spherical joints only one point in each link is given, we replicate
    if as two points. */
  for(k=0;k<3;k++)
    {
      j->points[0][k]=points[0][k];
      j->points[1][k]=points[0][k];
    }
  for(k=0;k<3;k++)
    {
      j->points[2][k]=points[2][k];
      j->points[3][k]=points[2][k];
    }

  j->hasLimits=hasLimits;

  if (j->hasLimits)
    {
      NewInterval(-range,range,&(j->range));
      /*normalize the reference vectors*/
      for(i=0;i<2;i++)
        {
          nr=0.0;
          for(k=0;k<3;k++)
            {
              j->vrange[i][k]=rPoints[i][k]-points[2*i][k];
              nr+=(j->vrange[i][k]*j->vrange[i][k]);
            }
          nr=sqrt(nr);
          for(k=0;k<3;k++)
            j->vrange[i][k]/=nr;
        }
      j->avoidLimits=avoidLimits;
      j->avoidLimitsWeight=avoidLimitsWeight;
    }
  else
    {
      /*Set some empty information for the range (not used)*/
      NewInterval(-INF,INF,&(j->range));
      for(i=0;i<2;i++)
        {
          for(k=0;k<3;k++)
            j->vrange[i][k]=0.0;
        }
      j->avoidLimits=FALSE;
      j->avoidLimitsWeight=0.0;
    }

  NewInterval(-INF,INF,&(j->range2));

  j->obj3d=NO_UINT;
  j->rad=0;
  j->length=0;
  NewColor(0,0,0,&(j->color));  
  HTransformIdentity(&(j->trs));
}

void NewPrismaticJoint(unsigned int id,
                       unsigned int linkID1,Tlink *link1,
                       unsigned int linkID2,Tlink *link2,
                       double **points,Tinterval *range,
                       boolean avoidLimits,double avoidLimitsWeight,
                       Tjoint *j)
{
  unsigned int i,k;
  double nr;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=PRS_JOINT;

  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;

  for(i=0;i<4;i++)
    {
      for(k=0;k<3;k++)
        j->points[i][k]=points[i][k];
    }

  for(i=0;i<2;i++)
    {
      nr=0.0;
      for(k=0;k<3;k++)
        {
          j->normals[i][k]=points[2*i+1][k]-points[2*i][k];
          nr+=(j->normals[i][k]*j->normals[i][k]);
        }
      nr=sqrt(nr);
      for(k=0;k<3;k++)
        j->normals[i][k]/=nr;
    }

  j->hasLimits=TRUE;
  CopyInterval(&(j->range),range);

  /*This information is not used for the prismatic joint*/
  for(i=0;i<2;i++)
    {
      for(k=0;k<3;k++)
        j->vrange[i][k]=0.0;
    }

  NewInterval(-INF,INF,&(j->range2));

  j->obj3d=NO_UINT;
  j->rad=0;
  j->length=0;
  NewColor(0,0,0,&(j->color));  
  HTransformIdentity(&(j->trs));

  j->avoidLimits=avoidLimits;
  j->avoidLimitsWeight=avoidLimitsWeight;
}

void NewSphSphJoint(unsigned int id,
                    unsigned int linkID1,Tlink *link1,
                    unsigned int linkID2,Tlink *link2,
                    double **points,
                    double l,double r,
                    Tcolor *color,
                    Tjoint *j)
{
  unsigned int i,k;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=SPH_SPH_JOINT;

  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;

  /*for spherical joints only one point in each link is given, we replicate
    if as two points. */
  for(k=0;k<3;k++)
    {
      j->points[0][k]=points[0][k];
      j->points[1][k]=points[0][k];
    }
  for(k=0;k<3;k++)
    {
      j->points[2][k]=points[1][k];
      j->points[3][k]=points[1][k];
    }

  j->hasLimits=FALSE;
  j->avoidLimits=FALSE;

  /*Set some empty information for the range (not used)*/
  NewInterval(-INF,INF,&(j->range));
  NewInterval(-INF,INF,&(j->range2));
  for(i=0;i<2;i++)
    {
      for(k=0;k<3;k++)
        j->vrange[i][k]=0.0;
    }

  j->obj3d=NO_UINT;
  j->rad=r;
  j->length=l;
  CopyColor(&(j->color),color);  
  HTransformIdentity(&(j->trs));
}

void NewInPatchJoint(unsigned int id,
                     unsigned int linkID1,Tlink *link1,
                     unsigned int linkID2,Tlink *link2,
                     double **points,
                     double **patch,
                     boolean avoidLimits,double avoidLimitsWeight,
                     Tjoint *j)
{
  unsigned int k;
  double aux[3],t1[3],t2[3];
  double n1,n2;
  double p[3][3];
  Tinterval tx,ty,tz,a;

  if (linkID1==linkID2)
    Error("Setting up a joint on a single link");

  j->t=IN_PATCH_JOINT;

  j->id=id;

  j->linkID[0]=linkID1;
  j->linkID[1]=linkID2;
  j->link[0]=link1;
  j->link[1]=link2;
  
  /*Store the 2 points defining the point and normal on the first link*/
  for(k=0;k<3;k++)
    {
      j->points[0][k]=points[0][k];
      j->points[1][k]=points[1][k]-points[0][k];
    }

  /*Store the 4 points defining the patch in the second link. Actually we do
    not directly store the points but differences between them*/

  for(k=0;k<3;k++)
    {
      j->points[2][k]=patch[0][k];
      j->points[3][k]=patch[2][k]-patch[0][k];
      j->points[4][k]=patch[1][k]-patch[0][k];
      j->points[5][k]=patch[0][k]-patch[1][k]-patch[2][k]+patch[3][k];
    }

  /* Size of the sides of the patch: must be larger than 0 */
  n1=DotProduct(j->points[3],j->points[3]);
  n2=DotProduct(j->points[4],j->points[4]);

  if ((n1<ZERO)||(n2<ZERO))
    Error("Patch is too small");

  /*Store information to recover the normal at any point in the patch*/
  CrossProduct(j->points[3],j->points[4],j->normals[0]);

  CrossProduct(j->points[3],j->points[5],j->normals[1]);

  CrossProduct(j->points[5],j->points[4],j->normals[2]);

  /*Check if the input points correctly define a patch.
    Points defining a patch are given as
              p2------p3
              |       |
              |       |
              p0------p1
    Thus, in a correct patch vector p0->p3 is enclosed in 
    vectors p0->p2 and p0->p1.
    We check if this is so ensuring that angle p2-p0-p3 has the
    same sign as angle p3-p0-p1
   */
  for(k=0;k<3;k++)
    aux[k]=patch[3][k]-patch[0][k]; /*This is p0->p3*/

  if ((DotProduct(aux,j->points[3])<ZERO)||
      (DotProduct(aux,j->points[4])<ZERO))
    Error("Patch with overlapping points");

  CrossProduct(j->points[3],aux,t1); /* t1=(p0->p2) x (p0->p3) */
  CrossProduct(aux,j->points[4],t2); /* t2=(p0->p3) x (p0->p1) */
  
  /* Now we check that the above vectors have the same orientation */
  if (DotProduct(t1,t2)<ZERO)
    Error("Patch points given in the wrong order");

  /*Now we compute the minimum/maximum values of the norm of the normal vector*/
  for(k=0;k<3;k++)
    {
      p[k][0]=j->normals[0][k];
      p[k][1]=t1[k];
      p[k][2]=t2[k];
    }

  ComputeBoundingBox3d(3,p[0],p[1],p[2],&tx,&ty,&tz);
  IntervalPow(&tx,2,&(j->normRange));
  IntervalPow(&ty,2,&a);
  IntervalAdd(&(j->normRange),&a,&(j->normRange));
  IntervalPow(&tz,2,&a);
  IntervalAdd(&(j->normRange),&a,&(j->normRange));
  IntervalSqrt(&(j->normRange),&(j->normRange));
  
  if (LowerLimit(&(j->normRange))<ZERO)
    Error("The lower limit for the norm vector bounds for a IN_PATCH_JOINT should not be zero.");

  /*Now we store the avoid limit information, if any*/
  j->hasLimits=TRUE; /*Limits are given by the patch*/
  j->avoidLimits=avoidLimits;
  j->avoidLimitsWeight=avoidLimitsWeight;
}

void CopyJoint(Tjoint *j_dst,Tjoint *j_src)
{
  unsigned int i,k;

  j_dst->t=j_src->t;

  j_dst->id=j_src->id;

  for(i=0;i<2;i++)
    {
      j_dst->linkID[i]=j_src->linkID[i];
      j_dst->link[i]=j_src->link[i];
    }

  for(i=0;i<6;i++)
    {
      for(k=0;k<3;k++)
        j_dst->points[i][k]=j_src->points[i][k];
    }

  for(i=0;i<3;i++)
    {
      for(k=0;k<3;k++)
        j_dst->normals[i][k]=j_src->normals[i][k];
    }

  j_dst->hasLimits=j_src->hasLimits;
  j_dst->avoidLimits=j_src->avoidLimits;
  j_dst->avoidLimitsWeight=j_src->avoidLimitsWeight;

  CopyInterval(&(j_dst->normRange),&(j_src->normRange));

  CopyInterval(&(j_dst->range),&(j_src->range)); 
  CopyInterval(&(j_dst->range2),&(j_src->range2)); 

  for(i=0;i<2;i++)
    {
      for(k=0;k<3;k++)
        j_dst->vrange[i][k]=j_src->vrange[i][k];
    }

  j_dst->obj3d=j_src->obj3d;
  j_dst->rad=j_src->rad;
  j_dst->length=j_src->length;
  CopyColor(&(j_dst->color),&(j_src->color));

  HTransformCopy(&(j_dst->trs),&(j_src->trs));
}

unsigned int GetJointType(Tjoint *j)
{
  return(j->t);
}

unsigned int GetJointID(Tjoint *j)
{
  return(j->id);
}

unsigned int JointFromID(Tjoint *j)
{
  return(j->linkID[0]);
}

Tlink *JointFrom(Tjoint *j)
{
  return(j->link[0]);
}

unsigned int JointToID(Tjoint *j)
{
  return(j->linkID[1]);
}

Tlink *JointTo(Tjoint *j)
{
  return(j->link[1]);
}

void GetJointPoint(unsigned int link,unsigned int point,double *p,Tjoint *j)
{
  unsigned int i,k;

  i=link*2+point;

  for(k=0;k<3;k++)
    p[k]=j->points[i][k];
}

boolean LimitedJoint(Tjoint *j)
{
  return(j->hasLimits);
}

Tinterval *GetJointRange(Tjoint *j)
{ 
  return(&(j->range));
}

Tinterval *GetJointSecondRange(Tjoint *j)
{ 
  return(&(j->range2));
}

signed int GetJointDOF(Tjoint *j)
{
  signed int ndof;

  switch(j->t)
    {
      case FREE_JOINT:
        ndof=6;
        break;
      case FIX_JOINT:
        ndof=0;
        break;
      case PRS_JOINT:
        ndof=1;
        break;
      case SPH_JOINT:
        ndof=3;
        break;
      case REV_JOINT:
        ndof=1;
        break;
      case UNV_JOINT:
        ndof=2;
        break;
      case SPH_SPH_JOINT:
        ndof=5;
        break;
      case IN_PATCH_JOINT:
        ndof=2;
        break;
    default:
      Error("Undefined joint type in GetJointDOF");
    }
  return(ndof);
}

double GetJointLength(Tjoint *j)
{
  if (j->t!=SPH_SPH_JOINT)
    Error("The length is only defined for spherical-spherical composite joints");

  return(j->length);
}

THTransform *GetJointTransform(Tjoint *j)
{
  if (j->t!=FIX_JOINT)
    Error("The transform is only defined for spherical-spherical composite joints");

  return(&(j->trs));
}

void GenerateJointRangeEquations(Tparameters *p,TCuikSystem *cs,Tjoint *j)
{
  /* Equations associated with the joint (not joint equations as in the paper :) */

  char *vname;
  unsigned int i,k,id,nv;
  Tequation eq[3],eq1;
  Tinterval rangeOne,rangeInf;
  Tmonomial f;
  unsigned int jointVars[4][3]; /*Reference vector used in REVOLUTE and SPHERICAL joints*/
  Tvariable var;
  char *ln1,*ln2;
  Tinterval ic;
  double r;
  
  NewInterval(-1.0,1.0,&rangeOne);
  NewInterval(-INF,INF,&rangeInf);

  InitMonomial(&f);

  ln1=GetLinkName(j->link[0]);
  ln2=GetLinkName(j->link[1]);

  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);

  switch(j->t)
    {
    case FREE_JOINT:
      /*We generate 3 new variables representing the (unlimited) displacement from
        the origin of the first link to the origin of the second link.
        No equations constraint these variables. */
      for(k=0;k<3;k++)
        {
          FREE_JOINT_VAR(vname,j->id,j->linkID[0],ln1,j->linkID[1],ln2,k);
          
          NewVariable(SECONDARY_VAR,vname,&var);
          SetVariableInterval(&rangeInf,&var);

          AddVariable2CS(&var,cs);
          
          DeleteVariable(&var);
        }
      break;

    case FIX_JOINT:
      break;

    case PRS_JOINT:
      /*Add one variable for the displacement along the prismatic axis*/
      PRS_JOINT_VAR(vname,j->id,ln1,ln2);
      
      NewVariable(SYSTEM_VAR,vname,&var);
      SetVariableInterval(&(j->range),&var);

      nv=AddVariable2CS(&var,cs);

      DeleteVariable(&var);

      if (j->avoidLimits)
        {
          r=IntervalSize(&(j->range))/2;
          AddTerm2SearchCriterion(j->avoidLimitsWeight/(r*r),nv,
                                  IntervalCenter(&(j->range)),cs);
        }

      break;
   
    case UNV_JOINT:
    case REV_JOINT:
    case SPH_JOINT:
      if (j->hasLimits)
        {
          /*We define two vectors normal to the rotation axis, one attached
            to link1 (k==0) and another attached to link2 (k==1)*/
          for(k=0;k<2;k++)
            { 
              /*Define the variables for the rotation reference vectors*/
              for(i=0;i<3;i++)
                {             
                  InitEquation(&(eq[i]));
                  SetEquationValue(0.0,&(eq[i]));

                  if (j->t==REV_JOINT)
                    ROT_JOINT_VAR_REF(vname,j->id,k,ln1,ln2,i);
                  else
                    {
                      if (j->t==SPH_JOINT)
                        SPH_JOINT_VAR_REF(vname,j->id,k,ln1,ln2,i);
                      else
                        UNV_JOINT_VAR_REF(vname,j->id,k,ln1,ln2,i);
                    }

                  NewVariable(SECONDARY_VAR,vname,&var);
                  SetVariableInterval(&rangeOne,&var);

                  id=AddVariable2CS(&var,cs);

                  DeleteVariable(&var);

                  jointVars[k][i]=id;
                  AddVariable2Monomial(id,1,&f);
                  AddMonomial(&f,&(eq[i]));
                  ResetMonomial(&f);
                }

              ApplyLinkRot(-1.0,NO_UINT,j->vrange[k],eq,cs,IsGroundLink(j->linkID[k]),j->link[k]);

              for(i=0;i<3;i++)
                {
                  SetEquationCmp(EQU,&(eq[i]));
                  SetEquationType(SYSTEM_EQ,&(eq[i]));

                  AddEquation2CS(p,&(eq[i]),cs);
                  DeleteEquation(&(eq[i]));
                }
            }  

          if (j->t==UNV_JOINT)
            {
              /* The angles for a universal joint are defined from the two vectors
                 defining the two copunctual, orthogonal rotation axis towards the
                 two reference vectors given by the user.
                 The first angle is from the 1st reference vector to the second rotation vector.
                 The second angle is from the 2nd reference vector to the 1st rotation vector.

                 Above we defined variables for the two reference vectors. Below we
                 retrive the variable identifiers for the two vectors defining the
                 copunctual, orthogonal rotation axis (those variables are defined
                 in function GenerateJointEquationsInBranch.
              */
              for(k=2;k<4;k++)
                {
                  for(i=0;i<3;i++)
                    {  
                      UNV_JOINT_VAR(vname,j->id,ln1,ln2,k-2,i);
                      jointVars[k][i]=GetCSVariableID(vname,cs);
                    }
                }
              
              for(k=0;k<2;k++)
                {
                  GenerateDotProductEquation(jointVars[3-k][0],jointVars[3-k][1],jointVars[3-k][2],
                                             jointVars[k][0],jointVars[k][1],jointVars[k][2],
                                             NO_UINT,0,&eq1);
                   
                  COS_VAR_UNI(vname,j->id,ln1,ln2,k);
                  NewVariable(SECONDARY_VAR,vname,&var);
                  if (k==0)
                    IntervalCosinus(&(j->range),&ic);
                  else
                    IntervalCosinus(&(j->range2),&ic);
                  SetVariableInterval(&ic,&var);
                  DeleteInterval(&ic);
                  id=AddVariable2CS(&var,cs);
                  DeleteVariable(&var);
                   
                  AddVariable2Monomial(id,1,&f);
                  AddCt2Monomial(-1,&f);
                  AddMonomial(&f,&eq1);
                  ResetMonomial(&f);
                   
                  SetEquationCmp(EQU,&eq1);
                  SetEquationType(SYSTEM_EQ,&eq1);
                  AddEquation2CS(p,&eq1,cs);
                  DeleteEquation(&eq1);

                  if (j->avoidLimits)
                    {
                      r=IntervalSize(&ic);
                      AddTerm2SearchCriterion(j->avoidLimitsWeight/(r*r),id,1,cs);
                    }
                }
            }
          else
            {
              GenerateDotProductEquation(jointVars[0][0],jointVars[0][1],jointVars[0][2],
                                         jointVars[1][0],jointVars[1][1],jointVars[1][2],
                                         NO_UINT,0,&eq1);
              
              COS_VAR(vname,j->id,ln1,ln2);
              NewVariable(SECONDARY_VAR,vname,&var);
              IntervalCosinus(&(j->range),&ic);
              SetVariableInterval(&ic,&var);
              DeleteInterval(&ic);
              id=AddVariable2CS(&var,cs);
              DeleteVariable(&var);
              
              AddVariable2Monomial(id,1,&f);
              AddCt2Monomial(-1,&f);
              AddMonomial(&f,&eq1);
              ResetMonomial(&f);
              
              SetEquationCmp(EQU,&eq1);
              SetEquationType(SYSTEM_EQ,&eq1);
              AddEquation2CS(p,&eq1,cs);
              DeleteEquation(&eq1);

              if (j->avoidLimits)
                {
                  r=IntervalSize(&ic);
                  AddTerm2SearchCriterion(j->avoidLimitsWeight/(r*r),id,1,cs);
                }
            }
          
        }
      break;

    case SPH_SPH_JOINT:
      /*We assume non-constrained spherical-spherical joints*/
      break;

    case IN_PATCH_JOINT:
      /*In patch joints are limited by the 4 points defining the patch. These limits
        are implicitly given by the ranges for the control variables giving the patch. */
      if (j->avoidLimits)
        {
          IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,0);
          nv=GetCSVariableID(vname,cs);
          AddTerm2SearchCriterion(j->avoidLimitsWeight,nv,0.5,cs);
          
          IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,1);
          nv=GetCSVariableID(vname,cs);
          AddTerm2SearchCriterion(j->avoidLimitsWeight,nv,0.5,cs);
        }
      break;
    default:
      Error("Undefined joint type in GenerateJointRangeEquations");
    }

  DeleteInterval(&rangeOne);
  DeleteInterval(&rangeInf);

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

void GenerateJointEquationsInBranch(double s,TCuikSystem *cs,Tequation *eq,Tjoint *j)
{

  char *l1name,*l2name,*vname;
  unsigned int i,vID,d;
  Tmonomial f;

  l1name=GetLinkName(j->link[0]);
  l2name=GetLinkName(j->link[1]);

  switch (j->t)
    {
    case FREE_JOINT:

      NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
      InitMonomial(&f);
      for(i=0;i<3;i++)
        {
          FREE_JOINT_VAR(vname,j->id,j->linkID[0],l1name,j->linkID[1],l2name,i);
          
          vID=GetCSVariableID(vname,cs);
          if (vID==NO_UINT)
            Error("Undefined FREE_JOINT variable in GenerateTransEquationsFromBranch");
      
          AddVariable2Monomial(vID,1,&f);
          AddCt2Monomial(s,&f);
          AddMonomial(&f,&(eq[i]));
          ResetMonomial(&f);
        }
      DeleteMonomial(&f);
      free(vname);

      break;

    case FIX_JOINT:
      /*The translation part of the homogeneous transform between the two links has
        been stored in points[0] (see NewFixJoint). We apply it here.
        The rotation constraints are imposed in GenerateJointEquations. */
      ApplyLinkRot( s,NO_UINT,j->points[0],eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);
      break;

    case UNV_JOINT:
    case REV_JOINT:
    case SPH_JOINT:
      ApplyLinkRot( s,NO_UINT,j->points[0],eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);
      ApplyLinkRot(-s,NO_UINT,j->points[2],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
      break;

    case PRS_JOINT:
      ApplyLinkRot( s,NO_UINT,j->points[0],eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);
                  
      NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
      PRS_JOINT_VAR(vname,j->id,l1name,l2name);
      d=GetCSVariableID(vname,cs);
      free(vname);
      if (d==NO_UINT)
        Error("Undefined PRS_JOINT variable in GenerateTransEquationsFromBranch");
      
      ApplyLinkRot( s,d,j->normals[0],eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);

      ApplyLinkRot(-s,NO_UINT,j->points[2],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
      break;

    case SPH_SPH_JOINT:
      ApplyLinkRot( s,NO_UINT,j->points[0],eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);

      NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
      InitMonomial(&f);
      for(i=0;i<3;i++)
        { 
          SPH_SPH_JOINT_VAR(vname,j->id,l1name,l2name,i);
          
          vID=GetCSVariableID(vname,cs);
          if (vID==NO_UINT)
            Error("Undefined SPH_SPH_JOINT variable in GenerateTransEquationsFromBranch");

          AddVariable2Monomial(vID,1,&f);
          AddCt2Monomial(s*j->length,&f);
          AddMonomial(&f,&(eq[i]));
          ResetMonomial(&f);
        }
      DeleteMonomial(&f);
      free(vname);

      ApplyLinkRot(-s,NO_UINT,j->points[2],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
      break;
     
    case IN_PATCH_JOINT:
      ApplyLinkRot( s,NO_UINT,j->points[0],eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);
      ApplyLinkRot(-s,NO_UINT,j->points[2],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
      
      NEW(vname,strlen(l1name)+strlen(l2name)+100,char);
      for(i=0;i<3;i++)
        {
          IN_PATCH_JOINT_CTRL_VAR(vname,j->id,l1name,l2name,i);
          vID=GetCSVariableID(vname,cs);
          if (vID==NO_UINT)
            Error("Undefined IN_PATCH_JOINT variable in GenerateTransEquationsFromBranch");

          ApplyLinkRot(-s,vID,j->points[3+i],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
        }
      free(vname);
      break;

    default:
      Error("Unkownd joint type"); 
    }
}

void GenerateJointEquations(Tparameters *p,double maxCoord,
                            TCuikSystem *cs,Tjoint *j)
{
  double v[3];
  unsigned int i,k;
  Tequation eq[3],eq1;
  char *ln1,*ln2,*vname;
  Tinterval rangeOne,rangeInf,rangeZeroOne;     
  Tmonomial f;
  unsigned int vID[3]; /* variables generated */
  unsigned int vIDuni[2][3]; /* for the universal joints 2 vectors are generated */
  unsigned int l; /*scale variable of the normal vectors in the patch joint*/
  Tvariable var;

  ln1=GetLinkName(j->link[0]);
  ln2=GetLinkName(j->link[1]);

  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);

  NewInterval(-1,1,&rangeOne);
  NewInterval(0,1,&rangeZeroOne);

  InitMonomial(&f);

  switch(j->t)
    {
    case FREE_JOINT:
      NewInterval(-maxCoord,maxCoord,&rangeInf);
      for(i=0;i<3;i++)
        {
          FREE_JOINT_VAR(vname,j->id,j->linkID[0],ln1,j->linkID[1],ln2,i);
          
          NewVariable(SYSTEM_VAR,vname,&var);
          SetVariableInterval(&rangeInf,&var);
          AddVariable2CS(&var,cs);
              
          DeleteVariable(&var);
        }
      break;

    case FIX_JOINT: 
      /* link1 * transform = link2 * identity */
      /* link1 * transform - link2 * identity = 0 */
      for(i=0;i<3;i++)
        {    
          for(k=0;k<3;k++)
            InitEquation(&(eq[k]));

          for(k=0;k<3;k++)
            v[k]=HTransformGetElement(k,i,&(j->trs));

          ApplyLinkRot(1,NO_UINT,v,eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);

          v[0]=v[1]=v[2]=0;
          v[i]=1;

          ApplyLinkRot(-1,NO_UINT,v,eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
          
          for(k=0;k<3;k++)
            {
              SetEquationType(SYSTEM_EQ,&(eq[k]));
              SetEquationCmp(EQU,&(eq[k]));
              AddEquation2CS(p,&(eq[k]),cs);
              DeleteEquation(&(eq[k])); 
            }
        }
      break;

    case PRS_JOINT:
      /* The three reference vectors (X,Y,Z) for the two links 
         are the same when transformed to global coordinates. */
      for(k=0;k<3;k++)
        InitEquation(&(eq[k]));
      
      for(i=0;i<3;i++)
        {
          v[0]=v[1]=v[2]=0.0;
          v[i]=1.0;

          for(k=0;k<3;k++)
            InitEquation(&(eq[k]));
          
          for(k=0;k<2;k++)
            ApplyLinkRot((k==0?+1.0:-1.0),NO_UINT,v,eq,cs,IsGroundLink(j->linkID[k]),j->link[k]);

          for(k=0;k<3;k++)
            {
              SetEquationType(SYSTEM_EQ,&(eq[k]));
              SetEquationCmp(EQU,&(eq[k]));
              AddEquation2CS(p,&(eq[k]),cs);
              DeleteEquation(&(eq[k])); 
            }
        }
      break;
      
    case REV_JOINT:
      /* The vector defining the rotation axis is the same as seen from the two links. */
      for(k=0;k<3;k++)
        InitEquation(&(eq[k]));

      for(k=0;k<2;k++) /* for the two links involved in the joint */
        ApplyLinkRot((k==0?+1.0:-1.0),NO_UINT,j->normals[k],eq,cs,IsGroundLink(j->linkID[k]),j->link[k]);

      for(k=0;k<3;k++)
        {
          SetEquationType(SYSTEM_EQ,&(eq[k]));
          SetEquationCmp(EQU,&(eq[k]));
          AddEquation2CS(p,&(eq[k]),cs);
          DeleteEquation(&(eq[k])); 
        }
      break;

    case UNV_JOINT:
      /* Two vectors one attached to link1 and another attached to link2 */
      for(i=0;i<2;i++)
        {
          for(k=0;k<3;k++)
            InitEquation(&(eq[k]));

          ApplyLinkRot(1,NO_UINT,j->normals[i],eq,cs,IsGroundLink(j->linkID[i]),j->link[i]);

          for(k=0;k<3;k++)
            {
              UNV_JOINT_VAR(vname,j->id,ln1,ln2,i,k);
              
              NewVariable(SECONDARY_VAR,vname,&var);
              SetVariableInterval(&rangeOne,&var);
              
              vIDuni[i][k]=AddVariable2CS(&var,cs);
              
              DeleteVariable(&var);

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

              SetEquationType(SYSTEM_EQ,&(eq[k]));
              SetEquationCmp(EQU,&(eq[k]));
              AddEquation2CS(p,&(eq[k]),cs);
              DeleteEquation(&(eq[k])); 
            }
          
        }

      /* the two vectors must be orthogonal */
      GenerateDotProductEquation(vIDuni[0][0],vIDuni[0][1],vIDuni[0][2],
                                 vIDuni[1][0],vIDuni[1][1],vIDuni[1][2],
                                 NO_UINT,0,&eq1);
      AddEquation2CS(p,&eq1,cs);
      DeleteEquation(&eq1);
      break;

    case SPH_JOINT:
      /*spherical joints introduce no joint equations (all the constraints of
        a spherical joint are in the loop equations.*/
      break;

    case SPH_SPH_JOINT:
      /* We define a  vector (with norm 1) along the sph-sph axis. These variables are
         later used in the loops. */
      for(i=0;i<3;i++)
        {
          SPH_SPH_JOINT_VAR(vname,j->id,ln1,ln2,i);  
          
          NewVariable(SYSTEM_VAR,vname,&var);
          SetVariableInterval(&rangeOne,&var);
          
          vID[i]=AddVariable2CS(&var,cs);
          
          DeleteVariable(&var);
        }
      GenerateNormEquation(vID[0],vID[1],vID[2],1.0,&eq1);
      AddEquation2CS(p,&eq1,cs);
      DeleteEquation(&eq1); 

      break;

    case IN_PATCH_JOINT:
      for(i=0;i<3;i++)
        {
          IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,i);

          NewVariable((i<2?SYSTEM_VAR:DUMMY_VAR),vname,&var);
          SetVariableInterval(&rangeZeroOne,&var);

          vID[i]=AddVariable2CS(&var,cs);

          DeleteVariable(&var);
        }

      GenerateSaddleEquation(vID[0],vID[1],vID[2],&eq1);
      AddEquation2CS(p,&eq1,cs);
      DeleteEquation(&eq1);
      
      for(k=0;k<3;k++)
        InitEquation(&(eq[k]));
      
      IN_PATCH_JOINT_SCALE_VAR(vname,j->id,ln1,ln2);

      NewVariable(SYSTEM_VAR,vname,&var);
      SetVariableInterval(&(j->normRange),&var);
      l=AddVariable2CS(&var,cs);
      DeleteVariable(&var);

      ApplyLinkRot( 1,l,j->points[1],eq,cs,IsGroundLink(j->linkID[0]),j->link[0]);

      ApplyLinkRot(-1,NO_UINT,j->normals[0],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
      ApplyLinkRot(-1, vID[0],j->normals[1],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);
      ApplyLinkRot(-1, vID[1],j->normals[2],eq,cs,IsGroundLink(j->linkID[1]),j->link[1]);

      for(k=0;k<3;k++)
        {
          SetEquationType(SYSTEM_EQ,&(eq[k]));
          SetEquationCmp(EQU,&(eq[k]));
          AddEquation2CS(p,&(eq[k]),cs);
          DeleteEquation(&(eq[k])); 
        }

      break;

    default:
      Error("Unknown joint type in GenerateJointEquations");
    }

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


void RegenerateJointSolution(TCuikSystem *cs,double *sol,Tjoint *j)
{
  unsigned int i;
  unsigned int vID[3];
  char *vname,*ln1,*ln2;

  ln1=GetLinkName(j->link[0]);
  ln2=GetLinkName(j->link[1]);

  NEW(vname,strlen(ln1)+strlen(ln2)+100,char);

  switch(j->t)
    {
      case IN_PATCH_JOINT:
        for(i=0;i<3;i++)
          {
            IN_PATCH_JOINT_CTRL_VAR(vname,j->id,ln1,ln2,i);
            vID[i]=GetCSVariableID(vname,cs);
          }
        
        sol[vID[2]]=sol[vID[0]]*sol[vID[1]];

        break;
    }

  free(vname);
}

double GetJointMaxCoordinate(Tjoint *j)
{
  double mc;

  switch(j->t)
    {
    case SPH_SPH_JOINT:
      mc=j->length;
      break;
    case PRS_JOINT:
      {
        double l,u;

        l=fabs(LowerLimit(&(j->range)));
        u=fabs(UpperLimit(&(j->range)));
        mc=(l>u?l:u);
      }
      break;
    default:
      mc=0.0;
    }

  return(mc);
}

void PlotJoint(Tplot3d *pt,Tjoint *j)
{
  if ((j->t==SPH_SPH_JOINT)&&
      (j->rad>0)) /*sph_slj joints with 0 radius are hidden (used in fix joints)*/
    {
      double zero[3]={0,0,0};
      double end[3]={0,0,0};
      
      end[0]=j->length;

      j->obj3d=StartNew3dObject(&(j->color),pt);
      PlotSphere(j->rad*1.5,0,0,0,pt);
      PlotCylinder(j->rad,zero,end,pt);
      PlotSphere(j->rad*1.5,end[0],end[1],end[2],pt);
      Close3dObject(pt);
    }
}

void MoveJoint(Tplot3d *pt,TCuikSystem *cs,double *sol,
               double **r,Tjoint *j)
{
  if (j->t==SPH_SPH_JOINT)
    {
      THTransform t;
      double p[2][3];
      unsigned int k,i;
      double d,n;

      /*We use the anchor points of the spherical-spherical composite
        joint to the links to define the rotation/translation of the
        X aligned cylinder generated in PlotJoint */
      for(k=0;k<2;k++)
        {
          GetTransform2Link(cs,sol,IsGroundLink(j->linkID[k]),r[j->linkID[k]],
                            &t,j->link[k]);
          
          for(i=0;i<3;i++)
            HTransformApply(j->points[2*k],p[k],&t);
          
          HTransformDelete(&t);
        }
      /*normalize the distance between p0 and p1 so that no deformation is produced
        in the X axis of the cylinder (the length is already set when plotting) */
      n=0;
      for(k=0;k<3;k++)
        {
          d=p[1][k]-p[0][k];
          n+=(d*d);
        }
      n=sqrt(n);

      for(k=0;k<3;k++)
        p[1][k]=p[0][k]+(p[1][k]-p[0][k])/n;

      HTransformX2Vect(1,1,p[0],p[1],&t);
      
      Move3dObject(j->obj3d,&t,pt);
      
      HTransformDelete(&t);
    }
}

void DeleteJoint(Tjoint *j)
{
  DeleteInterval(&(j->range));  
  DeleteInterval(&(j->range2));  
  HTransformDelete(&(j->trs));
}