diff options
Diffstat (limited to 'Runtime/Math/Matrix4x4.h')
| -rw-r--r-- | Runtime/Math/Matrix4x4.h | 410 |
1 files changed, 410 insertions, 0 deletions
diff --git a/Runtime/Math/Matrix4x4.h b/Runtime/Math/Matrix4x4.h new file mode 100644 index 0000000..2e14091 --- /dev/null +++ b/Runtime/Math/Matrix4x4.h @@ -0,0 +1,410 @@ +#ifndef MATRIX4X4_H +#define MATRIX4X4_H + +#include "Vector3.h" +#include "Vector4.h" +#include "Runtime/Utilities/Prefetch.h" +#include "Runtime/Misc/CPUInfo.h" +#include "Runtime/Modules/ExportModules.h" + +#if (UNITY_SUPPORTS_SSE || UNITY_SUPPORTS_VMX) +# include "Simd/SimdMath.h" +#endif + + +class Matrix3x3f; +class Matrix4x4f; +class Quaternionf; + +/// Uniform transform scales x, y, z in the same amount, +/// NonUniform transform scales x, y, z differently and might contain skew. +/// kOddNegativeScaleTransform means that FrontFace(CCW) should be used (An odd number of scale axes is negative) +enum TransformType +{ + kNoScaleTransform = 0, + kUniformScaleTransform = 1 << 0, + kNonUniformScaleTransform = 1 << 1, + kOddNegativeScaleTransform = 1 << 2 +}; +ENUM_FLAGS(TransformType); + +inline bool IsNoScaleTransform (TransformType type) { return type == kNoScaleTransform; } +inline bool IsNonUniformScaleTransform (TransformType type) { return (type & kNonUniformScaleTransform) != 0; } + +TransformType ComputeTransformType (const Matrix4x4f& matrix, float& outUniformScale, float epsilon = Vector3f::epsilon); + +bool InvertMatrix4x4_Full( const float* m, float* out ); +bool InvertMatrix4x4_General3D( const float* m, float* out ); + + +/// Matrices in unity are column major. +class EXPORT_COREMODULE Matrix4x4f +{ + public: + float m_Data[16]; + + + ///@todo: Can't be Transfer optimized because Transfer doesn't write the same as memory layout + DECLARE_SERIALIZE_NO_PPTR (Matrix4x4f) + + Matrix4x4f () {} + Matrix4x4f (const Matrix3x3f &other); + explicit Matrix4x4f (const float data[16]); + + float& Get (int row, int column) { return m_Data[row + (column*4)]; } + const float& Get (int row, int column)const { return m_Data[row + (column*4)]; } + float* GetPtr () { return m_Data; } + const float* GetPtr ()const { return m_Data; } + + float operator [] (int index) const { return m_Data[index]; } + float& operator [] (int index) { return m_Data[index]; } + + Matrix4x4f& operator *= (const Matrix4x4f& inM); + + Matrix4x4f& operator = (const Matrix3x3f& m); + + Vector3f MultiplyVector3 (const Vector3f& inV) const; + void MultiplyVector3 (const Vector3f& inV, Vector3f& output) const; + bool PerspectiveMultiplyVector3( const Vector3f& inV, Vector3f& output ) const; + Vector3f MultiplyPoint3 (const Vector3f& inV) const; + void MultiplyPoint3 (const Vector3f& inV, Vector3f& output) const; + bool PerspectiveMultiplyPoint3( const Vector3f& inV, Vector3f& output ) const; + Vector3f InverseMultiplyPoint3Affine (const Vector3f& inV) const; + Vector3f InverseMultiplyVector3Affine (const Vector3f& inV) const; + + bool IsIdentity (float epsilon = Vector3f::epsilon) const; + + double GetDeterminant() const; + + Matrix4x4f& Invert_Full() { + InvertMatrix4x4_Full( m_Data, m_Data ); + return *this; + } + static bool Invert_Full( const Matrix4x4f &inM, Matrix4x4f &outM ) { + return InvertMatrix4x4_Full( inM.m_Data, outM.m_Data ); + } + static bool Invert_General3D( const Matrix4x4f &inM, Matrix4x4f &outM ) { + return InvertMatrix4x4_General3D( inM.m_Data, outM.m_Data ); + } + + Matrix4x4f& Transpose (); + + Matrix4x4f& Copy (const Matrix4x4f& inM); + + Matrix4x4f& SetIdentity (); + Matrix4x4f& SetPerspective( float fovy, float aspect, float zNear, float zFar ); + // rad = Deg2Rad(fovy/2), contanHalfFOV = cos(rad)/sin(rad) + Matrix4x4f& SetPerspectiveCotan( float cotanHalfFOV, float zNear, float zFar ); + Matrix4x4f& SetOrtho( float left, float right, float bottom, float top, float zNear, float zFar ); + Matrix4x4f& SetFrustum( float left, float right, float bottom, float top, float nearval, float farval ); + + Vector3f GetAxisX() const; + Vector3f GetAxisY() const; + Vector3f GetAxisZ() const; + Vector3f GetPosition() const; + Vector4f GetRow(int row) const; + Vector4f GetColumn(int col) const; + // these set only these components of the matrix, everything else is untouched! + void SetAxisX( const Vector3f& v ); + void SetAxisY( const Vector3f& v ); + void SetAxisZ( const Vector3f& v ); + void SetPosition( const Vector3f& v ); + void SetRow( int row, const Vector4f& v ); + void SetColumn( int col, const Vector4f& v ); + + Matrix4x4f& SetTranslate (const Vector3f& inTrans); + Matrix4x4f& SetOrthoNormalBasis (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ); + Matrix4x4f& SetOrthoNormalBasisInverse (const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ); + Matrix4x4f& SetScale (const Vector3f& inScale); + Matrix4x4f& SetPositionAndOrthoNormalBasis (const Vector3f& inPosition, const Vector3f& inX, const Vector3f& inY, const Vector3f& inZ); + + Matrix4x4f& Translate (const Vector3f& inTrans); + Matrix4x4f& Scale (const Vector3f& inScale); + + Matrix4x4f& SetFromToRotation (const Vector3f& from, const Vector3f& to); + + void SetTR (const Vector3f& pos, const Quaternionf& q); + void SetTRS (const Vector3f& pos, const Quaternionf& q, const Vector3f& s); + void SetTRInverse (const Vector3f& pos, const Quaternionf& q); + + static const Matrix4x4f identity; +}; + +bool CompareApproximately (const Matrix4x4f& lhs, const Matrix4x4f& rhs, float dist = Vector3f::epsilon); + +/// Transforms an array of vertices. input may be the same as output. +void EXPORT_COREMODULE TransformPoints3x3 (const Matrix4x4f &matrix, const Vector3f* input, Vector3f* ouput, int count); +void EXPORT_COREMODULE TransformPoints3x4 (const Matrix4x4f &matrix, const Vector3f* input, Vector3f* ouput, int count); +void EXPORT_COREMODULE TransformPoints3x3 (const Matrix4x4f &matrix, const Vector3f* input, size_t inStride, Vector3f* ouput, size_t outStride, int count); +void EXPORT_COREMODULE TransformPoints3x4 (const Matrix4x4f &matrix, const Vector3f* input, size_t inStride, Vector3f* ouput, size_t outStride, int count); + +void MultiplyMatrices3x4( const Matrix4x4f& lhs, const Matrix4x4f& rhs, Matrix4x4f& res); + +void MultiplyMatrices4x4REF(const Matrix4x4f* __restrict lhs, const Matrix4x4f* __restrict rhs, Matrix4x4f* __restrict res); +void CopyMatrixREF( const float* __restrict lhs, float* __restrict res); +void TransposeMatrix4x4REF (const Matrix4x4f* __restrict lhs, Matrix4x4f* __restrict res); + +// foreach R[i] = A[i] * B[i] +void MultiplyMatrixArray4x4REF(const Matrix4x4f* __restrict arrayA, const Matrix4x4f* __restrict arrayB, + Matrix4x4f* __restrict arrayRes, size_t count); +// foreach R[i] = BASE * A[i] * B[i] +void MultiplyMatrixArrayWithBase4x4REF (const Matrix4x4f* __restrict base, + const Matrix4x4f* __restrict arrayA, const Matrix4x4f* __restrict arrayB, + Matrix4x4f* __restrict arrayRes, size_t count); + +#if (UNITY_AUTO_DETECT_VECTOR_UNIT && UNITY_SUPPORTS_SSE) +# define DECLARE_SIMD_FUNC(f) f##Simd +#else +# define DECLARE_SIMD_FUNC(f) f +#endif + +#if (UNITY_SUPPORTS_SSE || UNITY_SUPPORTS_VMX) +# include "Simd/Matrix4x4Simd.h" +#elif UNITY_SUPPORTS_NEON + +#if UNITY_ANDROID || UNITY_WINRT || UNITY_BB10 || UNITY_TIZEN + #define MultiplyMatrices4x4_NEON _MultiplyMatrices4x4_NEON + #define CopyMatrix_NEON _CopyMatrix_NEON + #define TransposeMatrix4x4_NEON _TransposeMatrix4x4_NEON + + #define MultiplyMatrixArray4x4_NEON _MultiplyMatrixArray4x4_NEON + #define MultiplyMatrixArrayWithBase4x4_NEON _MultiplyMatrixArrayWithBase4x4_NEON +#endif + +extern "C" +{ + void CopyMatrix_NEON(const float* __restrict lhs, float* __restrict res); + void TransposeMatrix4x4_NEON(const Matrix4x4f* __restrict lhs, Matrix4x4f* __restrict res); + + void MultiplyMatrices4x4_NEON(const Matrix4x4f* __restrict lhs, const Matrix4x4f* __restrict rhs, Matrix4x4f* __restrict res); + void MultiplyMatrixArray4x4_NEON(const Matrix4x4f* __restrict arrayA, const Matrix4x4f* __restrict arrayB, + Matrix4x4f* __restrict arrayRes, size_t count); + void MultiplyMatrixArrayWithBase4x4_NEON(const Matrix4x4f* __restrict base, + const Matrix4x4f* __restrict arrayA, const Matrix4x4f* __restrict arrayB, + Matrix4x4f* __restrict arrayRes, size_t count); +} + +#if UNITY_ANDROID && UNITY_SUPPORTS_NEON && UNITY_SUPPORTS_VFP + + #define MultiplyMatrices4x4_VFP _MultiplyMatrices4x4_VFP + #define MultiplyMatrixArray4x4_VFP _MultiplyMatrixArray4x4_VFP + + extern "C" + { + void MultiplyMatrices4x4_VFP(const Matrix4x4f* __restrict lhs, const Matrix4x4f* __restrict rhs, Matrix4x4f* __restrict res); + void MultiplyMatrixArray4x4_VFP(const Matrix4x4f* __restrict arrayA, const Matrix4x4f* __restrict arrayB, + Matrix4x4f* __restrict arrayRes, size_t count); + } + + #define CopyMatrix(a,b) CPUInfo::HasNEONSupport() ? CopyMatrix_NEON(a,b) : CopyMatrixREF(a,b) + #define TransposeMatrix4x4(a,b) CPUInfo::HasNEONSupport() ? TransposeMatrix4x4_NEON(a,b) : TransposeMatrix4x4REF(a,b) + + #define MultiplyMatrices4x4(a,b,c) CPUInfo::HasNEONSupport() ? MultiplyMatrices4x4_NEON(a,b,c) : MultiplyMatrices4x4_VFP(a,b,c) + #define MultiplyMatrixArray4x4(a,b,c,d) CPUInfo::HasNEONSupport() ? MultiplyMatrixArray4x4_NEON(a,b,c,d) : MultiplyMatrixArray4x4_VFP(a,b,c,d) + #define MultiplyMatrixArrayWithBase4x4(a,b,c,d,e) CPUInfo::HasNEONSupport() ? MultiplyMatrixArrayWithBase4x4_NEON(a,b,c,d,e) : MultiplyMatrixArrayWithBase4x4REF(a,b,c,d,e) + +#else + + #define CopyMatrix CopyMatrix_NEON + #define TransposeMatrix4x4 TransposeMatrix4x4_NEON + + #define MultiplyMatrices4x4 MultiplyMatrices4x4_NEON + #define MultiplyMatrixArray4x4 MultiplyMatrixArray4x4_NEON + #define MultiplyMatrixArrayWithBase4x4 MultiplyMatrixArrayWithBase4x4_NEON + +#endif + +#elif UNITY_SUPPORTS_VFP + +#if UNITY_ANDROID + #define MultiplyMatrices4x4_VFP _MultiplyMatrices4x4_VFP + #define MultiplyMatrixArray4x4_VFP _MultiplyMatrixArray4x4_VFP +#endif + +extern "C" +{ + void MultiplyMatrices4x4_VFP(const Matrix4x4f* __restrict lhs, const Matrix4x4f* __restrict rhs, Matrix4x4f* __restrict res); + void MultiplyMatrixArray4x4_VFP(const Matrix4x4f* __restrict arrayA, const Matrix4x4f* __restrict arrayB, + Matrix4x4f* __restrict arrayRes, size_t count); +} + + #define CopyMatrix CopyMatrixREF + #define TransposeMatrix4x4 TransposeMatrix4x4REF + + #define MultiplyMatrices4x4 MultiplyMatrices4x4_VFP + #define MultiplyMatrixArray4x4 MultiplyMatrixArray4x4_VFP + #define MultiplyMatrixArrayWithBase4x4 MultiplyMatrixArrayWithBase4x4REF + + +#else + + #define CopyMatrix CopyMatrixREF + #define TransposeMatrix4x4 TransposeMatrix4x4REF + + #define MultiplyMatrices4x4 MultiplyMatrices4x4REF + #define MultiplyMatrixArray4x4 MultiplyMatrixArray4x4REF + #define MultiplyMatrixArrayWithBase4x4 MultiplyMatrixArrayWithBase4x4REF + +#endif + + +inline Vector3f Matrix4x4f::GetAxisX() const { + return Vector3f( Get(0,0), Get(1,0), Get(2,0) ); +} +inline Vector3f Matrix4x4f::GetAxisY() const { + return Vector3f( Get(0,1), Get(1,1), Get(2,1) ); +} +inline Vector3f Matrix4x4f::GetAxisZ() const { + return Vector3f( Get(0,2), Get(1,2), Get(2,2) ); +} +inline Vector3f Matrix4x4f::GetPosition() const { + return Vector3f( Get(0,3), Get(1,3), Get(2,3) ); +} +inline Vector4f Matrix4x4f::GetRow(int row) const { + return Vector4f( Get(row,0), Get(row,1), Get(row,2), Get(row,3) ); +} +inline Vector4f Matrix4x4f::GetColumn(int col) const { + return Vector4f( Get(0,col), Get(1,col), Get(2,col), Get(3,col) ); +} +inline void Matrix4x4f::SetAxisX( const Vector3f& v ) { + Get(0,0) = v.x; Get(1,0) = v.y; Get(2,0) = v.z; +} +inline void Matrix4x4f::SetAxisY( const Vector3f& v ) { + Get(0,1) = v.x; Get(1,1) = v.y; Get(2,1) = v.z; +} +inline void Matrix4x4f::SetAxisZ( const Vector3f& v ) { + Get(0,2) = v.x; Get(1,2) = v.y; Get(2,2) = v.z; +} +inline void Matrix4x4f::SetPosition( const Vector3f& v ) { + Get(0,3) = v.x; Get(1,3) = v.y; Get(2,3) = v.z; +} +inline void Matrix4x4f::SetRow( int row, const Vector4f& v ) { + Get(row,0) = v.x; Get(row,1) = v.y; Get(row,2) = v.z; Get(row,3) = v.w; +} +inline void Matrix4x4f::SetColumn( int col, const Vector4f& v ) { + Get(0,col) = v.x; Get(1,col) = v.y; Get(2,col) = v.z; Get(3,col) = v.w; +} + + +inline Vector3f Matrix4x4f::MultiplyPoint3 (const Vector3f& v) const +{ + Vector3f res; + res.x = m_Data[0] * v.x + m_Data[4] * v.y + m_Data[ 8] * v.z + m_Data[12]; + res.y = m_Data[1] * v.x + m_Data[5] * v.y + m_Data[ 9] * v.z + m_Data[13]; + res.z = m_Data[2] * v.x + m_Data[6] * v.y + m_Data[10] * v.z + m_Data[14]; + return res; +} + +inline void Matrix4x4f::MultiplyPoint3 (const Vector3f& v, Vector3f& output) const +{ + output.x = m_Data[0] * v.x + m_Data[4] * v.y + m_Data[ 8] * v.z + m_Data[12]; + output.y = m_Data[1] * v.x + m_Data[5] * v.y + m_Data[ 9] * v.z + m_Data[13]; + output.z = m_Data[2] * v.x + m_Data[6] * v.y + m_Data[10] * v.z + m_Data[14]; +} + + +inline Vector3f Matrix4x4f::MultiplyVector3 (const Vector3f& v) const +{ + Vector3f res; + res.x = m_Data[0] * v.x + m_Data[4] * v.y + m_Data[ 8] * v.z; + res.y = m_Data[1] * v.x + m_Data[5] * v.y + m_Data[ 9] * v.z; + res.z = m_Data[2] * v.x + m_Data[6] * v.y + m_Data[10] * v.z; + return res; +} + +inline void Matrix4x4f::MultiplyVector3 (const Vector3f& v, Vector3f& output) const +{ + output.x = m_Data[0] * v.x + m_Data[4] * v.y + m_Data[ 8] * v.z; + output.y = m_Data[1] * v.x + m_Data[5] * v.y + m_Data[ 9] * v.z; + output.z = m_Data[2] * v.x + m_Data[6] * v.y + m_Data[10] * v.z; +} + + +inline bool Matrix4x4f::PerspectiveMultiplyPoint3( const Vector3f& v, Vector3f& output ) const +{ + Vector3f res; + float w; + res.x = Get (0, 0) * v.x + Get (0, 1) * v.y + Get (0, 2) * v.z + Get (0, 3); + res.y = Get (1, 0) * v.x + Get (1, 1) * v.y + Get (1, 2) * v.z + Get (1, 3); + res.z = Get (2, 0) * v.x + Get (2, 1) * v.y + Get (2, 2) * v.z + Get (2, 3); + w = Get (3, 0) * v.x + Get (3, 1) * v.y + Get (3, 2) * v.z + Get (3, 3); + if( Abs(w) > 1.0e-7f ) + { + float invW = 1.0f / w; + output.x = res.x * invW; + output.y = res.y * invW; + output.z = res.z * invW; + return true; + } + else + { + output.x = 0.0f; + output.y = 0.0f; + output.z = 0.0f; + return false; + } +} + +inline bool Matrix4x4f::PerspectiveMultiplyVector3( const Vector3f& v, Vector3f& output ) const +{ + Vector3f res; + float w; + res.x = Get (0, 0) * v.x + Get (0, 1) * v.y + Get (0, 2) * v.z; + res.y = Get (1, 0) * v.x + Get (1, 1) * v.y + Get (1, 2) * v.z; + res.z = Get (2, 0) * v.x + Get (2, 1) * v.y + Get (2, 2) * v.z; + w = Get (3, 0) * v.x + Get (3, 1) * v.y + Get (3, 2) * v.z; + if( Abs(w) > 1.0e-7f ) + { + float invW = 1.0f / w; + output.x = res.x * invW; + output.y = res.y * invW; + output.z = res.z * invW; + return true; + } + else + { + output.x = 0.0f; + output.y = 0.0f; + output.z = 0.0f; + return false; + } +} + +inline Vector3f Matrix4x4f::InverseMultiplyPoint3Affine (const Vector3f& inV) const +{ + Vector3f v (inV.x - Get (0, 3), inV.y - Get (1, 3), inV.z - Get (2, 3)); + Vector3f res; + res.x = Get (0, 0) * v.x + Get (1, 0) * v.y + Get (2, 0) * v.z; + res.y = Get (0, 1) * v.x + Get (1, 1) * v.y + Get (2, 1) * v.z; + res.z = Get (0, 2) * v.x + Get (1, 2) * v.y + Get (2, 2) * v.z; + return res; +} + +inline Vector3f Matrix4x4f::InverseMultiplyVector3Affine (const Vector3f& v) const +{ + Vector3f res; + res.x = Get (0, 0) * v.x + Get (1, 0) * v.y + Get (2, 0) * v.z; + res.y = Get (0, 1) * v.x + Get (1, 1) * v.y + Get (2, 1) * v.z; + res.z = Get (0, 2) * v.x + Get (1, 2) * v.y + Get (2, 2) * v.z; + return res; +} + +template<class TransferFunction> inline +void Matrix4x4f::Transfer (TransferFunction& t) +{ + t.Transfer (Get (0, 0), "e00"); t.Transfer (Get (0, 1), "e01"); t.Transfer (Get (0, 2), "e02"); t.Transfer (Get (0, 3), "e03"); + t.Transfer (Get (1, 0), "e10"); t.Transfer (Get (1, 1), "e11"); t.Transfer (Get (1, 2), "e12"); t.Transfer (Get (1, 3), "e13"); + t.Transfer (Get (2, 0), "e20"); t.Transfer (Get (2, 1), "e21"); t.Transfer (Get (2, 2), "e22"); t.Transfer (Get (2, 3), "e23"); + t.Transfer (Get (3, 0), "e30"); t.Transfer (Get (3, 1), "e31"); t.Transfer (Get (3, 2), "e32"); t.Transfer (Get (3, 3), "e33"); +} + +inline bool IsFinite (const Matrix4x4f& f) +{ + return + IsFinite(f.m_Data[0]) & IsFinite(f.m_Data[1]) & IsFinite(f.m_Data[2]) & + IsFinite(f.m_Data[4]) & IsFinite(f.m_Data[5]) & IsFinite(f.m_Data[6]) & + IsFinite(f.m_Data[8]) & IsFinite(f.m_Data[9]) & IsFinite(f.m_Data[10]) & + IsFinite(f.m_Data[12]) & IsFinite(f.m_Data[13]) & IsFinite(f.m_Data[14]) & IsFinite(f.m_Data[15]); +} + +#endif |