#include "ZGo.h"



//---- ZGoMATRIX

ZGoMATRIX::ZGoMATRIX(ZGo *zgo)
{
  this->zgo = zgo;

  identity();
}



//---- ZGoMATRIX
/*
ZGoMATRIX::ZGoMATRIX(ZGo *zgo, idx3d_Vector right, idx3d_Vector up, idx3d_Vector forward)
{
  this->zgo = zgo;

  identity();

	m00 = right->x;
	m10 = right->y;
	m20 = right->z;
	m01 = up->x;
	m11 = up->y;
	m21 = up->z;
	m02 = forward->x;
	m12 = forward->y;
	m22 = forward->z;
}
*/


//---- identity

void ZGoMATRIX::identity()
{
  m00 = 1; m01 = 0; m02 = 0; m03 = 0;
  m10 = 0; m11 = 1; m12 = 0; m13 = 0;
  m20 = 0; m21 = 0; m22 = 1; m23 = 0;
  m30 = 0; m31 = 0; m32 = 0; m33 = 1;
}



//---- setMatrix

void ZGoMATRIX::setMatrix(ZGoMATRIX *m)
{
  m00 = m->m00; m01 = m->m01; m02 = m->m02; m03 = m->m03;
  m10 = m->m10; m11 = m->m11; m12 = m->m12; m13 = m->m13;
  m20 = m->m20; m21 = m->m21; m22 = m->m22; m23 = m->m23;
  m30 = m->m30; m31 = m->m31; m32 = m->m32; m33 = m->m33;
}



//---- shiftMatrix

void ZGoMATRIX::shiftMatrix(float dx, float dy, float dz)
{
  identity();
	m03 = dx;
	m13 = dy;
	m23 = dz;
}



//---- scaleMatrix

void ZGoMATRIX::scaleMatrix(float dx, float dy, float dz)
{
  identity();
	m00 = dx;
	m11 = dy;
	m22 = dz;
}



//---- scaleMatrix

void ZGoMATRIX::scaleMatrix(float d)
{
  identity();
	m00 = d;
	m11 = d;
	m22 = d;
}



//---- rotateXMatrix

void ZGoMATRIX::rotateXMatrix(float a)
{
  identity();

	double s = sin(a);
	double c = cos(a);

	m11 = c;
	m12 = s;
	m21 = -s;
	m22 = c;
}



//---- rotateYMatrix

void ZGoMATRIX::rotateYMatrix(float a)
{
  identity();

	double s = sin(a);
	double c = cos(a);

	m00 = c;
	m02 = s;
	m20 = -s;
	m22 = c;
}



//---- rotateZMatrix

void ZGoMATRIX::rotateZMatrix(float a)
{
  identity();

	double s = sin(a);
	double c = cos(a);

	m00 = c;
	m01 = s;
	m10 = -s;
	m11 = c;
}



//---- transform

void ZGoMATRIX::transform(ZGoMATRIX *src, ZGoMATRIX *dest)
{
	dest->m00 = src->m00*m00 + src->m01*m10 + src->m02*m20;
	dest->m01 = src->m00*m01 + src->m01*m11 + src->m02*m21;
	dest->m02 = src->m00*m02 + src->m01*m12 + src->m02*m22;
	dest->m03 = src->m00*m03 + src->m01*m13 + src->m02*m23 + src->m03;
	dest->m10 = src->m10*m00 + src->m11*m10 + src->m12*m20;
	dest->m11 = src->m10*m01 + src->m11*m11 + src->m12*m21;
	dest->m12 = src->m10*m02 + src->m11*m12 + src->m12*m22;
	dest->m13 = src->m10*m03 + src->m11*m13 + src->m12*m23 + src->m13;
	dest->m20 = src->m20*m00 + src->m21*m10 + src->m22*m20;
	dest->m21 = src->m20*m01 + src->m21*m11 + src->m22*m21;
	dest->m22 = src->m20*m02 + src->m21*m12 + src->m22*m22;
	dest->m23 = src->m20*m03 + src->m21*m13 + src->m22*m23 + src->m23;			
}



//---- preTransform

void ZGoMATRIX::preTransform(ZGoMATRIX *src, ZGoMATRIX *dest)
{
	dest->m00 = m00*src->m00 + m01*src->m10 + m02*src->m20;
	dest->m01 = m00*src->m01 + m01*src->m11 + m02*src->m21;
	dest->m02 = m00*src->m02 + m01*src->m12 + m02*src->m22;
	dest->m03 = m00*src->m03 + m01*src->m13 + m02*src->m23 + m03;
	dest->m10 = m10*src->m00 + m11*src->m10 + m12*src->m20;
	dest->m11 = m10*src->m01 + m11*src->m11 + m12*src->m21;
	dest->m12 = m10*src->m02 + m11*src->m12 + m12*src->m22;
	dest->m13 = m10*src->m03 + m11*src->m13 + m12*src->m23 + m13;
	dest->m20 = m20*src->m00 + m21*src->m10 + m22*src->m20;
	dest->m21 = m20*src->m01 + m21*src->m11 + m22*src->m21;
	dest->m22 = m20*src->m02 + m21*src->m12 + m22*src->m22;
	dest->m23 = m20*src->m03 + m21*src->m13 + m22*src->m23 + m23;			
}



//---- inverse

void ZGoMATRIX::inverse(ZGoMATRIX *m)
{
	float q1  = m12;      float q6  = m10*m01;  float q7  = m10*m21;  float q8  = m02;  
	float q13 = m20*m01;  float q14 = m20*m11;  float q21 = m02*m21;  float q22 = m03*m21;  
	float q25 = m01*m12;  float q26 = m01*m13;  float q27 = m02*m11;  float q28 = m03*m11;  
	float q29 = m10*m22;  float q30 = m10*m23;  float q31 = m20*m12;  float q32 = m20*m13;  
	float q35 = m00*m22;  float q36 = m00*m23;  float q37 = m20*m02;  float q38 = m20*m03;  
	float q41 = m00*m12;  float q42 = m00*m13;  float q43 = m10*m02;  float q44 = m10*m03;  
	float q45 = m00*m11;  float q48 = m00*m21;  

	float q49 = q45*m22 - q48*q1 - q6*m22 + q7*q8;
	float q50 = q13*q1 - q14*q8;

	float q51 = 1/(q49+q50);
		
	m->m00 = (m11*m22*m33-m11*m23*m32-m21*m12*m33+m21*m13*m32+m31*m12*m23-m31*m13*m22)*q51;
	m->m01 = -(m01*m22*m33-m01*m23*m32-q21*m33+q22*m32)*q51;
	m->m02 = (q25*m33-q26*m32-q27*m33+q28*m32)*q51;
	m->m03 = -(q25*m23-q26*m22-q27*m23+q28*m22+q21*m13-q22*m12)*q51;
	m->m10 = -(q29*m33-q30*m32-q31*m33+q32*m32)*q51;
	m->m11 = (q35*m33-q36*m32-q37*m33+q38*m32)*q51;
	m->m12 = -(q41*m33-q42*m32-q43*m33+q44*m32)*q51;
	m->m13 = (q41*m23-q42*m22-q43*m23+q44*m22+q37*m13-q38*m12)*q51;
	m->m20 = (q7*m33-q30*m31-q14*m33+q32*m31)*q51;
	m->m21 = -(q48*m33-q36*m31-q13*m33+q38*m31)*q51;
	m->m22 = (q45*m33-q42*m31-q6*m33+q44*m31)*q51;
	m->m23 = -(q45*m23-q42*m21-q6*m23+q44*m21+q13*m13-q38*m11)*q51;
}



//---- end of file -----------------------------------------------------------//