#include <iostream>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

class CMatrix4x4f
{
public: // attributes

  float m_vector[16];

  CMatrix4x4f()
  {
    SetIdentity();
  }

  ~CMatrix4x4f()
  {
  }

  void SetIdentity()
  {
    memset(m_vector, 0x00, 16*sizeof(float));
    m_vector[0] = m_vector[5] = m_vector[10] = m_vector[15]= 1.0f;
  }

  void Rotate(float angle, float x, float y, float z)
  {
    float u[3];
    float norma = x*x+y*y+z*z;
    if (norma != 0.0f)
    {
      norma = (float)(1.0/sqrt(norma));
      u[0] = x*norma;
      u[1] = y*norma;
      u[2] = z*norma;
    }
    else
      u[0] = u[1] = u[2] = 0.0f;
    float seno   = (float)sin(angle);
    float coseno = (float)cos(angle);

    float aux;
    float sen_x_u[3];
    sen_x_u[0] = seno*u[0];
    sen_x_u[1] = seno*u[1];
    sen_x_u[2] = seno*u[2];

    aux = u[0]*u[0];  m_vector[0] = aux + coseno * (1.0f - aux);
    aux = u[0]*u[1];  m_vector[1] = aux - coseno * aux - sen_x_u[2];
    aux = u[0]*u[2];  m_vector[2] = aux - coseno * aux + sen_x_u[1];

    aux = u[1]*u[0];  m_vector[4] = aux - coseno * aux + sen_x_u[2];
    aux = u[1]*u[1];  m_vector[5] = aux + coseno * (1.0f - aux);
    aux = u[1]*u[2];  m_vector[6] = aux - coseno * aux - sen_x_u[0];

    aux = u[2]*u[0];  m_vector[8] = aux - coseno * aux - sen_x_u[1];
    aux = u[2]*u[1];  m_vector[9] = aux - coseno * aux + sen_x_u[0];
    aux = u[2]*u[2];  m_vector[10]= aux + coseno * (1.0f - aux);

    m_vector[3] = m_vector[7] = m_vector[11] = m_vector[12]= m_vector[13] = m_vector[14] = 0.0f;
    m_vector[15] = 1.0f;
  }

  void PreMult(CMatrix4x4f & leftMatrix)
  {
    CMatrix4x4f res;
    for (int i=0; i<4; i++)
      for (int j=0; j<4; j++)
      {
        res[i][j] = 0;
        for (int k=0; k<4; k++)
          res[i][j] += m_vector[(k<<2)+j]*leftMatrix[i][k];
      }
    *this = res;
  }

  float* operator [](int i)
  {
    return &m_vector[i<<2];
  }

  CMatrix4x4f& Transpose()
  {
    for (int i=0; i<4; i++)
      for (int j=i+1; j<4; j++)
      {
        float aux = m_vector[(i<<2)+j];
        m_vector[(i<<2)+j] = m_vector[(j<<2)+i];
        m_vector[(j<<2)+i] = aux;
      }
    return *this;
  }

};