Biomechanical Joint Model  Author: Anderson Maciel

---

mat.h

Go to the documentation of this file.

#ifndef AMAURY_MAT_H
#define AMAURY_MAT_H

#include "vec.h"
#include <iostream> //iostream.h
#include <sstream>

namespace LinAlg{

//  using namespace std;
 
class Matrix_ 
{
public:
   typedef         double   value_type;
   typedef         double*  pointer;
   typedef         double*  iterator;
   typedef         double&  reference;
   typedef const   double*  const_iterator;
   typedef const   double&  const_reference;

   // constructors
   Matrix_();
   Matrix_(const Matrix_ &A);
   Matrix_(unsigned M, unsigned N, const double& value = double(0));
   Matrix_(unsigned M, unsigned N, const double* v);
   Matrix_(const Vector &V);
   Matrix_(unsigned M, unsigned N, const char *s);

   // destructor
   ~Matrix_();

   // methods
   Matrix_& newsize(unsigned M, unsigned N);
   void addToDiagonal(const Vector &V);
   void addToDiagonal(const double& value);
   void setBlock(const int m, const unsigned nrow, 
                 const int n, const unsigned ncol, 
                 const double& value = double(0));
   void dim(unsigned &M, unsigned &N) const { M=m_;N=n_;};
   unsigned row() const { return m_; };
   unsigned col() const { return n_; };

   void safeOutOfBounds(const bool on=true) { safe=on; };
   void setNeutral(const double& value) { neutral=value; };
   
   double det();

   // assignments
   Matrix_& operator=(const Matrix_ &A);
   Matrix_& operator=(const double& scalar);
   Matrix_& operator+=(const Matrix_ &A);
   Matrix_& operator-=(const Matrix_ &A);
   Matrix_& operator*=(const double& scalar);

    
   // access
   iterator begin() { return t_;}
   iterator end()   { return t_ + m_*n_; }
   const_iterator begin() const { return t_;}
   const_iterator end() const { return t_ + m_*n_; }
   // normal 2-index access
   reference operator()(int i, int j);
   const_reference operator()(int i, int j) const;
   // array-type index access
   reference operator[](int k);
   const_reference operator[](int k) const;

   virtual Vector operator*(const Vector &V) const;

   // basic I/O
   friend std::ostream& operator<<(std::ostream &s, const Matrix_ &A);

   // binary operators
   friend Matrix_ operator+ (const Matrix_ &A, const Matrix_ &B);
   friend Matrix_ operator- (const Matrix_ &A, const Matrix_ &B);
   friend Matrix_ operator- (const Matrix_ &A);

   friend Matrix_ operator* (const Matrix_ &A, const Matrix_ &B);
   friend Matrix_ operator* (const Matrix_ &A, double s);
   friend Matrix_ operator* (double s, const Matrix_ &A);
   friend bool operator == (const Matrix_ &A, const Matrix_ &B);
   friend bool operator != (const Matrix_ &A, const Matrix_ &B);

   // friend functions
   friend Matrix_ transpose(const Matrix_ &A);    
   friend Matrix_ inverse(const Matrix_ &A);    

protected:
   double **v_;
   double *t_;
   unsigned m_, n_;  // m rows, n cols
   bool safe;
   double neutral;        // neutral element
   void initialize(unsigned M, unsigned N);
#ifdef BOUNDS_CHECK
   void check_bounds(unsigned i, unsigned j) { assert (i>=0 && j>=0 && i<m_ && j<n_); };
#endif
   void copy(const double** v);
   void copy(const double*  t);
   void set(const double& val);
   void destroy();     
};

 // typedefs
  typedef Matrix_ MAT;

  // binary operators
  Matrix_ operator+ (const Matrix_ &A, const Matrix_ &B);
  Matrix_ operator- (const Matrix_ &A, const Matrix_ &B);
  Matrix_ operator- (const Matrix_ &A);
  
  Matrix_ operator* (const Matrix_ &A, const Matrix_ &B);
  Matrix_ operator* (const Matrix_ &A, double s);
  Matrix_ operator* (double s, const Matrix_ &A);
  bool operator == (const Matrix_ &A, const Matrix_ &B);
  bool operator != (const Matrix_ &A, const Matrix_ &B);
  
  // friend functions
  Matrix_ transpose(const Matrix_ &A);    
  Matrix_ inverse(const Matrix_ &A);  
  std::ostream& operator<<(std::ostream &s, const Matrix_ &A);  

inline  void Matrix_::copy(const double**  v)
{
   for (int i=0; i<m_; i++)
      for (int j=0; j<n_; j++)
         v_[i][j] = v[i][j];
}


inline void Matrix_::copy(const double*  t)
{
   memcpy(t_, t, m_*n_*sizeof(double));
}


inline void Matrix_::set(const double& val)
{
   for (int i=0; i<m_; i++)
      for (int j=0; j<n_; j++)
         v_[i][j] = val;
}


inline void Matrix_::destroy()
{     
   delete [] t_;
   delete [] v_;
   v_ = NULL;
   t_ = NULL;
   n_ = m_ = 0;
}

inline  void Matrix_::initialize(unsigned M, unsigned N)
{
   t_ = new double[M*N];
   v_ = new double*[M];
   for (int i=0;i<M;i++) v_[i] = &t_[i*N];
   m_ = M;
   n_ = N;
}


// restricted for the moment to 3x3 matrices

/**************[ implementation of class Matrix_ ]********************/

// constructors
inline  Matrix_::Matrix_() : t_(0), v_(0), n_(0), m_(0), safe(false), neutral(double(0))
{}


inline  Matrix_::Matrix_(const Matrix_ &A) : t_(0), v_(0), n_(0), m_(0), 
                                                   safe(A.safe), neutral(A.neutral)
{
   initialize(A.m_, A.n_);
   copy(A.t_);
}


inline  Matrix_::Matrix_(unsigned M, unsigned N, const double& value) :  t_(0), v_(0), n_(0), m_(0), 
                                                                       safe(false), neutral(double(0))
{
   initialize(M, N);
   set(value);
}


inline  Matrix_::Matrix_(unsigned M, unsigned N, const double* v) : t_(0), v_(0), n_(0), m_(0), 
                                                                  safe(false), neutral(double(0))
{
   initialize(M, N);
   copy(v);
}



inline Matrix_::Matrix_(const Vector &V) : t_(0), v_(0), n_(0), m_(0), 
                                                 safe(false), neutral(double(0))
{
   initialize(V.dim(), V.dim());
   for (int i=0; i<V.dim(); i++) 
      v_[i][i] = V[i];
}


inline Matrix_::Matrix_(unsigned M, unsigned N, const char *s)
{
   initialize(M, N);
   std::istringstream ins(s);

   for (int i=0; i<M; i++) 
      for (int j=0; j<N; j++) 
         ins >> v_[i][j];

}


inline  Matrix_::~Matrix_()
{ destroy(); }

// methods


inline Matrix_::reference Matrix_::operator()(int i, int j)
{   
   if ( safe && (i<0 || j<0 || i>=m_ || j>=n_) )
   {
     printf("Accesing matrix ellement out of matrix dimensions");
     exit (-1);
   }
#ifdef BOUNDS_CHECK
   check_bounds(i,j);
#endif
   return v_[i][j]; 
}


inline Matrix_::const_reference Matrix_::operator()(int i, int j) const
{
#ifdef BOUNDS_CHECK
   check_bounds(i,j);
#endif
   return v_[i][j]; 
}



inline  Matrix_& Matrix_::newsize(unsigned M, unsigned N)
{
   if (m_ == M && n_ == N) return *this;

   destroy();
   initialize(M,N);

   return *this;
}


inline void Matrix_::addToDiagonal(const Vector &V)
{
   assert (m_==n_ && n_==V.dim());

   for (int i=0; i<m_; i++)
      v_[i][i] += V[i];
}


inline void Matrix_::addToDiagonal(const double& value)
{
   for (int i=0; i<m_; i++)
      v_[i][i] += value;
}


inline void Matrix_::setBlock(const int m, const unsigned nrow, 
                                const int n, const unsigned ncol, const double& value)
{
   for (int i=0; i<nrow; i++)
      for (int j=0; j<ncol; j++)
         this->operator()(m+i, n+j) = value;
}


// assignments
//

inline  Matrix_& Matrix_::operator=(const Matrix_ &A)
{
   if (v_ == A.v_)
      return *this;

   if (m_ == A.m_ && n_ == A.n_)         // no need to re-alloc
      copy(A.t_);
   else
   {
      destroy();
      initialize(A.m_, A.n_);
      copy(A.t_);
   }

   return *this;
}
        

inline  Matrix_& Matrix_::operator=(const double& scalar)
{ 
   set(scalar);  
   return *this;
}


inline  Matrix_& Matrix_::operator+=(const Matrix_ &A)
{
   assert(n_==A.n_ && m_==A.m_);

   for (int i=0; i<m_; i++)
      for (int j=0; j<n_; j++)
         v_[i][j] += A.v_[i][j];
   return *this;
}


inline  Matrix_& Matrix_::operator-=(const Matrix_ &A)
{
   assert(n_==A.n_ && m_==A.m_);

   for (int i=0; i<m_; i++)
      for (int j=0; j<n_; j++)
         v_[i][j] -= A.v_[i][j];
   return *this;
}


inline  Matrix_& Matrix_::operator*=(const double& scalar)
{
   for (int i=0; i<m_; i++)
      for (int j=0; j<n_; j++)
         v_[i][j] *= scalar; 
   return *this;
}

inline Matrix_::reference Matrix_::operator[](int k)
{   
   if ( safe && (k<0 || k>=m_*n_))
   {
     printf("Accesing matrix ellement out of matrix dimensions");
     exit (-1);
   }

   int i = k / n_;
   int j = k % n_;
#ifdef BOUNDS_CHECK
   check_bounds(i,j);
#endif
   return v_[i][j]; 
}


inline Matrix_::const_reference Matrix_::operator[](int k) const
{
   int i = k / n_;
   int j = k % n_;
#ifdef BOUNDS_CHECK
   check_bounds(i,j);
#endif
   return v_[i][j]; 
}



inline  double Matrix_::det()
{
   assert(row()==3 && col()==3);

   return (
      v_[0][0]*v_[1][1]*v_[2][2]
      -v_[0][0]*v_[1][2]*v_[2][1]
      -v_[1][0]*v_[0][1]*v_[2][2]
      +v_[1][0]*v_[0][2]*v_[2][1]
      +v_[2][0]*v_[0][1]*v_[1][2]
      -v_[2][0]*v_[0][2]*v_[1][1] );   
}

// private members


inline Matrix_ inverse(const Matrix_ &A)
{
   unsigned M=A.row(), N=A.col();
   assert(M==3 && N==3);
   
   double det = 
      A.v_[0][0]*A.v_[1][1]*A.v_[2][2]
      -A.v_[0][0]*A.v_[1][2]*A.v_[2][1]
      -A.v_[1][0]*A.v_[0][1]*A.v_[2][2]
      +A.v_[1][0]*A.v_[0][2]*A.v_[2][1]
      +A.v_[2][0]*A.v_[0][1]*A.v_[1][2]
      -A.v_[2][0]*A.v_[0][2]*A.v_[1][1];
   
   assert(fabs(det) > 1E-7);  
 
   static Matrix_ tmp(3,3);

   tmp.v_[0][0] = A.v_[1][1]*A.v_[2][2] - A.v_[2][1]*A.v_[1][2];
   tmp.v_[0][1] = -A.v_[0][1]*A.v_[2][2] + A.v_[2][1]*A.v_[0][2];
   tmp.v_[0][2] = A.v_[0][1]*A.v_[1][2] - A.v_[1][1]*A.v_[0][2];

   tmp.v_[1][0] = -A.v_[1][0]*A.v_[2][2] + A.v_[2][0]*A.v_[1][2];
   tmp.v_[1][1] = A.v_[0][0]*A.v_[2][2] - A.v_[2][0]*A.v_[0][2];
   tmp.v_[1][2] = -A.v_[0][0]*A.v_[1][2] + A.v_[1][0]*A.v_[0][2];
   
   tmp.v_[2][0] = A.v_[1][0]*A.v_[2][1] - A.v_[2][0]*A.v_[1][1];
   tmp.v_[2][1] = -A.v_[0][0]*A.v_[2][1] + A.v_[2][0]*A.v_[0][1];
   tmp.v_[2][2] = A.v_[0][0]*A.v_[1][1] - A.v_[1][0]*A.v_[0][1];
   
   return (tmp*(1/det));
}

inline Matrix_ operator+(const Matrix_ &A, 
                           const Matrix_ &B)
{
   unsigned M=A.row(), N=A.col();
   assert(M==B.row() && N==B.col());

   Matrix_ tmp(M, N);

   for (int i=0; i<M; i++)
      for (int j=0; j<N; j++)
         tmp.v_[i][j] = A.v_[i][j] + B.v_[i][j];
  
   return tmp;
}


inline Matrix_ operator-(const Matrix_ &A, 
                           const Matrix_ &B)
{
   unsigned M=A.row(), N=A.col();

   assert(M==B.row() && N==B.col());
  
   Matrix_ tmp(M, N);

   for (int i=0; i<M; i++)
      for (int j=0; j<N; j++)
         tmp.v_[i][j] = A.v_[i][j] - B.v_[i][j];  

   return tmp;
}



inline Matrix_ operator-(const Matrix_ &A)
{
   unsigned M=A.row(), N=A.col();

   Matrix_ tmp(M,N);

   for (int i=0; i<M; i++)
      for (int j=0; j<N; j++)
         tmp.v_[i][j] = -A.v_[i][j];

   return tmp;
}


inline Matrix_ operator*(const Matrix_ &A, 
                           const Matrix_ &B)
{
   assert(A.col() == B.row());
   unsigned M = A.row();
   unsigned N = A.col();
   unsigned K = B.col();

   Matrix_ tmp(M,K);
   unsigned i,j,k;
   double sum; 

   for (i=0; i<M; i++)
   {
      for (k=0; k<K; k++)
      {
         for (sum=0, j=0; j<N; j++)
         {
            sum += A.v_[i][j]*B.v_[j][k];
         }        
         tmp.v_[i][k] = sum; 
      }
   }
   
   return tmp;
}


inline Matrix_ operator*(const Matrix_ &A, double s)
{
   Matrix_ tmp(A);
   tmp *= s;
   return tmp;
}


inline Matrix_ operator*(double s, const Matrix_ &A)
{
   return A*s;
}



inline  bool operator ==(const Matrix_ &A, const Matrix_ &B)
{
   unsigned M=A.row(), N=A.col();
   if (M==B.row() && N == B.col())
   {
      for (int i=0; i<M; i++)
         for (int j=0; j<N; j++)
            if (A.v_[i][j] != B.v_[i][j]) return false;
      return true;
   }
   return false;
}


inline  bool operator !=(const Matrix_ &A, const Matrix_ &B)      
{
   return !(A==B);
}


inline Matrix_ transpose(const Matrix_ &A)
{
   unsigned M=A.row(), N=A.col();
   Matrix_ tmp(N,M);

   for (int i=0; i<M; i++)
      for (int j=0; j<N; j++)
         tmp.v_[j][i] = A.v_[i][j];

   return tmp;
}

/****************************  I/O  ********************************/

inline std::ostream& operator<<(std::ostream &s, const Matrix_ &A)
{
   unsigned M=A.row(), N=A.col();

   s << M << "x" << N << std::endl;

   for (unsigned i=0; i<M; i++)
   {
      for (unsigned j=0; j<N; j++)
         s  << A.v_[i][j] << " ";
      s << std::endl;
   }
   s << std::endl;

   return s;
}

}
#endif

---