/************************************************************
 *															*
 *						matrix.c							*
 *			   											    *															*
 *				LPGE     1997								*		
 *															*
 *	Copyright (C) 1996 Sony Computer Entertainment Inc.		*
 *	All Rights Reserved										*
 *															*
 ***********************************************************/
 
 

/****************************************************************************
					includes
****************************************************************************/


 
#include "matrix.h"




/****************************************************************************
					macros
****************************************************************************/



#define setVECTOR(vector, x, y, z)				\
				(vector)->vx = (x), (vector)->vy = (y), (vector)->vz = (z)	




/****************************************************************************
					functions
****************************************************************************/






void InitMatrix (MATRIX* matrix)
{
	matrix->m[0][0] = ONE;
	matrix->m[0][1] = 0;
	matrix->m[0][2] = 0;
	matrix->m[1][0] = 0;
	matrix->m[1][1] = ONE;
	matrix->m[1][2] = 0;
	matrix->m[2][0] = 0;
	matrix->m[2][1] = 0;
	matrix->m[2][2] = ONE;

	matrix->t[0] = 0;
	matrix->t[1] = 0;
	matrix->t[2] = 0;
}


  



void CopyMatrix (MATRIX* from, MATRIX* to)
{
	assert(from != to);

	to->m[0][0] = from->m[0][0];
	to->m[0][1] = from->m[0][1];
	to->m[0][2] = from->m[0][2];
	to->m[1][0] = from->m[1][0];
	to->m[1][1] = from->m[1][1];
	to->m[1][2] = from->m[1][2];
	to->m[2][0] = from->m[2][0];
	to->m[2][1] = from->m[2][1];
	to->m[2][2] = from->m[2][2];

	to->t[0] = from->t[0];
	to->t[1] = from->t[1];
	to->t[2] = from->t[2];
}
	



void ExpressSuperPointInSubMatrix (VECTOR* subPoint, 
			MATRIX *matrix, VECTOR* superPointOutput)
{
	setVECTOR(superPointOutput, 
				((subPoint->vx * matrix->m[0][0]
				+ subPoint->vy * matrix->m[0][1]
				+ subPoint->vz * matrix->m[0][2]) >> 12),

				((subPoint->vx * matrix->m[1][0]
				+ subPoint->vy * matrix->m[1][1]
				+ subPoint->vz * matrix->m[1][2]) >> 12),

				((subPoint->vx * matrix->m[2][0]
				+ subPoint->vy * matrix->m[2][1]
				+ subPoint->vz * matrix->m[2][2]) >> 12) );
}



	
	
	// eg know position or vector in object space,
	// want to find it in world coordinate terms
	
	// NOTE: Only takes account of rotation
	// NOT displacement as well			   

void ExpressSubPointInSuperMatrix (VECTOR* superPoint, 
			MATRIX *matrix, VECTOR* subPointOutput)
{
	setVECTOR(subPointOutput, 
				((superPoint->vx * matrix->m[0][0]
				+ superPoint->vy * matrix->m[1][0]
				+ superPoint->vz * matrix->m[2][0]) >> 12),

				((superPoint->vx * matrix->m[0][1]
				+ superPoint->vy * matrix->m[1][1]
				+ superPoint->vz * matrix->m[2][1]) >> 12),

				((superPoint->vx * matrix->m[0][2]
				+ superPoint->vy * matrix->m[1][2]
				+ superPoint->vz * matrix->m[2][2]) >> 12) );
}



	// translation part NOT altered

void GetMatrixFlippedOnDiagonal (MATRIX *input, MATRIX *output)
{
	assert(input != output);

	output->m[0][0] = input->m[0][0];
	output->m[0][1] = input->m[1][0];
	output->m[0][2] = input->m[2][0];
	output->m[1][0] = input->m[0][1];
	output->m[1][1] = input->m[1][1];
	output->m[1][2] = input->m[2][1];
	output->m[2][0] = input->m[0][2];
	output->m[2][1] = input->m[1][2];
	output->m[2][2] = input->m[2][2];

	output->t[0] = input->t[0];
	output->t[1] = input->t[1];
	output->t[2] = input->t[2];
}