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


 
#include "coord.h"



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




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





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

	




void CopyCoordinateSystem (GsCOORDINATE2* from, GsCOORDINATE2* to)
{
	assert(from != to);

	to->super = from->super;

	CopyMatrix( &from->coord, &to->coord);

	to->flg = 0;
}




	



	// eg know position or vector in world space,
	// want to find it in object coordinate terms

	// NOTE: Only takes account of rotation
	// NOT displacement as well

void ExpressSuperPointInSub (VECTOR* subPoint, 
			GsCOORDINATE2* subSystem, VECTOR* superPointOutput)
{
	MATRIX* matrix;

	matrix = &subSystem->coord;

	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 ExpressSubPointInSuper (VECTOR* superPoint, 
			GsCOORDINATE2* subSystem, VECTOR* subPointOutput)
{
	MATRIX* matrix;

	matrix = &subSystem->coord;

	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) );
}


 	





	// rotating around x and y only: theta-Z always zero
	// rotation order: always y then x

void DeriveNewCoordSystemFromRotation (GsCOORDINATE2* original,
			SVECTOR* rotation, GsCOORDINATE2* output)
{
	SVECTOR realRotation;
	MATRIX rotationMatrix;
	SVECTOR xVector, yVector;

		// z component of rotation has no place in cylindrical tunnel
	setVECTOR( &realRotation, rotation->vx, rotation->vy, 0);

	if (realRotation.vx == 0)		 // just a theta-Y rotation
		{
		RotMatrix(&realRotation, &rotationMatrix);
		MulMatrix0(&original->coord, &rotationMatrix, &output->coord);
		}
	else
		{
		if (realRotation.vy == 0)		 // just a theta-X rotation
			{
			RotMatrix(&realRotation, &rotationMatrix);
			MulMatrix0(&original->coord, &rotationMatrix, &output->coord);
			}
		else	 // compound rotation: first by Y, then by X
			{
			setVECTOR( &yVector, 0, realRotation.vy, 0);	   // just y rotation
			RotMatrix(&yVector, &rotationMatrix);
			MulMatrix0(&original->coord, &rotationMatrix, &output->coord);

			setVECTOR( &xVector, realRotation.vx, 0, 0);	  // just x rotation
			RotMatrix(&xVector, &rotationMatrix);
			MulMatrix0(&output->coord, &rotationMatrix, &output->coord);
			}
		}

		// tell GTE that coordinate system has been updated
	output->flg = 0;
}






	// self-relative rotation, order is zyx

void RotateCoordinateSystem (GsCOORDINATE2* original,
			SVECTOR* rotation, GsCOORDINATE2* output)
{
	MATRIX xMatrix, yMatrix, zMatrix;
	SVECTOR xVector, yVector, zVector;

	CopyCoordinateSystem(original, output);

	if (rotation->vz != 0)
		{
		setVECTOR( &zVector, 0, 0, rotation->vz);
		RotMatrix( &zVector, &zMatrix);
		MulMatrix0(&output->coord, &zMatrix, &output->coord);
		}
	if (rotation->vy != 0)
		{
		setVECTOR( &yVector, 0, rotation->vy, 0);
		RotMatrix( &yVector, &yMatrix);
		MulMatrix0(&output->coord, &yMatrix, &output->coord);
		}
	if (rotation->vx != 0)
		{
		setVECTOR( &xVector, rotation->vx, 0, 0);
		RotMatrix( &xVector, &xMatrix);
		MulMatrix0(&output->coord, &xMatrix, &output->coord);
		}

		// tell GTE that coordinate system has been updated
	output->flg = 0;
}




	// ALSO NEED HERE: convert world coords to screen coords
	// get translation and rotation of the view,
	// express in view coord system, adjust for ProjectionDistance