/////////////////////////////////////////////////////////////////////////
//                                                                     //
// This file contains functions to generate rotation matrices.         //
//                                                                     //
/////////////////////////////////////////////////////////////////////////
#ifndef __ROTATIONMATRIX_HPP__
#define __ROTATIONMATRIX_HPP__


#include <cmath>
#include "CSquareMatrix.hpp"




// This function generates a rotation matrix from an axis and an
// angle.
template <typename T, int N>
inline void rotationMatrix(T angle, T axeX, T axeY, T axeZ,
                           CSquareMatrix<T,N>& matrix)
{
  T angleCos    = cos(angle);
  T angleSin    = sin(angle);
  T oneMinusCos = 1 - angleCos;

  matrix(0,0) = angleCos + oneMinusCos * axeX * axeX;
  matrix(0,1) = oneMinusCos * axeX * axeY - axeZ * angleSin;
  matrix(0,2) = oneMinusCos * axeX * axeZ + axeY * angleSin;

  matrix(1,0) = oneMinusCos * axeX * axeY + axeZ * angleSin;
  matrix(1,1) = angleCos + oneMinusCos * axeY * axeY;
  matrix(1,2) = oneMinusCos * axeY * axeZ - axeX * angleSin;

  matrix(2,0) = oneMinusCos * axeX * axeZ - axeY * angleSin;
  matrix(2,1) = oneMinusCos * axeY * axeZ + axeX * angleSin;
  matrix(2,2) = angleCos + oneMinusCos * axeZ * axeZ;
}




// This function generates a rotation 4x4 matrix from an axis and an
// angle.
template <typename T>
inline void rotationMatrix(T angle, T axeX, T axeY, T axeZ,
                           CSquareMatrix<T,4>& matrix)
{
  T angleCos    = cos(angle);
  T angleSin    = sin(angle);
  T oneMinusCos = 1 - angleCos;

  matrix(0,0) = angleCos + oneMinusCos * axeX * axeX;
  matrix(0,1) = oneMinusCos * axeX * axeY - axeZ * angleSin;
  matrix(0,2) = oneMinusCos * axeX * axeZ + axeY * angleSin;
  matrix(0,3) = 0;

  matrix(1,0) = oneMinusCos * axeX * axeY + axeZ * angleSin;
  matrix(1,1) = angleCos + oneMinusCos * axeY * axeY;
  matrix(1,2) = oneMinusCos * axeY * axeZ - axeX * angleSin;
  matrix(1,3) = 0;

  matrix(2,0) = oneMinusCos * axeX * axeZ - axeY * angleSin;
  matrix(2,1) = oneMinusCos * axeY * axeZ + axeX * angleSin;
  matrix(2,2) = angleCos + oneMinusCos * axeZ * axeZ;
  matrix(2,3) = 0;

  matrix(3,0) = 0;
  matrix(3,1) = 0;
  matrix(3,2) = 0;
  matrix(3,3) = 1;
}


#endif // __ROTATIONMATRIX_HPP__
