mechanism.c

Go to the documentation of this file.

#include "mechanism.h"

#include <string.h>


void InitMechanism(Tmechanism *m)
{
  InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,INIT_NUM_JOINTS,&(m->joints)); 
  InitVector(sizeof(void *),CopyVoidPtr,DeleteVoidPtr,INIT_NUM_LINKS,&(m->links));  
  m->nbodies=0;
  m->allSpheres=TRUE;
  m->maxCoord=0.0;
}

unsigned int GetMechanismNLinks(Tmechanism *m)
{
  return(VectorSize(&(m->links)));
}

unsigned int GetMechanismNJoints(Tmechanism *m)
{
  return(VectorSize(&(m->joints)));
}

unsigned int GetMechanismNBodies(Tmechanism *m)
{
  return(m->nbodies);
}

boolean IsMechanismAllSpheres(Tmechanism *m)
{
  return(m->allSpheres);
}

double GetMechanismMaxCoordinate(Tmechanism *m)
{
  return(m->maxCoord);
}

unsigned int GetLinkID(char *name,Tmechanism *m)
{
  unsigned int id,nl;
  boolean found;
  Tlink *l;
  
  nl=GetMechanismNLinks(m);
  found=FALSE;
  id=0;
  while((!found)&&(id<nl))
    {
      l=GetMechanismLink(id,m);
      if ((l!=NULL)&&(strcmp(name,GetLinkName(l))==0))
        found=TRUE;
      else
        id++;
    }
  if (found)
    return(id);
  else
    return(NO_UINT);
}

signed int GetMechanismMobility(Tmechanism *m)
{
  signed int ndof,constraints;
  unsigned int i,nj;

  ndof=6*(GetMechanismNLinks(m)-1); /*Ground link is fixed !!*/

  nj=GetMechanismNJoints(m);
  constraints=0;
  for(i=0;i<nj;i++)
    constraints+=(6-GetJointDOF(GetMechanismJoint(i,m)));

  return(ndof-constraints);
}

void GetMechanismDefiningPoint(unsigned int lID,unsigned int bID,unsigned int pID,
                               double *p,Tmechanism *m)
{
  GetPolyhedronDefiningPoint(pID,p,GetLinkBody(bID,GetMechanismLink(lID,m)));
}

unsigned int AddLink2Mechanism(Tlink *l,Tmechanism *m)
{
  Tlink *li;
  unsigned int k;

  NEW(li,1,Tlink);
  CopyLink(li,l);
  k=GetMechanismNLinks(m);
  SetVectorElement(k,&li,&(m->links));

  m->maxCoord+=GetLinkMaxCoordinate(l);
  m->nbodies+=LinkNBodies(l);
  m->allSpheres=((m->allSpheres)&&(IsLinkAllSpheres(l)));

  return(k);
}

unsigned int AddJoint2Mechanism(Tjoint *j,Tmechanism *m)
{
  Tjoint *ji;
  unsigned int k;

  NEW(ji,1,Tjoint);
  CopyJoint(ji,j);
  k=GetMechanismNJoints(m);
  SetVectorElement(k,&ji,&(m->joints));

  m->maxCoord+=GetJointMaxCoordinate(j);
  m->allSpheres=((m->allSpheres)&&(IsJointAllSpheres(j)));

  return(k);
}

unsigned int AddBody2Mechanism(unsigned int lID,Tpolyhedron *b,Tmechanism *m)
{
  Tlink *lk;

  lk=GetMechanismLink(lID,m);
  if (lk==NULL)
    Error("Unkown link in AddBody2Mechanism");

  
  m->maxCoord-=GetLinkMaxCoordinate(lk);
  AddBody2Link(b,lk);

  m->maxCoord+=GetLinkMaxCoordinate(lk);
  m->nbodies++;
  m->allSpheres=((m->allSpheres)&&(IsLinkAllSpheres(lk)));
  
  return(m->nbodies);
}

Tlink *GetMechanismLink(unsigned int i,Tmechanism *m)
{
  Tlink **l;

  l=(Tlink **)GetVectorElement(i,&(m->links));
  if (l==NULL)
    return(NULL);
  else
    return(*l);
}

unsigned int GetMechanismLinkID(char *ln,Tmechanism *m)
{
  unsigned int i,nl;
  Tlink **l;
  boolean found;

  nl=VectorSize(&(m->links));
  found=FALSE;
  i=0;
  while((!found)&&(i<nl))
    {
      l=(Tlink **)GetVectorElement(i,&(m->links));
      
      if ((l!=NULL)&&(strcmp(ln,GetLinkName(*l))==0))
        found=TRUE;
      else
        i++;
    }
    
  if (found)
    return(i);
  else
    return(NO_UINT);
}

Tjoint *GetMechanismJoint(unsigned int i,Tmechanism *m)
{
  Tjoint **j;

  j=(Tjoint **)GetVectorElement(i,&(m->joints));
  if (j==NULL)
    return(NULL);
  else
    return(*j);
}

boolean AllRevolute(Tmechanism *m)
{
  unsigned int nj,i;
  boolean all;
  
  nj=GetMechanismNJoints(m);
  all=TRUE;
  i=0;
  while((all)&&(i<nj))
    {
      all=(GetJointType(GetMechanismJoint(i,m))==REV_JOINT);
      i++;
    }
  return(all);
}

void PlotMechanism(Tplot3d *pt,double axesLength,Tmechanism *m)
{
  unsigned int i;
  unsigned int n;
  Tlink *l;

  n=GetMechanismNLinks(m);  
  if (n>0) Start3dBlock(pt);
  for(i=0;i<n;i++)
    {
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        PlotLink(pt,axesLength,l);
    } 

  /*We only plot joints if there is at least one link to plot*/ 
  if (n>0)
    {
      Tjoint *j;
      unsigned int nj;

      nj=GetMechanismNJoints(m);  
      for(i=0;i<nj;i++)
        {
          j=GetMechanismJoint(i,m);
          if (j!=NULL)
            PlotJoint(pt,j);
        }  
    }

  if (n>0) Close3dBlock(pt);
}

void RegenerateMechanismSolution(Tparameters *p,TCuikSystem *cs,
                                 double *sol,Tmechanism *m)
{ 
  unsigned int i;
  unsigned int n;
  Tlink *l;
  Tjoint *j;

  n=GetMechanismNLinks(m); 
  for(i=0;i<n;i++)
    {
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        RegenerateLinkSolution(p,cs,sol,IsGroundLink(i),l);
    }

  n=GetMechanismNJoints(m); 
  for(i=0;i<n;i++)
    {
      j=GetMechanismJoint(i,m);
      if (j!=NULL)
        RegenerateJointSolution(p,cs,sol,j);
    }
}

void RegenerateMechanismBox(Tparameters *p,TCuikSystem *cs,Tbox *b,Tmechanism *m)
{
  unsigned int i;
  unsigned int n;
  Tlink *l;
  Tjoint *j;
  unsigned int r;

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

  /* for DOF representations -> nothing to regenerate */
  if (r!=REP_JOINTS)
    {
      n=GetMechanismNLinks(m);
      for(i=0;i<n;i++)
        {
          l=GetMechanismLink(i,m);
          if (l!=NULL)
            RegenerateLinkBox(p,cs,b,IsGroundLink(i),l);
        }

      n=GetMechanismNJoints(m); 
      for(i=0;i<n;i++)
        {
          j=GetMechanismJoint(i,m);
          if (j!=NULL)
            RegenerateJointBox(p,cs,b,j);
        }
    }
}

void GetMechanismDOFsFromTransforms(Tparameters *p,THTransform *tl,double *dof,
                                    Tmechanism *m)
{
  Tjoint *j;
  unsigned int i,nj,k;
  unsigned int l1,l2;
    
  nj=GetMechanismNJoints(m);  
  k=0;
  for(i=0;i<nj;i++)
    {
      j=GetMechanismJoint(i,m);
      if (j!=NULL)
        {
          l1=JointFromID(j);
          l2=JointToID(j);
          GetJointDOFValues(p,&(tl[l1]),&(tl[l2]),&(dof[k]),j);
          k+=GetJointDOF(j);
        }
    }
}

void MechanismPrintAtoms(FILE *f,THTransform *tl,Tmechanism *m)
{
  unsigned int i,n;
  Tlink *l;

  n=GetMechanismNLinks(m);
  for(i=0;i<n;i++) 
    {
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        LinkPrintAtoms(f,&(tl[i]),l);
    }
  fprintf(f,"\n");
}

void MechanismStoreRigidAtoms(FILE *f,THTransform *tl,Tmechanism *m)
{
  unsigned int i,n,nr,na;
  Tlink *l;

  n=GetMechanismNLinks(m);

  /* number of rigid = number of links with balls */
  nr=0;
  for(i=0;i<n;i++) 
    {
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        {
          if (LinkNAtoms(l)>0)
            nr++;
        }
    }
  fprintf(f,"%u\n",nr);

  /* now store  */
  for(i=0;i<n;i++) 
    {
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        {
          na=LinkNAtoms(l);
          if (na>0)
            {
              fprintf(f,"%u %u\n",na,IsGroundLink(i)); 
              LinkStoreAtoms(f,&(tl[i]),l);
            }
        }
    }
  fprintf(f,"\n");
}

void MoveMechanismFromTransforms(Tparameters *pr,
                                 Tplot3d *pt,THTransform *tl,Tmechanism *m)
{
  unsigned int i;
  unsigned int n;
  unsigned int l1,l2;
  Tlink *l;

  n=GetMechanismNLinks(m);
  if (n>0) Start3dBlock(pt);
  for(i=1;i<n;i++) /*Ground link (ID==0) does not move*/
    {
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        MoveLinkFromTransform(pt,&(tl[i]),l);
    }

  if(n>0)
    {
      Tjoint *j;
      unsigned int nj,k;

      /*We only attempt to move joints if there is at least one link to plot*/

      nj=GetMechanismNJoints(m); 
      k=0;
      for(i=0;i<nj;i++)
        {
          j=GetMechanismJoint(i,m);
          if (j!=NULL)
            {
              l1=JointFromID(j);
              l2=JointToID(j);
              MoveJointFromTransforms(pr,pt,&(tl[l1]),&(tl[l2]),j);
              k+=GetJointDOF(j);
            }
        }
    }

  if (n>0) Close3dBlock(pt);
}

void PrintMechanism(FILE *f,char *path,char *prefix,Tmechanism *m)
{
  unsigned int i,n;
  Tlink *l;
  Tjoint *j;
  
  n=GetMechanismNLinks(m);
  if (n>0)
    fprintf(f,"[LINKS]\n\n");
  for(i=0;i<n;i++)
    { 
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        PrintLink(f,path,prefix,l);
    }

  n=GetMechanismNJoints(m);
  if (n>0)
    fprintf(f,"[JOINTS]\n\n");
  for(i=0;i<n;i++)
    {
      j=GetMechanismJoint(i,m);
      if (j!=NULL)
        PrintJoint(f,j);
    }
}

void DeleteMechanism(Tmechanism *m)
{

  unsigned int i;
  unsigned int n;
  Tlink *l;
  Tjoint *j;

  n=GetMechanismNLinks(m);
  for(i=0;i<n;i++)
    {
      l=GetMechanismLink(i,m);
      if (l!=NULL)
        {
          DeleteLink(l);
          free(l);
        } 
    }

  n=GetMechanismNJoints(m);
  for(i=0;i<n;i++)
    {
      j=GetMechanismJoint(i,m);
      if (j!=NULL)
        {
          DeleteJoint(j);
          free(j);
        }
    }

  DeleteVector(&(m->links)); 
  DeleteVector(&(m->joints)); 
}