Inter SSE指令集实现的3D数学库

#pragma once #include "WYVector4.h" class WYMatrix4x4; __declspec(align(16)) extern WYMatrix4x4 g_MatrixSSE_Identity; __declspec(align(16)) extern WYVector4 g_VectorSSE_quat2mat_x0; __declspec(align(16)) extern WYVector4 g_VectorSSE_quat2mat_x1; __declspec(align(16)) extern WYVector4 g_VectorSSE_quat2mat_x2; inline WYVector4 operator*(WYVector4 &v, WYMatrix4x4 &matrix); inline WYMatrix4x4 operator*(WYMatrix4x4 &a, WYMatrix4x4 &b); class WYMatrix4x4 { public: union { struct { float m_00, m_01, m_02, m_03; float m_10, m_11, m_12, m_13; float m_20, m_21, m_22, m_23; float m_30, m_31, m_32, m_33; }; struct { WYVector4 m_vec0, m_vec1, m_vec2, m_vec3; }; }; public: inline WYMatrix4x4() { } inline WYMatrix4x4( float f00, float f01, float f02, float f03, float f10, float f11, float f12, float f13, float f20, float f21, float f22, float f23, float f30, float f31, float f32, float f33 ) { m_00 = f00; m_01 = f01; m_02 = f02; m_03 = f03; m_10 = f10; m_11 = f11; m_12 = f12; m_13 = f13; m_20 = f20; m_21 = f21; m_22 = f22; m_23 = f23; m_30 = f30; m_31 = f31; m_32 = f32; m_33 = f33; } inline void Identity(void) { /* *this = g_MatrixSSE_Identity; */ m_vec0.Set(1.0f, 0.0f, 0.0f, 0.0f); m_vec1.Set(0.0f, 1.0f, 0.0f, 0.0f); m_vec2.Set(0.0f, 0.0f, 1.0f, 0.0f); m_vec3.Set(0.0f, 0.0f, 0.0f, 1.0f); } static WYMatrix4x4 IdentityMatrix(void) { return g_MatrixSSE_Identity; } inline void SetRow(int row, WYVector4 &vec) { assert(row>=0 && row<4); WYVector4 *rows = &m_vec0; rows[row] = vec; } inline void SetColumn(int column, WYVector4 &vec) { assert(column>=0 && column<4); float *p = &m_00 + column; p[0] = vec.GetX(); p[4] = vec.GetY(); p[8] = vec.GetZ(); p[12] = vec.GetW(); } inline WYVector4 &GetRow(int row) { assert(row>=0 && row<4); WYVector4 *rows = &m_vec0; return rows[row]; } inline WYVector4 GetColumn(int column) { assert(column>=0 && column<4); WYVector4 vec; float *p = &m_00 + column; vec.SetX(p[0]); vec.SetY(p[4]); vec.SetZ(p[8]); vec.SetW(p[12]); return vec; } // assume input vec.w equals 1 // result.w is garbage inline WYVector4 TransformVec3(WYVector4 &vec) { WYVector4 result; __m128 vxxxx = _mm_shuffle_ps(vec.m_vec, vec.m_vec, _MM_SHUFFLE(0, 0, 0, 0) ); __m128 vyyyy = _mm_shuffle_ps(vec.m_vec, vec.m_vec, _MM_SHUFFLE(1, 1, 1, 1) ); __m128 vzzzz = _mm_shuffle_ps(vec.m_vec, vec.m_vec, _MM_SHUFFLE(2, 2, 2, 2) ); __m128 temp; result.m_vec = _mm_mul_ps(vxxxx, m_vec0.m_vec); temp = _mm_mul_ps(vyyyy, m_vec1.m_vec); result.m_vec = _mm_add_ps(result.m_vec, temp); temp = _mm_mul_ps(vzzzz, m_vec2.m_vec); result.m_vec = _mm_add_ps(result.m_vec, temp); result.m_vec = _mm_add_ps(result.m_vec, m_vec3.m_vec); return result; } // this = R * this void Rotate( WYVector4 &v, float radian) { float fSin, fCos; WYFastMath::SinCos(radian, fSin, fCos); float fOneMinusCos = 1.0f - fCos; WYVector4 xxxx = v.GetXXXX(); WYVector4 yyyy = v.GetYYYY(); WYVector4 zzzz = v.GetZZZZ(); WYVector4 SinVector(fSin); WYVector4 v_Mul_Sin = v * SinVector; WYVector4 multipler = fOneMinusCos * v; multipler.SetW(0.0f); WYVector4 add; WYMatrix4x4 local; local.Identity(); add.Set(fCos, v_Mul_Sin.GetZ(), -v_Mul_Sin.GetY(), 0.0f); local.m_vec0 = xxxx * multipler + add; add.Set(-v_Mul_Sin.GetZ(), fCos, v_Mul_Sin.GetX(), 0.0f); local.m_vec1 = yyyy * multipler + add; add.Set(v_Mul_Sin.GetY(), -v_Mul_Sin.GetX(), fCos, 0.0f); local.m_vec2 = zzzz * multipler + add; *this = local * (*this); } // replace current matrix void Rotate_Replace(WYVector4 &v, float radian) { float x = v.x; float y = v.y; float z = v.z; float w = v.w; float fSin, fCos; WYFastMath::SinCos(radian, fSin, fCos); m_00 = ( x * x ) * ( 1.0f - fCos ) + fCos; m_01 = ( x * y ) * ( 1.0f - fCos ) + (z * fSin); m_02 = ( x * z ) * ( 1.0f - fCos ) - (y * fSin); m_03 = 0.0f; m_10 = ( y * x ) * ( 1.0f - fCos ) - (z * fSin); m_11 = ( y * y ) * ( 1.0f - fCos ) + fCos ; m_12 = ( y * z ) * ( 1.0f - fCos ) + (x * fSin); m_13 = 0.0f; m_20 = ( z * x ) * ( 1.0f - fCos ) + (y * fSin); m_21 = ( z * y ) * ( 1.0f - fCos ) - (x * fSin); m_22 = ( z * z ) * ( 1.0f - fCos ) + fCos; m_23 = 0.0f; m_30 = 0.0f; m_31 = 0.0f; m_32 = 0.0f; m_33 = 1.0f; } // this = Rx * this void RotateX( const float radian ) { float Sin, Cos; WYFastMath::SinCos(radian, Sin, Cos); WYVector4 SinVector(Sin); WYVector4 CosVector(Cos); WYVector4 t0 = m_vec1 * CosVector + m_vec2 * SinVector; WYVector4 t1 = m_vec2 * CosVector - m_vec1 * SinVector; m_vec1 = t0; m_vec2 = t1; } // replace current matrix void RotateX_Replace( const float radian ) { float fSin, fCos; WYFastMath::SinCos(radian, fSin, fCos); m_vec0.Set(1.0f, 0.0f, 0.0f, 0.0f); m_vec1.Set(0.0f, fCos, fSin, 0.0f); m_vec2.Set(0.0f, -fSin, fCos, 0.0f); m_vec3.Set(0.0f, 0.0f, 0.0f, 1.0f); } // this = Ry * this void RotateY( const float radian ) { float Sin, Cos; WYFastMath::SinCos(radian, Sin, Cos); WYVector4 SinVector(Sin); WYVector4 CosVector(Cos); WYVector4 t0 = m_vec0 * CosVector - m_vec2 * SinVector; WYVector4 t1 = m_vec0 * SinVector + m_vec2 * CosVector; m_vec0 = t0; m_vec2 = t1; } // replace current matrix void RotateY_Replace( const float radian ) { float fSin, fCos; WYFastMath::SinCos(radian, fSin, fCos); m_vec0.Set( fCos, 0.0f,-fSin, 0.0f); m_vec1.Set( 0.0f, 1.0f, 0.0f, 0.0f); m_vec2.Set( fSin, 0.0f, fCos, 0.0f); m_vec3.Set( 0.0f, 0.0f, 0.0f, 1.0f); } // this = Rz * this void RotateZ( const float radian ) { float Sin, Cos; WYFastMath::SinCos(radian, Sin, Cos); WYVector4 SinVector(Sin); WYVector4 CosVector(Cos); WYVector4 t0 = m_vec0 * CosVector + m_vec1 * SinVector; WYVector4 t1 = m_vec1 * CosVector - m_vec0 * SinVector; m_vec0 = t0; m_vec1 = t1; } // replace current matrix void RotateZ_Replace( const float radian ) { float fSin, fCos; WYFastMath::SinCos(radian, fSin, fCos); m_vec0.Set( fCos, fSin, 0.0f, 0.0f); m_vec1.Set(-fSin, fCos, 0.0f, 0.0f); m_vec2.Set( 0.0f, 0.0f, 1.0f, 0.0f); m_vec3.Set( 0.0f, 0.0f, 0.0f, 1.0f); } void Scale(WYVector4 &scale) { m_vec0 *= scale; m_vec1 *= scale; m_vec2 *= scale; m_vec3 *= scale; } void Scale(float x, float y, float z) { WYVector4 vScale(x, y, z, 1.0f); Scale(vScale); //Matrix4x4 scale_replace(x, y, z); //*this = *this * scale; } void Scale_Replace(WYVector4 &scale) { Scale_Replace(scale[0], scale[1], scale[2]); } void Scale_Replace(float x, float y, float z) { m_vec0.Set(x, 0, 0, 0); m_vec1.Set(0, y, 0, 0); m_vec2.Set(0, 0, z, 0); m_vec3.Set(0, 0, 0, 1); } inline WYMatrix4x4 &FastInvert(void) { WYVector4 pos = VectorInvertSign(m_vec3); m_vec3 = g_MatrixSSE_Identity.m_vec3; Transpose(); Translate(pos); return *this; } WYMatrix4x4 &Invert(void); inline WYMatrix4x4 &Transpose(void) { _MM_TRANSPOSE4_PS(m_vec0.m_vec, m_vec1.m_vec, m_vec2.m_vec, m_vec3.m_vec); return *this; } // this = T * this void Translate(float x, float y, float z) { WYVector4 xxxx(x); WYVector4 yyyy(y); WYVector4 zzzz(z); m_vec3 += m_vec0 * xxxx + m_vec1 * yyyy + m_vec2 * zzzz; } // this = T * this void Translate(WYVector4 &v) { WYVector4 xxxx = v.GetXXXX(); WYVector4 yyyy = v.GetYYYY(); WYVector4 zzzz = v.GetZZZZ(); m_vec3 += m_vec0 * xxxx + m_vec1 * yyyy + m_vec2 * zzzz; } void Translate_Replace(float x, float y, float z) { Identity(); m_vec3[0] = x; m_vec3[1] = y;