Biomechanical Joint Model  Author: Anderson Maciel

---

hmat.h

Go to the documentation of this file.

#ifndef AMAURY_HMAT_H
#define AMAURY_HMAT_H

#include "mat.h"


namespace LinAlg{

  // HMAT: homgoneous matrix; internal representation is column-vector, 3x4.
  // conversion to/from Scenelib, Inventor is easy with defined operators.
  //
class HMAT : public MAT
{
public:
   typedef float   MatriX[4][4]; // nicely mirrors Scenelib's geom definition

   // constructors
   HMAT(const HMAT &A);
   explicit HMAT(const REAL& value = REAL(0));
   HMAT(const MatriX mat);
   HMAT(const MAT &rot, const VEC &trans);
   
   static HMAT getIdentity();
   void setIdt() { set(0); MAT::addToDiagonal(1); };

   VEC getColumn(int i) const;
   VEC getRow(int i) const;
   VEC getTranslation() const;
   
   void setColumn(int i, const VEC &V);
   void setRow(int i, const VEC &V);
   void setTranslation(const VEC &trans);

   // make a rotation matrix that maps v0 onto v1
   static void makeVecRotVec(HMAT &mat, const VEC &v0, const VEC &v1);

   virtual VEC operator*(const VEC &V) const; // point multiplication
   VEC fullMult(const VEC &V) const;          // point multiplication
   VEC mult(const VEC &V) const;              // vector multiplication
   operator MatriX&();

   friend HMAT inverse(const HMAT &A);
private:
   MatriX mx;
};

// constructors

inline HMAT::HMAT(const REAL& value) : MAT(3,4,value) {}

inline HMAT::HMAT(const HMAT &A) : MAT(A) {}

inline HMAT::HMAT(const MatriX mat) : MAT() 
{
   initialize(3,4);
   for (int i=0;i<3;i++)
      for (int j=0;j<4;j++)
         v_[i][j]=mat[j][i];  // transpose because of switch from row-vector to column-vector
}
   
inline HMAT::HMAT(const MAT &rot, const VEC &trans) : MAT() 
{
   int i;
   initialize(3,4);

   // rotation
   for (i=0;i<3;i++)
      for (int j=0;j<3;j++)
         v_[i][j]=rot(i,j);

   // translation
   for (i=0; i<3; i++)
      v_[i][3] = trans[i];
}

inline HMAT 
HMAT::getIdentity()
{
   HMAT Id;
   Id.addToDiagonal(1);
   return Id;
}

inline VEC 
HMAT::getColumn(int i) const
{
#ifdef BOUNDS_CHECK
   assert (i>=0 && i<4);
#endif
   static VEC V(3);
   V[0] = v_[0][i];
   V[1] = v_[1][i];
   V[2] = v_[2][i];
   return V;
}

inline VEC HMAT::getRow(int i) const
{
#ifdef BOUNDS_CHECK
   assert (i>=0 && i<3);
#endif
   static VEC V(3);
   V[0] = v_[i][0];
   V[1] = v_[i][1];
   V[2] = v_[i][2];
   return V;
}

inline VEC HMAT::getTranslation() const 
{ 
   return getColumn(3); 
}

inline void HMAT::setColumn(int i, const VEC &V)
{
#ifdef BOUNDS_CHECK
   assert (i>=0 && i<4);
   assert (V.dim() == 3 || V.dim() == 4);
#endif
   v_[0][i] = V[0];
   v_[1][i] = V[1];
   v_[2][i] = V[2];
}

inline void HMAT::setRow(int i, const VEC &V)
{
#ifdef BOUNDS_CHECK
   assert (i>=0 && i<3);
   assert (V.dim() == 3 || V.dim() == 4);
#endif
   v_[i][0] = V[0];
   v_[i][1] = V[1];
   v_[i][2] = V[2];   
}

inline void HMAT::setTranslation(const VEC &trans)
{
   setColumn(3, trans);
}


inline VEC HMAT::fullMult(const VEC &V) const
{
   return this->operator*(V);
}

inline VEC HMAT::mult(const VEC &V) const
{
   assert (V.dim()==3);
   static VEC tmp(3);
   for (int i=0; i<3; i++)
   {
      REAL sum=0;
      for (int j=0;j<3;j++)
         sum += V[j] * v_[i][j];          
      tmp[i]=sum;
   }
   return tmp;
}

inline HMAT::operator HMAT::MatriX&()
{   
   mx[0][3]=mx[1][3]=mx[2][3]=0; mx[3][3]=1;
   for (int i=0;i<4;i++)
      for (int j=0;j<3;j++)
         mx[i][j]=v_[j][i];  
   return mx;
}

   
inline HMAT inverse(const HMAT &A)
{
   static MAT  rot(3,3);

   // inverse rotation part (take matrix's transpose)
   for (int i=0; i<3; i++)
      for (int j=0; j<3; j++)
         rot(j,i) = A.v_[i][j];

   // negate translation
   static VEC tmp(3);
   tmp = -A.getTranslation();
   // rotate translational part
   
   return HMAT(rot, rot*tmp);
}

}
#endif

---