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





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



#include "vector.h"

#include "trig.h"



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

   

	 


int SizeOfVector (VECTOR* vector)
{
	int size, square;

	assert (abs(vector->vx) <= MAX_VECTOR_ELEMENT_SIZE
		&& abs(vector->vy) <= MAX_VECTOR_ELEMENT_SIZE
		&& abs(vector->vz) <= MAX_VECTOR_ELEMENT_SIZE);

	square = (vector->vx * vector->vx)
				+ (vector->vy * vector->vy)
				+ (vector->vz * vector->vz); 

	size = pow(square, 0.5);
	
	return size;
}







void ScaleVectorToUnit (VECTOR* vector)
{
	int size;

	size = SizeOfVector(vector);
	assert(size != 0);			  // never allow division by zero

	vector->vx *= ONE;
	vector->vy *= ONE;
	vector->vz *= ONE;

	vector->vx /= size;
	vector->vy /= size;
	vector->vz /= size;
}





	// use this to SET size absolutely: NOT a multiply-vector function
void ScaleVectorToSize (VECTOR* vector, int size)
{
	int actualSize;

	actualSize = SizeOfVector(vector);
	assert(actualSize != 0);	   // never allow division by zero

	vector->vx *= size;
	vector->vy *= size;
	vector->vz *= size;

	vector->vx /= actualSize;
	vector->vy /= actualSize;
	vector->vz /= actualSize;
}






	// scaling arg: unit is ONE (4096)
	// takes no account of previous vector size
void ScaleVector (VECTOR* vector, int relativeScale)
{
	vector->vx *= relativeScale;
	vector->vy *= relativeScale;
	vector->vz *= relativeScale;

	vector->vx /= ONE;
	vector->vy /= ONE;
	vector->vz /= ONE;
}

  

  


	

	// chop vector into two pieces;
	// firstPortion expresses the ratio of fristpart to original,
	// scaled up by 4096
void SplitVectorInTwo (VECTOR* original, int firstPortion, 
					VECTOR* firstPart, VECTOR* secondPart)
{
	int secondPortion;

	assert(firstPortion >= 0 && firstPortion <= ONE);
	secondPortion = ONE - firstPortion;

	setVECTOR(firstPart,
			original->vx,
			original->vy,
			original->vz);
	ScaleVector(firstPart, firstPortion);

	setVECTOR(secondPart,
			original->vx,
			original->vy,
			original->vz);
	ScaleVector(secondPart, secondPortion);
}		  








void ResolveVector (VECTOR* original, VECTOR* resolver,
						VECTOR* parallelPart, VECTOR* orthogonalPart)
{
	int sizeOfResolverSquared;
	int dotProduct;
	int smallest, scaleFactor;

	sizeOfResolverSquared = (resolver->vx * resolver->vx)
							+ (resolver->vy * resolver->vy)
							+ (resolver->vz * resolver->vz);
	assert(sizeOfResolverSquared > 0); 
		
   	dotProduct = (original->vx * resolver->vx)
				+ (original->vy * resolver->vy)
				+ (original->vz * resolver->vz); 

		// scaling to prevent integer overflow
	if (abs(dotProduct) > abs(sizeOfResolverSquared))
		smallest = abs(sizeOfResolverSquared);
	else
		smallest = abs(dotProduct);

		 if (smallest > 1000000)
		scaleFactor = 200000;
	else if (smallest > 100000)
		scaleFactor = 20000;
	else if (smallest > 10000)
		scaleFactor = 2000;
	else if (smallest > 1000)
		scaleFactor = 200;
	else if (smallest > 100)
		scaleFactor = 20;
	else if (smallest > 10)
		scaleFactor = 2;
	else
		scaleFactor = 1;
		

	dotProduct /= scaleFactor;
	sizeOfResolverSquared /= scaleFactor;

	setVECTOR(parallelPart,
			((resolver->vx * dotProduct) / sizeOfResolverSquared),
			((resolver->vy * dotProduct) / sizeOfResolverSquared),
			((resolver->vz * dotProduct) / sizeOfResolverSquared) );

	setVECTOR(orthogonalPart, 
				original->vx - parallelPart->vx,
				original->vy - parallelPart->vy,
				original->vz - parallelPart->vz);
}







	

	// wrapper
int SizeOfVector2 (VECTOR *vector)
{
	int result;

	result = (int) jbVectorMeasure(vector);

	return result;
}


	// use this to SET size absolutely: NOT a multiply-vector function
void ScaleVectorToSize2 (VECTOR* vector, int size)
{
	int actualSize;

	actualSize = SizeOfVector2(vector);
	assert(actualSize != 0);	   // never allow division by zero

	vector->vx *= size;
	vector->vy *= size;
	vector->vz *= size;

	vector->vx /= actualSize;
	vector->vy /= actualSize;
	vector->vz /= actualSize;
}





/******************************************************
	All credit to Jim Blackler for this section below
******************************************************/

#if (DEVELOPMENT_ENVIRONMENT==YAROZE)
	// define 2d vector type
typedef struct
{
	short vx, vy;
} DVECTOR;
#endif

typedef struct
{
	long vx, vy;
} DVECTOR_lng;






 

	// HALF = ONE/2
#define HALF 2048

#define NORMALISE_TABLE_ENTRIES__NUMBER_ENTRIES 512

struct cosarctan_and_sinarctan
{
	signed short cosarctan;
	signed short sinarctan;
};


#include "mathtab.h"

#define VN_FUNCTION_D(x,y,outx,xeffect,outy,yeffect)								\
		 	{																						\
		 		unsigned short lookup_idx =												\
		 				(NORMALISE_TABLE_ENTRIES__NUMBER_ENTRIES*						\
		 				(y)/*+(x)/2*/)/(x);													\
		 		(outx) = xeffect cosarctan_and_sinarctan_table[lookup_idx].cosarctan;\
		 		(outy) = yeffect cosarctan_and_sinarctan_table[lookup_idx].sinarctan;\
		 	}


/* guaranteed y>0 */
/* guaranteed x>0 */
#define VN_FUNCTION_C(x,y,outx,xeffect,outy,yeffect)								\
	 	{												  									  		\
 			if(x<y)											  		  			  	   		\
	 			VN_FUNCTION_D(y,x,outy,yeffect,outx,xeffect)							\
 			else												  		  			  	   		\
	 			VN_FUNCTION_D(x,y,outx,xeffect,outy,yeffect)							\
	 	}

/* guaranteed x>0 */
#define VN_FUNCTION_B(x,y,outx,xeffect,outy,yeffect)								\
	{													  							 				\
		if(y<0)											  						  				\
			VN_FUNCTION_C(x,-y,outx,xeffect,outy,-yeffect)							\
		else												  						 	 			\
			VN_FUNCTION_C(x,y,outx,xeffect,outy,yeffect)								\
	}

#define VN_FUNCTION_A(x,y,outx,outy)													\
{														  											\
	if(x<0)											  											\
		VN_FUNCTION_B(-x,y,outx,-,outy,+)												\
	else																							\
		VN_FUNCTION_B(x,y,outx,+,outy,+)		 											\
}


static __inline__ void inline_jbVectorNormalD(DVECTOR_lng * original, DVECTOR * out)
{
	VN_FUNCTION_A(original->vx,original->vy,out->vx,out->vy)
}

long jbVectorMeasure(VECTOR *v0)
{

	if(v0->vx || v0->vz)
	{


	/* START common lines (A) */
		DVECTOR_lng vector_xz = {v0->vx, v0->vz};

		DVECTOR unit_vector_xz;

		DVECTOR unit_vector_xz_y;

		DVECTOR_lng vector_xz_y;

		inline_jbVectorNormalD(&vector_xz,&unit_vector_xz);	
			/* 20% speed increase for inline*/

		vector_xz_y.vx =
				(unit_vector_xz.vx * vector_xz.vx +
				unit_vector_xz.vy * vector_xz.vy + HALF)/ONE;
				/* length of xz part of vector */
		vector_xz_y.vy = v0->vy;

		inline_jbVectorNormalD(&vector_xz_y, &unit_vector_xz_y);
			/* 20% speed increase for inline*/
	/* END common lines (A) */

		return(vector_xz_y.vx * unit_vector_xz_y.vx +
				vector_xz_y.vy * unit_vector_xz_y.vy + HALF) / ONE;

	}
	else
	{
		return(ABS(v0->vy));
	}

}