PhxMatrix.h

Go to the documentation of this file.

#ifndef PHX_MATRIX_H
#define PHX_MATRIX_H

#include <Phx/PhxConfig.h>
#include <Phx/PhxTypes.h>
#include <Phx/Core/PhxCore.h>
#include <Phx/Util/PhxLog.h>
#include <math.h>
#include <iostream>
#include <sstream>

namespace Phx
{

  template <uint16_t, uint16_t, typename> class BasicMatrix;

  template <uint16_t Rows, uint16_t Cols, typename value_type>
  class MatrixRow
  {
  private:
    MatrixRow()
    {
      // doesn't initialize!!!!!!!
    }
  public:
    value_type operator[](uint16_t col) const
    {
      if (col < Cols) { return mData[col]; }
      std::ostringstream oss;
      oss << "Invalid column index " << col;
      Core::systemLog()->newMessage("MatrixRow", PHX_ERROR, oss.str());
      return mData[0];
    }
    value_type& operator[](uint16_t col)
    {
      if (col < Cols) { return mData[col]; }
      std::ostringstream oss;
      oss << "Invalid column index " << col;
      Core::systemLog()->newMessage("MatrixRow", PHX_ERROR, oss.str());
      return mData[0];
    }
  private:
    value_type mData[Cols];

    template <uint16_t Rows2, uint16_t Cols2, typename value_type2>
    friend class BasicMatrix;
  };

  template <uint16_t Rows, uint16_t Cols, typename value_type>
  class BasicIndexMatrix : public BasicMatrix<Rows,Cols,value_type>
  {
    typedef BasicMatrix<Rows,Cols,value_type> BaseType;
  protected:
    BasicIndexMatrix(bool dummy) : BaseType(dummy) { }
  public:
    BasicIndexMatrix() { }
    BasicIndexMatrix(const value_type* data) : BaseType(data) {}

    const MatrixRow<Rows,Cols,value_type>& operator[](uint16_t r) const
    {
      if (r < Rows)
      {
        return BaseType::mData[r];
      }
      std::ostringstream oss;
      oss << "Invalid row index " << r;
      Core::systemLog()->newMessage("BasicIndexMatrix", PHX_ERROR, oss.str());
      return BaseType::mData[0];
    }
    MatrixRow<Rows,Cols,value_type>& operator[](uint16_t r)
    {
      if (r < Rows)
      {
        return BaseType::mData[r];
      }
      std::ostringstream oss;
      oss << "Invalid row index " << r;
      Core::systemLog()->newMessage("BasicIndexMatrix", PHX_ERROR, oss.str());
      return BaseType::mData[0];
    }
  };

  template <uint16_t Rows, typename value_type>
  class BasicIndexMatrix<Rows,1,value_type> :
        public BasicMatrix<Rows,1,value_type>
  {
    typedef BasicMatrix<Rows,1,value_type> BaseType;
  protected:
    BasicIndexMatrix(bool dummy) : BaseType(dummy) {}
  public:
    BasicIndexMatrix() { }
    BasicIndexMatrix(const value_type* data) : BaseType(data) {}

    value_type operator[](uint16_t r) const
    {
      if (r < Rows)
      {
        return BaseType::mData[r][0];
      }
      std::ostringstream oss;
      oss << "Invalid vector index " << r;
      Core::systemLog()->newMessage("BasicIndexMatrix", PHX_ERROR, oss.str());
      return BaseType::mData[0][0];
    }
    value_type& operator[](uint16_t r)
    {
      if (r < Rows)
      {
        return BaseType::mData[r][0];
      }
      std::ostringstream oss;
      oss << "Invalid vector index " << r;
      Core::systemLog()->newMessage("BasicIndexMatrix", PHX_ERROR, oss.str());
      return BaseType::mData[0][0];
    }

    value_type norm(void) const {
      return sqrt(norm2());
    }

    value_type norm2(void) const {
      double sum = 0;
      for (uint16_t i = 0; i < Rows; i++) {
        sum += BaseType::mData[i][0]*BaseType::mData[i][0];
      }
      return sum;
    }
  };

  template <uint16_t Rows, uint16_t Cols, typename value_type = double>
  class Matrix : public BasicIndexMatrix<Rows, Cols, value_type>
  {
    typedef BasicIndexMatrix<Rows, Cols, value_type>  BaseType;
  public:
    Matrix() : BaseType() { }
    Matrix(const value_type* data) : BaseType(data) {}}
  ;

  template <typename value_type>
  class Matrix<4,1, value_type> : public BasicIndexMatrix<4,1, value_type>
  {
    typedef BasicIndexMatrix<4, 1, value_type>  BaseType;
  public:
    Matrix() { }
    Matrix(const value_type* data) : BaseType(data) {}
    Matrix(value_type a0, value_type a1, value_type a2, value_type a3) : BaseType(false)
    {
      BaseType::mData[0][0] = a0;
      BaseType::mData[1][0] = a1;
      BaseType::mData[2][0] = a2;
      BaseType::mData[3][0] = a3;
    }

    void normalize()
    {
      double t = sqrt(BaseType::mData[0][0]*BaseType::mData[0][0] + BaseType::mData[1][0]*BaseType::mData[1][0] + BaseType::mData[2][0]*BaseType::mData[2][0] + BaseType::mData[3][0]*BaseType::mData[3][0]);

      BaseType::mData[0][0] = BaseType::mData[0][0]/t;
      BaseType::mData[1][0] = BaseType::mData[1][0]/t;
      BaseType::mData[2][0] = BaseType::mData[2][0]/t;
      BaseType::mData[3][0] = BaseType::mData[3][0]/t;
    }
  };

  template <typename value_type>
  class Matrix<3,1, value_type> : public BasicIndexMatrix<3,1, value_type>
  {
    typedef BasicIndexMatrix<3, 1, value_type>  BaseType;
  public:
    Matrix() { }
    Matrix(const value_type* data) : BaseType(data) {}
    Matrix(value_type a0, value_type a1, value_type a2) : BaseType(false)
    {
      BaseType::mData[0][0] = a0;
      BaseType::mData[1][0] = a1;
      BaseType::mData[2][0] = a2;
    }

    void normalize()
    {
      double t = sqrt(BaseType::mData[0][0]*BaseType::mData[0][0] + BaseType::mData[1][0]*BaseType::mData[1][0] + BaseType::mData[2][0]*BaseType::mData[2][0]);
      BaseType::mData[0][0] = BaseType::mData[0][0]/t;
      BaseType::mData[1][0] = BaseType::mData[1][0]/t;
      BaseType::mData[2][0] = BaseType::mData[2][0]/t;
    }
    
    value_type x(){return BaseType::mData[0][0];}
    value_type y(){return BaseType::mData[1][0];}
    value_type z(){return BaseType::mData[2][0];}
  };

  template <typename value_type>
  class Matrix<2,1, value_type> : public BasicIndexMatrix<2,1, value_type>
  {
    typedef BasicIndexMatrix<2, 1, value_type>  BaseType;
  public:
    Matrix() { }
    Matrix(const value_type* data) : BaseType(data) {}
    Matrix(value_type a0, value_type a1) : BaseType(false)
    {
      BaseType::mData[0][0] = a0;
      BaseType::mData[1][0] = a1;
    }

    void normalize()
    {
      double t = sqrt(BaseType::mData[0][0]*BaseType::mData[0][0] + BaseType::mData[1][0]*BaseType::mData[1][0]);
      BaseType::mData[0][0] = BaseType::mData[0][0]/t;
      BaseType::mData[1][0] = BaseType::mData[1][0]/t;
    }
  };

  template <typename value_type>
  class Matrix<1,1, value_type> : public BasicIndexMatrix<1,1, value_type>
  {
    typedef BasicIndexMatrix<1, 1, value_type>  BaseType;
  public:
    Matrix() { }
    Matrix(const value_type* data) : BaseType(data) {}

    // explicit conversion from value_type
    explicit Matrix(value_type a0) : BaseType(false)
    { // don't initialize
      BaseType::mData[0][0] = a0;
    }

    // implicit conversion to value_type
    operator value_type() const
    {
      return BaseType::mData[0][0];
    }

    const Matrix<1,1, value_type>&
    operator=(const Matrix<1,1, value_type>& m)
    {
      BaseType::operator=(m);
      return *this;
    }

    double operator=(double a0)
    {
      BaseType::mData[0][0] = a0;
      return BaseType::mData[0][0];
    }
  };

  template <typename value_type>
  class Matrix<2,2, value_type> : public BasicIndexMatrix<2,2, value_type>
  {
    typedef BasicIndexMatrix<2, 2, value_type>  BaseType;
  public:
    Matrix() { }
    Matrix(const value_type* data) : BaseType(data) {}
    Matrix(value_type a0, value_type a1,
           value_type b0, value_type b1) : BaseType(false)
    {
      BaseType::mData[0][0]=a0; BaseType::mData[0][1]=a1;
      BaseType::mData[1][0]=b0; BaseType::mData[1][1]=b1;
    }

    value_type determinant()
    {
      return BaseType::mData[0][0]*BaseType::mData[1][1]-BaseType::mData[0][1]*BaseType::mData[1][0];
    }

    Matrix<2,2,value_type> inverse(double tolerance=0.0005)
    {
      Matrix<2,2,value_type> mr;
      value_type det = determinant();
      if(fabs(det)>tolerance)
      {
        mr[0][0] = BaseType::mData[1][1]/det;
        mr[0][1] = -BaseType::mData[0][1]/det;
        mr[1][0] = -BaseType::mData[1][0]/det;
        mr[1][1] = BaseType::mData[0][0]/det;
      }else{
        Core::systemLog()->newMessage("BasicIndexMatrix", PHX_ERROR, 
                                      "Unable to compute inverse.");
      }
      return mr;
    }
  };

  template <typename value_type>
  class Matrix<3,3, value_type> : public BasicIndexMatrix<3,3, value_type>
  {
    typedef BasicIndexMatrix<3, 3, value_type>  BaseType;
  public:
    Matrix() { }
    Matrix(const value_type* data) : BaseType(data) {}
    Matrix(value_type a0, value_type a1, value_type a2,
           value_type b0, value_type b1, value_type b2,
           value_type c0, value_type c1, value_type c2) : BaseType(false)
    {
      BaseType::mData[0][0]=a0; BaseType::mData[0][1]=a1; BaseType::mData[0][2]=a2;
      BaseType::mData[1][0]=b0; BaseType::mData[1][1]=b1; BaseType::mData[1][2]=b2;
      BaseType::mData[2][0]=c0; BaseType::mData[2][1]=c1; BaseType::mData[2][2]=c2;
    }

    value_type determinant()
    {
      return 
BaseType::mData[0][0] * BaseType::mData[1][1] * BaseType::mData[2][2] + BaseType::mData[0][1] * BaseType::mData[1][2] * BaseType::mData[2][0] + BaseType::mData[0][2] * BaseType::mData[1][0] * BaseType::mData[2][1] - BaseType::mData[0][0] * BaseType::mData[1][2] * BaseType::mData[2][1] -
BaseType::mData[0][1] * BaseType::mData[1][0] * BaseType::mData[2][2] -
BaseType::mData[0][2] * BaseType::mData[1][1] * BaseType::mData[2][0];
    }

    Matrix<3,3,value_type> inverse(double tolerance=0.0005)
    {
      Matrix<3,3,value_type> mr;
      value_type det = determinant();
      if(fabs(det)>tolerance)
      {
        mr[0][0] =  ( BaseType::mData[1][1]*BaseType::mData[2][2] - BaseType::mData[1][2]*BaseType::mData[2][1] ) / det;
        mr[0][1] =  ( BaseType::mData[2][1]*BaseType::mData[0][2] - BaseType::mData[0][1]*BaseType::mData[2][2] ) / det;
        mr[0][2] =  ( BaseType::mData[0][1]*BaseType::mData[1][2] - BaseType::mData[1][1]*BaseType::mData[0][2] ) / det;
        mr[1][0] =  ( BaseType::mData[1][2]*BaseType::mData[2][0] - BaseType::mData[1][0]*BaseType::mData[2][2] ) / det;
        mr[1][1] =  ( BaseType::mData[0][0]*BaseType::mData[2][2] - BaseType::mData[2][0]*BaseType::mData[0][2] ) / det;
        mr[1][2] =  ( BaseType::mData[1][0]*BaseType::mData[0][2] - BaseType::mData[0][0]*BaseType::mData[1][2] ) / det;
        mr[2][0] =  ( BaseType::mData[1][0]*BaseType::mData[2][1] - BaseType::mData[2][0]*BaseType::mData[1][1] ) / det;
        mr[2][1] =  ( BaseType::mData[2][0]*BaseType::mData[0][1] - BaseType::mData[0][0]*BaseType::mData[2][1] ) / det;
        mr[2][2] =  ( BaseType::mData[0][0]*BaseType::mData[1][1] - BaseType::mData[0][1]*BaseType::mData[1][0] ) / det;
      }else{
        Core::systemLog()->newMessage("BasicIndexMatrix", PHX_ERROR, 
                                      "Unable to create inverse.");
      }
      return mr;
    }
  };

  template <typename value_type>
  class Matrix<4,4, value_type> : public BasicIndexMatrix<4,4, value_type>
  {
    typedef BasicIndexMatrix<4, 4, value_type>  BaseType;
  private:
    Matrix<3,3,value_type> submat(int i, int j)
    {
      Matrix<3,3,value_type> sm;
      int si, sj;
      for(int di=0;di<3;di++)
      {
        for(int dj=0; dj<3; dj++)
        {
          si = di + ( ( di >= i ) ? 1 : 0 );
          sj = dj + ( ( dj >= j ) ? 1 : 0 );

          sm[di][dj] = BaseType::mData[si][sj];
        }
      }
      return sm;
    }

  public:
    Matrix() { }
    Matrix(const value_type* data) : BaseType(data) {}
    Matrix(value_type a0, value_type a1, value_type a2, value_type a3,
           value_type b0, value_type b1, value_type b2, value_type b3,
           value_type c0, value_type c1, value_type c2, value_type c3,
           value_type d0, value_type d1, value_type d2, value_type d3) : BaseType(false)
    {
      BaseType::mData[0][0]=a0; BaseType::mData[0][1]=a1; BaseType::mData[0][2]=a2; BaseType::mData[0][3]=a3;
      BaseType::mData[1][0]=b0; BaseType::mData[1][1]=b1; BaseType::mData[1][2]=b2; BaseType::mData[1][3]=b3;
      BaseType::mData[2][0]=c0; BaseType::mData[2][1]=c1; BaseType::mData[2][2]=c2; BaseType::mData[2][3]=c3;
      BaseType::mData[3][0]=d0; BaseType::mData[3][1]=d1; BaseType::mData[3][2]=d2; BaseType::mData[3][3]=d3;
    }

    value_type determinant()
    {
      Matrix<3,3,value_type> msub3;
      value_type result=0,det;
      int i=1;
      for ( int n = 0; n < 4; n++, i *= -1 )
      {

        msub3   = submat( 0, n );
        det     = msub3.determinant();
        result += BaseType::mData[0][n] * det * i;
      }
      return result;
    }

    Matrix<4,4,value_type> inverse(double tolerance=0.0005)
    {
      value_type det=determinant();
      Matrix<3,3,value_type> mtemp;
      Matrix<4,4,value_type> mr;
      if(fabs(det)>tolerance)
      {
        int sign;
        for ( int i = 0; i < 4; i++ )
          for ( int j = 0; j < 4; j++ )
          {
            sign = 1 - ( (i +j) % 2 ) * 2;
            mtemp = submat( i, j );
            mr[(i+j*4)/4][(i+j*4)%4] = ( mtemp.determinant() * sign ) / det;
          }
      }else{
        Core::systemLog()->newMessage("BasicIndexMatrix", PHX_ERROR, 
                                      "Unable to compute inverse.");
      }
      return mr;
    }
  };


  template <uint16_t Rows, uint16_t Cols, typename value_type = double>
  class BasicMatrix
  {
  protected:
    MatrixRow<Rows,Cols,value_type> mData[Rows]; // data in row major order

    // Constructs uninitialized matrix.
    BasicMatrix(bool dummy) {}
  public:
    BasicMatrix()
    { // constructs zero matrix
      for (uint16_t i = 0; i < Rows; ++i)
      {
        for (uint16_t j = 0; j < Cols; ++j)
        {
          mData[i][j] = 0;
        }
      }
    }
    BasicMatrix(const value_type* data)
    { // constructs from array
      if (!data)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR,
                                      "Constructing matrix from 0 array.");
        throw RangeException();
      }

      for (uint16_t i = 0; i < Rows; ++i)
      {
        for (uint16_t j = 0; j < Cols; ++j)
        {
          mData[i][j] = data[i*Cols + j];
        }
      }
    }

    uint32_t rows() const { return Rows; }
    uint32_t columns() const { return Cols; }

    value_type element(uint16_t row, uint16_t col) const
    {
      if (row >= rows() || col >= columns())
      {
        std::ostringstream oss;
        oss << "Illegal read access: " << row << ", " << col 
        << " in " << Rows << "x" << Cols << " matrix.";
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, oss.str());
        return mData[0][0];
      }
      return mData[row][col];
    }

    value_type& element(uint16_t row, uint16_t col)
    {
      if (row >= rows() || col >= columns())
      {
        std::ostringstream oss;
        oss << "Illegal read access: " << row << ", " << col << " in " 
        << Rows << "x" << Cols << " matrix.";
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, oss.str());
        return mData[0][0];
      }
      return mData[row][col];
    }

    void element(uint16_t row, uint16_t col, value_type value)
    {
      if (row >= rows() || col >= columns())
      {
        std::ostringstream oss;
        oss << "Illegal write access: " << row << ", " << col
            << " in " << Rows << "x" << Cols << " matrix.";
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, oss.str());
        return ;
      }
      mData[row][col] = value;
    }

    template <uint16_t RowRangeSize, uint16_t ColRangeSize>
    Matrix<RowRangeSize, ColRangeSize, value_type>
    subMatrix(const Matrix<1, RowRangeSize, uint16_t>& rowRange,
              const Matrix<1, ColRangeSize, uint16_t>& colRange)
    {
      Matrix<RowRangeSize, ColRangeSize, value_type> result;
      for (uint32_t i = 0; i < RowRangeSize; i++)
      {
        for (uint32_t j = 0; j < ColRangeSize; j++)
        {
          result.element(i,j, element(rowRange.element(0, i), colRange.element(0,j)));
        }
      }
      return result;
    }

    template <uint32_t RowRangeSize, uint32_t ColRangeSize>
    Matrix<RowRangeSize, ColRangeSize, value_type>
    operator()(const Matrix<1, RowRangeSize, uint16_t>& rowRange,
               const Matrix<1, ColRangeSize, uint16_t>& colRange)
    {
      return subMatrix(rowRange, colRange);
    }

    template <uint16_t RhsCols>
    Matrix<Rows, RhsCols, value_type> operator*(const Matrix<Cols, RhsCols, value_type>& rhs) const
    {
      Matrix<Rows, RhsCols, value_type> result;
      for (uint32_t i = 0; i < Rows; i++)
      {
        for (uint32_t j = 0; j < RhsCols; j++)
        {
          for (uint32_t k = 0; k < Cols; k++)
          {
            result.element(i,j) += element(i,k)*rhs.element(k,j);
          }
        }
      }
      return result;
    }

    Matrix<Rows, Cols, value_type> operator+(const Matrix<Rows, Cols, value_type>& rhs) const
    {
      Matrix<Rows, Cols, value_type> result;
      for (uint32_t i = 0;  i < Rows;  i++)
      {
        for (uint32_t j = 0;  j < Cols;  j++)
        {
          result.mData[i][j] = mData[i][j] + rhs.data()[i][j];
        }
      }
      return result;
    }

    Matrix<Rows, Cols, value_type> operator-(const Matrix<Rows, Cols, value_type>& rhs) const
    {
      Matrix<Rows, Cols, value_type> result;
      for (uint32_t i = 0;  i < Rows;  i++)
      {
        for (uint32_t j = 0;  j < Cols;  j++)
        {
          result.mData[i][j] = mData[i][j] - rhs.data()[i][j];
        }
      }
      return result;
    }

    Matrix<Rows, Cols, value_type> operator*(value_type s) const
    {
      Matrix<Rows, Cols, value_type> result;
      for (uint16_t i = 0;  i < Rows;  i++)
      {
        for (uint16_t j = 0;  j < Cols;  j++)
        {
          result.mData[i][j] = mData[i][j] * s;
        }
      }
      return result;
    }

    Matrix<Rows, Cols, value_type> operator/(value_type s) const
    {
      Matrix<Rows, Cols, value_type> result;
      for (uint16_t i = 0;  i < Rows;  i++)
      {
        for (uint16_t j = 0;  j < Cols;  j++)
        {
          result.mData[i][j] = mData[i][j] / s;
        }
      }
      return result;
    }

    Matrix<Rows, Cols, value_type> operator-(void) const
    {
      Matrix<Rows, Cols, value_type> result;
      for (uint16_t i = 0;  i < Rows;  i++)
      {
        for (uint16_t j = 0;  j < Cols;  j++)
        {
          result.mData[i][j] = -mData[i][j];
        }
      }
      return result;
    }

    Matrix<Cols, Rows, value_type> transpose(void) const
    {
      Matrix<Cols, Rows, value_type> result;
      for (uint16_t i = 0;  i < Rows;  i++)
      {
        for (uint16_t j = 0;  j < Cols;  j++)
        {
          result.element(j,i, element(i,j));
        }
      }
      return result;
    }

    static Matrix<Rows, Cols, value_type> identity()
    {
      Matrix<Rows, Cols, value_type> id;
      for (uint16_t i = 0; i < Rows && i < Cols; i++)
      {
        id.element(i,i) = 1;
      }
      return id;
    }

    static Matrix<Rows, Cols, value_type> zero()
    {
      return Matrix<Rows, Cols, value_type>();
    }

    MatrixRow<Rows,Cols,value_type>* data() { return mData; }
    const MatrixRow<Rows,Cols,value_type>* data() const { return mData; }

    static Matrix<Rows, Cols, value_type> rotationMatrixX(double rad)
    {
      Matrix<Rows, Cols, value_type> rotMatrix;
      if(Rows != Cols)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                      "Can't create rotation matrix for non-square matrix.");
        return rotMatrix;
      }
      if(Rows<3)
        {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                      "Matrix is too small for rotation.");
        return rotMatrix;
      }
      rotMatrix[0][0]=1; rotMatrix[0][1]=0; rotMatrix[0][2]=0;
      rotMatrix[1][0]=0; rotMatrix[1][1]=cos(rad); rotMatrix[1][2]=-sin(rad);
      rotMatrix[2][0]=0; rotMatrix[2][1]=sin(rad); rotMatrix[2][2]=cos(rad);

      //Set the rest of the diagonal to 1
      for(int i=3;i<Rows;i++)
      {
        rotMatrix[i][i]=1;
      }
      return rotMatrix;
    }

    static Matrix<Rows, Cols, value_type> rotationMatixY(double rad)
    {
      Matrix<Rows, Cols, value_type> rotMatrix;
      if(Rows != Cols)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR,
                                      "Can't create rotation matrix from non-square matrix.");
        return rotMatrix;
      }
      if(Rows<3)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, "Matrix is too small for rotation.");
        return rotMatrix;
      }
      rotMatrix[0][0]=cos(rad); rotMatrix[0][1]=0; rotMatrix[0][2]=sin(rad);
      rotMatrix[1][0]=0; rotMatrix[1][1]=1; rotMatrix[1][2]=0;
      rotMatrix[2][0]=-sin(rad); rotMatrix[2][1]=0; rotMatrix[2][2]=cos(rad);

      //Set the rest of the diagonal to 1
      for(int i=3;i<Rows;i++)
      {
        rotMatrix[i][i]=1;
      }
      return rotMatrix;
    }

    static Matrix<Rows, Cols, value_type> rotationMatixZ(double rad)
    {
      Matrix<Rows, Cols, value_type> rotMatrix;
      if(Rows != Cols)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                     "Can't create rotation matrix for non-square matrix");
        return rotMatrix;
      }
      if(Rows<2)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                     "Matrix is too small for rotation.");
        return rotMatrix;
      }
      rotMatrix[0][0]=cos(rad); rotMatrix[0][1]=-sin(rad);
      rotMatrix[1][0]=sin(rad); rotMatrix[1][1]=cos(rad);

      //Set the rest of the diagonal to 1
      for(int i=2;i<Rows;i++)
      {
        rotMatrix[i][i]=1;
      }
      return rotMatrix;
    }

    static Matrix<Rows, Cols, value_type> rotationMatix(double roll, double pitch, double yaw)
    {
      Matrix<Rows, Cols, value_type> rotMatrix;

      if(Rows != Cols)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                     "Can't create rotation matrix for non-square matrix");
        return rotMatrix;
      }
      if(Rows<3)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR,
                                     "Matrix is too small for rotation.");
        return rotMatrix;
      }

      double cr = cos(roll);
      double sr = sin(roll);

      double cp = cos(pitch);
      double sp = sin(pitch);

      double cy = cos(yaw);
      double sy = sin(yaw);

      //used more than once.
      double srsp = sr*sp;
      double crsp = cr*sp;

      rotMatrix[0][0]=cp*cy;
      rotMatrix[0][1]=-cp*sy;
      rotMatrix[0][2]=sp;
      rotMatrix[1][0]=srsp*cy+cr*sy;
      rotMatrix[1][1]=-srsp*sy+cr*cy;
      rotMatrix[1][2]=-sr*cp;
      rotMatrix[2][0]=-crsp*cy+sr*sy;
      rotMatrix[2][1]=crsp*sy+sr*cy;
      rotMatrix[2][2]=cr*cp;

      //Set the rest of the diagonal to 1
      for(int i=3;i<Rows;i++)
      {
        rotMatrix[i][i]=1;
      }
      return rotMatrix;
    }

    static Matrix<Rows, Cols, value_type> rotationMatix(Matrix<3,1> axis, double rad)
    {
      Matrix<Rows, Cols, value_type> rotMatrix;

      if(Rows != Cols)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                     "Can't create rotation matrix for non-square matrix.");
        return rotMatrix;
      }
      if(Rows<3)
      {
        Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                     "Matrix is too small for rotation.");
        return rotMatrix;
      }

      axis.normalize();
      double s = sin( rad/2 );
      double w = cos( rad/2 );

      double x = axis[0]*s;
      double y = axis[1]*s;
      double z = axis[2]*s;

      rotMatrix[0][0]=1-2*y*y-2*z*z;rotMatrix[0][1]=2*x*y - 2*z*w ;rotMatrix[0][2]=2*x*z + 2*y*w;
      rotMatrix[1][0]=2*x*y + 2*z*w;rotMatrix[1][1]= 1-2*x*x-2*z*z;rotMatrix[1][2]=2*y*z - 2*x*w;
      rotMatrix[2][0]=2*x*z - 2*y*w;rotMatrix[2][1]= 2*y*z + 2*x*w;rotMatrix[2][2]=1-2*x*x-2*y*y;

      //Set the rest of the diagonal to 1
      for(int i=3;i<Rows;i++)
      {
        rotMatrix[i][i]=1;
      }
      return rotMatrix;
    }

  private:

    void print(std::ostream& os) const
    {
      for (uint16_t i = 0; i < Rows; i++)
      {
        for (uint16_t j = 0; j < Cols; j++)
        {
          os.width(11);
          os.precision(11);
          os << element(i,j) << " ";
        }
        os << std::endl;
      }
    }

    void input(std::istream& is)
    {
      for (uint16_t i = 0; i < Rows; i++)
      {
        for (uint16_t j = 0; j < Cols; j++)
        {
          if (is.good())
          {
            is >> element(i,j);
          }
          else
          {
            Core::systemLog()->newMessage("BasicMatrix", PHX_ERROR, 
                                         "Unable to read matrix from input stream.");
            throw IOException();
          }
        }
      }
    }

    template <uint16_t Rows2, uint16_t Cols2, typename value_type2>
    friend Matrix<Rows2,Cols2,value_type2> operator*(double s, const Matrix<Rows2, Cols2, value_type2>& m);

    template <uint16_t Rows2, uint16_t Cols2, typename value_type2>
    friend std::ostream& operator<<(std::ostream& os, const Matrix<Rows2, Cols2, value_type2>& rhs);

    template <uint16_t Rows2, uint16_t Cols2, typename value_type2>
    friend std::istream& operator>>(std::istream& is, Matrix<Rows2, Cols2, value_type2>& rhs);
  };

  typedef Matrix<2,2>  Matrix2;
  typedef Matrix<3,3>  Matrix3;
  typedef Matrix<4,4>  Matrix4;
  typedef Matrix<2,1>  Vector2;
  typedef Matrix<3,1>  Vector3;
  typedef Matrix<4,1>  Vector4;

  // left-side scalar multiply
  template <uint16_t Rows, uint16_t Cols, typename value_type>
  Matrix<Rows,Cols,value_type> operator*(double s, const Matrix<Rows, Cols, value_type>& m)
  {
    return m * s;
  }

  // input / output operators
  template <uint16_t Rows, uint16_t Cols, typename value_type>
  std::ostream& operator<<(std::ostream& os, const Matrix<Rows, Cols, value_type>& rhs)
  {
    rhs.print(os);
    return os;
  }

  template <uint16_t Rows, uint16_t Cols, typename value_type>
  std::istream& operator>>(std::istream& is, Matrix<Rows, Cols, value_type>& rhs)
  {
    rhs.input(is);
    return is;
  }

}
; // namespace Phx


#endif /* PHX_MATRIX_H */

---