Altivec optimized point3F_normalize

// ------------
//	The following code is from here: http://developer.apple.com/hardwaredrivers/ve/algorithms.html

//result = v-0.5
inline vector float ReciprocalSquareRoot( vector float v )
{
	//Get the square root reciprocal estimate
	vector float zero = (vector float)(0);
	vector float oneHalf = (vector float)(0.5);
	vector float one = (vector float)(1.0);
	vector float estimate = vec_rsqrte( v );

	//One round of Newton-Raphson refinement
	vector float estimateSquared = vec_madd( estimate, estimate, zero );
	vector float halfEstimate = vec_madd( estimate, oneHalf, zero );
	return vec_madd( vec_nmsub( v, estimateSquared, one ), halfEstimate, estimate );
}
// ------------

static void vec_point3F_normalize(F32 *p)
{	
	vector float	zeros = (vector float)(0.0f);
	vector float	pv;

	F32	*loader = (F32*) &pv;

	loader[0] = p[0];
	loader[1] = p[1];
	loader[2] = p[2];
	loader[3] = 0.0f;

	// Square each of the components of the vector, then add them together
	//	After this, all components of the vector are the same value
	//	This is the recommended way to operate across a vector
	vector float	squared = vec_madd( pv, pv, zeros );
	squared = vec_add( squared, vec_sld( squared, squared, 8 ) );
	squared = vec_add( squared, vec_sld( squared, squared, 4 ) );
	
	if ( vec_all_eq( squared, zeros ) )
	{
		p[0] = 0;
		p[1] = 0;
		p[2] = 1;
	}
	else
	{
		// If you're happy with a faster estimated reciprocal sqrt, use vec_rsqrte() instead of ReciprocalSquareRoot()
	//	pv = vec_madd( pv, vec_rsqrte( squared ), zeros );
		pv = vec_madd( pv, ReciprocalSquareRoot( squared ), zeros );
		
		p[0] = loader[0];
		p[1] = loader[1];
		p[2] = loader[2];
	}
}

m_point3F_normalize     = vec_point3F_normalize;