From 7ceba7761637443692384ece1d54ce3fde09b635 Mon Sep 17 00:00:00 2001 From: nillerusr Date: Wed, 26 Apr 2023 19:56:15 +0000 Subject: [PATCH] WIP: move vmatrix functions to header and inline them --- mathlib/mathlib_base.cpp | 58 -- mathlib/vmatrix.cpp | 1264 -------------------------- public/mathlib/math_pfns.h | 12 +- public/mathlib/mathlib.h | 31 +- public/mathlib/vector4d.h | 2 +- public/mathlib/vmatrix.h | 1730 +++++++++++++++++++++++++++++------- 6 files changed, 1443 insertions(+), 1654 deletions(-) diff --git a/mathlib/mathlib_base.cpp b/mathlib/mathlib_base.cpp index 3b8fbe2f..60113379 100644 --- a/mathlib/mathlib_base.cpp +++ b/mathlib/mathlib_base.cpp @@ -420,13 +420,6 @@ void MatrixGetColumn( const matrix3x4_t& in, int column, Vector &out ) out.z = in[2][column]; } -void MatrixSetColumn( const Vector &in, int column, matrix3x4_t& out ) -{ - out[0][column] = in.x; - out[1][column] = in.y; - out[2][column] = in.z; -} - void MatrixScaleBy ( const float flScale, matrix3x4_t &out ) { out[0][0] *= flScale; @@ -1092,57 +1085,6 @@ void SetScaleMatrix( float x, float y, float z, matrix3x4_t &dst ) dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = z; dst[2][3] = 0.0f; } - -//----------------------------------------------------------------------------- -// Purpose: Builds the matrix for a counterclockwise rotation about an arbitrary axis. -// -// | ax2 + (1 - ax2)cosQ axay(1 - cosQ) - azsinQ azax(1 - cosQ) + aysinQ | -// Ra(Q) = | axay(1 - cosQ) + azsinQ ay2 + (1 - ay2)cosQ ayaz(1 - cosQ) - axsinQ | -// | azax(1 - cosQ) - aysinQ ayaz(1 - cosQ) + axsinQ az2 + (1 - az2)cosQ | -// -// Input : mat - -// vAxisOrRot - -// angle - -//----------------------------------------------------------------------------- -void MatrixBuildRotationAboutAxis( const Vector &vAxisOfRot, float angleDegrees, matrix3x4_t &dst ) -{ - float radians; - float axisXSquared; - float axisYSquared; - float axisZSquared; - float fSin; - float fCos; - - radians = angleDegrees * ( M_PI / 180.0 ); - fSin = sin( radians ); - fCos = cos( radians ); - - axisXSquared = vAxisOfRot[0] * vAxisOfRot[0]; - axisYSquared = vAxisOfRot[1] * vAxisOfRot[1]; - axisZSquared = vAxisOfRot[2] * vAxisOfRot[2]; - - // Column 0: - dst[0][0] = axisXSquared + (1 - axisXSquared) * fCos; - dst[1][0] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) + vAxisOfRot[2] * fSin; - dst[2][0] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) - vAxisOfRot[1] * fSin; - - // Column 1: - dst[0][1] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) - vAxisOfRot[2] * fSin; - dst[1][1] = axisYSquared + (1 - axisYSquared) * fCos; - dst[2][1] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) + vAxisOfRot[0] * fSin; - - // Column 2: - dst[0][2] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) + vAxisOfRot[1] * fSin; - dst[1][2] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) - vAxisOfRot[0] * fSin; - dst[2][2] = axisZSquared + (1 - axisZSquared) * fCos; - - // Column 3: - dst[0][3] = 0; - dst[1][3] = 0; - dst[2][3] = 0; -} - - //----------------------------------------------------------------------------- // Computes the transpose //----------------------------------------------------------------------------- diff --git a/mathlib/vmatrix.cpp b/mathlib/vmatrix.cpp index 01a987b4..31c1000e 100644 --- a/mathlib/vmatrix.cpp +++ b/mathlib/vmatrix.cpp @@ -24,1270 +24,6 @@ // Helper functions. // ------------------------------------------------------------------------------------------- // -#ifndef VECTOR_NO_SLOW_OPERATIONS -VMatrix SetupMatrixIdentity() -{ - return VMatrix( - 1.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f); -} - -VMatrix SetupMatrixTranslation(const Vector &vTranslation) -{ - return VMatrix( - 1.0f, 0.0f, 0.0f, vTranslation.x, - 0.0f, 1.0f, 0.0f, vTranslation.y, - 0.0f, 0.0f, 1.0f, vTranslation.z, - 0.0f, 0.0f, 0.0f, 1.0f - ); -} - -VMatrix SetupMatrixScale(const Vector &vScale) -{ - return VMatrix( - vScale.x, 0.0f, 0.0f, 0.0f, - 0.0f, vScale.y, 0.0f, 0.0f, - 0.0f, 0.0f, vScale.z, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f - ); -} - -VMatrix SetupMatrixReflection(const VPlane &thePlane) -{ - VMatrix mReflect, mBack, mForward; - Vector vOrigin, N; - - N = thePlane.m_Normal; - - mReflect.Init( - -2.0f*N.x*N.x + 1.0f, -2.0f*N.x*N.y, -2.0f*N.x*N.z, 0.0f, - -2.0f*N.y*N.x, -2.0f*N.y*N.y + 1.0f, -2.0f*N.y*N.z, 0.0f, - -2.0f*N.z*N.x, -2.0f*N.z*N.y, -2.0f*N.z*N.z + 1.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f - ); - - vOrigin = thePlane.GetPointOnPlane(); - - mBack.Identity(); - mBack.SetTranslation(-vOrigin); - - mForward.Identity(); - mForward.SetTranslation(vOrigin); - - // (multiplied in reverse order, so it translates to the origin point, - // reflects, and translates back). - return mForward * mReflect * mBack; -} - -VMatrix SetupMatrixProjection(const Vector &vOrigin, const VPlane &thePlane) -{ - vec_t dot; - VMatrix mRet; - - - #define PN thePlane.m_Normal - #define PD thePlane.m_Dist; - - dot = PN[0]*vOrigin.x + PN[1]*vOrigin.y + PN[2]*vOrigin.z - PD; - - mRet.m[0][0] = dot - vOrigin.x * PN[0]; - mRet.m[0][1] = -vOrigin.x * PN[1]; - mRet.m[0][2] = -vOrigin.x * PN[2]; - mRet.m[0][3] = -vOrigin.x * -PD; - - mRet.m[1][0] = -vOrigin.y * PN[0]; - mRet.m[1][1] = dot - vOrigin.y * PN[1]; - mRet.m[1][2] = -vOrigin.y * PN[2]; - mRet.m[1][3] = -vOrigin.y * -PD; - - mRet.m[2][0] = -vOrigin.z * PN[0]; - mRet.m[2][1] = -vOrigin.z * PN[1]; - mRet.m[2][2] = dot - vOrigin.z * PN[2]; - mRet.m[2][3] = -vOrigin.z * -PD; - - mRet.m[3][0] = -PN[0]; - mRet.m[3][1] = -PN[1]; - mRet.m[3][2] = -PN[2]; - mRet.m[3][3] = dot + PD; - - #undef PN - #undef PD - - return mRet; -} - -VMatrix SetupMatrixAxisRot(const Vector &vAxis, vec_t fDegrees) -{ - vec_t s, c, t; - vec_t tx, ty, tz; - vec_t sx, sy, sz; - vec_t fRadians; - - - fRadians = fDegrees * (M_PI / 180.0f); - - s = (vec_t)sin(fRadians); - c = (vec_t)cos(fRadians); - t = 1.0f - c; - - tx = t * vAxis.x; ty = t * vAxis.y; tz = t * vAxis.z; - sx = s * vAxis.x; sy = s * vAxis.y; sz = s * vAxis.z; - - return VMatrix( - tx*vAxis.x + c, tx*vAxis.y - sz, tx*vAxis.z + sy, 0.0f, - tx*vAxis.y + sz, ty*vAxis.y + c, ty*vAxis.z - sx, 0.0f, - tx*vAxis.z - sy, ty*vAxis.z + sx, tz*vAxis.z + c, 0.0f, - 0.0f, 0.0f, 0.0f, 1.0f); -} - -VMatrix SetupMatrixAngles(const QAngle &vAngles) -{ - VMatrix mRet; - MatrixFromAngles( vAngles, mRet ); - return mRet; -} - -VMatrix SetupMatrixOrgAngles(const Vector &origin, const QAngle &vAngles) -{ - VMatrix mRet; - mRet.SetupMatrixOrgAngles( origin, vAngles ); - return mRet; -} - -#endif // VECTOR_NO_SLOW_OPERATIONS - - -bool PlaneIntersection( const VPlane &vp1, const VPlane &vp2, const VPlane &vp3, Vector &vOut ) -{ - VMatrix mMat, mInverse; - - mMat.Init( - vp1.m_Normal.x, vp1.m_Normal.y, vp1.m_Normal.z, -vp1.m_Dist, - vp2.m_Normal.x, vp2.m_Normal.y, vp2.m_Normal.z, -vp2.m_Dist, - vp3.m_Normal.x, vp3.m_Normal.y, vp3.m_Normal.z, -vp3.m_Dist, - 0.0f, 0.0f, 0.0f, 1.0f - ); - - if(mMat.InverseGeneral(mInverse)) - { - //vOut = mInverse * Vector(0.0f, 0.0f, 0.0f); - mInverse.GetTranslation( vOut ); - return true; - } - else - { - return false; - } -} - - - -// ------------------------------------------------------------------------------------------- // -// VMatrix functions. -// ------------------------------------------------------------------------------------------- // - -VMatrix& VMatrix::operator=(const VMatrix &mOther) -{ - m[0][0] = mOther.m[0][0]; - m[0][1] = mOther.m[0][1]; - m[0][2] = mOther.m[0][2]; - m[0][3] = mOther.m[0][3]; - - m[1][0] = mOther.m[1][0]; - m[1][1] = mOther.m[1][1]; - m[1][2] = mOther.m[1][2]; - m[1][3] = mOther.m[1][3]; - - m[2][0] = mOther.m[2][0]; - m[2][1] = mOther.m[2][1]; - m[2][2] = mOther.m[2][2]; - m[2][3] = mOther.m[2][3]; - - m[3][0] = mOther.m[3][0]; - m[3][1] = mOther.m[3][1]; - m[3][2] = mOther.m[3][2]; - m[3][3] = mOther.m[3][3]; - - return *this; -} - -bool VMatrix::operator==( const VMatrix& src ) const -{ - return !memcmp( src.m, m, sizeof(m) ); -} - -void VMatrix::MatrixMul( const VMatrix &vm, VMatrix &out ) const -{ - out.Init( - m[0][0]*vm.m[0][0] + m[0][1]*vm.m[1][0] + m[0][2]*vm.m[2][0] + m[0][3]*vm.m[3][0], - m[0][0]*vm.m[0][1] + m[0][1]*vm.m[1][1] + m[0][2]*vm.m[2][1] + m[0][3]*vm.m[3][1], - m[0][0]*vm.m[0][2] + m[0][1]*vm.m[1][2] + m[0][2]*vm.m[2][2] + m[0][3]*vm.m[3][2], - m[0][0]*vm.m[0][3] + m[0][1]*vm.m[1][3] + m[0][2]*vm.m[2][3] + m[0][3]*vm.m[3][3], - - m[1][0]*vm.m[0][0] + m[1][1]*vm.m[1][0] + m[1][2]*vm.m[2][0] + m[1][3]*vm.m[3][0], - m[1][0]*vm.m[0][1] + m[1][1]*vm.m[1][1] + m[1][2]*vm.m[2][1] + m[1][3]*vm.m[3][1], - m[1][0]*vm.m[0][2] + m[1][1]*vm.m[1][2] + m[1][2]*vm.m[2][2] + m[1][3]*vm.m[3][2], - m[1][0]*vm.m[0][3] + m[1][1]*vm.m[1][3] + m[1][2]*vm.m[2][3] + m[1][3]*vm.m[3][3], - - m[2][0]*vm.m[0][0] + m[2][1]*vm.m[1][0] + m[2][2]*vm.m[2][0] + m[2][3]*vm.m[3][0], - m[2][0]*vm.m[0][1] + m[2][1]*vm.m[1][1] + m[2][2]*vm.m[2][1] + m[2][3]*vm.m[3][1], - m[2][0]*vm.m[0][2] + m[2][1]*vm.m[1][2] + m[2][2]*vm.m[2][2] + m[2][3]*vm.m[3][2], - m[2][0]*vm.m[0][3] + m[2][1]*vm.m[1][3] + m[2][2]*vm.m[2][3] + m[2][3]*vm.m[3][3], - - m[3][0]*vm.m[0][0] + m[3][1]*vm.m[1][0] + m[3][2]*vm.m[2][0] + m[3][3]*vm.m[3][0], - m[3][0]*vm.m[0][1] + m[3][1]*vm.m[1][1] + m[3][2]*vm.m[2][1] + m[3][3]*vm.m[3][1], - m[3][0]*vm.m[0][2] + m[3][1]*vm.m[1][2] + m[3][2]*vm.m[2][2] + m[3][3]*vm.m[3][2], - m[3][0]*vm.m[0][3] + m[3][1]*vm.m[1][3] + m[3][2]*vm.m[2][3] + m[3][3]*vm.m[3][3] - ); -} - -#ifndef VECTOR_NO_SLOW_OPERATIONS - -VMatrix VMatrix::operator*(const VMatrix &vm) const -{ - VMatrix ret; - MatrixMul( vm, ret ); - return ret; -} - -#endif - -bool VMatrix::InverseGeneral(VMatrix &vInverse) const -{ - return MatrixInverseGeneral( *this, vInverse ); -} - - -bool MatrixInverseGeneral(const VMatrix& src, VMatrix& dst) -{ - int iRow, i, j, iTemp, iTest; - vec_t mul, fTest, fLargest; - vec_t mat[4][8]; - int rowMap[4], iLargest; - vec_t *pOut, *pRow, *pScaleRow; - - - // How it's done. - // AX = I - // A = this - // X = the matrix we're looking for - // I = identity - - // Setup AI - for(i=0; i < 4; i++) - { - const vec_t *pIn = src[i]; - pOut = mat[i]; - - for(j=0; j < 4; j++) - { - pOut[j] = pIn[j]; - } - - pOut[4] = 0.0f; - pOut[5] = 0.0f; - pOut[6] = 0.0f; - pOut[7] = 0.0f; - pOut[i+4] = 1.0f; - - rowMap[i] = i; - } - - // Use row operations to get to reduced row-echelon form using these rules: - // 1. Multiply or divide a row by a nonzero number. - // 2. Add a multiple of one row to another. - // 3. Interchange two rows. - - for(iRow=0; iRow < 4; iRow++) - { - // Find the row with the largest element in this column. - fLargest = 0.00001f; - iLargest = -1; - for(iTest=iRow; iTest < 4; iTest++) - { - fTest = (vec_t)FloatMakePositive(mat[rowMap[iTest]][iRow]); - if(fTest > fLargest) - { - iLargest = iTest; - fLargest = fTest; - } - } - - // They're all too small.. sorry. - if(iLargest == -1) - { - return false; - } - - // Swap the rows. - iTemp = rowMap[iLargest]; - rowMap[iLargest] = rowMap[iRow]; - rowMap[iRow] = iTemp; - - pRow = mat[rowMap[iRow]]; - - // Divide this row by the element. - mul = 1.0f / pRow[iRow]; - for(j=0; j < 8; j++) - pRow[j] *= mul; - - pRow[iRow] = 1.0f; // Preserve accuracy... - - // Eliminate this element from the other rows using operation 2. - for(i=0; i < 4; i++) - { - if(i == iRow) - continue; - - pScaleRow = mat[rowMap[i]]; - - // Multiply this row by -(iRow*the element). - mul = -pScaleRow[iRow]; - for(j=0; j < 8; j++) - { - pScaleRow[j] += pRow[j] * mul; - } - - pScaleRow[iRow] = 0.0f; // Preserve accuracy... - } - } - - // The inverse is on the right side of AX now (the identity is on the left). - for(i=0; i < 4; i++) - { - const vec_t *pIn = mat[rowMap[i]] + 4; - pOut = dst.m[i]; - - for(j=0; j < 4; j++) - { - pOut[j] = pIn[j]; - } - } - - return true; -} - - -//----------------------------------------------------------------------------- -// Does a fast inverse, assuming the matrix only contains translation and rotation. -//----------------------------------------------------------------------------- -void MatrixInverseTR( const VMatrix& src, VMatrix &dst ) -{ - Vector vTrans, vNewTrans; - - // Transpose the upper 3x3. - dst.m[0][0] = src.m[0][0]; dst.m[0][1] = src.m[1][0]; dst.m[0][2] = src.m[2][0]; - dst.m[1][0] = src.m[0][1]; dst.m[1][1] = src.m[1][1]; dst.m[1][2] = src.m[2][1]; - dst.m[2][0] = src.m[0][2]; dst.m[2][1] = src.m[1][2]; dst.m[2][2] = src.m[2][2]; - - // Transform the translation. - vTrans.Init( -src.m[0][3], -src.m[1][3], -src.m[2][3] ); - Vector3DMultiply( dst, vTrans, vNewTrans ); - MatrixSetColumn( dst, 3, vNewTrans ); - - // Fill in the bottom row. - dst.m[3][0] = dst.m[3][1] = dst.m[3][2] = 0.0f; - dst.m[3][3] = 1.0f; -} - - -void VMatrix::InverseTR( VMatrix &ret ) const -{ - MatrixInverseTR( *this, ret ); -} - -void MatrixInverseTranspose( const VMatrix& src, VMatrix& dst ) -{ - src.InverseGeneral( dst ); - MatrixTranspose( dst, dst ); -} - -//----------------------------------------------------------------------------- -// Computes the inverse transpose -//----------------------------------------------------------------------------- -void MatrixInverseTranspose( const matrix3x4_t& src, matrix3x4_t& dst ) -{ - VMatrix tmp, out; - tmp.CopyFrom3x4( src ); - ::MatrixInverseTranspose( tmp, out ); - out.Set3x4( dst ); -} - - -#ifndef VECTOR_NO_SLOW_OPERATIONS - -VMatrix VMatrix::InverseTR() const -{ - VMatrix ret; - MatrixInverseTR( *this, ret ); - return ret; -} - -Vector VMatrix::GetScale() const -{ - Vector vecs[3]; - - GetBasisVectors(vecs[0], vecs[1], vecs[2]); - - return Vector( - vecs[0].Length(), - vecs[1].Length(), - vecs[2].Length() - ); -} - -VMatrix VMatrix::Scale(const Vector &vScale) -{ - return VMatrix( - m[0][0]*vScale.x, m[0][1]*vScale.y, m[0][2]*vScale.z, m[0][3], - m[1][0]*vScale.x, m[1][1]*vScale.y, m[1][2]*vScale.z, m[1][3], - m[2][0]*vScale.x, m[2][1]*vScale.y, m[2][2]*vScale.z, m[2][3], - m[3][0]*vScale.x, m[3][1]*vScale.y, m[3][2]*vScale.z, 1.0f - ); -} - -VMatrix VMatrix::NormalizeBasisVectors() const -{ - Vector vecs[3]; - VMatrix mRet; - - - GetBasisVectors(vecs[0], vecs[1], vecs[2]); - - VectorNormalize( vecs[0] ); - VectorNormalize( vecs[1] ); - VectorNormalize( vecs[2] ); - - mRet.SetBasisVectors(vecs[0], vecs[1], vecs[2]); - - // Set everything but basis vectors to identity. - mRet.m[3][0] = mRet.m[3][1] = mRet.m[3][2] = 0.0f; - mRet.m[3][3] = 1.0f; - - return mRet; -} - -VMatrix VMatrix::Transpose() const -{ - return VMatrix( - m[0][0], m[1][0], m[2][0], m[3][0], - m[0][1], m[1][1], m[2][1], m[3][1], - m[0][2], m[1][2], m[2][2], m[3][2], - m[0][3], m[1][3], m[2][3], m[3][3]); -} - -// Transpose upper-left 3x3. -VMatrix VMatrix::Transpose3x3() const -{ - return VMatrix( - m[0][0], m[1][0], m[2][0], m[0][3], - m[0][1], m[1][1], m[2][1], m[1][3], - m[0][2], m[1][2], m[2][2], m[2][3], - m[3][0], m[3][1], m[3][2], m[3][3]); -} - -#endif // VECTOR_NO_SLOW_OPERATIONS - - -bool VMatrix::IsRotationMatrix() const -{ - Vector &v1 = (Vector&)m[0][0]; - Vector &v2 = (Vector&)m[1][0]; - Vector &v3 = (Vector&)m[2][0]; - - return - FloatMakePositive( 1 - v1.Length() ) < 0.01f && - FloatMakePositive( 1 - v2.Length() ) < 0.01f && - FloatMakePositive( 1 - v3.Length() ) < 0.01f && - FloatMakePositive( v1.Dot(v2) ) < 0.01f && - FloatMakePositive( v1.Dot(v3) ) < 0.01f && - FloatMakePositive( v2.Dot(v3) ) < 0.01f; -} - -static void SetupMatrixAnglesInternal( vec_t m[4][4], const QAngle & vAngles ) -{ - float sr, sp, sy, cr, cp, cy; - - SinCos( DEG2RAD( vAngles[YAW] ), &sy, &cy ); - SinCos( DEG2RAD( vAngles[PITCH] ), &sp, &cp ); - SinCos( DEG2RAD( vAngles[ROLL] ), &sr, &cr ); - - // matrix = (YAW * PITCH) * ROLL - m[0][0] = cp*cy; - m[1][0] = cp*sy; - m[2][0] = -sp; - m[0][1] = sr*sp*cy+cr*-sy; - m[1][1] = sr*sp*sy+cr*cy; - m[2][1] = sr*cp; - m[0][2] = (cr*sp*cy+-sr*-sy); - m[1][2] = (cr*sp*sy+-sr*cy); - m[2][2] = cr*cp; - m[0][3] = 0.f; - m[1][3] = 0.f; - m[2][3] = 0.f; -} - -void VMatrix::SetupMatrixOrgAngles( const Vector &origin, const QAngle &vAngles ) -{ - SetupMatrixAnglesInternal( m, vAngles ); - - // Add translation - m[0][3] = origin.x; - m[1][3] = origin.y; - m[2][3] = origin.z; - m[3][0] = 0.0f; - m[3][1] = 0.0f; - m[3][2] = 0.0f; - m[3][3] = 1.0f; -} - - -void VMatrix::SetupMatrixAngles( const QAngle &vAngles ) -{ - SetupMatrixAnglesInternal( m, vAngles ); - - // Zero everything else - m[0][3] = 0.0f; - m[1][3] = 0.0f; - m[2][3] = 0.0f; - m[3][0] = 0.0f; - m[3][1] = 0.0f; - m[3][2] = 0.0f; - m[3][3] = 1.0f; -} - - -//----------------------------------------------------------------------------- -// Sets matrix to identity -//----------------------------------------------------------------------------- -void MatrixSetIdentity( VMatrix &dst ) -{ - dst[0][0] = 1.0f; dst[0][1] = 0.0f; dst[0][2] = 0.0f; dst[0][3] = 0.0f; - dst[1][0] = 0.0f; dst[1][1] = 1.0f; dst[1][2] = 0.0f; dst[1][3] = 0.0f; - dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = 1.0f; dst[2][3] = 0.0f; - dst[3][0] = 0.0f; dst[3][1] = 0.0f; dst[3][2] = 0.0f; dst[3][3] = 1.0f; -} - - -//----------------------------------------------------------------------------- -// Setup a matrix from euler angles. -//----------------------------------------------------------------------------- -void MatrixFromAngles( const QAngle& vAngles, VMatrix& dst ) -{ - dst.SetupMatrixOrgAngles( vec3_origin, vAngles ); -} - - -//----------------------------------------------------------------------------- -// Creates euler angles from a matrix -//----------------------------------------------------------------------------- -void MatrixToAngles( const VMatrix& src, QAngle& vAngles ) -{ - float forward[3]; - float left[3]; - float up[3]; - - // Extract the basis vectors from the matrix. Since we only need the Z - // component of the up vector, we don't get X and Y. - forward[0] = src[0][0]; - forward[1] = src[1][0]; - forward[2] = src[2][0]; - left[0] = src[0][1]; - left[1] = src[1][1]; - left[2] = src[2][1]; - up[2] = src[2][2]; - - float xyDist = sqrtf( forward[0] * forward[0] + forward[1] * forward[1] ); - - // enough here to get angles? - if ( xyDist > 0.001f ) - { - // (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis - vAngles[1] = RAD2DEG( atan2f( forward[1], forward[0] ) ); - - // The engine does pitch inverted from this, but we always end up negating it in the DLL - // UNDONE: Fix the engine to make it consistent - // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); - vAngles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); - - // (roll) z = ATAN( left.z, up.z ); - vAngles[2] = RAD2DEG( atan2f( left[2], up[2] ) ); - } - else // forward is mostly Z, gimbal lock- - { - // (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw - vAngles[1] = RAD2DEG( atan2f( -left[0], left[1] ) ); - - // The engine does pitch inverted from this, but we always end up negating it in the DLL - // UNDONE: Fix the engine to make it consistent - // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); - vAngles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); - - // Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll) - vAngles[2] = 0; - } -} - - -//----------------------------------------------------------------------------- -// Transpose -//----------------------------------------------------------------------------- -inline void Swap( float& a, float& b ) -{ - float tmp = a; - a = b; - b = tmp; -} - -void MatrixTranspose( const VMatrix& src, VMatrix& dst ) -{ - if (&src == &dst) - { - Swap( dst[0][1], dst[1][0] ); - Swap( dst[0][2], dst[2][0] ); - Swap( dst[0][3], dst[3][0] ); - Swap( dst[1][2], dst[2][1] ); - Swap( dst[1][3], dst[3][1] ); - Swap( dst[2][3], dst[3][2] ); - } - else - { - dst[0][0] = src[0][0]; dst[0][1] = src[1][0]; dst[0][2] = src[2][0]; dst[0][3] = src[3][0]; - dst[1][0] = src[0][1]; dst[1][1] = src[1][1]; dst[1][2] = src[2][1]; dst[1][3] = src[3][1]; - dst[2][0] = src[0][2]; dst[2][1] = src[1][2]; dst[2][2] = src[2][2]; dst[2][3] = src[3][2]; - dst[3][0] = src[0][3]; dst[3][1] = src[1][3]; dst[3][2] = src[2][3]; dst[3][3] = src[3][3]; - } -} - - -//----------------------------------------------------------------------------- -// Matrix copy -//----------------------------------------------------------------------------- - -void MatrixCopy( const VMatrix& src, VMatrix& dst ) -{ - if (&src != &dst) - { - memcpy( dst.m, src.m, 16 * sizeof(float) ); - } -} - -//----------------------------------------------------------------------------- -// Matrix multiply -//----------------------------------------------------------------------------- -typedef float VMatrixRaw_t[4]; - -void MatrixMultiply( const VMatrix& src1, const VMatrix& src2, VMatrix& dst ) -{ - // Make sure it works if src1 == dst or src2 == dst - VMatrix tmp1, tmp2; - const VMatrixRaw_t* s1 = (&src1 == &dst) ? tmp1.m : src1.m; - const VMatrixRaw_t* s2 = (&src2 == &dst) ? tmp2.m : src2.m; - - if (&src1 == &dst) - { - MatrixCopy( src1, tmp1 ); - } - if (&src2 == &dst) - { - MatrixCopy( src2, tmp2 ); - } - - dst[0][0] = s1[0][0] * s2[0][0] + s1[0][1] * s2[1][0] + s1[0][2] * s2[2][0] + s1[0][3] * s2[3][0]; - dst[0][1] = s1[0][0] * s2[0][1] + s1[0][1] * s2[1][1] + s1[0][2] * s2[2][1] + s1[0][3] * s2[3][1]; - dst[0][2] = s1[0][0] * s2[0][2] + s1[0][1] * s2[1][2] + s1[0][2] * s2[2][2] + s1[0][3] * s2[3][2]; - dst[0][3] = s1[0][0] * s2[0][3] + s1[0][1] * s2[1][3] + s1[0][2] * s2[2][3] + s1[0][3] * s2[3][3]; - - dst[1][0] = s1[1][0] * s2[0][0] + s1[1][1] * s2[1][0] + s1[1][2] * s2[2][0] + s1[1][3] * s2[3][0]; - dst[1][1] = s1[1][0] * s2[0][1] + s1[1][1] * s2[1][1] + s1[1][2] * s2[2][1] + s1[1][3] * s2[3][1]; - dst[1][2] = s1[1][0] * s2[0][2] + s1[1][1] * s2[1][2] + s1[1][2] * s2[2][2] + s1[1][3] * s2[3][2]; - dst[1][3] = s1[1][0] * s2[0][3] + s1[1][1] * s2[1][3] + s1[1][2] * s2[2][3] + s1[1][3] * s2[3][3]; - - dst[2][0] = s1[2][0] * s2[0][0] + s1[2][1] * s2[1][0] + s1[2][2] * s2[2][0] + s1[2][3] * s2[3][0]; - dst[2][1] = s1[2][0] * s2[0][1] + s1[2][1] * s2[1][1] + s1[2][2] * s2[2][1] + s1[2][3] * s2[3][1]; - dst[2][2] = s1[2][0] * s2[0][2] + s1[2][1] * s2[1][2] + s1[2][2] * s2[2][2] + s1[2][3] * s2[3][2]; - dst[2][3] = s1[2][0] * s2[0][3] + s1[2][1] * s2[1][3] + s1[2][2] * s2[2][3] + s1[2][3] * s2[3][3]; - - dst[3][0] = s1[3][0] * s2[0][0] + s1[3][1] * s2[1][0] + s1[3][2] * s2[2][0] + s1[3][3] * s2[3][0]; - dst[3][1] = s1[3][0] * s2[0][1] + s1[3][1] * s2[1][1] + s1[3][2] * s2[2][1] + s1[3][3] * s2[3][1]; - dst[3][2] = s1[3][0] * s2[0][2] + s1[3][1] * s2[1][2] + s1[3][2] * s2[2][2] + s1[3][3] * s2[3][2]; - dst[3][3] = s1[3][0] * s2[0][3] + s1[3][1] * s2[1][3] + s1[3][2] * s2[2][3] + s1[3][3] * s2[3][3]; -} - -//----------------------------------------------------------------------------- -// Matrix/vector multiply -//----------------------------------------------------------------------------- - -void Vector4DMultiply( const VMatrix& src1, Vector4D const& src2, Vector4D& dst ) -{ - // Make sure it works if src2 == dst - Vector4D tmp; - Vector4D const&v = (&src2 == &dst) ? tmp : src2; - - if (&src2 == &dst) - { - Vector4DCopy( src2, tmp ); - } - - dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2] + src1[0][3] * v[3]; - dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2] + src1[1][3] * v[3]; - dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2] + src1[2][3] * v[3]; - dst[3] = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2] + src1[3][3] * v[3]; -} - -//----------------------------------------------------------------------------- -// Matrix/vector multiply -//----------------------------------------------------------------------------- - -void Vector4DMultiplyPosition( const VMatrix& src1, Vector const& src2, Vector4D& dst ) -{ - // Make sure it works if src2 == dst - Vector tmp; - Vector const&v = ( &src2 == &dst.AsVector3D() ) ? static_cast(tmp) : src2; - - if (&src2 == &dst.AsVector3D()) - { - VectorCopy( src2, tmp ); - } - - dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2] + src1[0][3]; - dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2] + src1[1][3]; - dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2] + src1[2][3]; - dst[3] = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2] + src1[3][3]; -} - - - -//----------------------------------------------------------------------------- -// Matrix/vector multiply -//----------------------------------------------------------------------------- - -void Vector3DMultiply( const VMatrix &src1, const Vector &src2, Vector &dst ) -{ - // Make sure it works if src2 == dst - Vector tmp; - const Vector &v = (&src2 == &dst) ? static_cast(tmp) : src2; - - if( &src2 == &dst ) - { - VectorCopy( src2, tmp ); - } - - dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2]; - dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2]; - dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2]; -} - - -//----------------------------------------------------------------------------- -// Vector3DMultiplyPositionProjective treats src2 as if it's a point -// and does the perspective divide at the end -//----------------------------------------------------------------------------- -void Vector3DMultiplyPositionProjective( const VMatrix& src1, const Vector &src2, Vector& dst ) -{ - // Make sure it works if src2 == dst - Vector tmp; - const Vector &v = (&src2 == &dst) ? static_cast(tmp): src2; - if( &src2 == &dst ) - { - VectorCopy( src2, tmp ); - } - - float w = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2] + src1[3][3]; - if ( w != 0.0f ) - { - w = 1.0f / w; - } - - dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2] + src1[0][3]; - dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2] + src1[1][3]; - dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2] + src1[2][3]; - dst *= w; -} - - -//----------------------------------------------------------------------------- -// Vector3DMultiplyProjective treats src2 as if it's a direction -// and does the perspective divide at the end -//----------------------------------------------------------------------------- -void Vector3DMultiplyProjective( const VMatrix& src1, const Vector &src2, Vector& dst ) -{ - // Make sure it works if src2 == dst - Vector tmp; - const Vector &v = (&src2 == &dst) ? static_cast(tmp) : src2; - if( &src2 == &dst ) - { - VectorCopy( src2, tmp ); - } - - float w; - dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2]; - dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2]; - dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2]; - w = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2]; - if (w != 0.0f) - { - dst /= w; - } - else - { - dst = vec3_origin; - } -} - - -//----------------------------------------------------------------------------- -// Multiplies the vector by the transpose of the matrix -//----------------------------------------------------------------------------- -void Vector4DMultiplyTranspose( const VMatrix& src1, Vector4D const& src2, Vector4D& dst ) -{ - // Make sure it works if src2 == dst - bool srcEqualsDst = (&src2 == &dst); - - Vector4D tmp; - Vector4D const&v = srcEqualsDst ? tmp : src2; - - if (srcEqualsDst) - { - Vector4DCopy( src2, tmp ); - } - - dst[0] = src1[0][0] * v[0] + src1[1][0] * v[1] + src1[2][0] * v[2] + src1[3][0] * v[3]; - dst[1] = src1[0][1] * v[0] + src1[1][1] * v[1] + src1[2][1] * v[2] + src1[3][1] * v[3]; - dst[2] = src1[0][2] * v[0] + src1[1][2] * v[1] + src1[2][2] * v[2] + src1[3][2] * v[3]; - dst[3] = src1[0][3] * v[0] + src1[1][3] * v[1] + src1[2][3] * v[2] + src1[3][3] * v[3]; -} - -//----------------------------------------------------------------------------- -// Multiplies the vector by the transpose of the matrix -//----------------------------------------------------------------------------- -void Vector3DMultiplyTranspose( const VMatrix& src1, const Vector& src2, Vector& dst ) -{ - // Make sure it works if src2 == dst - bool srcEqualsDst = (&src2 == &dst); - - Vector tmp; - const Vector&v = srcEqualsDst ? static_cast(tmp) : src2; - - if (srcEqualsDst) - { - VectorCopy( src2, tmp ); - } - - dst[0] = src1[0][0] * v[0] + src1[1][0] * v[1] + src1[2][0] * v[2]; - dst[1] = src1[0][1] * v[0] + src1[1][1] * v[1] + src1[2][1] * v[2]; - dst[2] = src1[0][2] * v[0] + src1[1][2] * v[1] + src1[2][2] * v[2]; -} - - -//----------------------------------------------------------------------------- -// Transform a plane -//----------------------------------------------------------------------------- -void MatrixTransformPlane( const VMatrix &src, const cplane_t &inPlane, cplane_t &outPlane ) -{ - // What we want to do is the following: - // 1) transform the normal into the new space. - // 2) Determine a point on the old plane given by plane dist * plane normal - // 3) Transform that point into the new space - // 4) Plane dist = DotProduct( new normal, new point ) - - // An optimized version, which works if the plane is orthogonal. - // 1) Transform the normal into the new space - // 2) Realize that transforming the old plane point into the new space - // is given by [ d * n'x + Tx, d * n'y + Ty, d * n'z + Tz ] - // where d = old plane dist, n' = transformed normal, Tn = translational component of transform - // 3) Compute the new plane dist using the dot product of the normal result of #2 - - // For a correct result, this should be an inverse-transpose matrix - // but that only matters if there are nonuniform scale or skew factors in this matrix. - Vector vTrans; - Vector3DMultiply( src, inPlane.normal, outPlane.normal ); - outPlane.dist = inPlane.dist * DotProduct( outPlane.normal, outPlane.normal ); - outPlane.dist += DotProduct( outPlane.normal, src.GetTranslation(vTrans) ); -} - - -#ifndef VECTOR_NO_SLOW_OPERATIONS - -VPlane VMatrix::operator*(const VPlane &thePlane) const -{ - VPlane ret; - TransformPlane( thePlane, ret ); - return ret; -} - -#endif - - -//----------------------------------------------------------------------------- -// Builds a rotation matrix that rotates one direction vector into another -//----------------------------------------------------------------------------- -void MatrixBuildTranslation( VMatrix& dst, float x, float y, float z ) -{ - MatrixSetIdentity( dst ); - dst[0][3] = x; - dst[1][3] = y; - dst[2][3] = z; -} - -void MatrixBuildTranslation( VMatrix& dst, const Vector &translation ) -{ - MatrixSetIdentity( dst ); - dst[0][3] = translation[0]; - dst[1][3] = translation[1]; - dst[2][3] = translation[2]; -} - - -//----------------------------------------------------------------------------- -// Purpose: Builds the matrix for a counterclockwise rotation about an arbitrary axis. -// -// | ax2 + (1 - ax2)cosQ axay(1 - cosQ) - azsinQ azax(1 - cosQ) + aysinQ | -// Ra(Q) = | axay(1 - cosQ) + azsinQ ay2 + (1 - ay2)cosQ ayaz(1 - cosQ) - axsinQ | -// | azax(1 - cosQ) - aysinQ ayaz(1 - cosQ) + axsinQ az2 + (1 - az2)cosQ | -// -// Input : mat - -// vAxisOrRot - -// angle - -//----------------------------------------------------------------------------- -void MatrixBuildRotationAboutAxis( VMatrix &dst, const Vector &vAxisOfRot, float angleDegrees ) -{ - MatrixBuildRotationAboutAxis( vAxisOfRot, angleDegrees, const_cast< matrix3x4_t &> ( dst.As3x4() ) ); - dst[3][0] = 0; - dst[3][1] = 0; - dst[3][2] = 0; - dst[3][3] = 1; -} - - -//----------------------------------------------------------------------------- -// Builds a rotation matrix that rotates one direction vector into another -//----------------------------------------------------------------------------- -void MatrixBuildRotation( VMatrix &dst, const Vector& initialDirection, const Vector& finalDirection ) -{ - float angle = DotProduct( initialDirection, finalDirection ); - Assert( IsFinite(angle) ); - - Vector axis; - - // No rotation required - if (angle - 1.0 > -1e-3) - { - // parallel case - MatrixSetIdentity(dst); - return; - } - else if (angle + 1.0 < 1e-3) - { - // antiparallel case, pick any axis in the plane - // perpendicular to the final direction. Choose the direction (x,y,z) - // which has the minimum component of the final direction, use that - // as an initial guess, then subtract out the component which is - // parallel to the final direction - int idx = 0; - if (FloatMakePositive(finalDirection[1]) < FloatMakePositive(finalDirection[idx])) - idx = 1; - if (FloatMakePositive(finalDirection[2]) < FloatMakePositive(finalDirection[idx])) - idx = 2; - - axis.Init( 0, 0, 0 ); - axis[idx] = 1.0f; - VectorMA( axis, -DotProduct( axis, finalDirection ), finalDirection, axis ); - VectorNormalize(axis); - angle = 180.0f; - } - else - { - CrossProduct( initialDirection, finalDirection, axis ); - VectorNormalize( axis ); - angle = acos(angle) * 180 / M_PI; - } - - MatrixBuildRotationAboutAxis( dst, axis, angle ); - -#ifdef _DEBUG - Vector test; - Vector3DMultiply( dst, initialDirection, test ); - test -= finalDirection; - Assert( test.LengthSqr() < 1e-3 ); -#endif -} - -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -void MatrixBuildRotateZ( VMatrix &dst, float angleDegrees ) -{ - float radians = angleDegrees * ( M_PI / 180.0f ); - - float fSin = ( float )sin( radians ); - float fCos = ( float )cos( radians ); - - dst[0][0] = fCos; dst[0][1] = -fSin; dst[0][2] = 0.0f; dst[0][3] = 0.0f; - dst[1][0] = fSin; dst[1][1] = fCos; dst[1][2] = 0.0f; dst[1][3] = 0.0f; - dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = 1.0f; dst[2][3] = 0.0f; - dst[3][0] = 0.0f; dst[3][1] = 0.0f; dst[3][2] = 0.0f; dst[3][3] = 1.0f; -} - -// Builds a scale matrix -void MatrixBuildScale( VMatrix &dst, float x, float y, float z ) -{ - dst[0][0] = x; dst[0][1] = 0.0f; dst[0][2] = 0.0f; dst[0][3] = 0.0f; - dst[1][0] = 0.0f; dst[1][1] = y; dst[1][2] = 0.0f; dst[1][3] = 0.0f; - dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = z; dst[2][3] = 0.0f; - dst[3][0] = 0.0f; dst[3][1] = 0.0f; dst[3][2] = 0.0f; dst[3][3] = 1.0f; -} - -void MatrixBuildScale( VMatrix &dst, const Vector& scale ) -{ - MatrixBuildScale( dst, scale.x, scale.y, scale.z ); -} - -void MatrixBuildPerspective( VMatrix &dst, float fovX, float fovY, float zNear, float zFar ) -{ - // FIXME: collapse all of this into one matrix after we figure out what all should be in here. - float width = 2 * zNear * tan( fovX * ( M_PI/180.0f ) * 0.5f ); - float height = 2 * zNear * tan( fovY * ( M_PI/180.0f ) * 0.5f ); - - memset( dst.Base(), 0, sizeof( dst ) ); - dst[0][0] = 2.0F * zNear / width; - dst[1][1] = 2.0F * zNear / height; - dst[2][2] = -zFar / ( zNear - zFar ); - dst[3][2] = 1.0f; - dst[2][3] = zNear * zFar / ( zNear - zFar ); - - // negate X and Y so that X points right, and Y points up. - VMatrix negateXY; - negateXY.Identity(); - negateXY[0][0] = -1.0f; - negateXY[1][1] = -1.0f; - MatrixMultiply( negateXY, dst, dst ); - - VMatrix addW; - addW.Identity(); - addW[0][3] = 1.0f; - addW[1][3] = 1.0f; - addW[2][3] = 0.0f; - MatrixMultiply( addW, dst, dst ); - - VMatrix scaleHalf; - scaleHalf.Identity(); - scaleHalf[0][0] = 0.5f; - scaleHalf[1][1] = 0.5f; - MatrixMultiply( scaleHalf, dst, dst ); -} - -static inline void CalculateAABBForNormalizedFrustum_Helper( float x, float y, float z, const VMatrix &volumeToWorld, Vector &mins, Vector &maxs ) -{ - Vector volumeSpacePos( x, y, z ); - - // Make sure it's been clipped - Assert( volumeSpacePos[0] >= -1e-3f ); - Assert( volumeSpacePos[0] - 1.0f <= 1e-3f ); - Assert( volumeSpacePos[1] >= -1e-3f ); - Assert( volumeSpacePos[1] - 1.0f <= 1e-3f ); - Assert( volumeSpacePos[2] >= -1e-3f ); - Assert( volumeSpacePos[2] - 1.0f <= 1e-3f ); - - Vector worldPos; - Vector3DMultiplyPositionProjective( volumeToWorld, volumeSpacePos, worldPos ); - AddPointToBounds( worldPos, mins, maxs ); -} - -//----------------------------------------------------------------------------- -// Given an inverse projection matrix, take the extremes of the space in transformed into world space and -// get a bounding box. -//----------------------------------------------------------------------------- -void CalculateAABBFromProjectionMatrixInverse( const VMatrix &volumeToWorld, Vector *pMins, Vector *pMaxs ) -{ - // FIXME: Could maybe do better than the compile with all of these multiplies by 0 and 1. - ClearBounds( *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 0, 0, 0, volumeToWorld, *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 0, 0, 1, volumeToWorld, *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 0, 1, 0, volumeToWorld, *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 0, 1, 1, volumeToWorld, *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 1, 0, 0, volumeToWorld, *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 1, 0, 1, volumeToWorld, *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 1, 1, 0, volumeToWorld, *pMins, *pMaxs ); - CalculateAABBForNormalizedFrustum_Helper( 1, 1, 1, volumeToWorld, *pMins, *pMaxs ); -} - -void CalculateAABBFromProjectionMatrix( const VMatrix &worldToVolume, Vector *pMins, Vector *pMaxs ) -{ - VMatrix volumeToWorld; - MatrixInverseGeneral( worldToVolume, volumeToWorld ); - CalculateAABBFromProjectionMatrixInverse( volumeToWorld, pMins, pMaxs ); -} - -//----------------------------------------------------------------------------- -// Given an inverse projection matrix, take the extremes of the space in transformed into world space and -// get a bounding sphere. -//----------------------------------------------------------------------------- -void CalculateSphereFromProjectionMatrixInverse( const VMatrix &volumeToWorld, Vector *pCenter, float *pflRadius ) -{ - // FIXME: Could maybe do better than the compile with all of these multiplies by 0 and 1. - - // Need 3 points: the endpoint of the line through the center of the near + far planes, - // and one point on the far plane. From that, we can derive a point somewhere on the center line - // which would produce the smallest bounding sphere. - Vector vecCenterNear, vecCenterFar, vecNearEdge, vecFarEdge; - Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.5f, 0.5f, 0.0f ), vecCenterNear ); - Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.5f, 0.5f, 1.0f ), vecCenterFar ); - Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.0f, 0.0f, 0.0f ), vecNearEdge ); - Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.0f, 0.0f, 1.0f ), vecFarEdge ); - - // Let the distance between the near + far center points = l - // Let the distance between the near center point + near edge point = h1 - // Let the distance between the far center point + far edge point = h2 - // Let the distance along the center line from the near point to the sphere center point = x - // Then let the distance between the sphere center point + near edge point == - // the distance between the sphere center point + far edge point == r == radius of sphere - // Then h1^2 + x^2 == r^2 == (l-x)^2 + h2^2 - // h1^x + x^2 = l^2 - 2 * l * x + x^2 + h2^2 - // 2 * l * x = l^2 + h2^2 - h1^2 - // x = (l^2 + h2^2 - h1^2) / (2 * l) - // r = sqrt( hl^1 + x^2 ) - Vector vecDelta; - VectorSubtract( vecCenterFar, vecCenterNear, vecDelta ); - float l = vecDelta.Length(); - float h1Sqr = vecCenterNear.DistToSqr( vecNearEdge ); - float h2Sqr = vecCenterFar.DistToSqr( vecFarEdge ); - float x = (l*l + h2Sqr - h1Sqr) / (2.0f * l); - VectorMA( vecCenterNear, (x / l), vecDelta, *pCenter ); - *pflRadius = sqrt( h1Sqr + x*x ); -} - -//----------------------------------------------------------------------------- -// Given a projection matrix, take the extremes of the space in transformed into world space and -// get a bounding sphere. -//----------------------------------------------------------------------------- -void CalculateSphereFromProjectionMatrix( const VMatrix &worldToVolume, Vector *pCenter, float *pflRadius ) -{ - VMatrix volumeToWorld; - MatrixInverseGeneral( worldToVolume, volumeToWorld ); - CalculateSphereFromProjectionMatrixInverse( volumeToWorld, pCenter, pflRadius ); -} - - -static inline void FrustumPlanesFromMatrixHelper( const VMatrix &shadowToWorld, const Vector &p1, const Vector &p2, const Vector &p3, - Vector &normal, float &dist ) -{ - Vector world1, world2, world3; - Vector3DMultiplyPositionProjective( shadowToWorld, p1, world1 ); - Vector3DMultiplyPositionProjective( shadowToWorld, p2, world2 ); - Vector3DMultiplyPositionProjective( shadowToWorld, p3, world3 ); - - Vector v1, v2; - VectorSubtract( world2, world1, v1 ); - VectorSubtract( world3, world1, v2 ); - - CrossProduct( v1, v2, normal ); - VectorNormalize( normal ); - dist = DotProduct( normal, world1 ); -} - -void FrustumPlanesFromMatrix( const VMatrix &clipToWorld, Frustum_t &frustum ) -{ - Vector normal; - float dist; - - FrustumPlanesFromMatrixHelper( clipToWorld, - Vector( 0.0f, 0.0f, 0.0f ), Vector( 1.0f, 0.0f, 0.0f ), Vector( 0.0f, 1.0f, 0.0f ), normal, dist ); - frustum.SetPlane( FRUSTUM_NEARZ, PLANE_ANYZ, normal, dist ); - - FrustumPlanesFromMatrixHelper( clipToWorld, - Vector( 0.0f, 0.0f, 1.0f ), Vector( 0.0f, 1.0f, 1.0f ), Vector( 1.0f, 0.0f, 1.0f ), normal, dist ); - frustum.SetPlane( FRUSTUM_FARZ, PLANE_ANYZ, normal, dist ); - - FrustumPlanesFromMatrixHelper( clipToWorld, - Vector( 1.0f, 0.0f, 0.0f ), Vector( 1.0f, 1.0f, 1.0f ), Vector( 1.0f, 1.0f, 0.0f ), normal, dist ); - frustum.SetPlane( FRUSTUM_RIGHT, PLANE_ANYZ, normal, dist ); - - FrustumPlanesFromMatrixHelper( clipToWorld, - Vector( 0.0f, 0.0f, 0.0f ), Vector( 0.0f, 1.0f, 1.0f ), Vector( 0.0f, 0.0f, 1.0f ), normal, dist ); - frustum.SetPlane( FRUSTUM_LEFT, PLANE_ANYZ, normal, dist ); - - FrustumPlanesFromMatrixHelper( clipToWorld, - Vector( 1.0f, 1.0f, 0.0f ), Vector( 1.0f, 1.0f, 1.0f ), Vector( 0.0f, 1.0f, 1.0f ), normal, dist ); - frustum.SetPlane( FRUSTUM_TOP, PLANE_ANYZ, normal, dist ); - - FrustumPlanesFromMatrixHelper( clipToWorld, - Vector( 1.0f, 0.0f, 0.0f ), Vector( 0.0f, 0.0f, 1.0f ), Vector( 1.0f, 0.0f, 1.0f ), normal, dist ); - frustum.SetPlane( FRUSTUM_BOTTOM, PLANE_ANYZ, normal, dist ); -} - -void MatrixBuildOrtho( VMatrix& dst, double left, double top, double right, double bottom, double zNear, double zFar ) -{ - // FIXME: This is being used incorrectly! Should read: - // D3DXMatrixOrthoOffCenterRH( &matrix, left, right, bottom, top, zNear, zFar ); - // Which is certainly why we need these extra -1 scales in y. Bleah - - // NOTE: The camera can be imagined as the following diagram: - // /z - // / - // /____ x Z is going into the screen - // | - // | - // |y - // - // (0,0,z) represents the upper-left corner of the screen. - // Our projection transform needs to transform from this space to a LH coordinate - // system that looks thusly: - // - // y| /z - // | / - // |/____ x Z is going into the screen - // - // Where x,y lies between -1 and 1, and z lies from 0 to 1 - // This is because the viewport transformation from projection space to pixels - // introduces a -1 scale in the y coordinates - // D3DXMatrixOrthoOffCenterRH( &matrix, left, right, top, bottom, zNear, zFar ); - - dst.Init( 2.0f / ( right - left ), 0.0f, 0.0f, ( left + right ) / ( left - right ), - 0.0f, 2.0f / ( bottom - top ), 0.0f, ( bottom + top ) / ( top - bottom ), - 0.0f, 0.0f, 1.0f / ( zNear - zFar ), zNear / ( zNear - zFar ), - 0.0f, 0.0f, 0.0f, 1.0f ); -} - -void MatrixBuildPerspectiveZRange( VMatrix& dst, double flZNear, double flZFar ) -{ - dst.m[2][0] = 0.0f; - dst.m[2][1] = 0.0f; - dst.m[2][2] = flZFar / ( flZNear - flZFar ); - dst.m[2][3] = flZNear * flZFar / ( flZNear - flZFar ); -} - -void MatrixBuildPerspectiveX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar ) -{ - float flWidthScale = 1.0f / tanf( flFovX * M_PI / 360.0f ); - float flHeightScale = flAspect * flWidthScale; - dst.Init( flWidthScale, 0.0f, 0.0f, 0.0f, - 0.0f, flHeightScale, 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, -1.0f, 0.0f ); - - MatrixBuildPerspectiveZRange ( dst, flZNear, flZFar ); -} - -void MatrixBuildPerspectiveOffCenterX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar, double bottom, double top, double left, double right ) -{ - float flWidth = tanf( flFovX * M_PI / 360.0f ); - float flHeight = flWidth / flAspect; - - // bottom, top, left, right are 0..1 so convert to -/2../2 - float flLeft = -(flWidth/2.0f) * (1.0f - left) + left * (flWidth/2.0f); - float flRight = -(flWidth/2.0f) * (1.0f - right) + right * (flWidth/2.0f); - float flBottom = -(flHeight/2.0f) * (1.0f - bottom) + bottom * (flHeight/2.0f); - float flTop = -(flHeight/2.0f) * (1.0f - top) + top * (flHeight/2.0f); - - dst.Init( 1.0f / (flRight-flLeft), 0.0f, (flLeft+flRight)/(flRight-flLeft), 0.0f, - 0.0f, 1.0f /(flTop-flBottom), (flTop+flBottom)/(flTop-flBottom), 0.0f, - 0.0f, 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, -1.0f, 0.0f ); - - MatrixBuildPerspectiveZRange ( dst, flZNear, flZFar ); -} #endif // !_STATIC_LINKED || _SHARED_LIB diff --git a/public/mathlib/math_pfns.h b/public/mathlib/math_pfns.h index d43411ce..5ff8a53b 100644 --- a/public/mathlib/math_pfns.h +++ b/public/mathlib/math_pfns.h @@ -22,10 +22,16 @@ extern float (*pfFastCos)(float x); // The following are not declared as macros because they are often used in limiting situations, // and sometimes the compiler simply refuses to inline them for some reason -#define FastSqrt(x) (*pfSqrt)(x) -#define FastRSqrt(x) (*pfRSqrt)(x) -#define FastRSqrtFast(x) (*pfRSqrtFast)(x) +#define FastSqrt(x) sqrtf(x) +#define FastRSqrt(x) (1.f/sqrtf(x)) +#define FastRSqrtFast(x) (1.f/sqrtf(x)) + +#ifdef _WIN32 #define FastSinCos(x,s,c) (*pfFastSinCos)(x,s,c) +#else +#define FastSinCos(x,s,c) sincosf(x,s,c) +#endif + #define FastCos(x) (*pfFastCos)(x) #if defined(__i386__) || defined(_M_IX86) diff --git a/public/mathlib/mathlib.h b/public/mathlib/mathlib.h index 75a21ad7..ca752def 100644 --- a/public/mathlib/mathlib.h +++ b/public/mathlib/mathlib.h @@ -237,8 +237,8 @@ bool R_CullBoxSkipNear( const Vector& mins, const Vector& maxs, const Frustum_t struct matrix3x4_t { - matrix3x4_t() = default; - matrix3x4_t( + inline matrix3x4_t() = default; + inline matrix3x4_t( float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23 ) @@ -252,7 +252,7 @@ struct matrix3x4_t // Creates a matrix where the X axis = forward // the Y axis = left, and the Z axis = up //----------------------------------------------------------------------------- - void Init( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector &vecOrigin ) + inline void Init( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector &vecOrigin ) { m_flMatVal[0][0] = xAxis.x; m_flMatVal[0][1] = yAxis.x; m_flMatVal[0][2] = zAxis.x; m_flMatVal[0][3] = vecOrigin.x; m_flMatVal[1][0] = xAxis.y; m_flMatVal[1][1] = yAxis.y; m_flMatVal[1][2] = zAxis.y; m_flMatVal[1][3] = vecOrigin.y; @@ -263,26 +263,23 @@ struct matrix3x4_t // Creates a matrix where the X axis = forward // the Y axis = left, and the Z axis = up //----------------------------------------------------------------------------- - matrix3x4_t( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector &vecOrigin ) + inline matrix3x4_t( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector &vecOrigin ) { Init( xAxis, yAxis, zAxis, vecOrigin ); } inline void Invalidate( void ) { - for (int i = 0; i < 3; i++) + for( int i=0; i < 12; i++ ) { - for (int j = 0; j < 4; j++) - { - m_flMatVal[i][j] = VEC_T_NAN; - } + ((float*)m_flMatVal)[i] = VEC_T_NAN; } } - float *operator[]( int i ) { Assert(( i >= 0 ) && ( i < 3 )); return m_flMatVal[i]; } - const float *operator[]( int i ) const { Assert(( i >= 0 ) && ( i < 3 )); return m_flMatVal[i]; } - float *Base() { return &m_flMatVal[0][0]; } - const float *Base() const { return &m_flMatVal[0][0]; } + inline float *operator[]( int i ) { Assert(( i >= 0 ) && ( i < 3 )); return m_flMatVal[i]; } + inline const float *operator[]( int i ) const { Assert(( i >= 0 ) && ( i < 3 )); return m_flMatVal[i]; } + inline float *Base() { return &m_flMatVal[0][0]; } + inline const float *Base() const { return &m_flMatVal[0][0]; } float m_flMatVal[3][4]; }; @@ -565,7 +562,13 @@ void MatrixInvert( const matrix3x4_t &in, matrix3x4_t &out ); bool MatricesAreEqual( const matrix3x4_t &src1, const matrix3x4_t &src2, float flTolerance = 1e-5 ); void MatrixGetColumn( const matrix3x4_t &in, int column, Vector &out ); -void MatrixSetColumn( const Vector &in, int column, matrix3x4_t &out ); + +inline void MatrixSetColumn( const Vector &in, int column, matrix3x4_t& out ) +{ + out[0][column] = in.x; + out[1][column] = in.y; + out[2][column] = in.z; +} inline void MatrixGetTranslation( const matrix3x4_t &in, Vector &out ) { diff --git a/public/mathlib/vector4d.h b/public/mathlib/vector4d.h index 63aebe85..99e0995f 100644 --- a/public/mathlib/vector4d.h +++ b/public/mathlib/vector4d.h @@ -35,7 +35,7 @@ class Vector2D; // 4D Vector4D //========================================================= -class Vector4D +class alignas(16) Vector4D { public: // Members diff --git a/public/mathlib/vmatrix.h b/public/mathlib/vmatrix.h index 48738df0..2170f9c7 100644 --- a/public/mathlib/vmatrix.h +++ b/public/mathlib/vmatrix.h @@ -30,6 +30,7 @@ #pragma once #endif +#include #include #include "mathlib/vector.h" #include "mathlib/vplane.h" @@ -38,8 +39,11 @@ struct cplane_t; +#ifndef M_PI + #define M_PI 3.14159265358979323846 // matches value in gcc v2 math.h +#endif -class VMatrix +class alignas(16) VMatrix { public: @@ -58,6 +62,7 @@ public: // Construct from a 3x4 matrix VMatrix( const matrix3x4_t& matrix3x4 ); + VMatrix( const VMatrix& ) = default; // Set the values in the matrix. void Init( @@ -111,7 +116,9 @@ public: void CopyFrom3x4( const matrix3x4_t &m3x4 ); void Set3x4( matrix3x4_t& matrix3x4 ) const; - bool operator==( const VMatrix& src ) const; + bool operator==( const VMatrix& src ) const { + return !memcmp( src.m, m, sizeof(m) ); + } bool operator!=( const VMatrix& src ) const { return !( *this == src ); } #ifndef VECTOR_NO_SLOW_OPERATIONS @@ -234,189 +241,286 @@ public: public: // The matrix. - vec_t m[4][4]; + vec_t m[4][4]; }; +inline void MatrixSetColumn( VMatrix &src, int nCol, const Vector &column ) +{ + Assert( (nCol >= 0) && (nCol <= 3) ); + src.m[0][nCol] = column.x; + src.m[1][nCol] = column.y; + src.m[2][nCol] = column.z; +} //----------------------------------------------------------------------------- -// Helper functions. -//----------------------------------------------------------------------------- - -#ifndef VECTOR_NO_SLOW_OPERATIONS - -// Setup an identity matrix. -VMatrix SetupMatrixIdentity(); - -// Setup as a scaling matrix. -VMatrix SetupMatrixScale(const Vector &vScale); - -// Setup a translation matrix. -VMatrix SetupMatrixTranslation(const Vector &vTranslation); - -// Setup a matrix to reflect around the plane. -VMatrix SetupMatrixReflection(const VPlane &thePlane); - -// Setup a matrix to project from vOrigin onto thePlane. -VMatrix SetupMatrixProjection(const Vector &vOrigin, const VPlane &thePlane); - -// Setup a matrix to rotate the specified amount around the specified axis. -VMatrix SetupMatrixAxisRot(const Vector &vAxis, vec_t fDegrees); - -// Setup a matrix from euler angles. Just sets identity and calls MatrixAngles. -VMatrix SetupMatrixAngles(const QAngle &vAngles); - -// Setup a matrix for origin and angles. -VMatrix SetupMatrixOrgAngles(const Vector &origin, const QAngle &vAngles); - -#endif - -#define VMatToString(mat) (static_cast(CFmtStr("[ (%f, %f, %f), (%f, %f, %f), (%f, %f, %f), (%f, %f, %f) ]", mat.m[0][0], mat.m[0][1], mat.m[0][2], mat.m[0][3], mat.m[1][0], mat.m[1][1], mat.m[1][2], mat.m[1][3], mat.m[2][0], mat.m[2][1], mat.m[2][2], mat.m[2][3], mat.m[3][0], mat.m[3][1], mat.m[3][2], mat.m[3][3] ))) // ** Note: this generates a temporary, don't hold reference! - -//----------------------------------------------------------------------------- -// Returns the point at the intersection on the 3 planes. -// Returns false if it can't be solved (2 or more planes are parallel). -//----------------------------------------------------------------------------- -bool PlaneIntersection( const VPlane &vp1, const VPlane &vp2, const VPlane &vp3, Vector &vOut ); - - -//----------------------------------------------------------------------------- -// These methods are faster. Use them if you want faster code -//----------------------------------------------------------------------------- -void MatrixSetIdentity( VMatrix &dst ); -void MatrixTranspose( const VMatrix& src, VMatrix& dst ); -void MatrixCopy( const VMatrix& src, VMatrix& dst ); -void MatrixMultiply( const VMatrix& src1, const VMatrix& src2, VMatrix& dst ); - -// Accessors -void MatrixGetColumn( const VMatrix &src, int nCol, Vector *pColumn ); -void MatrixSetColumn( VMatrix &src, int nCol, const Vector &column ); -void MatrixGetRow( const VMatrix &src, int nCol, Vector *pColumn ); -void MatrixSetRow( VMatrix &src, int nCol, const Vector &column ); - -// Vector3DMultiply treats src2 as if it's a direction vector -void Vector3DMultiply( const VMatrix& src1, const Vector& src2, Vector& dst ); - // Vector3DMultiplyPosition treats src2 as if it's a point (adds the translation) -inline void Vector3DMultiplyPosition( const VMatrix& src1, const VectorByValue src2, Vector& dst ); - -// Vector3DMultiplyPositionProjective treats src2 as if it's a point -// and does the perspective divide at the end -void Vector3DMultiplyPositionProjective( const VMatrix& src1, const Vector &src2, Vector& dst ); - -// Vector3DMultiplyPosition treats src2 as if it's a direction -// and does the perspective divide at the end -// NOTE: src1 had better be an inverse transpose to use this correctly -void Vector3DMultiplyProjective( const VMatrix& src1, const Vector &src2, Vector& dst ); - -void Vector4DMultiply( const VMatrix& src1, const Vector4D& src2, Vector4D& dst ); - -// Same as Vector4DMultiply except that src2 has an implicit W of 1 -void Vector4DMultiplyPosition( const VMatrix& src1, const Vector &src2, Vector4D& dst ); - -// Multiplies the vector by the transpose of the matrix -void Vector3DMultiplyTranspose( const VMatrix& src1, const Vector& src2, Vector& dst ); -void Vector4DMultiplyTranspose( const VMatrix& src1, const Vector4D& src2, Vector4D& dst ); - -// Transform a plane -void MatrixTransformPlane( const VMatrix &src, const cplane_t &inPlane, cplane_t &outPlane ); - -// Transform a plane that has an axis-aligned normal -void MatrixTransformAxisAlignedPlane( const VMatrix &src, int nDim, float flSign, float flDist, cplane_t &outPlane ); - -void MatrixBuildTranslation( VMatrix& dst, float x, float y, float z ); -void MatrixBuildTranslation( VMatrix& dst, const Vector &translation ); - -inline void MatrixTranslate( VMatrix& dst, const Vector &translation ) +//----------------------------------------------------------------------------- +// NJS: src2 is passed in as a full vector rather than a reference to prevent the need +// for 2 branches and a potential copy in the body. (ie, handling the case when the src2 +// reference is the same as the dst reference ). +inline void Vector3DMultiplyPosition( const VMatrix& src1, const VectorByValue src2, Vector& dst ) { - VMatrix matTranslation, temp; - MatrixBuildTranslation( matTranslation, translation ); - MatrixMultiply( dst, matTranslation, temp ); - dst = temp; + dst[0] = src1[0][0] * src2.x + src1[0][1] * src2.y + src1[0][2] * src2.z + src1[0][3]; + dst[1] = src1[1][0] * src2.x + src1[1][1] * src2.y + src1[1][2] * src2.z + src1[1][3]; + dst[2] = src1[2][0] * src2.x + src1[2][1] * src2.y + src1[2][2] * src2.z + src1[2][3]; +} + +//----------------------------------------------------------------------------- +// Sets matrix to identity +//----------------------------------------------------------------------------- +inline void MatrixSetIdentity( VMatrix &dst ) +{ + dst[0][0] = 1.0f; dst[0][1] = 0.0f; dst[0][2] = 0.0f; dst[0][3] = 0.0f; + dst[1][0] = 0.0f; dst[1][1] = 1.0f; dst[1][2] = 0.0f; dst[1][3] = 0.0f; + dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = 1.0f; dst[2][3] = 0.0f; + dst[3][0] = 0.0f; dst[3][1] = 0.0f; dst[3][2] = 0.0f; dst[3][3] = 1.0f; +} + +//----------------------------------------------------------------------------- +// Matrix/vector multiply +//----------------------------------------------------------------------------- +inline void Vector3DMultiply( const VMatrix &src1, const Vector &src2, Vector &dst ) +{ + // Make sure it works if src2 == dst + Vector tmp; + const Vector &v = (&src2 == &dst) ? static_cast(tmp) : src2; + + if( &src2 == &dst ) + VectorCopy( src2, tmp ); + + dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2]; + dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2]; + dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2]; } -void MatrixBuildRotationAboutAxis( VMatrix& dst, const Vector& vAxisOfRot, float angleDegrees ); -void MatrixBuildRotateZ( VMatrix& dst, float angleDegrees ); - -inline void MatrixRotate( VMatrix& dst, const Vector& vAxisOfRot, float angleDegrees ) +inline bool MatrixInverseGeneral(const VMatrix& src, VMatrix& dst) { - VMatrix rotation, temp; - MatrixBuildRotationAboutAxis( rotation, vAxisOfRot, angleDegrees ); - MatrixMultiply( dst, rotation, temp ); - dst = temp; + int iRow, i, j, iTemp, iTest; + vec_t mul, fTest, fLargest; + vec_t mat[4][8]; + int rowMap[4], iLargest; + vec_t *pOut, *pRow, *pScaleRow; + + + // How it's done. + // AX = I + // A = this + // X = the matrix we're looking for + // I = identity + + // Setup AI + for(i=0; i < 4; i++) + { + const vec_t *pIn = src[i]; + pOut = mat[i]; + + for(j=0; j < 4; j++) + { + pOut[j] = pIn[j]; + } + + pOut[4] = 0.0f; + pOut[5] = 0.0f; + pOut[6] = 0.0f; + pOut[7] = 0.0f; + pOut[i+4] = 1.0f; + + rowMap[i] = i; + } + + // Use row operations to get to reduced row-echelon form using these rules: + // 1. Multiply or divide a row by a nonzero number. + // 2. Add a multiple of one row to another. + // 3. Interchange two rows. + + for(iRow=0; iRow < 4; iRow++) + { + // Find the row with the largest element in this column. + fLargest = 0.00001f; + iLargest = -1; + for(iTest=iRow; iTest < 4; iTest++) + { + fTest = (vec_t)FloatMakePositive(mat[rowMap[iTest]][iRow]); + if(fTest > fLargest) + { + iLargest = iTest; + fLargest = fTest; + } + } + + // They're all too small.. sorry. + if(iLargest == -1) + { + return false; + } + + // Swap the rows. + iTemp = rowMap[iLargest]; + rowMap[iLargest] = rowMap[iRow]; + rowMap[iRow] = iTemp; + + pRow = mat[rowMap[iRow]]; + + // Divide this row by the element. + mul = 1.0f / pRow[iRow]; + for(j=0; j < 8; j++) + pRow[j] *= mul; + + pRow[iRow] = 1.0f; // Preserve accuracy... + + // Eliminate this element from the other rows using operation 2. + for(i=0; i < 4; i++) + { + if(i == iRow) + continue; + + pScaleRow = mat[rowMap[i]]; + + // Multiply this row by -(iRow*the element). + mul = -pScaleRow[iRow]; + for(j=0; j < 8; j++) + { + pScaleRow[j] += pRow[j] * mul; + } + + pScaleRow[iRow] = 0.0f; // Preserve accuracy... + } + } + + // The inverse is on the right side of AX now (the identity is on the left). + for(i=0; i < 4; i++) + { + const vec_t *pIn = mat[rowMap[i]] + 4; + pOut = dst.m[i]; + + for(j=0; j < 4; j++) + { + pOut[j] = pIn[j]; + } + } + + return true; } -// Builds a rotation matrix that rotates one direction vector into another -void MatrixBuildRotation( VMatrix &dst, const Vector& initialDirection, const Vector& finalDirection ); +static inline void SetupMatrixAnglesInternal( vec_t m[4][4], const QAngle & vAngles ) +{ + float sr, sp, sy, cr, cp, cy; -// Builds a scale matrix -void MatrixBuildScale( VMatrix &dst, float x, float y, float z ); -void MatrixBuildScale( VMatrix &dst, const Vector& scale ); + SinCos( DEG2RAD( vAngles[YAW] ), &sy, &cy ); + SinCos( DEG2RAD( vAngles[PITCH] ), &sp, &cp ); + SinCos( DEG2RAD( vAngles[ROLL] ), &sr, &cr ); + + // matrix = (YAW * PITCH) * ROLL + m[0][0] = cp*cy; + m[1][0] = cp*sy; + m[2][0] = -sp; + m[0][1] = sr*sp*cy+cr*-sy; + m[1][1] = sr*sp*sy+cr*cy; + m[2][1] = sr*cp; + m[0][2] = (cr*sp*cy+-sr*-sy); + m[1][2] = (cr*sp*sy+-sr*cy); + m[2][2] = cr*cp; + m[0][3] = 0.f; + m[1][3] = 0.f; + m[2][3] = 0.f; +} -// Build a perspective matrix. -// zNear and zFar are assumed to be positive. -// You end up looking down positive Z, X is to the right, Y is up. -// X range: [0..1] -// Y range: [0..1] -// Z range: [0..1] -void MatrixBuildPerspective( VMatrix &dst, float fovX, float fovY, float zNear, float zFar ); //----------------------------------------------------------------------------- -// Given a projection matrix, take the extremes of the space in transformed into world space and -// get a bounding box. +// Transpose //----------------------------------------------------------------------------- -void CalculateAABBFromProjectionMatrix( const VMatrix &worldToVolume, Vector *pMins, Vector *pMaxs ); +inline void Swap( float& a, float& b ) +{ + float tmp = a; + a = b; + b = tmp; +} -//----------------------------------------------------------------------------- -// Given a projection matrix, take the extremes of the space in transformed into world space and -// get a bounding sphere. -//----------------------------------------------------------------------------- -void CalculateSphereFromProjectionMatrix( const VMatrix &worldToVolume, Vector *pCenter, float *pflRadius ); - -//----------------------------------------------------------------------------- -// Given an inverse projection matrix, take the extremes of the space in transformed into world space and -// get a bounding box. -//----------------------------------------------------------------------------- -void CalculateAABBFromProjectionMatrixInverse( const VMatrix &volumeToWorld, Vector *pMins, Vector *pMaxs ); - -//----------------------------------------------------------------------------- -// Given an inverse projection matrix, take the extremes of the space in transformed into world space and -// get a bounding sphere. -//----------------------------------------------------------------------------- -void CalculateSphereFromProjectionMatrixInverse( const VMatrix &volumeToWorld, Vector *pCenter, float *pflRadius ); - -//----------------------------------------------------------------------------- -// Calculate frustum planes given a clip->world space transform. -//----------------------------------------------------------------------------- -void FrustumPlanesFromMatrix( const VMatrix &clipToWorld, Frustum_t &frustum ); - -//----------------------------------------------------------------------------- -// Setup a matrix from euler angles. -//----------------------------------------------------------------------------- -void MatrixFromAngles( const QAngle& vAngles, VMatrix& dst ); - -//----------------------------------------------------------------------------- -// Creates euler angles from a matrix -//----------------------------------------------------------------------------- -void MatrixToAngles( const VMatrix& src, QAngle& vAngles ); +inline void MatrixTranspose( const VMatrix& src, VMatrix& dst ) +{ + if (&src == &dst) + { + Swap( dst[0][1], dst[1][0] ); + Swap( dst[0][2], dst[2][0] ); + Swap( dst[0][3], dst[3][0] ); + Swap( dst[1][2], dst[2][1] ); + Swap( dst[1][3], dst[3][1] ); + Swap( dst[2][3], dst[3][2] ); + } + else + { + dst[0][0] = src[0][0]; dst[0][1] = src[1][0]; dst[0][2] = src[2][0]; dst[0][3] = src[3][0]; + dst[1][0] = src[0][1]; dst[1][1] = src[1][1]; dst[1][2] = src[2][1]; dst[1][3] = src[3][1]; + dst[2][0] = src[0][2]; dst[2][1] = src[1][2]; dst[2][2] = src[2][2]; dst[2][3] = src[3][2]; + dst[3][0] = src[0][3]; dst[3][1] = src[1][3]; dst[3][2] = src[2][3]; dst[3][3] = src[3][3]; + } +} //----------------------------------------------------------------------------- // Does a fast inverse, assuming the matrix only contains translation and rotation. //----------------------------------------------------------------------------- -void MatrixInverseTR( const VMatrix& src, VMatrix &dst ); +inline void MatrixInverseTR( const VMatrix& src, VMatrix &dst ) +{ + Vector vTrans, vNewTrans; -//----------------------------------------------------------------------------- -// Inverts any matrix at all -//----------------------------------------------------------------------------- -bool MatrixInverseGeneral(const VMatrix& src, VMatrix& dst); + // Transpose the upper 3x3. + dst.m[0][0] = src.m[0][0]; dst.m[0][1] = src.m[1][0]; dst.m[0][2] = src.m[2][0]; + dst.m[1][0] = src.m[0][1]; dst.m[1][1] = src.m[1][1]; dst.m[1][2] = src.m[2][1]; + dst.m[2][0] = src.m[0][2]; dst.m[2][1] = src.m[1][2]; dst.m[2][2] = src.m[2][2]; -//----------------------------------------------------------------------------- -// Computes the inverse transpose -//----------------------------------------------------------------------------- -void MatrixInverseTranspose( const VMatrix& src, VMatrix& dst ); + // Transform the translation. + vTrans.Init( -src.m[0][3], -src.m[1][3], -src.m[2][3] ); + Vector3DMultiply( dst, vTrans, vNewTrans ); + MatrixSetColumn( dst, 3, vNewTrans ); + + // Fill in the bottom row. + dst.m[3][0] = dst.m[3][1] = dst.m[3][2] = 0.0f; + dst.m[3][3] = 1.0f; +} + +inline void VMatrix::Init( + vec_t m00, vec_t m01, vec_t m02, vec_t m03, + vec_t m10, vec_t m11, vec_t m12, vec_t m13, + vec_t m20, vec_t m21, vec_t m22, vec_t m23, + vec_t m30, vec_t m31, vec_t m32, vec_t m33 + ) +{ + m[0][0] = m00; + m[0][1] = m01; + m[0][2] = m02; + m[0][3] = m03; + + m[1][0] = m10; + m[1][1] = m11; + m[1][2] = m12; + m[1][3] = m13; + + m[2][0] = m20; + m[2][1] = m21; + m[2][2] = m22; + m[2][3] = m23; + + m[3][0] = m30; + m[3][1] = m31; + m[3][2] = m32; + m[3][3] = m33; +} +//----------------------------------------------------------------------------- +// Initialize from a 3x4 +//----------------------------------------------------------------------------- +inline void VMatrix::Init( const matrix3x4_t& matrix3x4 ) +{ + memcpy(m, matrix3x4.Base(), sizeof( matrix3x4_t ) ); + + m[3][0] = 0.0f; + m[3][1] = 0.0f; + m[3][2] = 0.0f; + m[3][3] = 1.0f; +} //----------------------------------------------------------------------------- // VMatrix inlines. @@ -476,56 +580,153 @@ inline VMatrix::VMatrix( const Vector& xAxis, const Vector& yAxis, const Vector& ); } - -inline void VMatrix::Init( - vec_t m00, vec_t m01, vec_t m02, vec_t m03, - vec_t m10, vec_t m11, vec_t m12, vec_t m13, - vec_t m20, vec_t m21, vec_t m22, vec_t m23, - vec_t m30, vec_t m31, vec_t m32, vec_t m33 - ) -{ - m[0][0] = m00; - m[0][1] = m01; - m[0][2] = m02; - m[0][3] = m03; - - m[1][0] = m10; - m[1][1] = m11; - m[1][2] = m12; - m[1][3] = m13; - - m[2][0] = m20; - m[2][1] = m21; - m[2][2] = m22; - m[2][3] = m23; - - m[3][0] = m30; - m[3][1] = m31; - m[3][2] = m32; - m[3][3] = m33; -} - - -//----------------------------------------------------------------------------- -// Initialize from a 3x4 -//----------------------------------------------------------------------------- -inline void VMatrix::Init( const matrix3x4_t& matrix3x4 ) -{ - memcpy(m, matrix3x4.Base(), sizeof( matrix3x4_t ) ); - - m[3][0] = 0.0f; - m[3][1] = 0.0f; - m[3][2] = 0.0f; - m[3][3] = 1.0f; -} - - //----------------------------------------------------------------------------- // Methods related to the basis vectors of the matrix //----------------------------------------------------------------------------- +inline VMatrix& VMatrix::operator=(const VMatrix &mOther) +{ + m[0][0] = mOther.m[0][0]; + m[0][1] = mOther.m[0][1]; + m[0][2] = mOther.m[0][2]; + m[0][3] = mOther.m[0][3]; + + m[1][0] = mOther.m[1][0]; + m[1][1] = mOther.m[1][1]; + m[1][2] = mOther.m[1][2]; + m[1][3] = mOther.m[1][3]; + + m[2][0] = mOther.m[2][0]; + m[2][1] = mOther.m[2][1]; + m[2][2] = mOther.m[2][2]; + m[2][3] = mOther.m[2][3]; + + m[3][0] = mOther.m[3][0]; + m[3][1] = mOther.m[3][1]; + m[3][2] = mOther.m[3][2]; + m[3][3] = mOther.m[3][3]; + + return *this; +} + + #ifndef VECTOR_NO_SLOW_OPERATIONS +inline VMatrix VMatrix::operator+(const VMatrix &other) const +{ + VMatrix ret; + for(int i=0; i < 16; i++) + { + ((float*)ret.m)[i] = ((float*)m)[i] + ((float*)other.m)[i]; + } + return ret; +} + +inline VMatrix VMatrix::operator-(const VMatrix &other) const +{ + VMatrix ret; + for(int i=0; i < 16; i++) + { + ((float*)ret.m)[i] = ((float*)m)[i] - ((float*)other.m)[i]; + } + return ret; +} + +inline VMatrix VMatrix::operator-() const +{ + VMatrix ret; + for( int i=0; i < 16; i++ ) + { + ((float*)ret.m)[i] = ((float*)m)[i]; + } + return ret; +} + +//----------------------------------------------------------------------------- +// Matrix math operations +//----------------------------------------------------------------------------- +inline const VMatrix& VMatrix::operator+=(const VMatrix &other) +{ + for(int i=0; i < 16; i++) + { + ((float*)m)[i] += ((float*)other.m)[i]; + } + return *this; +} + + +inline Vector VMatrix::operator*(const Vector &vVec) const +{ + Vector vRet; + vRet.x = m[0][0]*vVec.x + m[0][1]*vVec.y + m[0][2]*vVec.z + m[0][3]; + vRet.y = m[1][0]*vVec.x + m[1][1]*vVec.y + m[1][2]*vVec.z + m[1][3]; + vRet.z = m[2][0]*vVec.x + m[2][1]*vVec.y + m[2][2]*vVec.z + m[2][3]; + + return vRet; +} + +//----------------------------------------------------------------------------- +// Plane transformation +//----------------------------------------------------------------------------- +inline void VMatrix::TransformPlane( const VPlane &inPlane, VPlane &outPlane ) const +{ + Vector vTrans; + Vector3DMultiply( *this, inPlane.m_Normal, outPlane.m_Normal ); + outPlane.m_Dist = inPlane.m_Dist * DotProduct( outPlane.m_Normal, outPlane.m_Normal ); + outPlane.m_Dist += DotProduct( outPlane.m_Normal, GetTranslation( vTrans ) ); +} + +inline VPlane VMatrix::operator*(const VPlane &thePlane) const +{ + VPlane ret; + TransformPlane( thePlane, ret ); + return ret; +} + +inline bool VMatrix::InverseGeneral(VMatrix &vInverse) const +{ + return MatrixInverseGeneral( *this, vInverse ); +} + +inline VMatrix VMatrix::operator~() const +{ + VMatrix mRet; + InverseGeneral(mRet); + return mRet; +} + +inline void VMatrix::MatrixMul( const VMatrix &vm, VMatrix &out ) const +{ + out.Init( + m[0][0]*vm.m[0][0] + m[0][1]*vm.m[1][0] + m[0][2]*vm.m[2][0] + m[0][3]*vm.m[3][0], + m[0][0]*vm.m[0][1] + m[0][1]*vm.m[1][1] + m[0][2]*vm.m[2][1] + m[0][3]*vm.m[3][1], + m[0][0]*vm.m[0][2] + m[0][1]*vm.m[1][2] + m[0][2]*vm.m[2][2] + m[0][3]*vm.m[3][2], + m[0][0]*vm.m[0][3] + m[0][1]*vm.m[1][3] + m[0][2]*vm.m[2][3] + m[0][3]*vm.m[3][3], + + m[1][0]*vm.m[0][0] + m[1][1]*vm.m[1][0] + m[1][2]*vm.m[2][0] + m[1][3]*vm.m[3][0], + m[1][0]*vm.m[0][1] + m[1][1]*vm.m[1][1] + m[1][2]*vm.m[2][1] + m[1][3]*vm.m[3][1], + m[1][0]*vm.m[0][2] + m[1][1]*vm.m[1][2] + m[1][2]*vm.m[2][2] + m[1][3]*vm.m[3][2], + m[1][0]*vm.m[0][3] + m[1][1]*vm.m[1][3] + m[1][2]*vm.m[2][3] + m[1][3]*vm.m[3][3], + + m[2][0]*vm.m[0][0] + m[2][1]*vm.m[1][0] + m[2][2]*vm.m[2][0] + m[2][3]*vm.m[3][0], + m[2][0]*vm.m[0][1] + m[2][1]*vm.m[1][1] + m[2][2]*vm.m[2][1] + m[2][3]*vm.m[3][1], + m[2][0]*vm.m[0][2] + m[2][1]*vm.m[1][2] + m[2][2]*vm.m[2][2] + m[2][3]*vm.m[3][2], + m[2][0]*vm.m[0][3] + m[2][1]*vm.m[1][3] + m[2][2]*vm.m[2][3] + m[2][3]*vm.m[3][3], + + m[3][0]*vm.m[0][0] + m[3][1]*vm.m[1][0] + m[3][2]*vm.m[2][0] + m[3][3]*vm.m[3][0], + m[3][0]*vm.m[0][1] + m[3][1]*vm.m[1][1] + m[3][2]*vm.m[2][1] + m[3][3]*vm.m[3][1], + m[3][0]*vm.m[0][2] + m[3][1]*vm.m[1][2] + m[3][2]*vm.m[2][2] + m[3][3]*vm.m[3][2], + m[3][0]*vm.m[0][3] + m[3][1]*vm.m[1][3] + m[3][2]*vm.m[2][3] + m[3][3]*vm.m[3][3] + ); +} + +inline VMatrix VMatrix::operator*(const VMatrix &vm) const +{ + VMatrix ret; + MatrixMul( vm, ret ); + return ret; +} + inline Vector VMatrix::GetForward() const { return Vector(m[0][0], m[1][0], m[2][0]); @@ -578,7 +779,6 @@ inline void VMatrix::SetBasisVectors(const Vector &vForward, const Vector &vLeft SetUp(vUp); } - //----------------------------------------------------------------------------- // Methods related to the translation component of the matrix //----------------------------------------------------------------------------- @@ -647,80 +847,8 @@ inline void VMatrix::Set3x4( matrix3x4_t& matrix3x4 ) const memcpy(matrix3x4.Base(), m, sizeof( matrix3x4_t ) ); } - -//----------------------------------------------------------------------------- -// Matrix math operations -//----------------------------------------------------------------------------- -inline const VMatrix& VMatrix::operator+=(const VMatrix &other) -{ - for(int i=0; i < 4; i++) - { - for(int j=0; j < 4; j++) - { - m[i][j] += other.m[i][j]; - } - } - - return *this; -} - - #ifndef VECTOR_NO_SLOW_OPERATIONS -inline VMatrix VMatrix::operator+(const VMatrix &other) const -{ - VMatrix ret; - for(int i=0; i < 16; i++) - { - ((float*)ret.m)[i] = ((float*)m)[i] + ((float*)other.m)[i]; - } - return ret; -} - -inline VMatrix VMatrix::operator-(const VMatrix &other) const -{ - VMatrix ret; - - for(int i=0; i < 4; i++) - { - for(int j=0; j < 4; j++) - { - ret.m[i][j] = m[i][j] - other.m[i][j]; - } - } - - return ret; -} - -inline VMatrix VMatrix::operator-() const -{ - VMatrix ret; - for( int i=0; i < 16; i++ ) - { - ((float*)ret.m)[i] = ((float*)m)[i]; - } - return ret; -} - -#endif // VECTOR_NO_SLOW_OPERATIONS - - -//----------------------------------------------------------------------------- -// Vector transformation -//----------------------------------------------------------------------------- - -#ifndef VECTOR_NO_SLOW_OPERATIONS - -inline Vector VMatrix::operator*(const Vector &vVec) const -{ - Vector vRet; - vRet.x = m[0][0]*vVec.x + m[0][1]*vVec.y + m[0][2]*vVec.z + m[0][3]; - vRet.y = m[1][0]*vVec.x + m[1][1]*vVec.y + m[1][2]*vVec.z + m[1][3]; - vRet.z = m[2][0]*vVec.x + m[2][1]*vVec.y + m[2][2]*vVec.z + m[2][3]; - - return vRet; -} - inline Vector VMatrix::VMul4x3(const Vector &vVec) const { Vector vResult; @@ -783,18 +911,6 @@ inline void VMatrix::V4Mul(const Vector4D &vIn, Vector4D &vOut) const } -//----------------------------------------------------------------------------- -// Plane transformation -//----------------------------------------------------------------------------- -inline void VMatrix::TransformPlane( const VPlane &inPlane, VPlane &outPlane ) const -{ - Vector vTrans; - Vector3DMultiply( *this, inPlane.m_Normal, outPlane.m_Normal ); - outPlane.m_Dist = inPlane.m_Dist * DotProduct( outPlane.m_Normal, outPlane.m_Normal ); - outPlane.m_Dist += DotProduct( outPlane.m_Normal, GetTranslation( vTrans ) ); -} - - //----------------------------------------------------------------------------- // Other random stuff //----------------------------------------------------------------------------- @@ -820,14 +936,529 @@ inline Vector VMatrix::ApplyRotation(const Vector &vVec) const return VMul3x3(vVec); } -inline VMatrix VMatrix::operator~() const +#endif + +inline void VMatrix::InverseTR( VMatrix &ret ) const { + MatrixInverseTR( *this, ret ); +} + +inline void MatrixInverseTranspose( const VMatrix& src, VMatrix& dst ) +{ + src.InverseGeneral( dst ); + MatrixTranspose( dst, dst ); +} + +//----------------------------------------------------------------------------- +// Computes the inverse transpose +//----------------------------------------------------------------------------- +inline void MatrixInverseTranspose( const matrix3x4_t& src, matrix3x4_t& dst ) +{ + VMatrix tmp, out; + tmp.CopyFrom3x4( src ); + ::MatrixInverseTranspose( tmp, out ); + out.Set3x4( dst ); +} + + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +inline VMatrix VMatrix::InverseTR() const +{ + VMatrix ret; + MatrixInverseTR( *this, ret ); + return ret; +} + +inline Vector VMatrix::GetScale() const +{ + Vector vecs[3]; + + GetBasisVectors(vecs[0], vecs[1], vecs[2]); + + return Vector( + vecs[0].Length(), + vecs[1].Length(), + vecs[2].Length() + ); +} + +inline VMatrix VMatrix::Scale(const Vector &vScale) +{ + return VMatrix( + m[0][0]*vScale.x, m[0][1]*vScale.y, m[0][2]*vScale.z, m[0][3], + m[1][0]*vScale.x, m[1][1]*vScale.y, m[1][2]*vScale.z, m[1][3], + m[2][0]*vScale.x, m[2][1]*vScale.y, m[2][2]*vScale.z, m[2][3], + m[3][0]*vScale.x, m[3][1]*vScale.y, m[3][2]*vScale.z, 1.0f + ); +} + +inline VMatrix VMatrix::NormalizeBasisVectors() const +{ + Vector vecs[3]; VMatrix mRet; - InverseGeneral(mRet); + + GetBasisVectors(vecs[0], vecs[1], vecs[2]); + + VectorNormalize( vecs[0] ); + VectorNormalize( vecs[1] ); + VectorNormalize( vecs[2] ); + + mRet.SetBasisVectors(vecs[0], vecs[1], vecs[2]); + + // Set everything but basis vectors to identity. + mRet.m[3][0] = mRet.m[3][1] = mRet.m[3][2] = 0.0f; + mRet.m[3][3] = 1.0f; + return mRet; } -#endif +inline VMatrix VMatrix::Transpose() const +{ + return VMatrix( + m[0][0], m[1][0], m[2][0], m[3][0], + m[0][1], m[1][1], m[2][1], m[3][1], + m[0][2], m[1][2], m[2][2], m[3][2], + m[0][3], m[1][3], m[2][3], m[3][3]); +} + +// Transpose upper-left 3x3. +inline VMatrix VMatrix::Transpose3x3() const +{ + return VMatrix( + m[0][0], m[1][0], m[2][0], m[0][3], + m[0][1], m[1][1], m[2][1], m[1][3], + m[0][2], m[1][2], m[2][2], m[2][3], + m[3][0], m[3][1], m[3][2], m[3][3]); +} + +#endif // VECTOR_NO_SLOW_OPERATIONS + + +inline bool VMatrix::IsRotationMatrix() const +{ + Vector &v1 = (Vector&)m[0][0]; + Vector &v2 = (Vector&)m[1][0]; + Vector &v3 = (Vector&)m[2][0]; + + return + FloatMakePositive( 1 - v1.Length() ) < 0.01f && + FloatMakePositive( 1 - v2.Length() ) < 0.01f && + FloatMakePositive( 1 - v3.Length() ) < 0.01f && + FloatMakePositive( v1.Dot(v2) ) < 0.01f && + FloatMakePositive( v1.Dot(v3) ) < 0.01f && + FloatMakePositive( v2.Dot(v3) ) < 0.01f; +} + +inline void VMatrix::SetupMatrixOrgAngles( const Vector &origin, const QAngle &vAngles ) +{ + SetupMatrixAnglesInternal( m, vAngles ); + + // Add translation + m[0][3] = origin.x; + m[1][3] = origin.y; + m[2][3] = origin.z; + m[3][0] = 0.0f; + m[3][1] = 0.0f; + m[3][2] = 0.0f; + m[3][3] = 1.0f; +} + + +inline void VMatrix::SetupMatrixAngles( const QAngle &vAngles ) +{ + SetupMatrixAnglesInternal( m, vAngles ); + + // Zero everything else + m[0][3] = 0.0f; + m[1][3] = 0.0f; + m[2][3] = 0.0f; + m[3][0] = 0.0f; + m[3][1] = 0.0f; + m[3][2] = 0.0f; + m[3][3] = 1.0f; +} + +//----------------------------------------------------------------------------- +// Creates euler angles from a matrix +//----------------------------------------------------------------------------- +inline void MatrixToAngles( const VMatrix& src, QAngle& vAngles ) +{ + float forward[3]; + float left[3]; + float up[3]; + + // Extract the basis vectors from the matrix. Since we only need the Z + // component of the up vector, we don't get X and Y. + forward[0] = src[0][0]; + forward[1] = src[1][0]; + forward[2] = src[2][0]; + left[0] = src[0][1]; + left[1] = src[1][1]; + left[2] = src[2][1]; + up[2] = src[2][2]; + + float xyDist = sqrtf( forward[0] * forward[0] + forward[1] * forward[1] ); + + // enough here to get angles? + if ( xyDist > 0.001f ) + { + // (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis + vAngles[1] = RAD2DEG( atan2f( forward[1], forward[0] ) ); + + // The engine does pitch inverted from this, but we always end up negating it in the DLL + // UNDONE: Fix the engine to make it consistent + // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); + vAngles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); + + // (roll) z = ATAN( left.z, up.z ); + vAngles[2] = RAD2DEG( atan2f( left[2], up[2] ) ); + } + else // forward is mostly Z, gimbal lock- + { + // (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw + vAngles[1] = RAD2DEG( atan2f( -left[0], left[1] ) ); + + // The engine does pitch inverted from this, but we always end up negating it in the DLL + // UNDONE: Fix the engine to make it consistent + // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); + vAngles[0] = RAD2DEG( atan2f( -forward[2], xyDist ) ); + + // Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll) + vAngles[2] = 0; + } +} + +//----------------------------------------------------------------------------- +// Transform a plane +//----------------------------------------------------------------------------- +inline void MatrixTransformPlane( const VMatrix &src, const cplane_t &inPlane, cplane_t &outPlane ) +{ + // What we want to do is the following: + // 1) transform the normal into the new space. + // 2) Determine a point on the old plane given by plane dist * plane normal + // 3) Transform that point into the new space + // 4) Plane dist = DotProduct( new normal, new point ) + + // An optimized version, which works if the plane is orthogonal. + // 1) Transform the normal into the new space + // 2) Realize that transforming the old plane point into the new space + // is given by [ d * n'x + Tx, d * n'y + Ty, d * n'z + Tz ] + // where d = old plane dist, n' = transformed normal, Tn = translational component of transform + // 3) Compute the new plane dist using the dot product of the normal result of #2 + + // For a correct result, this should be an inverse-transpose matrix + // but that only matters if there are nonuniform scale or skew factors in this matrix. + Vector vTrans; + Vector3DMultiply( src, inPlane.normal, outPlane.normal ); + outPlane.dist = inPlane.dist * DotProduct( outPlane.normal, outPlane.normal ); + outPlane.dist += DotProduct( outPlane.normal, src.GetTranslation(vTrans) ); +} + + +//----------------------------------------------------------------------------- +// Helper functions. +//----------------------------------------------------------------------------- + +#define VMatToString(mat) (static_cast(CFmtStr("[ (%f, %f, %f), (%f, %f, %f), (%f, %f, %f), (%f, %f, %f) ]", mat.m[0][0], mat.m[0][1], mat.m[0][2], mat.m[0][3], mat.m[1][0], mat.m[1][1], mat.m[1][2], mat.m[1][3], mat.m[2][0], mat.m[2][1], mat.m[2][2], mat.m[2][3], mat.m[3][0], mat.m[3][1], mat.m[3][2], mat.m[3][3] ))) // ** Note: this generates a temporary, don't hold reference! + +//----------------------------------------------------------------------------- +// Matrix multiply +//----------------------------------------------------------------------------- +typedef ALIGN16 float VMatrixRaw_t[4]; + +//----------------------------------------------------------------------------- +// Matrix/vector multiply +//----------------------------------------------------------------------------- + +inline void Vector4DMultiply( const VMatrix& src1, Vector4D const& src2, Vector4D& dst ) +{ + // Make sure it works if src2 == dst + Vector4D tmp; + Vector4D const&v = (&src2 == &dst) ? tmp : src2; + + if (&src2 == &dst) + { + Vector4DCopy( src2, tmp ); + } + + dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2] + src1[0][3] * v[3]; + dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2] + src1[1][3] * v[3]; + dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2] + src1[2][3] * v[3]; + dst[3] = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2] + src1[3][3] * v[3]; +} + +//----------------------------------------------------------------------------- +// Matrix/vector multiply +//----------------------------------------------------------------------------- + +inline void Vector4DMultiplyPosition( const VMatrix& src1, Vector const& src2, Vector4D& dst ) +{ + // Make sure it works if src2 == dst + Vector tmp; + Vector const&v = ( &src2 == &dst.AsVector3D() ) ? static_cast(tmp) : src2; + + if (&src2 == &dst.AsVector3D()) + { + VectorCopy( src2, tmp ); + } + + dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2] + src1[0][3]; + dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2] + src1[1][3]; + dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2] + src1[2][3]; + dst[3] = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2] + src1[3][3]; +} + +//----------------------------------------------------------------------------- +// Vector3DMultiplyPositionProjective treats src2 as if it's a point +// and does the perspective divide at the end +//----------------------------------------------------------------------------- +inline void Vector3DMultiplyPositionProjective( const VMatrix& src1, const Vector &src2, Vector& dst ) +{ + // Make sure it works if src2 == dst + Vector tmp; + const Vector &v = (&src2 == &dst) ? static_cast(tmp): src2; + if( &src2 == &dst ) + { + VectorCopy( src2, tmp ); + } + + float w = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2] + src1[3][3]; + if ( w != 0.0f ) + { + w = 1.0f / w; + } + + dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2] + src1[0][3]; + dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2] + src1[1][3]; + dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2] + src1[2][3]; + dst *= w; +} + + +//----------------------------------------------------------------------------- +// Vector3DMultiplyProjective treats src2 as if it's a direction +// and does the perspective divide at the end +//----------------------------------------------------------------------------- +inline void Vector3DMultiplyProjective( const VMatrix& src1, const Vector &src2, Vector& dst ) +{ + // Make sure it works if src2 == dst + Vector tmp; + const Vector &v = (&src2 == &dst) ? static_cast(tmp) : src2; + if( &src2 == &dst ) + { + VectorCopy( src2, tmp ); + } + + float w; + dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2]; + dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2]; + dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2]; + w = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2]; + if (w != 0.0f) + { + dst /= w; + } + else + { + dst = vec3_origin; + } +} + + +//----------------------------------------------------------------------------- +// Multiplies the vector by the transpose of the matrix +//----------------------------------------------------------------------------- +inline void Vector4DMultiplyTranspose( const VMatrix& src1, Vector4D const& src2, Vector4D& dst ) +{ + // Make sure it works if src2 == dst + bool srcEqualsDst = (&src2 == &dst); + + Vector4D tmp; + Vector4D const&v = srcEqualsDst ? tmp : src2; + + if (srcEqualsDst) + { + Vector4DCopy( src2, tmp ); + } + + dst[0] = src1[0][0] * v[0] + src1[1][0] * v[1] + src1[2][0] * v[2] + src1[3][0] * v[3]; + dst[1] = src1[0][1] * v[0] + src1[1][1] * v[1] + src1[2][1] * v[2] + src1[3][1] * v[3]; + dst[2] = src1[0][2] * v[0] + src1[1][2] * v[1] + src1[2][2] * v[2] + src1[3][2] * v[3]; + dst[3] = src1[0][3] * v[0] + src1[1][3] * v[1] + src1[2][3] * v[2] + src1[3][3] * v[3]; +} + +//----------------------------------------------------------------------------- +// Multiplies the vector by the transpose of the matrix +//----------------------------------------------------------------------------- +inline void Vector3DMultiplyTranspose( const VMatrix& src1, const Vector& src2, Vector& dst ) +{ + // Make sure it works if src2 == dst + bool srcEqualsDst = (&src2 == &dst); + + Vector tmp; + const Vector&v = srcEqualsDst ? static_cast(tmp) : src2; + + if (srcEqualsDst) + { + VectorCopy( src2, tmp ); + } + + dst[0] = src1[0][0] * v[0] + src1[1][0] * v[1] + src1[2][0] * v[2]; + dst[1] = src1[0][1] * v[0] + src1[1][1] * v[1] + src1[2][1] * v[2]; + dst[2] = src1[0][2] * v[0] + src1[1][2] * v[1] + src1[2][2] * v[2]; +} + + +//----------------------------------------------------------------------------- +// Matrix copy +//----------------------------------------------------------------------------- +inline void MatrixCopy( const VMatrix& src, VMatrix& dst ) +{ + if (&src != &dst) + dst = src; +} + +inline void MatrixMultiply( const VMatrix& src1, const VMatrix& src2, VMatrix& dst ) +{ + // Make sure it works if src1 == dst or src2 == dst + VMatrix tmp1, tmp2; + const VMatrixRaw_t* s1 = (&src1 == &dst) ? tmp1.m : src1.m; + const VMatrixRaw_t* s2 = (&src2 == &dst) ? tmp2.m : src2.m; + + if (&src1 == &dst) + MatrixCopy( src1, tmp1 ); + + if (&src2 == &dst) + MatrixCopy( src2, tmp2 ); + + dst[0][0] = s1[0][0] * s2[0][0] + s1[0][1] * s2[1][0] + s1[0][2] * s2[2][0] + s1[0][3] * s2[3][0]; + dst[0][1] = s1[0][0] * s2[0][1] + s1[0][1] * s2[1][1] + s1[0][2] * s2[2][1] + s1[0][3] * s2[3][1]; + dst[0][2] = s1[0][0] * s2[0][2] + s1[0][1] * s2[1][2] + s1[0][2] * s2[2][2] + s1[0][3] * s2[3][2]; + dst[0][3] = s1[0][0] * s2[0][3] + s1[0][1] * s2[1][3] + s1[0][2] * s2[2][3] + s1[0][3] * s2[3][3]; + + dst[1][0] = s1[1][0] * s2[0][0] + s1[1][1] * s2[1][0] + s1[1][2] * s2[2][0] + s1[1][3] * s2[3][0]; + dst[1][1] = s1[1][0] * s2[0][1] + s1[1][1] * s2[1][1] + s1[1][2] * s2[2][1] + s1[1][3] * s2[3][1]; + dst[1][2] = s1[1][0] * s2[0][2] + s1[1][1] * s2[1][2] + s1[1][2] * s2[2][2] + s1[1][3] * s2[3][2]; + dst[1][3] = s1[1][0] * s2[0][3] + s1[1][1] * s2[1][3] + s1[1][2] * s2[2][3] + s1[1][3] * s2[3][3]; + + dst[2][0] = s1[2][0] * s2[0][0] + s1[2][1] * s2[1][0] + s1[2][2] * s2[2][0] + s1[2][3] * s2[3][0]; + dst[2][1] = s1[2][0] * s2[0][1] + s1[2][1] * s2[1][1] + s1[2][2] * s2[2][1] + s1[2][3] * s2[3][1]; + dst[2][2] = s1[2][0] * s2[0][2] + s1[2][1] * s2[1][2] + s1[2][2] * s2[2][2] + s1[2][3] * s2[3][2]; + dst[2][3] = s1[2][0] * s2[0][3] + s1[2][1] * s2[1][3] + s1[2][2] * s2[2][3] + s1[2][3] * s2[3][3]; + + dst[3][0] = s1[3][0] * s2[0][0] + s1[3][1] * s2[1][0] + s1[3][2] * s2[2][0] + s1[3][3] * s2[3][0]; + dst[3][1] = s1[3][0] * s2[0][1] + s1[3][1] * s2[1][1] + s1[3][2] * s2[2][1] + s1[3][3] * s2[3][1]; + dst[3][2] = s1[3][0] * s2[0][2] + s1[3][1] * s2[1][2] + s1[3][2] * s2[2][2] + s1[3][3] * s2[3][2]; + dst[3][3] = s1[3][0] * s2[0][3] + s1[3][1] * s2[1][3] + s1[3][2] * s2[2][3] + s1[3][3] * s2[3][3]; +} + + +//----------------------------------------------------------------------------- +// Purpose: Builds the matrix for a counterclockwise rotation about an arbitrary axis. +// +// | ax2 + (1 - ax2)cosQ axay(1 - cosQ) - azsinQ azax(1 - cosQ) + aysinQ | +// Ra(Q) = | axay(1 - cosQ) + azsinQ ay2 + (1 - ay2)cosQ ayaz(1 - cosQ) - axsinQ | +// | azax(1 - cosQ) - aysinQ ayaz(1 - cosQ) + axsinQ az2 + (1 - az2)cosQ | +// +// Input : mat - +// vAxisOrRot - +// angle - +//----------------------------------------------------------------------------- +inline void MatrixBuildRotationAboutAxis( const Vector &vAxisOfRot, float angleDegrees, matrix3x4_t &dst ) +{ + float radians; + float axisXSquared; + float axisYSquared; + float axisZSquared; + float fSin; + float fCos; + + radians = angleDegrees * ( M_PI / 180.0 ); + fSin = sinf( radians ); + fCos = cosf( radians ); + + axisXSquared = vAxisOfRot[0] * vAxisOfRot[0]; + axisYSquared = vAxisOfRot[1] * vAxisOfRot[1]; + axisZSquared = vAxisOfRot[2] * vAxisOfRot[2]; + + // Column 0: + dst[0][0] = axisXSquared + (1 - axisXSquared) * fCos; + dst[1][0] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) + vAxisOfRot[2] * fSin; + dst[2][0] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) - vAxisOfRot[1] * fSin; + + // Column 1: + dst[0][1] = vAxisOfRot[0] * vAxisOfRot[1] * (1 - fCos) - vAxisOfRot[2] * fSin; + dst[1][1] = axisYSquared + (1 - axisYSquared) * fCos; + dst[2][1] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) + vAxisOfRot[0] * fSin; + + // Column 2: + dst[0][2] = vAxisOfRot[2] * vAxisOfRot[0] * (1 - fCos) + vAxisOfRot[1] * fSin; + dst[1][2] = vAxisOfRot[1] * vAxisOfRot[2] * (1 - fCos) - vAxisOfRot[0] * fSin; + dst[2][2] = axisZSquared + (1 - axisZSquared) * fCos; + + // Column 3: + dst[0][3] = 0; + dst[1][3] = 0; + dst[2][3] = 0; +} + +//----------------------------------------------------------------------------- +// Purpose: Builds the matrix for a counterclockwise rotation about an arbitrary axis. +// +// | ax2 + (1 - ax2)cosQ axay(1 - cosQ) - azsinQ azax(1 - cosQ) + aysinQ | +// Ra(Q) = | axay(1 - cosQ) + azsinQ ay2 + (1 - ay2)cosQ ayaz(1 - cosQ) - axsinQ | +// | azax(1 - cosQ) - aysinQ ayaz(1 - cosQ) + axsinQ az2 + (1 - az2)cosQ | +// +// Input : mat - +// vAxisOrRot - +// angle - +//----------------------------------------------------------------------------- +inline void MatrixBuildRotationAboutAxis( VMatrix &dst, const Vector &vAxisOfRot, float angleDegrees ) +{ + MatrixBuildRotationAboutAxis( vAxisOfRot, angleDegrees, const_cast< matrix3x4_t &> ( dst.As3x4() ) ); + dst[3][0] = 0; + dst[3][1] = 0; + dst[3][2] = 0; + dst[3][3] = 1; +} + + +//----------------------------------------------------------------------------- +// Builds a rotation matrix that rotates one direction vector into another +//----------------------------------------------------------------------------- +inline void MatrixBuildTranslation( VMatrix& dst, float x, float y, float z ) +{ + MatrixSetIdentity( dst ); + dst[0][3] = x; + dst[1][3] = y; + dst[2][3] = z; +} + +inline void MatrixBuildTranslation( VMatrix& dst, const Vector &translation ) +{ + MatrixSetIdentity( dst ); + dst[0][3] = translation[0]; + dst[1][3] = translation[1]; + dst[2][3] = translation[2]; +} + + +inline void MatrixTranslate( VMatrix& dst, const Vector &translation ) +{ + VMatrix matTranslation, temp; + MatrixBuildTranslation( matTranslation, translation ); + MatrixMultiply( dst, matTranslation, temp ); + dst = temp; +} + +inline void MatrixRotate( VMatrix& dst, const Vector& vAxisOfRot, float angleDegrees ) +{ + VMatrix rotation, temp; + MatrixBuildRotationAboutAxis( rotation, vAxisOfRot, angleDegrees ); + MatrixMultiply( dst, rotation, temp ); + dst = temp; +} //----------------------------------------------------------------------------- @@ -842,15 +1473,6 @@ inline void MatrixGetColumn( const VMatrix &src, int nCol, Vector *pColumn ) pColumn->z = src[2][nCol]; } -inline void MatrixSetColumn( VMatrix &src, int nCol, const Vector &column ) -{ - Assert( (nCol >= 0) && (nCol <= 3) ); - - src.m[0][nCol] = column.x; - src.m[1][nCol] = column.y; - src.m[2][nCol] = column.z; -} - inline void MatrixGetRow( const VMatrix &src, int nRow, Vector *pRow ) { Assert( (nRow >= 0) && (nRow <= 3) ); @@ -863,21 +1485,6 @@ inline void MatrixSetRow( VMatrix &dst, int nRow, const Vector &row ) *(Vector*)dst[nRow] = row; } - -//----------------------------------------------------------------------------- -// Vector3DMultiplyPosition treats src2 as if it's a point (adds the translation) -//----------------------------------------------------------------------------- -// NJS: src2 is passed in as a full vector rather than a reference to prevent the need -// for 2 branches and a potential copy in the body. (ie, handling the case when the src2 -// reference is the same as the dst reference ). -inline void Vector3DMultiplyPosition( const VMatrix& src1, const VectorByValue src2, Vector& dst ) -{ - dst[0] = src1[0][0] * src2.x + src1[0][1] * src2.y + src1[0][2] * src2.z + src1[0][3]; - dst[1] = src1[1][0] * src2.x + src1[1][1] * src2.y + src1[1][2] * src2.z + src1[1][3]; - dst[2] = src1[2][0] * src2.x + src1[2][1] * src2.y + src1[2][2] * src2.z + src1[2][3]; -} - - //----------------------------------------------------------------------------- // Transform a plane that has an axis-aligned normal //----------------------------------------------------------------------------- @@ -948,6 +1555,501 @@ inline void MatrixPerspectiveOffCenterX( VMatrix& dst, double flFovX, double flA dst = temp; } + +#ifndef VECTOR_NO_SLOW_OPERATIONS + +inline VMatrix SetupMatrixIdentity() +{ + return VMatrix( + 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); +} + +inline VMatrix SetupMatrixTranslation(const Vector &vTranslation) +{ + return VMatrix( + 1.0f, 0.0f, 0.0f, vTranslation.x, + 0.0f, 1.0f, 0.0f, vTranslation.y, + 0.0f, 0.0f, 1.0f, vTranslation.z, + 0.0f, 0.0f, 0.0f, 1.0f + ); +} + +inline VMatrix SetupMatrixScale(const Vector &vScale) +{ + return VMatrix( + vScale.x, 0.0f, 0.0f, 0.0f, + 0.0f, vScale.y, 0.0f, 0.0f, + 0.0f, 0.0f, vScale.z, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f + ); +} + +inline VMatrix SetupMatrixReflection(const VPlane &thePlane) +{ + VMatrix mReflect, mBack, mForward; + Vector vOrigin, N; + + N = thePlane.m_Normal; + + mReflect.Init( + -2.0f*N.x*N.x + 1.0f, -2.0f*N.x*N.y, -2.0f*N.x*N.z, 0.0f, + -2.0f*N.y*N.x, -2.0f*N.y*N.y + 1.0f, -2.0f*N.y*N.z, 0.0f, + -2.0f*N.z*N.x, -2.0f*N.z*N.y, -2.0f*N.z*N.z + 1.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f + ); + + vOrigin = thePlane.GetPointOnPlane(); + + mBack.Identity(); + mBack.SetTranslation(-vOrigin); + + mForward.Identity(); + mForward.SetTranslation(vOrigin); + + // (multiplied in reverse order, so it translates to the origin point, + // reflects, and translates back). + return mForward * mReflect * mBack; +} + +inline VMatrix SetupMatrixProjection(const Vector &vOrigin, const VPlane &thePlane) +{ + vec_t dot; + VMatrix mRet; + + + #define PN thePlane.m_Normal + #define PD thePlane.m_Dist; + + dot = PN[0]*vOrigin.x + PN[1]*vOrigin.y + PN[2]*vOrigin.z - PD; + + mRet.m[0][0] = dot - vOrigin.x * PN[0]; + mRet.m[0][1] = -vOrigin.x * PN[1]; + mRet.m[0][2] = -vOrigin.x * PN[2]; + mRet.m[0][3] = -vOrigin.x * -PD; + + mRet.m[1][0] = -vOrigin.y * PN[0]; + mRet.m[1][1] = dot - vOrigin.y * PN[1]; + mRet.m[1][2] = -vOrigin.y * PN[2]; + mRet.m[1][3] = -vOrigin.y * -PD; + + mRet.m[2][0] = -vOrigin.z * PN[0]; + mRet.m[2][1] = -vOrigin.z * PN[1]; + mRet.m[2][2] = dot - vOrigin.z * PN[2]; + mRet.m[2][3] = -vOrigin.z * -PD; + + mRet.m[3][0] = -PN[0]; + mRet.m[3][1] = -PN[1]; + mRet.m[3][2] = -PN[2]; + mRet.m[3][3] = dot + PD; + + #undef PN + #undef PD + + return mRet; +} + +inline VMatrix SetupMatrixAxisRot(const Vector &vAxis, vec_t fDegrees) +{ + vec_t s, c, t; + vec_t tx, ty, tz; + vec_t sx, sy, sz; + vec_t fRadians; + + + fRadians = fDegrees * (M_PI / 180.0f); + + s = (vec_t)sin(fRadians); + c = (vec_t)cos(fRadians); + t = 1.0f - c; + + tx = t * vAxis.x; ty = t * vAxis.y; tz = t * vAxis.z; + sx = s * vAxis.x; sy = s * vAxis.y; sz = s * vAxis.z; + + return VMatrix( + tx*vAxis.x + c, tx*vAxis.y - sz, tx*vAxis.z + sy, 0.0f, + tx*vAxis.y + sz, ty*vAxis.y + c, ty*vAxis.z - sx, 0.0f, + tx*vAxis.z - sy, ty*vAxis.z + sx, tz*vAxis.z + c, 0.0f, + 0.0f, 0.0f, 0.0f, 1.0f); +} + +//----------------------------------------------------------------------------- +// Setup a matrix from euler angles. +//----------------------------------------------------------------------------- +inline void MatrixFromAngles( const QAngle& vAngles, VMatrix& dst ) +{ + dst.SetupMatrixOrgAngles( vec3_origin, vAngles ); +} + +inline VMatrix SetupMatrixAngles(const QAngle &vAngles) +{ + VMatrix mRet; + MatrixFromAngles( vAngles, mRet ); + return mRet; +} + +inline VMatrix SetupMatrixOrgAngles(const Vector &origin, const QAngle &vAngles) +{ + VMatrix mRet; + mRet.SetupMatrixOrgAngles( origin, vAngles ); + return mRet; +} + +#endif // VECTOR_NO_SLOW_OPERATIONS + + +inline bool PlaneIntersection( const VPlane &vp1, const VPlane &vp2, const VPlane &vp3, Vector &vOut ) +{ + VMatrix mMat, mInverse; + + mMat.Init( + vp1.m_Normal.x, vp1.m_Normal.y, vp1.m_Normal.z, -vp1.m_Dist, + vp2.m_Normal.x, vp2.m_Normal.y, vp2.m_Normal.z, -vp2.m_Dist, + vp3.m_Normal.x, vp3.m_Normal.y, vp3.m_Normal.z, -vp3.m_Dist, + 0.0f, 0.0f, 0.0f, 1.0f + ); + + if(mMat.InverseGeneral(mInverse)) + { + //vOut = mInverse * Vector(0.0f, 0.0f, 0.0f); + mInverse.GetTranslation( vOut ); + return true; + } + else + { + return false; + } +} + +//----------------------------------------------------------------------------- +// Builds a rotation matrix that rotates one direction vector into another +//----------------------------------------------------------------------------- +inline void MatrixBuildRotation( VMatrix &dst, const Vector& initialDirection, const Vector& finalDirection ) +{ + float angle = DotProduct( initialDirection, finalDirection ); + Assert( IsFinite(angle) ); + + Vector axis; + + // No rotation required + if (angle - 1.0 > -1e-3) + { + // parallel case + MatrixSetIdentity(dst); + return; + } + else if (angle + 1.0 < 1e-3) + { + // antiparallel case, pick any axis in the plane + // perpendicular to the final direction. Choose the direction (x,y,z) + // which has the minimum component of the final direction, use that + // as an initial guess, then subtract out the component which is + // parallel to the final direction + int idx = 0; + if (FloatMakePositive(finalDirection[1]) < FloatMakePositive(finalDirection[idx])) + idx = 1; + if (FloatMakePositive(finalDirection[2]) < FloatMakePositive(finalDirection[idx])) + idx = 2; + + axis.Init( 0, 0, 0 ); + axis[idx] = 1.0f; + VectorMA( axis, -DotProduct( axis, finalDirection ), finalDirection, axis ); + VectorNormalize(axis); + angle = 180.0f; + } + else + { + CrossProduct( initialDirection, finalDirection, axis ); + VectorNormalize( axis ); + angle = acos(angle) * 180 / M_PI; + } + + MatrixBuildRotationAboutAxis( dst, axis, angle ); + +#ifdef _DEBUG + Vector test; + Vector3DMultiply( dst, initialDirection, test ); + test -= finalDirection; + Assert( test.LengthSqr() < 1e-3 ); +#endif +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +inline void MatrixBuildRotateZ( VMatrix &dst, float angleDegrees ) +{ + float radians = angleDegrees * ( M_PI / 180.0f ); + + float fSin = ( float )sin( radians ); + float fCos = ( float )cos( radians ); + + dst[0][0] = fCos; dst[0][1] = -fSin; dst[0][2] = 0.0f; dst[0][3] = 0.0f; + dst[1][0] = fSin; dst[1][1] = fCos; dst[1][2] = 0.0f; dst[1][3] = 0.0f; + dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = 1.0f; dst[2][3] = 0.0f; + dst[3][0] = 0.0f; dst[3][1] = 0.0f; dst[3][2] = 0.0f; dst[3][3] = 1.0f; +} + +// Builds a scale matrix +inline void MatrixBuildScale( VMatrix &dst, float x, float y, float z ) +{ + dst[0][0] = x; dst[0][1] = 0.0f; dst[0][2] = 0.0f; dst[0][3] = 0.0f; + dst[1][0] = 0.0f; dst[1][1] = y; dst[1][2] = 0.0f; dst[1][3] = 0.0f; + dst[2][0] = 0.0f; dst[2][1] = 0.0f; dst[2][2] = z; dst[2][3] = 0.0f; + dst[3][0] = 0.0f; dst[3][1] = 0.0f; dst[3][2] = 0.0f; dst[3][3] = 1.0f; +} + +inline void MatrixBuildScale( VMatrix &dst, const Vector& scale ) +{ + MatrixBuildScale( dst, scale.x, scale.y, scale.z ); +} + +inline void MatrixBuildPerspective( VMatrix &dst, float fovX, float fovY, float zNear, float zFar ) +{ + // FIXME: collapse all of this into one matrix after we figure out what all should be in here. + float width = 2 * zNear * tan( fovX * ( M_PI/180.0f ) * 0.5f ); + float height = 2 * zNear * tan( fovY * ( M_PI/180.0f ) * 0.5f ); + + memset( dst.Base(), 0, sizeof( dst ) ); + dst[0][0] = 2.0F * zNear / width; + dst[1][1] = 2.0F * zNear / height; + dst[2][2] = -zFar / ( zNear - zFar ); + dst[3][2] = 1.0f; + dst[2][3] = zNear * zFar / ( zNear - zFar ); + + // negate X and Y so that X points right, and Y points up. + VMatrix negateXY; + negateXY.Identity(); + negateXY[0][0] = -1.0f; + negateXY[1][1] = -1.0f; + MatrixMultiply( negateXY, dst, dst ); + + VMatrix addW; + addW.Identity(); + addW[0][3] = 1.0f; + addW[1][3] = 1.0f; + addW[2][3] = 0.0f; + MatrixMultiply( addW, dst, dst ); + + VMatrix scaleHalf; + scaleHalf.Identity(); + scaleHalf[0][0] = 0.5f; + scaleHalf[1][1] = 0.5f; + MatrixMultiply( scaleHalf, dst, dst ); +} + +static inline void CalculateAABBForNormalizedFrustum_Helper( float x, float y, float z, const VMatrix &volumeToWorld, Vector &mins, Vector &maxs ) +{ + Vector volumeSpacePos( x, y, z ); + + // Make sure it's been clipped + Assert( volumeSpacePos[0] >= -1e-3f ); + Assert( volumeSpacePos[0] - 1.0f <= 1e-3f ); + Assert( volumeSpacePos[1] >= -1e-3f ); + Assert( volumeSpacePos[1] - 1.0f <= 1e-3f ); + Assert( volumeSpacePos[2] >= -1e-3f ); + Assert( volumeSpacePos[2] - 1.0f <= 1e-3f ); + + Vector worldPos; + Vector3DMultiplyPositionProjective( volumeToWorld, volumeSpacePos, worldPos ); + AddPointToBounds( worldPos, mins, maxs ); +} + +//----------------------------------------------------------------------------- +// Given an inverse projection matrix, take the extremes of the space in transformed into world space and +// get a bounding box. +//----------------------------------------------------------------------------- +inline void CalculateAABBFromProjectionMatrixInverse( const VMatrix &volumeToWorld, Vector *pMins, Vector *pMaxs ) +{ + // FIXME: Could maybe do better than the compile with all of these multiplies by 0 and 1. + ClearBounds( *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 0, 0, 0, volumeToWorld, *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 0, 0, 1, volumeToWorld, *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 0, 1, 0, volumeToWorld, *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 0, 1, 1, volumeToWorld, *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 1, 0, 0, volumeToWorld, *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 1, 0, 1, volumeToWorld, *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 1, 1, 0, volumeToWorld, *pMins, *pMaxs ); + CalculateAABBForNormalizedFrustum_Helper( 1, 1, 1, volumeToWorld, *pMins, *pMaxs ); +} + +inline void CalculateAABBFromProjectionMatrix( const VMatrix &worldToVolume, Vector *pMins, Vector *pMaxs ) +{ + VMatrix volumeToWorld; + MatrixInverseGeneral( worldToVolume, volumeToWorld ); + CalculateAABBFromProjectionMatrixInverse( volumeToWorld, pMins, pMaxs ); +} + +//----------------------------------------------------------------------------- +// Given an inverse projection matrix, take the extremes of the space in transformed into world space and +// get a bounding sphere. +//----------------------------------------------------------------------------- +inline void CalculateSphereFromProjectionMatrixInverse( const VMatrix &volumeToWorld, Vector *pCenter, float *pflRadius ) +{ + // FIXME: Could maybe do better than the compile with all of these multiplies by 0 and 1. + + // Need 3 points: the endpoint of the line through the center of the near + far planes, + // and one point on the far plane. From that, we can derive a point somewhere on the center line + // which would produce the smallest bounding sphere. + Vector vecCenterNear, vecCenterFar, vecNearEdge, vecFarEdge; + Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.5f, 0.5f, 0.0f ), vecCenterNear ); + Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.5f, 0.5f, 1.0f ), vecCenterFar ); + Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.0f, 0.0f, 0.0f ), vecNearEdge ); + Vector3DMultiplyPositionProjective( volumeToWorld, Vector( 0.0f, 0.0f, 1.0f ), vecFarEdge ); + + // Let the distance between the near + far center points = l + // Let the distance between the near center point + near edge point = h1 + // Let the distance between the far center point + far edge point = h2 + // Let the distance along the center line from the near point to the sphere center point = x + // Then let the distance between the sphere center point + near edge point == + // the distance between the sphere center point + far edge point == r == radius of sphere + // Then h1^2 + x^2 == r^2 == (l-x)^2 + h2^2 + // h1^x + x^2 = l^2 - 2 * l * x + x^2 + h2^2 + // 2 * l * x = l^2 + h2^2 - h1^2 + // x = (l^2 + h2^2 - h1^2) / (2 * l) + // r = sqrt( hl^1 + x^2 ) + Vector vecDelta; + VectorSubtract( vecCenterFar, vecCenterNear, vecDelta ); + float l = vecDelta.Length(); + float h1Sqr = vecCenterNear.DistToSqr( vecNearEdge ); + float h2Sqr = vecCenterFar.DistToSqr( vecFarEdge ); + float x = (l*l + h2Sqr - h1Sqr) / (2.0f * l); + VectorMA( vecCenterNear, (x / l), vecDelta, *pCenter ); + *pflRadius = sqrt( h1Sqr + x*x ); +} + +//----------------------------------------------------------------------------- +// Given a projection matrix, take the extremes of the space in transformed into world space and +// get a bounding sphere. +//----------------------------------------------------------------------------- +inline void CalculateSphereFromProjectionMatrix( const VMatrix &worldToVolume, Vector *pCenter, float *pflRadius ) +{ + VMatrix volumeToWorld; + MatrixInverseGeneral( worldToVolume, volumeToWorld ); + CalculateSphereFromProjectionMatrixInverse( volumeToWorld, pCenter, pflRadius ); +} + + +static inline void FrustumPlanesFromMatrixHelper( const VMatrix &shadowToWorld, const Vector &p1, const Vector &p2, const Vector &p3, + Vector &normal, float &dist ) +{ + Vector world1, world2, world3; + Vector3DMultiplyPositionProjective( shadowToWorld, p1, world1 ); + Vector3DMultiplyPositionProjective( shadowToWorld, p2, world2 ); + Vector3DMultiplyPositionProjective( shadowToWorld, p3, world3 ); + + Vector v1, v2; + VectorSubtract( world2, world1, v1 ); + VectorSubtract( world3, world1, v2 ); + + CrossProduct( v1, v2, normal ); + VectorNormalize( normal ); + dist = DotProduct( normal, world1 ); +} + +inline void FrustumPlanesFromMatrix( const VMatrix &clipToWorld, Frustum_t &frustum ) +{ + Vector normal; + float dist; + + FrustumPlanesFromMatrixHelper( clipToWorld, + Vector( 0.0f, 0.0f, 0.0f ), Vector( 1.0f, 0.0f, 0.0f ), Vector( 0.0f, 1.0f, 0.0f ), normal, dist ); + frustum.SetPlane( FRUSTUM_NEARZ, PLANE_ANYZ, normal, dist ); + + FrustumPlanesFromMatrixHelper( clipToWorld, + Vector( 0.0f, 0.0f, 1.0f ), Vector( 0.0f, 1.0f, 1.0f ), Vector( 1.0f, 0.0f, 1.0f ), normal, dist ); + frustum.SetPlane( FRUSTUM_FARZ, PLANE_ANYZ, normal, dist ); + + FrustumPlanesFromMatrixHelper( clipToWorld, + Vector( 1.0f, 0.0f, 0.0f ), Vector( 1.0f, 1.0f, 1.0f ), Vector( 1.0f, 1.0f, 0.0f ), normal, dist ); + frustum.SetPlane( FRUSTUM_RIGHT, PLANE_ANYZ, normal, dist ); + + FrustumPlanesFromMatrixHelper( clipToWorld, + Vector( 0.0f, 0.0f, 0.0f ), Vector( 0.0f, 1.0f, 1.0f ), Vector( 0.0f, 0.0f, 1.0f ), normal, dist ); + frustum.SetPlane( FRUSTUM_LEFT, PLANE_ANYZ, normal, dist ); + + FrustumPlanesFromMatrixHelper( clipToWorld, + Vector( 1.0f, 1.0f, 0.0f ), Vector( 1.0f, 1.0f, 1.0f ), Vector( 0.0f, 1.0f, 1.0f ), normal, dist ); + frustum.SetPlane( FRUSTUM_TOP, PLANE_ANYZ, normal, dist ); + + FrustumPlanesFromMatrixHelper( clipToWorld, + Vector( 1.0f, 0.0f, 0.0f ), Vector( 0.0f, 0.0f, 1.0f ), Vector( 1.0f, 0.0f, 1.0f ), normal, dist ); + frustum.SetPlane( FRUSTUM_BOTTOM, PLANE_ANYZ, normal, dist ); +} + +inline void MatrixBuildOrtho( VMatrix& dst, double left, double top, double right, double bottom, double zNear, double zFar ) +{ + // FIXME: This is being used incorrectly! Should read: + // D3DXMatrixOrthoOffCenterRH( &matrix, left, right, bottom, top, zNear, zFar ); + // Which is certainly why we need these extra -1 scales in y. Bleah + + // NOTE: The camera can be imagined as the following diagram: + // /z + // / + // /____ x Z is going into the screen + // | + // | + // |y + // + // (0,0,z) represents the upper-left corner of the screen. + // Our projection transform needs to transform from this space to a LH coordinate + // system that looks thusly: + // + // y| /z + // | / + // |/____ x Z is going into the screen + // + // Where x,y lies between -1 and 1, and z lies from 0 to 1 + // This is because the viewport transformation from projection space to pixels + // introduces a -1 scale in the y coordinates + // D3DXMatrixOrthoOffCenterRH( &matrix, left, right, top, bottom, zNear, zFar ); + + dst.Init( 2.0f / ( right - left ), 0.0f, 0.0f, ( left + right ) / ( left - right ), + 0.0f, 2.0f / ( bottom - top ), 0.0f, ( bottom + top ) / ( top - bottom ), + 0.0f, 0.0f, 1.0f / ( zNear - zFar ), zNear / ( zNear - zFar ), + 0.0f, 0.0f, 0.0f, 1.0f ); +} + +inline void MatrixBuildPerspectiveZRange( VMatrix& dst, double flZNear, double flZFar ) +{ + dst.m[2][0] = 0.0f; + dst.m[2][1] = 0.0f; + dst.m[2][2] = flZFar / ( flZNear - flZFar ); + dst.m[2][3] = flZNear * flZFar / ( flZNear - flZFar ); +} + +inline void MatrixBuildPerspectiveX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar ) +{ + float flWidthScale = 1.0f / tanf( flFovX * M_PI / 360.0f ); + float flHeightScale = flAspect * flWidthScale; + dst.Init( flWidthScale, 0.0f, 0.0f, 0.0f, + 0.0f, flHeightScale, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, -1.0f, 0.0f ); + + MatrixBuildPerspectiveZRange ( dst, flZNear, flZFar ); +} + +inline void MatrixBuildPerspectiveOffCenterX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar, double bottom, double top, double left, double right ) +{ + float flWidth = tanf( flFovX * M_PI / 360.0f ); + float flHeight = flWidth / flAspect; + + // bottom, top, left, right are 0..1 so convert to -/2../2 + float flLeft = -(flWidth/2.0f) * (1.0f - left) + left * (flWidth/2.0f); + float flRight = -(flWidth/2.0f) * (1.0f - right) + right * (flWidth/2.0f); + float flBottom = -(flHeight/2.0f) * (1.0f - bottom) + bottom * (flHeight/2.0f); + float flTop = -(flHeight/2.0f) * (1.0f - top) + top * (flHeight/2.0f); + + dst.Init( 1.0f / (flRight-flLeft), 0.0f, (flLeft+flRight)/(flRight-flLeft), 0.0f, + 0.0f, 1.0f /(flTop-flBottom), (flTop+flBottom)/(flTop-flBottom), 0.0f, + 0.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, -1.0f, 0.0f ); + + MatrixBuildPerspectiveZRange ( dst, flZNear, flZFar ); +} + #endif