mequation.c

Go to the documentation of this file.

#include "mequation.h"

#include "error.h"
#include "basic_algebra.h"

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


void RecomputeScalarEquations(TMequation *me);

void RecomputeScalarEquations(TMequation *me)
{
#if (0)
  unsigned int nRot,nTrans;
  
  me->neq=0;
  nRot=1;
  nTrans=1;
#else
  unsigned int i,nRot,nTrans;
  boolean ctAllTrans;
  unsigned int dof[6]={0,0,0,0,0,0};

 for(i=0;i<me->n;i++)
   UpdateUsedDOF(dof,me->ts[i]);

 nRot=(dof[RX]+dof[RY]+dof[RZ]);
 nTrans=(dof[TX]+dof[TY]+dof[TZ]);

 /* Check if all ct transforms are translations */
 ctAllTrans=TRUE;
 for(i=0;((ctAllTrans)&&(i<me->n));i++)
   ctAllTrans=(!HasCtRotTransSeq(me->ts[i]));

 me->neq=0;
 if ((ctAllTrans)&&(nRot>0)&&(nTrans==0))
   {
     /* Check if all rotations are aligned -> plannar mechanism */
     if (dof[RX]==nRot)
       {
         me->r[me->neq]=AXIS_Y;me->c[me->neq]=AXIS_Z;me->neq++;
         me->r[me->neq]=AXIS_Y;me->c[me->neq]=AXIS_H;me->neq++;
         me->r[me->neq]=AXIS_Z;me->c[me->neq]=AXIS_H;me->neq++;
         me->r[me->neq]=AXIS_Y;me->c[me->neq]=AXIS_Y;me->neq++;
       }
    if (dof[RY]==nRot)
       {
         me->r[me->neq]=AXIS_X;me->c[me->neq]=AXIS_Z;me->neq++;
         me->r[me->neq]=AXIS_X;me->c[me->neq]=AXIS_H;me->neq++;
         me->r[me->neq]=AXIS_Z;me->c[me->neq]=AXIS_H;me->neq++;
         me->r[me->neq]=AXIS_X;me->c[me->neq]=AXIS_X;me->neq++;
       }
    if (dof[RZ]==nRot)
       {
         me->r[me->neq]=AXIS_X;me->c[me->neq]=AXIS_Y;me->neq++;
         me->r[me->neq]=AXIS_X;me->c[me->neq]=AXIS_H;me->neq++;
         me->r[me->neq]=AXIS_Y;me->c[me->neq]=AXIS_H;me->neq++;
         me->r[me->neq]=AXIS_X;me->c[me->neq]=AXIS_X;me->neq++;
       }
   }
#endif

 /* If none of the special cases -> general case  */
 /* Ct equations also define equations (ct equations) */
 if (me->neq==0)
   {
     me->r[0]=AXIS_X; me->c[0]=AXIS_Y;
     me->r[1]=AXIS_X; me->c[1]=AXIS_Z;
     me->r[2]=AXIS_Y; me->c[2]=AXIS_Z;
     me->r[3]=AXIS_X; me->c[3]=AXIS_H;
     me->r[4]=AXIS_Y; me->c[4]=AXIS_H;
     me->r[5]=AXIS_Z; me->c[5]=AXIS_H;
     me->r[6]=AXIS_X; me->c[6]=AXIS_X;
     me->r[7]=AXIS_Y; me->c[7]=AXIS_Y;
     me->r[8]=AXIS_Z; me->c[8]=AXIS_Z;
     me->neq=9;
   }
}

void InitMEquation(TMequation *me)
{
  me->n=1;
  me->simp=FALSE;
  NEW(me->ts,me->n,TTransSeq*);
  NEW(me->ts[0],1,TTransSeq);
  InitTransSeq(me->ts[0]);
  HTransformIdentity(&(me->rhs));
  AddCtTrans2TransSeq(&(me->rhs),me->ts[0]);

  me->neq=0;
  RecomputeScalarEquations(me);
}

void CopyMEquation(TMequation *me_dst,TMequation *me_src)
{
  unsigned int i;
  
  me_dst->n=me_src->n;
  me_dst->simp=me_src->simp;
  NEW(me_dst->ts,me_dst->n,TTransSeq*);
  for(i=0;i<me_dst->n;i++)
    {
      NEW(me_dst->ts[i],1,TTransSeq);
      CopyTransSeq(me_dst->ts[i],me_src->ts[i]);
    }
  HTransformCopy(&(me_dst->rhs),&(me_src->rhs));
  me_dst->neq=me_src->neq;
  for(i=0;i<me_dst->neq;i++)
    {
      me_dst->r[i]=me_src->r[i];
      me_dst->c[i]=me_src->c[i];
    }
}

void ResetMEquation(TMequation *me)
{
  if (me->n>1)
    Error("Reseting a non-basic matrix equation.");

  ResetTransSeq(me->ts[0]);
  HTransformIdentity(&(me->rhs));
  me->neq=0;
  me->simp=FALSE;
}

boolean IsEmptyMEquation(TMequation *me)
{
  return(me->neq==0);
}

unsigned int MEquationSize(TMequation *me)
{
  unsigned int i,s,l;

  s=0;
  for(i=0;i<me->n;i++)
    {
      l=TransSeqSize(me->ts[i]);
      if (l>s) s=l;
    }
  return(s);
}

boolean HasRotations(TMequation *me)
{
  unsigned int dof[6]={0,0,0,0,0,0};
  unsigned int i;

  for(i=0;i<me->n;i++)
    UpdateUsedDOF(dof,me->ts[i]);
  
  return((dof[RX]+dof[RY]+dof[RZ])>0);
}

void SetTranslationMEquation(TMequation *me)
{
  unsigned int i;

  if (me->neq==0)
    Error("Modifying an empty matrix equation");

  me->simp=TRUE;

  me->neq=3;
  for(i=0;i<3;i++)
    {
      me->r[i]=i;
      me->c[i]=AXIS_H;
    }
}

boolean VarIncludedinMEquation(unsigned int v,TMequation *me)
{
  unsigned int i;
  boolean found;

  found=FALSE;
  i=0;
  while((!found)&&(i<me->n))
    {
      found=VarIncludedinTransSeq(v,me->ts[i]);
      i++;
    }
  return(found);
}

void AddDispTrans2MEquation(int s,unsigned int v,
                            double *vect,TMequation *me)
{
  AddDispTrans2TransSeq(s,v,vect,me->ts[0]);
}

void AddVarTrans2MEquation(unsigned int t,int s,unsigned int v,TMequation *me)
{
  if (me->n>1)
    Error("Adding terms to a non-basic matrix equation.");
  if (me->simp)
    Error("Adding terms an already simplified matrix equation.");

  AddVarTrans2TransSeq(t,s,v,me->ts[0]);
  
  RecomputeScalarEquations(me);
}

void AddPatchTrans2MEquation(unsigned int t,int s,unsigned int u,unsigned int v,
                             double **p,TMequation *me)
{
  if (me->n>1)
    Error("Adding terms to a non-basic matrix equation.");
  if (me->simp)
    Error("Adding terms an already simplified matrix equation.");

  AddPatchTrans2TransSeq(t,s,u,v,p,me->ts[0]);
  
  RecomputeScalarEquations(me);
}


void AddCtTrans2MEquation(THTransform *t,TMequation *me)
{
  if (me->n>1)
    Error("Adding terms to a non-basic matrix equation.");
  if (me->simp)
    Error("Adding terms an already simplified matrix equation.");

  AddCtTrans2TransSeq(t,me->ts[0]);

  RecomputeScalarEquations(me);
}

void AddTransSeq2MEquation(int s,TTransSeq *ts,TMequation *me)
{
  int i;
  unsigned int n;
  TTrans taux;
  
  if (me->n>1)
    Error("Adding terms to a non-basic matrix equation.");

  if (me->simp)
    Error("Adding terms an already simplified matrix equation.");

  n=TransSeqSize(ts);
  if (n>0)
    {
      if (s<0)
        {
          for(i=n-1;i>=0;i--)
            {
              TransInvert(&taux,GetElementFromTransSeq(i,ts));
              AddTrans2TransSeq(&taux,me->ts[0]);
              DeleteTrans(&taux);
            }
        }
      else
        {
          for(i=0;i<n;i++)
            AddTrans2TransSeq(GetElementFromTransSeq(i,ts),me->ts[0]);
        }
      RecomputeScalarEquations(me);
    }
}

unsigned int NumberScalarEquations(TMequation *me)
{
  return(me->neq);
}

void  ShiftVariablesInMEquation(unsigned int nv,TMequation *me)
{
  unsigned int i;

  for(i=0;i<me->n;i++)
    ShiftVariablesInTransSeq(nv,me->ts[i]);
}

unsigned int FixVarInMEquation(unsigned int nv,double v,TMequation *me)
{
  unsigned int i;
  TTransSeq **tsOrig;
  unsigned int nOrig;
  unsigned int r;
  THTransform ctt;
  
  tsOrig=me->ts;
  nOrig=me->n;

  NEW(me->ts,me->n,TTransSeq *);
  me->n=0;
  
  for(i=0;i<nOrig;i++)
    {
      if (FixVarInTransSeq(nv,v,&ctt,tsOrig[i]))
        {
          /* tsOrig[i] is now constant */
          HTransformSubstract(&(me->rhs),&ctt,&(me->rhs));
          DeleteTransSeq(tsOrig[i]);
        }
      else
        {
          /* non-ct tsOrig[i] -> just add it to tne new equation */
          me->ts[me->n]=tsOrig[i];
          me->n++;
        }
      HTransformDelete(&ctt);
    }
  free(tsOrig);
  
  if (me->n==0)
    {
      /* The hole matrix equation is ct */
      if (HTransformIsZero(&(me->rhs)))
        r=1; /* ct and holds */
      else
        r=2; /* ct and does not hold */
    }
  else
    r=0; /* normal case */

  me->simp=FALSE;
  SimplifyMEquation(me);
  
  return(r);
}

void ReplaceVarInMEquation(unsigned int nv,unsigned int nvNew,TMequation *me)
{
  unsigned int i;

  for(i=0;i<me->n;i++)
    ReplaceVarInTransSeq(nv,nvNew,me->ts[i]);
}

boolean SimplifiedMEquation(TMequation *me)
{
  return(me->simp);
}

void SimplifyMEquation(TMequation *me)
{
  if (me->simp)
    Error("Re-simplifying a matrix equation?");

  me->simp=TRUE;
  
  if ((me->neq>0)&&(me->n==1))
    SimplifyTransSeq(me->ts[0]);
  
  RecomputeScalarEquations(me);
}

void DeriveMEquation(unsigned int v,TMequation *dme,TMequation *me)
{
  if (!me->simp)
    Error("Deriving a non simplified equation");

  if (me->neq==0)
    //InitMEquation(dme);
    Error("Deriving an empty equation?");
  else
    {
      unsigned int i,k,n;
      TTransSeq **ts;

      dme->n=0;
      dme->simp=TRUE; /* == me->simp */
      for(i=0;i<me->n;i++)
        {
          DeriveTransSeq(v,&n,&ts,me->ts[i]);
          if (n>0)
            {
              k=dme->n+n;
              if (dme->n==0)
                { NEW(dme->ts,k,TTransSeq*); }
              else
                { MEM_EXPAND(dme->ts,k,TTransSeq*); }
              memcpy(&(dme->ts[dme->n]),ts,sizeof(TTransSeq*)*n);
              free(ts);
              dme->n=k;
            }
        }
      if (dme->n==0)
        dme->ts=NULL;

      HTransformZero(&(dme->rhs));

      dme->neq=me->neq;
      for(i=0;i<me->neq;i++)
        {
          dme->r[i]=me->r[i];
          dme->c[i]=me->c[i];
        }
    }
}

unsigned int EvaluateMEquation(double *v,double *r,TMequation *me)
{
  if (me->neq>0)
    {
      unsigned int i,j;
      THTransform a;

      for(j=0;j<me->neq;j++)
        r[j]=-HTransformGetElement(me->r[j],me->c[j],&(me->rhs));

      for(i=0;i<me->n;i++)
        {  
          EvaluateTransSeq(v,&a,me->ts[i]);
          for(j=0;j<me->neq;j++)
            r[j]+=HTransformGetElement(me->r[j],me->c[j],&a);

          HTransformDelete(&a);
        }
    }
  return(me->neq);
}

void EvaluateMEquationXVectors(double *v,unsigned int n,double *p,
                               double *r,TMequation *me)
{
  if (me->neq>0)
    {
      unsigned int i,k;
      THTransform a;
      double p1[3];

      /* The rhs is not used since this function is only used
         for derivatives (rhs=0)*/

      /* Initialize the result to zero */
      for(i=0;i<n;i++)
        {
          for(k=0;k<3;k++)
            r[3*i+k]=0;
        }
        
      /* Sum for all the terms in the matrix equation */
      for(i=0;i<me->n;i++)
        {  
          EvaluateTransSeq(v,&a,me->ts[i]);

          /* Now apply the matrix to all vectors */
          for(k=0;k<n;k++)
            {
              HTransformApply(&(p[3*k]),p1,&a);
              AccumulateVector(3,p1,&(r[3*k]));
            }
              
          HTransformDelete(&a);
        }
    }
}

void PrintMEquation(FILE *f,char **varNames,TMequation *me)
{
  unsigned int i;

  fprintf(f,"   ");
  for(i=0;i<me->n;i++)
    {
      PrintTransSeq(f,varNames,me->ts[i]);
      if (i<(me->n-1))
        fprintf(f,"+");
    } 
  fprintf(f,"=");
  if (HTransformIsIdentity(&(me->rhs)))
    fprintf(f,"Id;\n");
  else
    fprintf(f,"Z;\n");
}


void DeleteMEquation(TMequation *me)
{
  unsigned int i;

  if (me->ts!=NULL)
    {
      for(i=0;i<me->n;i++)
        {
          DeleteTransSeq(me->ts[i]);
          free(me->ts[i]);
        }
      free(me->ts);
    }
  HTransformDelete(&(me->rhs));
}