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

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 GetMechanismDOF(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)
{
  GetCPolyhedronDefiningPoint(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);

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

  return(k);
}

unsigned int AddBody2Mechanism(unsigned int lID,Tcpolyhedron *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++;
  
  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);
}

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

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

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

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);
    }  
  if (n>0)
    {
      Tjoint *j;
      unsigned int nj;

      /*We only plot joints if there is at least one link to plot*/
      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(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(cs,sol,IsGroundLink(i),l);
    }

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

void MoveMechanism(Tplot3d *pt,TCuikSystem *cs,double *sol,
                   double **r,Tmechanism *m)
{
  unsigned int i;
  unsigned int n;
  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)
        MoveLink(pt,cs,sol,r[i],l);
    }

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

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

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

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

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