#include "math33.h"


void M4I( Mat4 m )
{
  m[0] = 1.0; m[1] =   0; m[2] =    0; m[3] =    0;
  m[4] =   0; m[5] = 1.0; m[6] =    0; m[7] =    0;
  m[8] =   0; m[9] =   0; m[10] = 1.0; m[11] =   0;
  m[12] =  0; m[13] =  0; m[14] =   0; m[15] = 1.0;
}

void V4xMt( Vec4 dst, Vec4 v, Mat4 m )
{
  dst[0] = (v[0] * m[0]) + (v[1] * m[4]) + (v[2] * m[8]) + (v[3] * m[12]);
  dst[1] = (v[0] * m[1]) + (v[1] * m[5]) + (v[2] * m[9]) + (v[3] * m[13]);
  dst[2] = (v[0] * m[2]) + (v[1] * m[6]) + (v[2] * m[10]) + (v[3] * m[14]);
  dst[3] = (v[0] * m[3]) + (v[1] * m[7]) + (v[2] * m[11]) + (v[3] * m[15]);
}

void MxM4( Mat4 a, Mat4 b )
{
  Mat4 t = {
    a[0], a[1], a[2], a[3],
    a[4], a[5], a[6], a[7],
    a[8], a[9], a[10], a[11],
    a[12], a[13], a[14], a[15]
  };

  a[ 0] = (t[ 0] * b[ 0]) + (t[ 1] * b[ 4]) + (t[ 2] * b[ 8]) + (t[ 3] * b[12]);
  a[ 1] = (t[ 0] * b[ 1]) + (t[ 1] * b[ 5]) + (t[ 2] * b[ 9]) + (t[ 3] * b[13]);
  a[ 2] = (t[ 0] * b[ 2]) + (t[ 1] * b[ 6]) + (t[ 2] * b[10]) + (t[ 3] * b[14]);
  a[ 3] = (t[ 0] * b[ 3]) + (t[ 1] * b[ 7]) + (t[ 2] * b[11]) + (t[ 3] * b[15]);

  a[ 4] = (t[ 4] * b[ 0]) + (t[ 5] * b[ 4]) + (t[ 6] * b[ 8]) + (t[ 7] * b[12]);
  a[ 5] = (t[ 4] * b[ 1]) + (t[ 5] * b[ 5]) + (t[ 6] * b[ 9]) + (t[ 7] * b[13]);
  a[ 6] = (t[ 4] * b[ 2]) + (t[ 5] * b[ 6]) + (t[ 6] * b[10]) + (t[ 7] * b[14]);
  a[ 7] = (t[ 4] * b[ 3]) + (t[ 5] * b[ 7]) + (t[ 6] * b[11]) + (t[ 7] * b[15]);

  a[ 8] = (t[ 8] * b[ 0]) + (t[ 9] * b[ 4]) + (t[10] * b[ 8]) + (t[11] * b[12]);
  a[ 9] = (t[ 8] * b[ 1]) + (t[ 9] * b[ 5]) + (t[10] * b[ 9]) + (t[11] * b[13]);
  a[10] = (t[ 8] * b[ 2]) + (t[ 9] * b[ 6]) + (t[10] * b[10]) + (t[11] * b[14]);
  a[11] = (t[ 8] * b[ 3]) + (t[ 9] * b[ 7]) + (t[10] * b[11]) + (t[11] * b[15]);

  a[12] = (t[12] * b[ 0]) + (t[13] * b[ 4]) + (t[14] * b[ 8]) + (t[15] * b[12]);
  a[13] = (t[12] * b[ 1]) + (t[13] * b[ 5]) + (t[14] * b[ 9]) + (t[15] * b[13]);
  a[14] = (t[12] * b[ 2]) + (t[13] * b[ 6]) + (t[14] * b[10]) + (t[15] * b[14]);
  a[15] = (t[12] * b[ 3]) + (t[13] * b[ 7]) + (t[14] * b[11]) + (t[15] * b[15]);
}

void VxP( Mat4 v, Mat4 p )
{
  Mat4 t = {
    v[0], v[1], v[2], v[3],
    v[4], v[5], v[6], v[7],
    v[8], v[9], v[10], v[11],
    v[12], v[13], v[14], v[15] };

  v[ 0] = t[ 0] * p[ 0];
  v[ 1] = t[ 1] * p[ 5];
  v[ 2] = t[ 2] * p[10] + t[ 3] * p[14];
  v[ 3] = t[ 2];

  v[ 4] = t[ 4] * p[ 0];
  v[ 5] = t[ 5] * p[ 5];
  v[ 6] = t[ 6] * p[10] + t[ 7] * p[14];
  v[ 7] = t[ 6];

  v[ 8] = t[ 8] * p[ 0];
  v[ 9] = t[ 9] * p[ 5];
  v[10] = t[10] * p[10] + t[11] * p[14];
  v[11] = t[10];

  v[12] = t[12] * p[ 0];
  v[13] = t[13] * p[ 5];
  v[14] = t[14] * p[10] + t[15] * p[14];
  v[15] = t[14];
}

void math33_create_translation_matrix( f64 x, f64 y, f64 z, Mat4 m )
{
  m[0] = 1.0; m[1] =   0; m[2] =    0; m[3] =    0;
  m[4] =   0; m[5] = 1.0; m[6] =    0; m[7] =    0;
  m[8] =   0; m[9] =   0; m[10] = 1.0; m[11] =   0;
  m[12] =  x; m[13] =  y; m[14] =   z; m[15] = 1.0;
}

void math33_create_view_rotation_matrix( f64 x, f64 y, Mat4 m )
{
  f64 sx, sy, cx, cy;

  sx = sin(x);  
  sy = sin(y);  
  cx = cos(x);  
  cy = cos(y);  

  Mat4 Rx = {
    1.0,   0,  0,   0,
      0,  cx, sx,   0,
      0, -sx, cx,   0,
      0,   0,  0, 1.0
  };

  m[ 0] = cy; m[ 1] =   0; m[ 2] = -sy; m[ 3] =   0;
  m[ 4] =  0; m[ 5] = 1.0; m[ 6] =   0; m[ 7] =   0;
  m[ 8] = sy; m[ 9] =   0; m[10] =  cy; m[11] =   0;
  m[12] =  0; m[13] =   0; m[14] =   0; m[15] = 1.0;

  MxM4(m, Rx);
}

void math33_create_view_matrix( Vec3 t, Vec3 r, Mat4 m )
{
  Mat4 rot;

  math33_create_translation_matrix(-t[0], -t[1], -t[2], m);
  math33_create_view_rotation_matrix(r[0], r[1], rot);
  MxM4(m, rot);
}

void math33_create_rotation_matrix( f64 x, f64 y, f64 z, Mat4 m )
{
  f64 sx, sy, sz, cx, cy, cz;

  sx = sin(x);  
  sy = sin(y);  
  sz = sin(z);  
  cx = cos(x);  
  cy = cos(y);  
  cz = cos(z);

  m[ 0] = 1.0; m[ 1] =   0; m[ 2] =  0; m[ 3] = 0;
  m[ 4] =   0; m[ 5] =  cx; m[ 6] = sx; m[ 7] = 0;
  m[ 8] =   0; m[ 9] = -sx; m[10] = cx; m[11] = 0;
  m[12] =   0; m[13] =   0; m[14] =  0; m[15] = 1.0;

  Mat4 Ry = {
     cy,   0, sy,   0,
      0, 1.0,  0,   0,
    -sy,   0, cy,   0,
      0,   0,  0, 1.0
  };

  Mat4 Rz = {
    cz, -sz,   0,   0,
    sz,  cz,   0,   0,
     0,   0, 1.0,   0,
     0,   0,   0, 1.0
  };

  MxM4(m, Ry);
  MxM4(m, Rz);
}

void math33_create_scale_matrix( f64 x, f64 y, f64 z, Mat4 m )
{
  m[0] =  x; m[1] =  0; m[2] =  0; m[3] =    0;
  m[4] =  0; m[5] =  y; m[6] =  0; m[7] =    0;
  m[8] =  0; m[9] =  0; m[10] = z; m[11] =   0;
  m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1.0;
}

#include "strings33.h"

void math33_create_world_matrix( Vec3 t, Vec3 r, Vec3 s, Mat4 m )
{
  Mat4 rot;


  math33_create_scale_matrix(s[0], s[1], s[2], m);
  math33_create_rotation_matrix(r[0], r[1], r[2], rot);
  MxM4(m, rot);
  math33_create_translation_matrix(t[0], t[1], t[2], rot);
  MxM4(m, rot);
}