/****************************/
/*		OPENGL.C			*/
/* (c)2004 Pangea Software  */
/* By Brian Greenstone      */
/****************************/


/****************************/
/*    PROTOTYPES            */
/****************************/

static void InitOurState(void);
static void OGLMatrix4x4_Multiply_Float(const OGLMatrix4x4	*mA, const OGLMatrix4x4 *mB, OGLMatrix4x4	*result);
static void OGLMatrix4x4_Multiply_Altivec(const OGLMatrix4x4	*mA, const OGLMatrix4x4 *mB, OGLMatrix4x4	*result);


/**************************/
/*    EXTERNS             */
/**************************/

extern	Boolean						gPlayInWindow, gAltiVec;
extern	WindowPtr					gGameWindow;
extern	short						gGameWindowWidth, gGameWindowHeight;


/**************************/
/*    CONSTANTS           */
/**************************/

#define	STATE_STACK_SIZE	20


#define	CAMERA_HITHER		1.0f
#define	CAMERA_YON			1000.0f
#define	CAMERA_FOV			90.0			// field of view of the camera (in degrees)

/****************************/
/*    VARIABLES             */
/****************************/

AGLDrawable			gAGLWin;
AGLContext			gAGLContext = nil;

GLuint				gFontList;
int					gFrameCounter = 0;


			/* STATE STACK */
			
int			gStateStackIndex = 0;

Boolean		gStateStack_Lighting[STATE_STACK_SIZE];					
Boolean		gStateStack_CullFace[STATE_STACK_SIZE];					
Boolean		gStateStack_DepthTest[STATE_STACK_SIZE];			
Boolean		gStateStack_Texture2D[STATE_STACK_SIZE];
Boolean		gStateStack_Blend[STATE_STACK_SIZE];
Boolean		gStateStack_Fog[STATE_STACK_SIZE];
GLboolean	gStateStack_DepthMask[STATE_STACK_SIZE];
GLint		gStateStack_BlendDst[STATE_STACK_SIZE];
GLint		gStateStack_BlendSrc[STATE_STACK_SIZE];
OGLColorRGBA gStateStack_Color[STATE_STACK_SIZE];

Boolean			gMyState_Lighting;
Boolean			gMyState_Blend;
Boolean			gMyState_Fog;
Boolean			gMyState_Texture2D;
Boolean			gMyState_CullFace;
Boolean			gMyState_DepthTest;
Boolean			gMyState_DepthMask;
OGLColorRGBA	gMyState_Color;
GLenum			gMyState_BlendFuncS, gMyState_BlendFuncD;

			/* MATRICES */
			
OGLMatrix4x4	gViewToFrustumMatrix,gWorldToViewMatrix,gWorldToFrustumMatrix, gLocalToViewMatrix, gLocalToFrustumMatrix;


/****************** CREATE OPENGL DRAW CONTEXT *********************/

void OGL_CreateDrawContext(void)
{
GDHandle 		gdevice;
AGLPixelFormat 	fmt;
GLint          attribWindow[]	= {AGL_RGBA, AGL_DOUBLEBUFFER, AGL_DEPTH_SIZE, 32, AGL_ALL_RENDERERS, AGL_ACCELERATED, AGL_NO_RECOVERY, AGL_NONE};
GLint          attribFullScreen[] = {AGL_RGBA, AGL_FULLSCREEN, AGL_DOUBLEBUFFER, AGL_DEPTH_SIZE, 32, AGL_ALL_RENDERERS, AGL_ACCELERATED, AGL_NO_RECOVERY, AGL_NONE};

	gdevice = GetMainDevice();						// get the main display

			/***********************/
			/* CHOOSE PIXEL FORMAT */
			/***********************/

			/* PLAY IN WINDOW */
												
	if (gPlayInWindow)
		fmt = aglChoosePixelFormat(&gdevice, 1, attribWindow);
		
			/* FULL-SCREEN */
	else
		fmt = aglChoosePixelFormat(&gdevice, 1, attribFullScreen);

	if ((fmt == nil) || (aglGetError() != AGL_NO_ERROR))
		DoFatalAlert("\pCannot make a draw context!");

			/**********************/
			/* CREATE AGL CONTEXT */
			/**********************/
			
	gAGLContext = aglCreateContext(fmt, nil);
	if ((gAGLContext == nil) || (aglGetError() != AGL_NO_ERROR))
		DoFatalAlert("\paglCreateContext failed!");
		
		
			/* PLAYING IN A WINDOW */
				
	if (gPlayInWindow)
	{
		gAGLWin = (AGLDrawable)GetWindowPort(gGameWindow);
		aglSetDrawable(gAGLContext, gAGLWin);
	}
	
			/* PLAYING FULL-SCREEN */
	else
	{
		gAGLWin = nil;
		aglEnable(gAGLContext, AGL_FS_CAPTURE_SINGLE);		
		aglSetFullScreen(gAGLContext, 0, 0, 0, 0);	
	}	

			/* ACTIVATE THE CONTEXT */

	aglSetCurrentContext(gAGLContext);


			/* NO LONGER NEED PIXEL FORMAT */
			
	aglDestroyPixelFormat(fmt);
	
	
			/* INITIALIZE OUR OPENGL STATE */
				
	InitOurState();
	OGL_InitVertexArrayRanges();

}

#pragma mark -

/******************* DRAW OPENGL SCENE *********************/
//
// Called from the main loop once each frame to draw the scene.
//

void OGL_DrawScene(void)
{
			/* MAKE OUR CONTEXT THE ACTIVE ONE */
			
  	aglSetCurrentContext(gAGLContext);
	
			/* CLEAR THE DRAW BUFFER & Z-BUFFER */
				
	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
	
		
				/* DO OUR GEOMETRY DRAWING HERE */
				
	OGL_UpdateVertexArrayRange();				// update VAR memory
	DrawSceneGeometry();						// draw stuff
	gFrameCounter++;
				

			/* END RENDER & SWAP THE BUFFER TO MAKE IT VISIBLE */
           		
	aglSwapBuffers(gAGLContext);	
}




/************** OGL: SET CAMERA **********************/
//
// This sets the Projection and model view matrix based on the input camera parameters.
//
//
//

void OGL_SetCamera(OGLPoint3D *cameraCoord, OGLPoint3D *lookAtPt, OGLVector3D *upVec)
{
float	aspect;
	
			/**************************/
			/* INIT PROJECTION MATRIX */
			/**************************/
			
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();	

	aspect = (float)gGameWindowWidth/(float)gGameWindowHeight;	// calc window aspect ratio
	
	gluPerspective (CAMERA_FOV,				// fov
					aspect,					// aspect
					CAMERA_HITHER,			// hither
					CAMERA_YON);			// yon

	
			/*************************/
			/* INIT MODELVIEW MATRIX */
			/*************************/
			
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(cameraCoord->x, cameraCoord->y, cameraCoord->z,	
			lookAtPt->x, lookAtPt->y, lookAtPt->z,	
			upVec->x, upVec->y, upVec->z);


			/*******************************/
			/* GET VARIOUS CAMERA MATRICES */
			/*******************************/
			//
			// Here we extract some matrices from OpenGL and
			// calculate some others which most games need from time to time.
			//
			
	glGetFloatv(GL_MODELVIEW_MATRIX, (GLfloat *)&gWorldToViewMatrix);
	glGetFloatv(GL_MODELVIEW_MATRIX, (GLfloat *)&gLocalToViewMatrix);
	glGetFloatv(GL_PROJECTION_MATRIX, (GLfloat *)&gViewToFrustumMatrix);
	
	OGLMatrix4x4_Multiply(&gLocalToViewMatrix, &gViewToFrustumMatrix, &gLocalToFrustumMatrix);
	OGLMatrix4x4_Multiply(&gWorldToViewMatrix, &gViewToFrustumMatrix, &gWorldToFrustumMatrix);


}




#pragma mark ______ AGL FONT FUNCTIONS _______

/****************** INIT OPENGL FONT ***********************/
//
// Initializes a font list that we can use for drawing text
// into a draw context for simple debug information.
//

void OGL_InitFont(void)
{	
	GLboolean	success;

	gFontList = glGenLists(256);
 
 	success = aglUseFont(gAGLContext,		// our draw context 
						kFontIDMonaco,		// font to use
						bold,				// style of font
						9,					// point size of font
						0,					// first character
						256,				// # characters
						gFontList);			// list to store font in


	if (!success)
		DoFatalAlert("\paglUseFont failed");
}


/******************* DRAW OPENGL STRING ***********************/
//
// Draws a pascal string to the 2D window coordinates specified by x,y.
//
// INPUT:	s = pascal string
//			x/y = screen coords (based on 640x480)
//

void OGL_DrawString(Str255 s, GLint x, GLint y)
{
	OGL_PushState();							// preserve the current state before changing it
	
			/* SET THE DRAWING MATRICES */

	glMatrixMode (GL_MODELVIEW);
	glLoadIdentity();
	glMatrixMode (GL_PROJECTION);
	glLoadIdentity();
	glOrtho(0, 640, 0, 480, -10.0, 10.0);

	
			/* SET SOME STATE VALUES */

	OGL_DisableLighting();
	OGL_DisableTexture2D();	
	OGL_SetColor4f (1,1,1,1);


		/* SET COORDINATE TO DRAW STRING */

	glRasterPos2i(x, 480-y);


			/* DRAW THE STRING */

	glListBase(gFontList);
	glCallLists(s[0], GL_UNSIGNED_BYTE, &s[1]);
	
	OGL_PopState();							// restore the state
}


/**************** DRAW INT WITH OPENGL FONT ********************/

void OGL_DrawInt(int f, GLint x, GLint y)
{
Str255	s;

	NumToString(f,s);
	OGL_DrawString(s,x,y);

}


#pragma mark _____STATE FUNCTIONS_______


/************** INIT OUR STATE ******************/
//
// Called when an OpenGL draw context is created.  This will initialize our
// state and our cached info to the desired default values.
//

static void InitOurState(void)
{
		/****************************/
		/* SET PERMANENT STATE INFO */
		/****************************/

	glClearColor(0,0,.5, 1.0);

	glEnable(GL_ALPHA_TEST);
	glAlphaFunc(GL_NOTEQUAL, 0);								// draw any pixel who's Alpha != 0

	glHint(GL_TRANSFORM_HINT_APPLE, GL_FASTEST);				// fastest transforms with minimal accuracy loss.

	glEnable(GL_RESCALE_NORMAL);								// do any vector scaling the fast way

	glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);	// use color material for faster color calls
	glEnable(GL_COLOR_MATERIAL);		


		/*************************/
		/* SET CACHED STATE INFO */
		/*************************/

		/* ENABLE LIGHTING */

	gMyState_Lighting = false;				// set state to false so that enable will set it to true
	OGL_EnableLighting();
	
		/* DISABLE FOG */
		
	gMyState_Fog = true;
	OGL_DisableFog();
		
		/* DISABLE TEXTURE2D */
			
	gMyState_Texture2D = true;
	OGL_DisableTexture2D();
	
		/* ENABLE BACKFACE CULLING */
			
	gMyState_CullFace = false;
	OGL_EnableCullFace();
	
		
		/* ENABLE Z-BUFFER READS */
		
	gMyState_DepthTest = false;
	OGL_EnableDepthTest();

		/* ENABLE Z-BUFFER WRITES */
		
	gMyState_DepthMask = false;
	OGL_DepthMask(true);
	
	
		/* TURN OFF BLENDING */
		
	gMyState_Blend = true;
	OGL_DisableBlend();
		
	gMyState_BlendFuncS = GL_ONE;			// even though blending is off, let's set the default blending mode
	gMyState_BlendFuncD = GL_ONE;
	
	
			/* SET COLOR */
			
	gMyState_Color.r =
	gMyState_Color.g =
	gMyState_Color.b = 1;
	gMyState_Color.a = 0;
	
	OGL_SetColor4f(1,1,1,1);
}


/********************* PUSH STATE **************************/

void OGL_PushState(void)
{
int	i;

		/* PUSH MATRIES WITH OPENGL */

	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();	

		/* SAVE STATE INFO */
	
	i = gStateStackIndex++;											// get stack index and increment
	
	if (i >= STATE_STACK_SIZE)
		DoFatalAlert("\pOGL_PushState: stack overflow");
	
	gStateStack_Lighting[i]		= gMyState_Lighting;					
	gStateStack_CullFace[i]		= gMyState_CullFace;					
	gStateStack_DepthTest[i]	= gMyState_DepthTest;		
	gStateStack_Texture2D[i]	= gMyState_Texture2D;		
	gStateStack_Fog[i]			= gMyState_Fog;		
	gStateStack_Blend[i]		= gMyState_Blend;		
	gStateStack_Color[i]		= gMyState_Color;
	gStateStack_DepthMask[i]	= gMyState_DepthMask;
	
	gStateStack_BlendSrc[i]		= gMyState_BlendFuncS;
	gStateStack_BlendDst[i]		= gMyState_BlendFuncD;
}


/********************* POP STATE **************************/

void OGL_PopState(void)
{
int		i;

		/* RETREIVE OPENGL MATRICES */
		
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	
		/* GET STATE INFO */
		
	i = --gStateStackIndex;												// dec stack index
		
	if (i < 0)
		DoFatalAlert("\pOGL_PopState: stack underflow!");

	if (gStateStack_Lighting[i])		// restore lighting
		OGL_EnableLighting();
	else
		OGL_DisableLighting();


	if (gStateStack_CullFace[i])		// restore backface culling
		OGL_EnableCullFace();
	else
		OGL_DisableCullFace();


	if (gStateStack_DepthTest[i])		// restore depth test
		OGL_EnableDepthTest();
	else
		OGL_DisableDepthTest();

	if (gStateStack_Texture2D[i])		// restore texture state
		OGL_EnableTexture2D();				
	else
		OGL_DisableTexture2D();				

	if (gStateStack_Blend[i])			// restore blending mode
		OGL_EnableBlend();
	else
		OGL_DisableBlend();

	if (gStateStack_Fog[i])				// restore fog state
		OGL_EnableFog();
	else
		OGL_DisableFog();

	OGL_DepthMask(gStateStack_DepthMask[i]);	// restore depth mask state

	OGL_BlendFunc(gStateStack_BlendSrc[i], gStateStack_BlendDst[i]);	// restore blending mode
	OGL_SetColor4fv(&gStateStack_Color[i]);	// restore color state

}


/******************* OGL ENABLE LIGHTING ****************************/

void OGL_EnableLighting(void)
{
	if (!gMyState_Lighting)
	{
		gMyState_Lighting = true;
		glEnable(GL_LIGHTING);	
	}
}


/******************* OGL DISABLE LIGHTING ****************************/

void OGL_DisableLighting(void)
{
	if (gMyState_Lighting)
	{
		gMyState_Lighting = false;
		glDisable(GL_LIGHTING);	
	}
}

/******************* OGL ENABLE DEPTH TEST ****************************/

void OGL_EnableDepthTest(void)
{
	if (!gMyState_DepthTest)
	{
		gMyState_DepthTest = true;
		glEnable(GL_DEPTH_TEST);	
	}
}


/******************* OGL DISABLE DEPTH TEST ****************************/

void OGL_DisableDepthTest(void)
{

	if (gMyState_DepthTest)
	{
		gMyState_DepthTest = false;
		glDisable(GL_DEPTH_TEST);	
	}
}




/********************** OGL ENABLE BLEND ********************/

void OGL_EnableBlend(void)
{
	if (!gMyState_Blend)
	{
		gMyState_Blend = true;
		glEnable(GL_BLEND);	
	}
}


/******************* OGL DISABLE BLEND ****************************/

void OGL_DisableBlend(void)
{
	if (gMyState_Blend)
	{
		gMyState_Blend = false;
		glDisable(GL_BLEND);	
	}
}


/********************** OGL ENABLE TEXTURE 2D ********************/

void OGL_EnableTexture2D(void)
{
	if (!gMyState_Texture2D)
	{
		gMyState_Texture2D = true;
		glEnable(GL_TEXTURE_2D);	
	}
}


/******************* OGL DISABLE TEXTURE 2D ****************************/

void OGL_DisableTexture2D(void)
{
	if (gMyState_Texture2D)
	{
		gMyState_Texture2D = false;
		glDisable(GL_TEXTURE_2D);	
	}
}


/********************** OGL SET COLOR 4fV ********************/

void OGL_SetColor4fv(OGLColorRGBA *color)
{
	if ((color->r != gMyState_Color.r) || 
		(color->g != gMyState_Color.g) ||
		(color->b != gMyState_Color.b) ||
		(color->a != gMyState_Color.a))
	{

		glColor4fv((GLfloat *)color);
		
		gMyState_Color = *color;
	}
}


/********************** OGL SET COLOR 4f ********************/

void OGL_SetColor4f(float r, float g, float b, float a)
{
	if ((r != gMyState_Color.r) || 
		(g != gMyState_Color.g) ||
		(b != gMyState_Color.b) ||
		(a != gMyState_Color.a))
	{
	
		glColor4f(r, g, b, a);
	
		gMyState_Color.r = r;
		gMyState_Color.g = g;
		gMyState_Color.b = b;
		gMyState_Color.a = a;
	}
}


/********************** OGL ENABLE CULL FACE ********************/

void OGL_EnableCullFace(void)
{
	if (!gMyState_CullFace)
	{
		gMyState_CullFace = true;
		glEnable(GL_CULL_FACE);		
	}
}


/******************* OGL DISABLE CULL FACE ****************************/

void OGL_DisableCullFace(void)
{

	if (gMyState_CullFace)
	{
		gMyState_CullFace = false;
		glDisable(GL_CULL_FACE);	
	}
}


/********************** OGL ENABLE FOG ********************/

void OGL_EnableFog(void)
{

	if (!gMyState_Fog)
	{
		gMyState_Fog = true;
		glEnable(GL_FOG);	
	}
}


/******************* OGL DISABLE FOG ****************************/

void OGL_DisableFog(void)
{

	if (gMyState_Fog)
	{
		gMyState_Fog = false;
		glDisable(GL_FOG);	
	}
}


/********************** OGL BLEND FUNC ********************/

void OGL_BlendFunc(GLenum sfactor, GLenum dfactor)
{
	if ((sfactor != gMyState_BlendFuncS) || (dfactor != gMyState_BlendFuncD))
	{
		glBlendFunc(sfactor, dfactor);

		gMyState_BlendFuncS = sfactor;
		gMyState_BlendFuncD = dfactor;
	}
}


/******************* OGL DEPTH MASK ****************************/
//
// Turns the z-buffer on/off.
//
// INPUT: useZbuffer = true if want depth mask turned on.
//

void OGL_DepthMask(Boolean useZbuffer)
{
	if (gMyState_DepthMask != useZbuffer)
	{
		glDepthMask(useZbuffer);
		gMyState_DepthMask = useZbuffer;
	}
}



#pragma mark _______ 3D MATH FUNCTIONS _______

/****************** OGL MATRIX 4X4 MULTIPLY ********************/

void OGLMatrix4x4_Multiply(const OGLMatrix4x4	*mA, const OGLMatrix4x4 *mB, OGLMatrix4x4	*result)
{
	if (gAltiVec)
		OGLMatrix4x4_Multiply_Altivec(mA, mB, result);
	else
		OGLMatrix4x4_Multiply_Float(mA,mB, result);
}


/****************** OGL MATRIX 4X4 MULTIPLY FLOAT ********************/

static void OGLMatrix4x4_Multiply_Float(const OGLMatrix4x4	*mA, const OGLMatrix4x4 *mB, OGLMatrix4x4	*result)
{
	float	b00, b01, b02, b03;
	float	b10, b11, b12, b13;
	float	b20, b21, b22, b23;
	float	b30, b31, b32, b33;
	float	ax0, ax1, ax2, ax3;
	
	b00 = mB->value[M00];	b01 = mB->value[M10];	b02 = mB->value[M20];	b03 = mB->value[M30];
	b10 = mB->value[M01];	b11 = mB->value[M11];	b12 = mB->value[M21];	b13 = mB->value[M31];
	b20 = mB->value[M02];	b21 = mB->value[M12];	b22 = mB->value[M22];	b23 = mB->value[M32];
	b30 = mB->value[M03];	b31 = mB->value[M13];	b32 = mB->value[M23];	b33 = mB->value[M33];
	
	ax0 = mA->value[M00];	ax1 = mA->value[M10];
	ax2 = mA->value[M20];	ax3 = mA->value[M30];
	
	result->value[M00] = ax0*b00 + ax1*b10 + ax2*b20 + ax3*b30;
	result->value[M10] = ax0*b01 + ax1*b11 + ax2*b21 + ax3*b31;
	result->value[M20] = ax0*b02 + ax1*b12 + ax2*b22 + ax3*b32;
	result->value[M30] = ax0*b03 + ax1*b13 + ax2*b23 + ax3*b33;
	
	ax0 = mA->value[M01];	ax1 = mA->value[M11];
	ax2 = mA->value[M21];	ax3 = mA->value[M31];
	
	result->value[M01] = ax0*b00 + ax1*b10 + ax2*b20 + ax3*b30;
	result->value[M11] = ax0*b01 + ax1*b11 + ax2*b21 + ax3*b31;
	result->value[M21] = ax0*b02 + ax1*b12 + ax2*b22 + ax3*b32;
	result->value[M31] = ax0*b03 + ax1*b13 + ax2*b23 + ax3*b33;
	
	ax0 = mA->value[M02];	ax1 = mA->value[M12];
	ax2 = mA->value[M22];	ax3 = mA->value[M32];
	
	result->value[M02] = ax0*b00 + ax1*b10 + ax2*b20 + ax3*b30;
	result->value[M12] = ax0*b01 + ax1*b11 + ax2*b21 + ax3*b31;
	result->value[M22] = ax0*b02 + ax1*b12 + ax2*b22 + ax3*b32;
	result->value[M32] = ax0*b03 + ax1*b13 + ax2*b23 + ax3*b33;
	
	ax0 = mA->value[M03];	ax1 = mA->value[M13];
	ax2 = mA->value[M23];	ax3 = mA->value[M33];
	
	result->value[M03] = ax0*b00 + ax1*b10 + ax2*b20 + ax3*b30;
	result->value[M13] = ax0*b01 + ax1*b11 + ax2*b21 + ax3*b31;
	result->value[M23] = ax0*b02 + ax1*b12 + ax2*b22 + ax3*b32;
	result->value[M33] = ax0*b03 + ax1*b13 + ax2*b23 + ax3*b33;
}



/****************** OGL MATRIX 4X4 MULTIPLY ALTIVEC ********************/

static void OGLMatrix4x4_Multiply_Altivec(const OGLMatrix4x4	*mA, const OGLMatrix4x4 *mB, OGLMatrix4x4	*result)
{
	vector float A1,A2,A3,A4, B1, B2, B3, B4;
	vector float zeroF = (vector float) vec_splat_u32(0);
	vector float C1, C2, C3, C4;		
	
	/* LOAD MATRIX A */		
	
	A1 = vec_ld(  0, (float*) mA );
	A2 = vec_ld( 16, (float*) mA );
	A3 = vec_ld( 32, (float*) mA );
	A4 = vec_ld( 48, (float*) mA );
	B1 = vec_ld(  0, (float*) mB );
	B2 = vec_ld( 16, (float*) mB );
	B3 = vec_ld( 32, (float*) mB );
	B4 = vec_ld( 48, (float*) mB );
	
	//Do the first scalar x vector multiply for each row
	C1 = vec_madd( vec_splat( A1, 0 ), B1, zeroF );
	C2 = vec_madd( vec_splat( A2, 0 ), B1, zeroF );
	C3 = vec_madd( vec_splat( A3, 0 ), B1, zeroF );
	C4 = vec_madd( vec_splat( A4, 0 ), B1, zeroF );
	
	//Accumulate in the second scalar x vector multiply for each row
	C1 = vec_madd( vec_splat( A1, 1 ), B2, C1 );
	C2 = vec_madd( vec_splat( A2, 1 ), B2, C2 );
	C3 = vec_madd( vec_splat( A3, 1 ), B2, C3 );
	C4 = vec_madd( vec_splat( A4, 1 ), B2, C4 );
	
	//Accumulate in the third scalar x vector multiply for each row
	C1 = vec_madd( vec_splat( A1, 2 ), B3, C1 );
	C2 = vec_madd( vec_splat( A2, 2 ), B3, C2 );
	C3 = vec_madd( vec_splat( A3, 2 ), B3, C3 );
	C4 = vec_madd( vec_splat( A4, 2 ), B3, C4 );
	
	//Accumulate in the fourth scalar x vector multiply for each row
	C1 = vec_madd( vec_splat( A1, 3 ), B4, C1 );
	C2 = vec_madd( vec_splat( A2, 3 ), B4, C2 );
	C3 = vec_madd( vec_splat( A3, 3 ), B4, C3 );
	C4 = vec_madd( vec_splat( A4, 3 ), B4, C4 );
	
	vec_st( C1,  0, (float*) result );
	vec_st( C2, 16, (float*) result );
	vec_st( C3, 32, (float*) result );
	vec_st( C4, 48, (float*) result );		
}


/****************** NORMALIZE VECTOR FAST **********************/
//
// Uses frsqrte and Newton-Rhapson refinement to quickly
// normalize the input vector.
//
// INPUT:  x,y,z = vector to normalize
//
// OUTPUT:	outV = normalized vector (estimate)
//

void OGL_Vector3D_NormalizeFast(float x, float y, float z, OGLVector3D *outV)
{
float	len;
float	isqrt, temp1, temp2;		
	
			/* CALC LENGTH OF INPUT VECTOR */

	len = (x * x) + (y * y) + (z * z);
	
		/* CALCULATE 1.0 / SQRT(len) ESTIMATE */

	isqrt = __frsqrte (len);				


		/* REFINE THE ESTIMATE WITH NEWTON-RHAPSON */

	temp1 = len * -.5f;						
	temp2 = isqrt * isqrt;				
	temp1 *= isqrt;						 
	isqrt *= (float)(3.0/2.0);			 
	len = isqrt + temp1 * temp2;	


			/* RETURN NORMALIZED VECTOR */

	outV->x = x * len;
	outV->y = y * len;
	outV->z = z * len;
}












