//	---------- MATRICES OPERATIONS ------------

float camG[16];
float nCamG[16];	// camera rotation mat

/* dst = transpose(src) */
// transform a row major matrix buffer in a column major matrix buffer(opengl) :)
void transposeMatrix(float *src)
{
  float dst[16];

  dst[0] = src[0];
  dst[1] = src[4];
  dst[2] = src[8];
  dst[3] = src[12];

  dst[4] = src[1];
  dst[5] = src[5];
  dst[6] = src[9];
  dst[7] = src[13];

  dst[8] = src[2];
  dst[9] = src[6];
  dst[10] = src[10];
  dst[11] = src[14];

  dst[12] = src[3];
  dst[13] = src[7];
  dst[14] = src[11];
  dst[15] = src[15];

  for(long i=0;i<16;i++) src[i] = dst[i];
}

// transpose rotation matrice
void transposeRMatrix(float *src)
{
  float dst[16];

  dst[0] = src[0];
  dst[1] = src[4];
  dst[2] = src[8];
 
  dst[4] = src[1];
  dst[5] = src[5];
  dst[6] = src[9];
 
  dst[8] = src[2];
  dst[9] = src[6];
  dst[10] = src[10];
 
  dst[12] = src[3];
  dst[13] = src[7];
  dst[14] = src[11];

  long i;
  for( i=0;i<3;i++) src[i] = dst[i];
  i++;
  for(;i<3;i++) src[i] = dst[i];
  i++;
  for(;i<3;i++) src[i] = dst[i];
}


// very important note!! : 'a' MUST(!) be a dynamic buffer!!! =|
float *matmult(float *a, float *b)
{
	float *res = new float[16];
	long i,j;


	for(i=0;i<4;i++)
	{
		for(j=0;j<4;j++)
		{
			res[j+(i<<2)] = ( a[(i<<2)] * b[j] )	 + ( a[1+(i<<2)] * b[j+4] ) +
							( a[2+(i<<2)] * b[j+8] ) + ( a[3+(i<<2)] * b[j+12] ) ;
		}
	}
	delete a;
	return res;
}


// transform point
void tPoint(float *vector, float *matrix)
{
	float x,y,z;

	x= matrix[0]*vector[0] + matrix[4]*vector[1] + matrix[8]*vector[2] + matrix[12];
	y= matrix[1]*vector[0] + matrix[5]*vector[1] + matrix[9]*vector[2] + matrix[13];
	z= matrix[2]*vector[0] + matrix[6]*vector[1] + matrix[10]*vector[2] + matrix[14];

	vector[0] = x;
	vector[1] = y;
	vector[2] = z;
}

// rotateVector
void rVector(float *vector,float *matrix)
{
	float x,y,z;

	x= matrix[0]*vector[0] + matrix[4]*vector[1] + matrix[8]*vector[2];
	y= matrix[1]*vector[0] + matrix[5]*vector[1] + matrix[9]*vector[2];
	z= matrix[2]*vector[0] + matrix[6]*vector[1] + matrix[10]*vector[2];

	vector[0] = x;
	vector[1] = y;
	vector[2] = z;

}

// inverse rotate vector
void iVector(float *vector, float *matrix)
{
	float x,y,z;

	x= matrix[0]*vector[0] + matrix[1]*vector[1] + matrix[2]*vector[2];
	y= matrix[4]*vector[0] + matrix[5]*vector[1] + matrix[6]*vector[2];
	z= matrix[8]*vector[0] + matrix[9]*vector[1] + matrix[10]*vector[2];

	vector[0] = x;
	vector[1] = y;
	vector[2] = z;
}

// distance from origin
float dPoint(float *v0)
{
	return (float)( sqrt((v0[0]*v0[0]) + (v0[1]*v0[1]) + (v0[2]*v0[2])) );
}

// calculate dot product v0.v1
float dot(float *v0, float *v1)
{
	return ( (v0[0]*v1[0]) + (v0[1]*v1[1]) + (v0[2]*v1[2]) );
}

// normaliz a vector
void nVector(float *vector)
{
	float nz = (float)(sqrt( (vector[0]*vector[0]) + (vector[1]*vector[1]) + (vector[2]*vector[2]) ));

	vector[0] /= nz;
	vector[1] /= nz;
	vector[2] /= nz;
}

void setToIdentity(float *mat)
{
	long i, j;
	i=0;
	for(long r=0;r<3;r++)
	{
		mat[i++] = 1;
		j = i+4;
		for(;i<j;i++) mat[i] = 0;
	}
	// dont forget last member ;)
	mat[15] = 1;

}

void copyMatrix(float *a, float *b)
{
	for(long i=0;i<16;i++) a[i] = b[i];
}

void fillMatrix(float aX, float aY, float aZ, float dx, float dy, float dz, float *matbuf)
{
	float *matrix_Object = new float[16];
	float matrix_Translation[16];
	float matrix_Xaxis[16];
	float matrix_Yaxis[16];
	float matrix_Zaxis[16];

	setToIdentity(matrix_Translation);
	matrix_Translation[12] = dx;
	matrix_Translation[13] = dy;
	matrix_Translation[14] = dz;

	float cosVal, sinVal;
	cosVal = (float)cos(aX);
	sinVal = (float)sin(aX);
	setToIdentity(matrix_Xaxis);
	matrix_Xaxis[5] = cosVal;
	matrix_Xaxis[6] = sinVal;
	matrix_Xaxis[9] = -sinVal;
	matrix_Xaxis[10] = cosVal;

	cosVal = (float)cos(aY);
	sinVal = (float)sin(aY);
	setToIdentity(matrix_Yaxis);
	matrix_Yaxis[0] = cosVal;
	matrix_Yaxis[2] = -sinVal;
	matrix_Yaxis[8] = sinVal;
	matrix_Yaxis[10] = cosVal;

	cosVal = (float)cos(aZ);
	sinVal = (float)sin(aZ);
	setToIdentity(matrix_Zaxis);
	matrix_Zaxis[0] = cosVal;
	matrix_Zaxis[1] = sinVal;
	matrix_Zaxis[4] = -sinVal;
	matrix_Zaxis[5] = cosVal;

	setToIdentity(matrix_Object);
	matrix_Object = matmult( matrix_Object, matrix_Translation);
	matrix_Object = matmult( matrix_Object, matrix_Yaxis);
	matrix_Object = matmult( matrix_Object, matrix_Xaxis);
	matrix_Object = matmult( matrix_Object, matrix_Zaxis);

	copyMatrix(matbuf, matrix_Object);
	delete matrix_Object;
}

//------------------------------------------------
