vmath.cpp - This C++ code provides a set of functions for c…

/src/shared/vmath.cpp

https://bitbucket.org/vivkin/gam3b00bs/ · C++ · 359 lines · 283 code · 51 blank · 25 comment · 4 complexity · 0f048b9d6716053978191e1fea7f1870 MD5 · raw file

#include "vmath.h"



///////////////////////////////////////////////////////////////////////////

// SIMD

///////////////////////////////////////////////////////////////////////////



const __m128 SIMD_EPSILON = _mm_set_ps1(EPSILON);

const __m128 SIMD_ZERO = _mm_setzero_ps();

const __m128 SIMD_ONE = _mm_set_ps1(1.0f);

const __m128 SIMD_1110 = _mm_set_ps(0.0f, 1.0f, 1.0, 1.0f);

const __m128 SIMD_1000 = _mm_set_ps(0.0f, 0.0f, 0.0, 1.0f);

const __m128 SIMD_0100 = _mm_set_ps(0.0f, 0.0f, 1.0, 0.0f);

const __m128 SIMD_0010 = _mm_set_ps(0.0f, 1.0f, 0.0, 0.0f);

const __m128 SIMD_0001 = _mm_set_ps(1.0f, 0.0f, 0.0, 0.0f);



__declspec(align(16)) const int SIMD_INT_MASK_AND_XYZ0[4] = {0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0};

__declspec(align(16)) const int SIMD_INT_MASK_AND_ABS[4] =  {0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};

__declspec(align(16)) const int SIMD_INT_MASK_XOR_SIGN_XYZ0[4] = {0x80000000, 0x80000000, 0x80000000, 0x00000000};

__declspec(align(16)) const int SIMD_INT_MASK_XOR_SIGN_0Y00[4] = {0, 0x80000000, 0, 0};

__declspec(align(16)) const int SIMD_INT_MASK_XOR_SIGN_000W[4] = {0, 0, 0, 0x80000000};



const __m128 SIMD_MASK_AND_XYZ0 = *reinterpret_cast<const __m128 *>(SIMD_INT_MASK_AND_XYZ0);

const __m128 SIMD_MASK_AND_ABS = *reinterpret_cast<const __m128 *>(SIMD_INT_MASK_AND_ABS);

const __m128 SIMD_MASK_XOR_SIGN_XYZ0 = *reinterpret_cast<const __m128 *>(SIMD_INT_MASK_XOR_SIGN_XYZ0);

const __m128 SIMD_MASK_XOR_SIGN_0Y00 = *reinterpret_cast<const __m128 *>(SIMD_INT_MASK_XOR_SIGN_0Y00);

const __m128 SIMD_MASK_XOR_SIGN_000W = *reinterpret_cast<const __m128 *>(SIMD_INT_MASK_XOR_SIGN_000W);



const __m128 SIMD_MATRIX_INVERSE_SIGN_A = _mm_set_ps(1.0f, -1.0f, 1.0f, -1.0f);

const __m128 SIMD_MATRIX_INVERSE_SIGN_B = _mm_set_ps(-1.0f, 1.0f, -1.0f, 1.0f);



///////////////////////////////////////////////////////////////////////////

// 4D vector

///////////////////////////////////////////////////////////////////////////



const vec4 vec4::ZERO = vec4(SIMD_ZERO);

const vec4 vec4::ONE = vec4(SIMD_ONE);

const vec4 vec4::UNIT_X = vec4(SIMD_1000);

const vec4 vec4::UNIT_Y = vec4(SIMD_0100);

const vec4 vec4::UNIT_Z = vec4(SIMD_0010);

const vec4 vec4::UNIT_W = vec4(SIMD_0001);



///////////////////////////////////////////////////////////////////////////

// Quaternion

///////////////////////////////////////////////////////////////////////////



const vec4 QUAT_IDENTITY = vec4(SIMD_0001);

	

// multiply quaternions

vec4 quat_mult(const vec4 &lhs, const vec4 &rhs)

{

	__m128 a1 = _mm_shuffle_ps(lhs.xyzw, lhs.xyzw, _MM_SHUFFLE(1, 3, 2, 0));

	__m128 a2 = _mm_shuffle_ps(lhs.xyzw, lhs.xyzw, _MM_SHUFFLE(0, 2, 3, 1));

	__m128 a3 = _mm_shuffle_ps(lhs.xyzw, lhs.xyzw, _MM_SHUFFLE(3, 1, 0, 2));

	__m128 a4 = _mm_shuffle_ps(lhs.xyzw, lhs.xyzw, _MM_SHUFFLE(2, 0, 1, 3));

	__m128 b1 = _mm_shuffle_ps(rhs.xyzw, rhs.xyzw, _MM_SHUFFLE(1, 2, 0, 3));

	__m128 b2 = _mm_shuffle_ps(rhs.xyzw, rhs.xyzw, _MM_SHUFFLE(0, 3, 1, 2));

	__m128 b3 = _mm_shuffle_ps(rhs.xyzw, rhs.xyzw, _MM_SHUFFLE(3, 0, 2, 1));

	__m128 b4 = _mm_shuffle_ps(rhs.xyzw, rhs.xyzw, _MM_SHUFFLE(2, 1, 3, 0));

	return vec4(_mm_xor_ps(

		_mm_add_ps(

			_mm_add_ps(_mm_mul_ps(a2, b2), _mm_mul_ps(a1, b1)),

			_mm_sub_ps(_mm_mul_ps(a4, b4), _mm_mul_ps(a3, b3))),

		SIMD_MASK_XOR_SIGN_000W));

}



// spherical linear interpolation

vec4 quat_slerp(const vec4 &v0, const vec4 &v1, float t)

{

	__m128 cosa = _mm_dot_ps(v0.xyzw, v1.xyzw);

	__m128 abscosa = _mm_and_ps(cosa, SIMD_MASK_AND_ABS);

	if (_mm_movemask_ps(_mm_cmpgt_ps(_mm_sub_ps(SIMD_ONE, abscosa), SIMD_EPSILON)) == 0xF)

	{

		float a;

		_mm_store_ss(&a, abscosa);

		a = acos(a);

		float sina = 1.0f / sin(a);

		float s0 = sin((1.0f - t) * a) * sina;

		float s1 = sin(t * a) * sina;

		__m128 scale0 = _mm_load_ps1(&s0);

		__m128 scale1 = _mm_load_ps1(&s1);

		if (_mm_movemask_ps(_mm_cmplt_ps(cosa, SIMD_ZERO)) == 0xF)

		{

			scale1 = _mm_sub_ps(SIMD_ZERO, scale1);

		}

		return vec4(_mm_add_ps(_mm_mul_ps(scale0, v0.xyzw), _mm_mul_ps(scale1, v1.xyzw)));

	}

	return v0;

}



///////////////////////////////////////////////////////////////////////////

// 4x4 matrix

///////////////////////////////////////////////////////////////////////////



const mat4 mat4::ZERO = mat4(SIMD_ZERO, SIMD_ZERO, SIMD_ZERO, SIMD_ZERO);

const mat4 mat4::IDENTITY = mat4(SIMD_1000, SIMD_0100, SIMD_0010, SIMD_0001);



// mul

mat4 &operator*=(mat4 &lhs, const mat4 &rhs)

{

	lhs = lhs * rhs;

	return lhs;

}



const mat4 operator*(const mat4 &lhs, const mat4 &rhs)

{

	mat4 m;



	m.row0 = _mm_add_ps(

		_mm_add_ps(

			_mm_mul_ps(rhs.row0, _mm_shuffle_ps(lhs.row0, lhs.row0, _MM_SHUFFLE(0, 0, 0, 0))),

			_mm_mul_ps(rhs.row1, _mm_shuffle_ps(lhs.row0, lhs.row0, _MM_SHUFFLE(1, 1, 1, 1)))),

		_mm_add_ps(

			_mm_mul_ps(rhs.row2, _mm_shuffle_ps(lhs.row0, lhs.row0, _MM_SHUFFLE(2, 2, 2, 2))),

			_mm_mul_ps(rhs.row3, _mm_shuffle_ps(lhs.row0, lhs.row0, _MM_SHUFFLE(3, 3, 3, 3)))));



	m.row1 = _mm_add_ps(

		_mm_add_ps(

			_mm_mul_ps(rhs.row0, _mm_shuffle_ps(lhs.row1, lhs.row1, _MM_SHUFFLE(0, 0, 0, 0))),

			_mm_mul_ps(rhs.row1, _mm_shuffle_ps(lhs.row1, lhs.row1, _MM_SHUFFLE(1, 1, 1, 1)))),

		_mm_add_ps(

			_mm_mul_ps(rhs.row2, _mm_shuffle_ps(lhs.row1, lhs.row1, _MM_SHUFFLE(2, 2, 2, 2))),

			_mm_mul_ps(rhs.row3, _mm_shuffle_ps(lhs.row1, lhs.row1, _MM_SHUFFLE(3, 3, 3, 3)))));



	m.row2 = _mm_add_ps(

		_mm_add_ps(

			_mm_mul_ps(rhs.row0, _mm_shuffle_ps(lhs.row2, lhs.row2, _MM_SHUFFLE(0, 0, 0, 0))),

			_mm_mul_ps(rhs.row1, _mm_shuffle_ps(lhs.row2, lhs.row2, _MM_SHUFFLE(1, 1, 1, 1)))),

		_mm_add_ps(

			_mm_mul_ps(rhs.row2, _mm_shuffle_ps(lhs.row2, lhs.row2, _MM_SHUFFLE(2, 2, 2, 2))),

			_mm_mul_ps(rhs.row3, _mm_shuffle_ps(lhs.row2, lhs.row2, _MM_SHUFFLE(3, 3, 3, 3)))));



	m.row3 = _mm_add_ps(

		_mm_add_ps(

			_mm_mul_ps(rhs.row0, _mm_shuffle_ps(lhs.row3, lhs.row3, _MM_SHUFFLE(0, 0, 0, 0))),

			_mm_mul_ps(rhs.row1, _mm_shuffle_ps(lhs.row3, lhs.row3, _MM_SHUFFLE(1, 1, 1, 1)))),

		_mm_add_ps(

			_mm_mul_ps(rhs.row2, _mm_shuffle_ps(lhs.row3, lhs.row3, _MM_SHUFFLE(2, 2, 2, 2))),

			_mm_mul_ps(rhs.row3, _mm_shuffle_ps(lhs.row3, lhs.row3, _MM_SHUFFLE(3, 3, 3, 3)))));



	return m;

}



// inverse matrix4x4

mat4 matrix_inverse(const mat4 &v)

{

	// http://www.devmaster.net/forums/showthread.php?t=14569



	__m128 Swp0a, Swp0b, Swp00, Swp01, Swp02, Swp03, Temp;



	Swp0a = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(3, 3, 3, 3));

	Swp0b = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(2, 2, 2, 2));

	Swp00 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(2, 2, 2, 2));

	Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));

	Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));

	Swp03 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(3, 3, 3, 3));

	__m128 Fac0 = _mm_sub_ps(_mm_mul_ps(Swp00, Swp01), _mm_mul_ps(Swp02, Swp03));



	Swp0a = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(3, 3, 3, 3));

	Swp0b = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(1, 1, 1, 1));

	Swp00 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(1, 1, 1, 1));

	Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));

	Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));

	Swp03 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(3, 3, 3, 3));

	__m128 Fac1 = _mm_sub_ps(_mm_mul_ps(Swp00, Swp01), _mm_mul_ps(Swp02, Swp03));



	Swp0a = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(2, 2, 2, 2));

	Swp0b = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(1, 1, 1, 1));

	Swp00 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(1, 1, 1, 1));

	Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));

	Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));

	Swp03 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(2, 2, 2, 2));

	__m128 Fac2 = _mm_sub_ps(_mm_mul_ps(Swp00, Swp01), _mm_mul_ps(Swp02, Swp03));



	Swp0a = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(3, 3, 3, 3));

	Swp0b = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(0, 0, 0, 0));

	Swp00 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(0, 0, 0, 0));

	Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));

	Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));

	Swp03 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(3, 3, 3, 3));

	__m128 Fac3 = _mm_sub_ps(_mm_mul_ps(Swp00, Swp01), _mm_mul_ps(Swp02, Swp03));



	Swp0a = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(2, 2, 2, 2));

	Swp0b = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(0, 0, 0, 0));

	Swp00 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(0, 0, 0, 0));

	Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));

	Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));

	Swp03 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(2, 2, 2, 2));

	__m128 Fac4 = _mm_sub_ps(_mm_mul_ps(Swp00, Swp01), _mm_mul_ps(Swp02, Swp03));



	Swp0a = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(1, 1, 1, 1));

	Swp0b = _mm_shuffle_ps(v.row3, v.row2, _MM_SHUFFLE(0, 0, 0, 0));

	Swp00 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(0, 0, 0, 0));

	Swp01 = _mm_shuffle_ps(Swp0a, Swp0a, _MM_SHUFFLE(2, 0, 0, 0));

	Swp02 = _mm_shuffle_ps(Swp0b, Swp0b, _MM_SHUFFLE(2, 0, 0, 0));

	Swp03 = _mm_shuffle_ps(v.row2, v.row1, _MM_SHUFFLE(1, 1, 1, 1));

	__m128 Fac5 = _mm_sub_ps(_mm_mul_ps(Swp00, Swp01), _mm_mul_ps(Swp02, Swp03));



	Temp = _mm_shuffle_ps(v.row1, v.row0, _MM_SHUFFLE(0, 0, 0, 0));

	__m128 Vec0 = _mm_shuffle_ps(Temp, Temp, _MM_SHUFFLE(2, 2, 2, 0));

	Temp = _mm_shuffle_ps(v.row1, v.row0, _MM_SHUFFLE(1, 1, 1, 1));

	__m128 Vec1 = _mm_shuffle_ps(Temp, Temp, _MM_SHUFFLE(2, 2, 2, 0));

	Temp = _mm_shuffle_ps(v.row1, v.row0, _MM_SHUFFLE(2, 2, 2, 2));

	__m128 Vec2 = _mm_shuffle_ps(Temp, Temp, _MM_SHUFFLE(2, 2, 2, 0));

	Temp = _mm_shuffle_ps(v.row1, v.row0, _MM_SHUFFLE(3, 3, 3, 3));

	__m128 Vec3 = _mm_shuffle_ps(Temp, Temp, _MM_SHUFFLE(2, 2, 2, 0));



	__m128 Inv0 = _mm_mul_ps(

		_mm_add_ps(

			_mm_mul_ps(Vec3, Fac2),

			_mm_sub_ps(

				_mm_mul_ps(Vec1, Fac0),

				_mm_mul_ps(Vec2, Fac1))),

		SIMD_MATRIX_INVERSE_SIGN_B);



	__m128 Inv1 = _mm_mul_ps(

		_mm_add_ps(

			_mm_mul_ps(Vec3, Fac4),

			_mm_sub_ps(

				_mm_mul_ps(Vec0, Fac0),

				_mm_mul_ps(Vec2, Fac3))),

		SIMD_MATRIX_INVERSE_SIGN_A);



	__m128 Inv2 = _mm_mul_ps(

		_mm_add_ps(

			_mm_mul_ps(Vec3, Fac5),

			_mm_sub_ps(

				_mm_mul_ps(Vec0, Fac1),

				_mm_mul_ps(Vec1, Fac3))),

		SIMD_MATRIX_INVERSE_SIGN_B);



	__m128 Inv3 = _mm_mul_ps(

		_mm_add_ps(

			_mm_mul_ps(Vec2, Fac5),

			_mm_sub_ps(

				_mm_mul_ps(Vec0, Fac2),

				_mm_mul_ps(Vec1, Fac4))),

		SIMD_MATRIX_INVERSE_SIGN_A);



	__m128 Det = _mm_dot_ps(

		_mm_shuffle_ps(

			_mm_shuffle_ps(Inv0, Inv1, _MM_SHUFFLE(0, 0, 0, 0)),

			_mm_shuffle_ps(Inv2, Inv3, _MM_SHUFFLE(0, 0, 0, 0)),

			_MM_SHUFFLE(2, 0, 2, 0)),

		v.row0);

	Det = _mm_div_ps(SIMD_ONE, Det);

	//Det = _mm_rcp_ps(Det);



	//	Inverse /= Determinant;

	mat4 m;

	m.row0 = _mm_mul_ps(Inv0, Det);

	m.row1 = _mm_mul_ps(Inv1, Det);

	m.row2 = _mm_mul_ps(Inv2, Det);

	m.row3 = _mm_mul_ps(Inv3, Det);

	return m;

}



// scale matrix

mat4 matrix_scaling(const vec4 &v)

{

	mat4 m;

	m.row0 = _mm_mul_ps(v.xyzw, SIMD_1000);

	m.row1 = _mm_mul_ps(v.xyzw, SIMD_0100);

	m.row2 = _mm_mul_ps(v.xyzw, SIMD_0010);

	m.row3 = SIMD_0001;

	return m;

}



// translation matrix

mat4 matrix_translation(const vec4 &v)

{

	mat4 m;

	m.row0 = SIMD_1000;

	m.row1 = SIMD_0100;

	m.row2 = SIMD_0010;

	m.row3 = _mm_merge_ps(v.xyzw, SIMD_1000);

	return m;

}



// rotation matrix from quaternion

mat4 matrix_rotation(const vec4 &v)

{

	__m128 v2 = _mm_and_ps(_mm_add_ps(v.xyzw, v.xyzw), SIMD_MASK_AND_XYZ0);

		

	__m128 wv = _mm_xor_ps(_mm_mul_ps(_mm_shuffle_ps(v.xyzw, v.xyzw, _MM_SHUFFLE(3, 3, 3, 3)), v2), SIMD_MASK_XOR_SIGN_0Y00);

	__m128 xy_yz_xz = _mm_mul_ps(_mm_shuffle_ps(v.xyzw, v.xyzw, _MM_SHUFFLE(0, 0, 2, 1)), v2);

	__m128 yz_xz_xy = _mm_shuffle_ps(xy_yz_xz, xy_yz_xz, _MM_SHUFFLE(3, 0, 2, 1));



	__m128 a = _mm_add_ps(yz_xz_xy, wv);

	__m128 b = _mm_sub_ps(yz_xz_xy, wv);



	__m128 vv = _mm_mul_ps(v.xyzw, v2);

	__m128 yy_xx_xx = _mm_shuffle_ps(vv, vv, _MM_SHUFFLE(3, 0, 0, 1));

	__m128 zz_zz_yy = _mm_shuffle_ps(vv, vv, _MM_SHUFFLE(3, 1, 2, 2));



	__m128 c = _mm_sub_ps(SIMD_1110, _mm_add_ps(yy_xx_xx, zz_zz_yy));



	mat4 m;

	m.row0 = _mm_shuffle_ps(c, a, _MM_SHUFFLE(1, 2, 3, 0));

	m.row0 = _mm_shuffle_ps(m.row0, m.row0, _MM_SHUFFLE(1, 3, 2, 0));

	m.row1 = _mm_shuffle_ps(_mm_shuffle_ps(b, c, _MM_SHUFFLE(1, 1, 2, 2)), a, _MM_SHUFFLE(3, 0, 2, 0));

	m.row2 = _mm_shuffle_ps(b, c, _MM_SHUFFLE(3, 2, 0, 1));

	m.row3 = SIMD_0001;

	return m;

}



mat4 matrix_lookat(const vec4 &eye, const vec4 &at, const vec4 &up)

{

	vec4 zaxis = normalize(at - eye);

	vec4 xaxis = normalize(cross(up, zaxis));

	vec4 yaxis = cross(zaxis, xaxis);



	__m128 tmp = _mm_hadd_ps(

		_mm_hadd_ps(_mm_mul_ps(xaxis.xyzw, eye.xyzw), _mm_mul_ps(yaxis.xyzw, eye.xyzw)),

		_mm_hadd_ps(_mm_mul_ps(zaxis.xyzw, eye.xyzw), _mm_setzero_ps()));

	tmp = _mm_merge_ps(_mm_sub_ps(_mm_setzero_ps(), tmp), SIMD_1000);



	mat4 m;

	m.row0 = xaxis.xyzw;

	m.row1 = yaxis.xyzw;

	m.row2 = zaxis.xyzw;

	m.row3 = _mm_setzero_ps();

	m.transpose();

	m.row3 = tmp;

		

	return m;

}



mat4 matrix_perspective(float fov, float aspect, float znear, float zfar)

{

	float y = 1.0f / tan(fov / 2.0f);

	float x = y / aspect;

	float a = zfar / (zfar - znear);

	float b = -znear * zfar / (zfar - znear);

	return mat4(

		x, 0.0f, 0.0f, 0.0f,

		0.0f, y, 0.0f, 0.0f,

		0.0f, 0.0f, a, 1.0f,

		0.0f, 0.0f, b, 0.0f);

}



mat4 matrix_ortho(float w, float h, float znear, float zfar)

{

	float x = w / 2.0f;

	float y = h / 2.0f;

	float a = 1.0f / (zfar - znear);

	float b = -znear / (zfar - znear);

	return mat4(

		x, 0.0f, 0.0f, 0.0f,

		0.0f, y, 0.0f, 0.0f,

		0.0f, 0.0f, a, 0.0f,

		0.0f, 0.0f, b, 1.0f);

}



///////////////////////////////////////////////////////////////////////////

// Axis aligned bounding box

///////////////////////////////////////////////////////////////////////////



const bbox bbox::ZERO = bbox(vec4::ZERO, vec4::ZERO);

const bbox bbox::DEGENERATE = bbox(vec4(INFINITY), vec4(-INFINITY));