SBSPSS/Utils/Libs/Maths/matrix4x4.h

490 lines
14 KiB
C
Raw Normal View History

2000-12-04 17:58:10 +01:00
#ifndef __MATRIX4X4_H__
#define __MATRIX4X4_H__
#include "mathtypes.h"
#include "vector3.h"
#include "plane.h"
//#include "user.h"
class Quaternion;
//AF - added this to suppress warning for nonstandard [unnamed union]
#pragma warning(push)
#pragma warning(disable:4201)
#if defined(__USER_Mike__)
#define USE_INTRINSICS
#endif
#if defined(USE_INTRINSICS)
#include "xmmintrin.h"
#endif
// there should be _NO_ constructor
struct Matrix4x4
{
union
{
real m_M[4][4];
struct
{
real _11, _12, _13, _14;
real _21, _22, _23, _24;
real _31, _32, _33, _34;
real _41, _42, _43, _44;
};
};
inline real & operator()(int ix, int iy)
{
return m_M[ix][iy];
}
inline real const & operator()(int ix, int iy) const
{
return m_M[ix][iy];
}
inline void Identity( void )
{
m_M[0][0] = 1.f;
m_M[0][1] = 0.f;
m_M[0][2] = 0.f;
m_M[0][3] = 0.f;
m_M[1][0] = 0.f;
m_M[1][1] = 1.f;
m_M[1][2] = 0.f;
m_M[1][3] = 0.f;
m_M[2][0] = 0.f;
m_M[2][1] = 0.f;
m_M[2][2] = 1.f;
m_M[2][3] = 0.f;
m_M[3][0] = 0.f;
m_M[3][1] = 0.f;
m_M[3][2] = 0.f;
m_M[3][3] = 1.f;
}
inline void Zero( void )
{
m_M[0][0] = 0.f;
m_M[0][1] = 0.f;
m_M[0][2] = 0.f;
m_M[0][3] = 0.f;
m_M[1][0] = 0.f;
m_M[1][1] = 0.f;
m_M[1][2] = 0.f;
m_M[1][3] = 0.f;
m_M[2][0] = 0.f;
m_M[2][1] = 0.f;
m_M[2][2] = 0.f;
m_M[2][3] = 0.f;
m_M[3][0] = 0.f;
m_M[3][1] = 0.f;
m_M[3][2] = 0.f;
m_M[3][3] = 0.f;
}
void ToQuaternion( Quaternion &q ) const;
void Invert( void );
inline void Transpose( void );
inline void SetRotX( real a );
inline void SetRotY( real a );
inline void SetRotZ( real a );
inline void SetTranslation( real x, real y, real z );
inline void SetTranslation( Vector3 const &v );
inline void GetTranslation(Vector3 &out) const;
inline void SetProjection( real fov, real aspect, real nearclip, real farclip );
inline void ApplyScale(Vector3 Scl);
inline Matrix4x4 &operator+=( Matrix4x4 const &other );
friend inline Matrix4x4 operator*( Matrix4x4 const &a, Matrix4x4 const &b );
friend inline Matrix4x4 operator+( Matrix4x4 const &a, Matrix4x4 const &b );
friend inline Vector3 operator*( Matrix4x4 const &a, Vector3 const &b );
};
#pragma warning(pop)
inline Matrix4x4 &Matrix4x4::operator +=( Matrix4x4 const &other )
{
for (int i=0; i<4; i++)
{
for (int j=0; j<4; j++)
{
m_M[i][j] += other.m_M[i][j];
}
}
return *this;
}
#if 0//defined(USE_INTRINSICS)
inline void Matrix4x4::Transpose( void )
{
_MM_TRANSPOSE4_PS(_114, _214, _314, _414);
}
#else
inline void Matrix4x4::Transpose( void )
{
Matrix4x4 temp = *this;
for (int i=0; i<4; i++)
{
for (int j=0; j<4; j++)
{
m_M[i][j] = temp(j,i);
}
}
}
#endif
inline void Matrix4x4::SetRotX( real a )
{
real ca = (real)cos( a );
real sa = (real)sin( a );
m_M[0][0] = 1.f;
m_M[0][1] = 0.f;
m_M[0][2] = 0.f;
m_M[0][3] = 0.f;
m_M[1][0] = 0.f;
m_M[1][1] = ca;
m_M[1][2] = sa;
m_M[1][3] = 0.f;
m_M[2][0] = 0.f;
m_M[2][1] = -sa;
m_M[2][2] = ca;
m_M[2][3] = 0.f;
m_M[3][0] = 0.f;
m_M[3][1] = 0.f;
m_M[3][2] = 0.f;
m_M[3][3] = 1.f;
}
inline void Matrix4x4::SetRotY( real a )
{
real ca = (real)cos( a );
real sa = (real)sin( a );
m_M[0][0] = ca;
m_M[0][1] = 0.f;
m_M[0][2] = -sa;
m_M[0][3] = 0.f;
m_M[1][0] = 0.f;
m_M[1][1] = 1.f;
m_M[1][2] = 0.f;
m_M[1][3] = 0.f;
m_M[2][0] = sa;
m_M[2][1] = 0.f;
m_M[2][2] = ca;
m_M[2][3] = 0.f;
m_M[3][0] = 0.f;
m_M[3][1] = 0.f;
m_M[3][2] = 0.f;
m_M[3][3] = 1.f;
}
inline void Matrix4x4::SetRotZ( real a )
{
real ca = (real)cos( a );
real sa = (real)sin( a );
m_M[0][0] = ca;
m_M[0][1] = sa;
m_M[0][2] = 0.f;
m_M[0][3] = 0.f;
m_M[1][0] = -sa;
m_M[1][1] = ca;
m_M[1][2] = 0.f;
m_M[1][3] = 0.f;
m_M[2][0] = 0.f;
m_M[2][1] = 0.f;
m_M[2][2] = 1.f;
m_M[2][3] = 0.f;
m_M[3][0] = 0.f;
m_M[3][1] = 0.f;
m_M[3][2] = 0.f;
m_M[3][3] = 1.f;
}
inline void Matrix4x4::SetTranslation( real x, real y, real z )
{
m_M[3][0] = x;
m_M[3][1] = y;
m_M[3][2] = z;
}
inline void Matrix4x4::SetTranslation( Vector3 const &v )
{
m_M[3][0] = v.x;
m_M[3][1] = v.y;
m_M[3][2] = v.z;
}
inline void Matrix4x4::GetTranslation(Vector3 &out) const
{
out.x=m_M[3][0];
out.y=m_M[3][1];
out.z=m_M[3][2];
}
inline void Matrix4x4::SetProjection( real fovx, real fovy, real nearclip, real farclip )
{
real w = 1.f/((real)tan( fovx*0.5f));
real h = 1.f/((real)tan( fovy*0.5f));
real q = farclip / (farclip - nearclip);
m_M[0][0] = w;
m_M[0][1] = 0.f;
m_M[0][2] = 0.f;
m_M[0][3] = 0.f;
m_M[1][0] = 0.f;
m_M[1][1] = h;
m_M[1][2] = 0.f;
m_M[1][3] = 0.f;
m_M[2][0] = 0.f;
m_M[2][1] = 0.f;
m_M[2][2] = q;
m_M[2][3] = 1.f;
m_M[3][0] = 0.f;
m_M[3][1] = 0.f;
m_M[3][2] = -q*nearclip;
m_M[3][3] = 0.f;
}
inline void Matrix4x4::ApplyScale(Vector3 Scl)
{
m_M[0][0] *= Scl.x;
m_M[1][0] *= Scl.x;
m_M[2][0] *= Scl.x;
m_M[0][1] *= Scl.y;
m_M[1][1] *= Scl.y;
m_M[2][1] *= Scl.y;
m_M[0][2] *= Scl.z;
m_M[1][2] *= Scl.z;
m_M[2][2] *= Scl.z;
}
#if defined(USE_INTRINSICS)
inline void MatrixMult( Matrix4x4 &out, Matrix4x4 const &a, Matrix4x4 const &b )
{
__m128 t114 = _mm_loadu_ps( b.m_M[0] );
__m128 t214 = _mm_loadu_ps( b.m_M[1] );
__m128 t314 = _mm_loadu_ps( b.m_M[2] );
__m128 t414 = _mm_loadu_ps( b.m_M[3] );
__m128 a114 = _mm_loadu_ps( a.m_M[0] );
__m128 a214 = _mm_loadu_ps( a.m_M[1] );
__m128 a314 = _mm_loadu_ps( a.m_M[2] );
__m128 a414 = _mm_loadu_ps( a.m_M[3] );
_MM_TRANSPOSE4_PS(t114, t214, t314, t414);
__m128 o11 = _mm_mul_ps( a114, t114 );
__m128 o12 = _mm_mul_ps( a114, t214 );
__m128 o13 = _mm_mul_ps( a114, t314 );
__m128 o14 = _mm_mul_ps( a114, t414 );
__m128 o21 = _mm_mul_ps( a214, t114 );
__m128 o22 = _mm_mul_ps( a214, t214 );
__m128 o23 = _mm_mul_ps( a214, t314 );
__m128 o24 = _mm_mul_ps( a214, t414 );
__m128 o31 = _mm_mul_ps( a314, t114 );
__m128 o32 = _mm_mul_ps( a314, t214 );
__m128 o33 = _mm_mul_ps( a314, t314 );
__m128 o34 = _mm_mul_ps( a314, t414 );
__m128 o41 = _mm_mul_ps( a414, t114 );
__m128 o42 = _mm_mul_ps( a414, t214 );
__m128 o43 = _mm_mul_ps( a414, t314 );
__m128 o44 = _mm_mul_ps( a414, t414 );
__m128 _11 = _mm_add_ss(o11,_mm_add_ss(_mm_shuffle_ps(o11, o11, 1),_mm_add_ss(_mm_shuffle_ps(o11, o11, 2),_mm_shuffle_ps(o11, o11, 3))));
__m128 _12 = _mm_add_ss(o12,_mm_add_ss(_mm_shuffle_ps(o12, o12, 1),_mm_add_ss(_mm_shuffle_ps(o12, o12, 2),_mm_shuffle_ps(o12, o12, 3))));
__m128 _13 = _mm_add_ss(o13,_mm_add_ss(_mm_shuffle_ps(o13, o13, 1),_mm_add_ss(_mm_shuffle_ps(o13, o13, 2),_mm_shuffle_ps(o13, o13, 3))));
__m128 _14 = _mm_add_ss(o14,_mm_add_ss(_mm_shuffle_ps(o14, o14, 1),_mm_add_ss(_mm_shuffle_ps(o14, o14, 2),_mm_shuffle_ps(o14, o14, 3))));
__m128 _21 = _mm_add_ss(o21,_mm_add_ss(_mm_shuffle_ps(o21, o21, 1),_mm_add_ss(_mm_shuffle_ps(o21, o21, 2),_mm_shuffle_ps(o21, o21, 3))));
__m128 _22 = _mm_add_ss(o22,_mm_add_ss(_mm_shuffle_ps(o22, o22, 1),_mm_add_ss(_mm_shuffle_ps(o22, o22, 2),_mm_shuffle_ps(o22, o22, 3))));
__m128 _23 = _mm_add_ss(o23,_mm_add_ss(_mm_shuffle_ps(o23, o23, 1),_mm_add_ss(_mm_shuffle_ps(o23, o23, 2),_mm_shuffle_ps(o23, o23, 3))));
__m128 _24 = _mm_add_ss(o24,_mm_add_ss(_mm_shuffle_ps(o24, o24, 1),_mm_add_ss(_mm_shuffle_ps(o24, o24, 2),_mm_shuffle_ps(o24, o24, 3))));
__m128 _31 = _mm_add_ss(o31,_mm_add_ss(_mm_shuffle_ps(o31, o31, 1),_mm_add_ss(_mm_shuffle_ps(o31, o31, 2),_mm_shuffle_ps(o31, o31, 3))));
__m128 _32 = _mm_add_ss(o32,_mm_add_ss(_mm_shuffle_ps(o32, o32, 1),_mm_add_ss(_mm_shuffle_ps(o32, o32, 2),_mm_shuffle_ps(o32, o32, 3))));
__m128 _33 = _mm_add_ss(o33,_mm_add_ss(_mm_shuffle_ps(o33, o33, 1),_mm_add_ss(_mm_shuffle_ps(o33, o33, 2),_mm_shuffle_ps(o33, o33, 3))));
__m128 _34 = _mm_add_ss(o34,_mm_add_ss(_mm_shuffle_ps(o34, o34, 1),_mm_add_ss(_mm_shuffle_ps(o34, o34, 2),_mm_shuffle_ps(o34, o34, 3))));
__m128 _41 = _mm_add_ss(o41,_mm_add_ss(_mm_shuffle_ps(o41, o41, 1),_mm_add_ss(_mm_shuffle_ps(o41, o41, 2),_mm_shuffle_ps(o41, o41, 3))));
__m128 _42 = _mm_add_ss(o42,_mm_add_ss(_mm_shuffle_ps(o42, o42, 1),_mm_add_ss(_mm_shuffle_ps(o42, o42, 2),_mm_shuffle_ps(o42, o42, 3))));
__m128 _43 = _mm_add_ss(o43,_mm_add_ss(_mm_shuffle_ps(o43, o43, 1),_mm_add_ss(_mm_shuffle_ps(o43, o43, 2),_mm_shuffle_ps(o43, o43, 3))));
__m128 _44 = _mm_add_ss(o44,_mm_add_ss(_mm_shuffle_ps(o44, o44, 1),_mm_add_ss(_mm_shuffle_ps(o44, o44, 2),_mm_shuffle_ps(o44, o44, 3))));
_mm_store_ss(&out._11, _11);
_mm_store_ss(&out._12, _12);
_mm_store_ss(&out._13, _13);
_mm_store_ss(&out._14, _14);
_mm_store_ss(&out._21, _21);
_mm_store_ss(&out._22, _22);
_mm_store_ss(&out._23, _23);
_mm_store_ss(&out._24, _24);
_mm_store_ss(&out._31, _31);
_mm_store_ss(&out._32, _32);
_mm_store_ss(&out._33, _33);
_mm_store_ss(&out._34, _34);
_mm_store_ss(&out._41, _41);
_mm_store_ss(&out._42, _42);
_mm_store_ss(&out._43, _43);
_mm_store_ss(&out._44, _44);
/* Matrix4x4 testout;
testout(0,0) = a(0,0) * b(0,0) + a(0,1) * b(1,0) + a(0,2) * b(2,0) + a(0,3) * b(3,0);
testout(0,1) = a(0,0) * b(0,1) + a(0,1) * b(1,1) + a(0,2) * b(2,1) + a(0,3) * b(3,1);
testout(0,2) = a(0,0) * b(0,2) + a(0,1) * b(1,2) + a(0,2) * b(2,2) + a(0,3) * b(3,2);
testout(0,3) = a(0,0) * b(0,3) + a(0,1) * b(1,3) + a(0,2) * b(2,3) + a(0,3) * b(3,3);
testout(1,0) = a(1,0) * b(0,0) + a(1,1) * b(1,0) + a(1,2) * b(2,0) + a(1,3) * b(3,0);
testout(1,1) = a(1,0) * b(0,1) + a(1,1) * b(1,1) + a(1,2) * b(2,1) + a(1,3) * b(3,1);
testout(1,2) = a(1,0) * b(0,2) + a(1,1) * b(1,2) + a(1,2) * b(2,2) + a(1,3) * b(3,2);
testout(1,3) = a(1,0) * b(0,3) + a(1,1) * b(1,3) + a(1,2) * b(2,3) + a(1,3) * b(3,3);
testout(2,0) = a(2,0) * b(0,0) + a(2,1) * b(1,0) + a(2,2) * b(2,0) + a(2,3) * b(3,0);
testout(2,1) = a(2,0) * b(0,1) + a(2,1) * b(1,1) + a(2,2) * b(2,1) + a(2,3) * b(3,1);
testout(2,2) = a(2,0) * b(0,2) + a(2,1) * b(1,2) + a(2,2) * b(2,2) + a(2,3) * b(3,2);
testout(2,3) = a(2,0) * b(0,3) + a(2,1) * b(1,3) + a(2,2) * b(2,3) + a(2,3) * b(3,3);
testout(3,0) = a(3,0) * b(0,0) + a(3,1) * b(1,0) + a(3,2) * b(2,0) + a(3,3) * b(3,0);
testout(3,1) = a(3,0) * b(0,1) + a(3,1) * b(1,1) + a(3,2) * b(2,1) + a(3,3) * b(3,1);
testout(3,2) = a(3,0) * b(0,2) + a(3,1) * b(1,2) + a(3,2) * b(2,2) + a(3,3) * b(3,2);
testout(3,3) = a(3,0) * b(0,3) + a(3,1) * b(1,3) + a(3,2) * b(2,3) + a(3,3) * b(3,3);*/
}
#else
inline void MatrixMult( Matrix4x4 &out, Matrix4x4 const &a, Matrix4x4 const &b )
{
out(0,0) = a(0,0) * b(0,0) + a(0,1) * b(1,0) + a(0,2) * b(2,0) + a(0,3) * b(3,0);
out(0,1) = a(0,0) * b(0,1) + a(0,1) * b(1,1) + a(0,2) * b(2,1) + a(0,3) * b(3,1);
out(0,2) = a(0,0) * b(0,2) + a(0,1) * b(1,2) + a(0,2) * b(2,2) + a(0,3) * b(3,2);
out(0,3) = a(0,0) * b(0,3) + a(0,1) * b(1,3) + a(0,2) * b(2,3) + a(0,3) * b(3,3);
out(1,0) = a(1,0) * b(0,0) + a(1,1) * b(1,0) + a(1,2) * b(2,0) + a(1,3) * b(3,0);
out(1,1) = a(1,0) * b(0,1) + a(1,1) * b(1,1) + a(1,2) * b(2,1) + a(1,3) * b(3,1);
out(1,2) = a(1,0) * b(0,2) + a(1,1) * b(1,2) + a(1,2) * b(2,2) + a(1,3) * b(3,2);
out(1,3) = a(1,0) * b(0,3) + a(1,1) * b(1,3) + a(1,2) * b(2,3) + a(1,3) * b(3,3);
out(2,0) = a(2,0) * b(0,0) + a(2,1) * b(1,0) + a(2,2) * b(2,0) + a(2,3) * b(3,0);
out(2,1) = a(2,0) * b(0,1) + a(2,1) * b(1,1) + a(2,2) * b(2,1) + a(2,3) * b(3,1);
out(2,2) = a(2,0) * b(0,2) + a(2,1) * b(1,2) + a(2,2) * b(2,2) + a(2,3) * b(3,2);
out(2,3) = a(2,0) * b(0,3) + a(2,1) * b(1,3) + a(2,2) * b(2,3) + a(2,3) * b(3,3);
out(3,0) = a(3,0) * b(0,0) + a(3,1) * b(1,0) + a(3,2) * b(2,0) + a(3,3) * b(3,0);
out(3,1) = a(3,0) * b(0,1) + a(3,1) * b(1,1) + a(3,2) * b(2,1) + a(3,3) * b(3,1);
out(3,2) = a(3,0) * b(0,2) + a(3,1) * b(1,2) + a(3,2) * b(2,2) + a(3,3) * b(3,2);
out(3,3) = a(3,0) * b(0,3) + a(3,1) * b(1,3) + a(3,2) * b(2,3) + a(3,3) * b(3,3);
}
#endif
inline void MatrixAdd( Matrix4x4 &out, Matrix4x4 const &a, Matrix4x4 const &b )
{
for (int i=0; i<4; i++)
{
for (int j=0; j<4; j++)
{
out(i,j) = a(i,j) + b(i,j);
}
}
}
inline Matrix4x4 operator*( Matrix4x4 const &a, Matrix4x4 const &b )
{
Matrix4x4 temp;
MatrixMult( temp, a, b );
return temp;
}
inline Matrix4x4 operator+( Matrix4x4 const &a, Matrix4x4 const &b )
{
Matrix4x4 temp;
MatrixAdd( temp, a, b );
return temp;
}
#if 0//defined(USE_INTRINSICS)
inline void MatrixMult( Vector3 &out, Matrix4x4 const &a, Vector3 const &b )
{
__m128 a114 = _mm_loadu_ps( a.m_M[0] );
__m128 a214 = _mm_loadu_ps( a.m_M[1] );
__m128 a314 = _mm_loadu_ps( a.m_M[2] );
__m128 a414 = _mm_loadu_ps( a.m_M[3] );
__m128 b114 = _mm_loadu_ps( b.m_Vec );
_MM_TRANSPOSE4_PS(a114, a214, a314, a414);
__m128 o11 = _mm_mul_ps( a114, b114 );
__m128 o12 = _mm_mul_ps( a214, b114 );
__m128 o13 = _mm_mul_ps( a314, b114 );
__m128 _11 = _mm_add_ss(o11,_mm_add_ss(_mm_shuffle_ps(o11, o11, 1),_mm_add_ss(_mm_shuffle_ps(o11, o11, 2),_mm_shuffle_ps(o11, o11, 3))));
__m128 _12 = _mm_add_ss(o12,_mm_add_ss(_mm_shuffle_ps(o12, o12, 1),_mm_add_ss(_mm_shuffle_ps(o12, o12, 2),_mm_shuffle_ps(o12, o12, 3))));
__m128 _13 = _mm_add_ss(o13,_mm_add_ss(_mm_shuffle_ps(o13, o13, 1),_mm_add_ss(_mm_shuffle_ps(o13, o13, 2),_mm_shuffle_ps(o13, o13, 3))));
}
#else
inline void MatrixMult( Vector3 &out, Matrix4x4 const &a, Vector3 const &b )
{
// out.x = a(0,0) * b.x + a(1,0) * b.y + a(2,0) * b.z + a(3,0);
// out.y = a(0,1) * b.x + a(1,1) * b.y + a(2,1) * b.z + a(3,1);
// out.z = a(0,2) * b.x + a(1,2) * b.y + a(2,2) * b.z + a(3,2);
out.x = a._11 * b.x + a._21 * b.y + a._31 * b.z + a._41;
out.y = a._12 * b.x + a._22 * b.y + a._32 * b.z + a._42;
out.z = a._13 * b.x + a._23 * b.y + a._33 * b.z + a._43;
}
#endif
inline Vector3 operator*( Matrix4x4 const &a, Vector3 const &b )
{
Vector3 temp;
MatrixMult( temp, a, b );
return temp;
}
inline void MatrixMult( Plane &out, Matrix4x4 const &a, Plane const &b )
{
Vector3 const &n = b.m_Normal;
Vector3 const &o = out.m_Normal;
float const d = b.m_Distance;
out.m_Normal.x = a._11 * n.x + a._21 * n.y + a._31 * n.z;
out.m_Normal.y = a._12 * n.x + a._22 * n.y + a._32 * n.z;
out.m_Normal.z = a._13 * n.x + a._23 * n.y + a._33 * n.z;
out.m_Distance =-a._41 * o.x - a._42 * o.y - a._43 * o.z - d;
// out.m_Distance = -b.m_Normal.x*M[3][0] - P.b*M[3][1] - P.c*M[3][2] - P.d
}
inline Plane operator*( Matrix4x4 const &a, Plane const &b )
{
Plane temp;
MatrixMult( temp, a, b );
return temp;
}
void MatrixInvert( Matrix4x4 &out, Matrix4x4 const &m );
#endif