room101/MATRIX.HPP

#ifndef __MATRIX_H
#define __MATRIX_H

#include "vector.hpp"
#include <string.h>
/*******************************************************************************
** MatrixCopy
*******************************************************************************/
inline void
MatrixCopy(Matrix dst, Matrix src)
{
   memcpy(dst, src, sizeof(Matrix));
}
/*******************************************************************************
** MatrixZero
*******************************************************************************/
inline void
MatrixZero(Matrix dst)
{
   Matrix src = 
   {
      {0.0f, 0.0f, 0.0f, 0.0f},
      {0.0f, 0.0f, 0.0f, 0.0f},
      {0.0f, 0.0f, 0.0f, 0.0f},      
      {0.0f, 0.0f, 0.0f, 0.0f}
   };
   MatrixCopy(dst, src);
}
/*******************************************************************************
** MatrixIdentity
*******************************************************************************/
inline void
MatrixIdentity(Matrix dst)
{
   Matrix src = 
   {
      {1.0f, 0.0f, 0.0f, 0.0f},
      {0.0f, 1.0f, 0.0f, 0.0f},
      {0.0f, 0.0f, 1.0f, 0.0f},      
      {0.0f, 0.0f, 0.0f, 1.0f}
   };
   MatrixCopy(dst, src);
}
/*******************************************************************************
** MatrixScale
*******************************************************************************/
inline void
MatrixScale(Matrix dst, float x, float y, float z)
{
   MatrixIdentity(dst);
   dst[0][0] = x;
   dst[1][1] = y;
   dst[2][2] = z;
}
/*******************************************************************************
** MatrixTranslate
*******************************************************************************/
inline void
MatrixTranslate(Matrix dst, float x, float y, float z)
{
   MatrixIdentity(dst);
   dst[0][3] = x;
   dst[1][3] = y;
   dst[2][3] = z;
}
/*******************************************************************************
** MatrixRotateX
*******************************************************************************/
inline void
MatrixRotateX(Matrix dst, float a)
{
   a *= DTOR;
   MatrixIdentity(dst);
   dst[1][1] =   (dst[2][2] = (float)cos(a));
   dst[1][2] = - (dst[2][1] = (float)sin(a));  
}
/*******************************************************************************
** MatrixRotateY
*******************************************************************************/
inline void
MatrixRotateY(Matrix dst, float a)
{
   a *= DTOR;   
   MatrixIdentity(dst);
   dst[0][0] =   (dst[2][2] = (float)cos(a));
   dst[0][2] = - (dst[2][0] = (float)sin(a));
}
/*******************************************************************************
** MatrixRotateZ
*******************************************************************************/
inline void
MatrixRotateZ(Matrix dst, float a)
{
   a *= DTOR;   
   MatrixIdentity(dst);
   dst[0][0] =   (dst[1][1] = (float)cos(a));
   dst[0][1] = - (dst[1][0] = (float)sin(a));  
}
/*******************************************************************************
** MatrixTranspose
*******************************************************************************/
inline void
MatrixTranspose(Matrix dst, Matrix src)
{
   for (int y = 0; y < 4; y++)
   {
      for (int x = 0; x < 4; x++)
      {
         dst[x][y] = src[y][x];
      }
   }
}
/*******************************************************************************
** MatrixMultiply
*******************************************************************************/
inline void
MatrixMultiply(Matrix dst, Matrix ma1, Matrix ma2)
{
   Matrix tmp;
   
   for (int i = 0; i < 4; i++)
   {
      for (int j = 0; j < 4; j++)
      {
         tmp[i][j] = 0.0f;
         for (int k = 0; k < 4; k++)
         {
            tmp[i][j] += ma2[i][k] * ma1[k][j];
         }
      }
   }
   MatrixCopy(dst, tmp);
}
/*******************************************************************************
** MatrixMultiply
*******************************************************************************/
inline void
MatrixMultiply(Matrix dst, Matrix ma1, Matrix ma2, Matrix ma3)
{
   Matrix tmp;
   MatrixMultiply(tmp, ma1, ma2);
   MatrixMultiply(dst, tmp, ma3);
}
/*******************************************************************************
** MatrixInvert
*******************************************************************************/
/*
inline void
MatrixInvert(Matrix dst, Matrix src)
{
   Matrix A, B;

   MatrixIdentity(A);
   
   for (int y = 0; y < 3; y++)
   {
      for (int x = 0; x < 3; x++)
      {
         A[x][y] = src[y][x];
      }
   }
   MatrixTranslate(B, -src[0][3], -src[1][3], -src[2][3]);

   MatrixMultiply(dst, B, A);
}
*/
/*******************************************************************************
** MatrixRotateXYZ
*******************************************************************************/
inline void
MatrixRotateXYZ(Matrix dst, Vector XYZ)
{
   Matrix xrot, yrot, zrot;

   MatrixRotateX(xrot, XYZ[0]);
   MatrixRotateY(yrot, XYZ[1]);
   MatrixRotateZ(zrot, XYZ[2]);

   MatrixMultiply(dst, zrot, yrot, xrot);   
}
/*******************************************************************************
** MatrixNormalize
*******************************************************************************/
inline void
MatrixNormalize(Matrix dst, Matrix src)
{
   VectorNormalize(dst[0], src[0]);
   VectorNormalize(dst[1], src[1]);
   VectorNormalize(dst[2], src[2]);      
}
/*******************************************************************************
** MatrixView
*******************************************************************************/
inline void
MatrixView(Matrix dst, Vector dir, Vector up, float angle)
{
   Matrix rotate;

   MatrixRotateZ(rotate, angle);

   MatrixIdentity(dst);
   
   v3dcpy(dst[2], dir);
   
   VectorCrossProduct(dst[0], up, dst[2]);
   
   if (EpsilonEqual(VectorMagnitude(dst[0]), 0.0f)) VectorCreate(dst[0], 1.0f, 0.0f, 0.0f);
   
   VectorCrossProduct(dst[1], dst[2], dst[0]);
      
   MatrixNormalize(dst, dst);

   MatrixMultiply(dst, dst, rotate);
}

#endif