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

#include "MathCommon.inl.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)
	{
		THROW_EXCEPTION("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;

	/*RASSERT_THROW_EXCEPTION(!isZero(M::sin(a) - sina));
	RASSERT_THROW_EXCEPTION(!isZero(M::cos(a) - cosa));*/
}


//======================================================================================================================
// 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