anki-3d-engine/src/Math/MathFuncs.inl.h

#include "MathDfltHeader.h"


namespace M {


// mathSanityChecks
inline void mathSanityChecks()
{
	const int fs = sizeof(float); // float size
	if( sizeof(Vec2)!=fs*2 || sizeof(Vec3)!=fs*3 || sizeof(Vec4)!=fs*4 || sizeof(Quat)!=fs*4 ||
	    sizeof(Euler)!=fs*3 || sizeof(Mat3)!=fs*9 || sizeof(Mat4)!=fs*16 )
	{
		FATAL( "Your compiler does class alignment" );
	}
}


// 1/sqrt(f)
inline float invSqrt( float f )
{
	#if defined( PLATFORM_WIN )
		float fhalf = 0.5*f;
		int i = *(int*)&f;
		i = 0x5F3759DF - (i>>1);
		f = *(float*)&i;
		f *= 1.5 - fhalf*f*f;
		return f;
	#elif defined( PLATFORM_LINUX )
		float fhalf = 0.5*f;
		asm
		(
			"mov %1, %%eax;"
			"sar %%eax;"
			"mov $0x5F3759DF, %%ebx;"
			"sub %%eax, %%ebx;"
			"mov %%ebx, %0"
			:"=g"(f)
			:"g"(f)
			:"%eax", "%ebx"
		);
		f *= 1.5 - fhalf*f*f;
		return f;
	#else
		#error "See file"
	#endif
}


// polynomialSinQuadrant
/**
 * Used in sinCos
 */
inline static float polynomialSinQuadrant( float a )
{
	return a * ( 1.0 + a * a * (-0.16666 + a * a * (0.0083143 - a * a * 0.00018542)));
}


// Sine and Cosine
inline void sinCos( float a, float& sina, float& cosa )
{
	bool negative = false;
	if (a < 0.0)
	{
		a = -a;
		negative = true;
	}
	const float kTwoOverPi = 1.0 / (PI/2.0);
	float floatA = kTwoOverPi * a;
	int intA = (int)floatA;

	const float k_rational_half_pi = 201 / 128.0;
	const float kRemainderHalfPi = 4.8382679e-4;

	floatA = (a - k_rational_half_pi * intA) - kRemainderHalfPi * intA;

	float floatAMinusHalfPi = (floatA - k_rational_half_pi) - kRemainderHalfPi;

	switch( intA & 3 )
	{
		case 0: // 0 - Pi/2
			sina = polynomialSinQuadrant(floatA);
			cosa = polynomialSinQuadrant(-floatAMinusHalfPi);
			break;
		case 1: // Pi/2 - Pi
			sina = polynomialSinQuadrant(-floatAMinusHalfPi);
			cosa = polynomialSinQuadrant(-floatA);
			break;
		case 2: // Pi - 3Pi/2
			sina = polynomialSinQuadrant(-floatA);
			cosa = polynomialSinQuadrant(floatAMinusHalfPi);
			break;
		case 3: // 3Pi/2 - 2Pi
			sina = polynomialSinQuadrant(floatAMinusHalfPi);
			cosa = polynomialSinQuadrant(floatA);
			break;
	};

	if( negative )
		sina = -sina;

	// a unit test
	#if defined( DEBUG_ENABLED )
		float sina_ = M::sin(a);
		float cosa_ = M::cos(a);

		DEBUG_ERR( isZero( sina_ - sina ) || isZero( cosa_ - cosa ) );
	#endif
}


//======================================================================================================================
// Small funcs                                                                                                         =
//======================================================================================================================
inline float sqrt( float f ) { return 1/invSqrt(f); }
inline float toRad( float degrees ) { return degrees*(PI/180.0); }
inline float toDegrees( float rad ) { return rad*(180.0/PI); }
inline float sin( float rad ) { return ::sin(rad); }
inline float cos( float rad ) { return ::cos(rad); }
inline bool  isZero( float f ) { return ( fabs(f) < EPSILON ); }


//  combineTransformations
//  mat4(t0,r0,s0)*mat4(t1,r1,s1) == mat4(tf,rf,sf)
inline void combineTransformations( const Vec3& t0, const Mat3& r0, float s0,
                                    const Vec3& t1, const Mat3& r1, float s1,
                                    Vec3& tf, Mat3& rf, float& sf )
{
	tf = t1.getTransformed( t0, r0, s0 );
	rf = r0 * r1;
	sf = s0 * s1;
}

//  combineTransformations as the above but without scale
inline void combineTransformations( const Vec3& t0, const Mat3& r0, const Vec3& t1, const Mat3& r1, Vec3& tf, Mat3& rf)
{
	tf = t1.getTransformed( t0, r0 );
	rf = r0 * r1;
}

} // end namespace