//========= Copyright (c) 1996-2005, Valve Corporation, All rights reserved. ============// // // Purpose: // // $NoKeywords: $ // //=============================================================================// #include "core/stdafx.h" #include "tier0/dbg.h" #if !defined(_STATIC_LINKED) || defined(_SHARED_LIB) #include "mathlib/vmatrix.h" #include "mathlib/mathlib.h" #include "mathlib/vector4d.h" #include "mathlib/ssemath.h" // memdbgon must be the last include file in a .cpp file!!! //#include "tier0/memdbgon.h" #pragma warning (disable : 4700) // local variable 'x' used without having been initialized // ------------------------------------------------------------------------------------------- // // 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 Vector3D& 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 Vector3D& 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; Vector3D 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 Vector3D& 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 Vector3D& vAxis, vec_t fDegrees) { vec_t s, c, t; // sin, cos, 1-cos 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); } // Basically takes a cross product and then does the same thing as SetupMatrixAxisRot // above, but takes advantage of the fact that the sin angle is precomputed. VMatrix SetupMatrixAxisToAxisRot(const Vector3D& vFromAxis, const Vector3D& vToAxis) { Assert(vFromAxis.LengthSqr() == 1); // these axes Assert(vToAxis.LengthSqr() == 1); // must be normal. vec_t s, c, t; // sin(theta), cos(theta), 1-cos vec_t tx, ty, tz; vec_t sx, sy, sz; Vector3D vAxis = vFromAxis.Cross(vToAxis); s = vAxis.Length(); c = vFromAxis.Dot(vToAxis); t = 1.0f - c; if (s > 0) { vAxis *= 1.0 / s; 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); } else { return SetupMatrixIdentity(); } } VMatrix SetupMatrixAngles(const QAngle& vAngles) { VMatrix mRet; MatrixFromAngles(vAngles, mRet); return mRet; } VMatrix SetupMatrixOrgAngles(const Vector3D& origin, const QAngle& vAngles) { VMatrix mRet; mRet.SetupMatrixOrgAngles(origin, vAngles); return mRet; } #endif // VECTOR_NO_SLOW_OPERATIONS #if 1 bool PlaneIntersection(const VPlane& vp1, const VPlane& vp2, const VPlane& vp3, Vector3D& vOut) { Vector3D v2Cross3 = CrossProduct(vp2.m_Normal, vp3.m_Normal); float flDenom = DotProduct(vp1.m_Normal, v2Cross3); if (fabs(flDenom) < FLT_EPSILON) return false; Vector3D vRet = vp1.m_Dist * v2Cross3 + vp2.m_Dist * CrossProduct(vp3.m_Normal, vp1.m_Normal) + vp3.m_Dist * CrossProduct(vp1.m_Normal, vp2.m_Normal); vOut = vRet * (1.0 / flDenom); return true; } #else // old slow innaccurate code 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; } } #endif // ------------------------------------------------------------------------------------------- // // 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 = 1e-6f; 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) { Vector3D 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; } Vector3D VMatrix::GetScale() const { Vector3D vecs[3]; GetBasisVectors(vecs[0], vecs[1], vecs[2]); return Vector3D( vecs[0].Length(), vecs[1].Length(), vecs[2].Length() ); } VMatrix VMatrix::Scale(const Vector3D& 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 { Vector3D 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 { Vector3D& v1 = (Vector3D&)m[0][0]; Vector3D& v2 = (Vector3D&)m[1][0]; Vector3D& v3 = (Vector3D&)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; } void VMatrix::SetupMatrixOrgAngles(const Vector3D& origin, 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; // 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; } //----------------------------------------------------------------------------- // 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, Vector3D const& src2, Vector4D& dst) { // Make sure it works if src2 == dst Vector3D tmp; Vector3D 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 Vector3D& src2, Vector3D& dst) { // Make sure it works if src2 == dst Vector3D tmp; const Vector3D& 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 Vector3D& src2, Vector3D& dst) { // Make sure it works if src2 == dst Vector3D tmp; const Vector3D& 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 Vector3D& src2, Vector3D& dst) { // Make sure it works if src2 == dst Vector3D tmp; const Vector3D& 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 Vector3D& src2, Vector3D& dst) { // Make sure it works if src2 == dst bool srcEqualsDst = (&src2 == &dst); Vector3D tmp; const Vector3D& 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. Vector3D 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 Vector3D& 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 Vector3D& vAxisOfRot, float angleDegrees) { MatrixBuildRotationAboutAxis(vAxisOfRot, angleDegrees, 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 Vector3D& initialDirection, const Vector3D& finalDirection) { float angle = DotProduct(initialDirection, finalDirection); Assert(IsFinite(angle)); Vector3D 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 Vector3D 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 Vector3D& 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, Vector3D& mins, Vector3D& maxs) { Vector3D 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); Vector3D 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, Vector3D* pMins, Vector3D* 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, Vector3D* pMins, Vector3D* 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, Vector3D* 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. Vector3D vecCenterNear, vecCenterFar, vecNearEdge, vecFarEdge; Vector3DMultiplyPositionProjective(volumeToWorld, Vector3D(0.5f, 0.5f, 0.0f), vecCenterNear); Vector3DMultiplyPositionProjective(volumeToWorld, Vector3D(0.5f, 0.5f, 1.0f), vecCenterFar); Vector3DMultiplyPositionProjective(volumeToWorld, Vector3D(0.0f, 0.0f, 0.0f), vecNearEdge); Vector3DMultiplyPositionProjective(volumeToWorld, Vector3D(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 ) Vector3D 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, Vector3D* pCenter, float* pflRadius) { VMatrix volumeToWorld; MatrixInverseGeneral(worldToVolume, volumeToWorld); CalculateSphereFromProjectionMatrixInverse(volumeToWorld, pCenter, pflRadius); } static inline void FrustumPlanesFromMatrixHelper(const VMatrix& shadowToWorld, const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, VPlane& plane) { Vector3D world1, world2, world3; Vector3DMultiplyPositionProjective(shadowToWorld, p1, world1); Vector3DMultiplyPositionProjective(shadowToWorld, p2, world2); Vector3DMultiplyPositionProjective(shadowToWorld, p3, world3); Vector3D v1, v2; VectorSubtract(world2, world1, v1); VectorSubtract(world3, world1, v2); CrossProduct(v1, v2, plane.m_Normal); VectorNormalize(plane.m_Normal); plane.m_Dist = DotProduct(plane.m_Normal, world1); } void FrustumPlanesFromMatrix(const VMatrix& clipToWorld, Frustum_t& frustum) { VPlane planes[6]; FrustumPlanesFromMatrixHelper(clipToWorld, Vector3D(0.0f, 0.0f, 0.0f), Vector3D(1.0f, 0.0f, 0.0f), Vector3D(0.0f, 1.0f, 0.0f), planes[FRUSTUM_NEARZ]); FrustumPlanesFromMatrixHelper(clipToWorld, Vector3D(0.0f, 0.0f, 1.0f), Vector3D(0.0f, 1.0f, 1.0f), Vector3D(1.0f, 0.0f, 1.0f), planes[FRUSTUM_FARZ]); FrustumPlanesFromMatrixHelper(clipToWorld, Vector3D(1.0f, 0.0f, 0.0f), Vector3D(1.0f, 1.0f, 1.0f), Vector3D(1.0f, 1.0f, 0.0f), planes[FRUSTUM_RIGHT]); FrustumPlanesFromMatrixHelper(clipToWorld, Vector3D(0.0f, 0.0f, 0.0f), Vector3D(0.0f, 1.0f, 1.0f), Vector3D(0.0f, 0.0f, 1.0f), planes[FRUSTUM_LEFT]); FrustumPlanesFromMatrixHelper(clipToWorld, Vector3D(1.0f, 1.0f, 0.0f), Vector3D(1.0f, 1.0f, 1.0f), Vector3D(0.0f, 1.0f, 1.0f), planes[FRUSTUM_TOP]); FrustumPlanesFromMatrixHelper(clipToWorld, Vector3D(1.0f, 0.0f, 0.0f), Vector3D(0.0f, 0.0f, 1.0f), Vector3D(1.0f, 0.0f, 1.0f), planes[FRUSTUM_BOTTOM]); frustum.SetPlanes(planes); } // BEWARE: top/bottom are FLIPPED relative to D3DXMatrixOrthoOffCenterRH(). 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 MatrixBuildPerspectiveX(VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar) { float flWidth = 2.0f * flZNear * tanf(flFovX * M_PI / 360.0f); float flHeight = flWidth / flAspect; dst.Init(2.0f * flZNear / flWidth, 0.0f, 0.0f, 0.0f, 0.0f, 2.0f * flZNear / flHeight, 0.0f, 0.0f, 0.0f, 0.0f, flZFar / (flZNear - flZFar), flZNear * flZFar / (flZNear - flZFar), 0.0f, 0.0f, -1.0f, 0.0f); } void MatrixBuildPerspectiveOffCenterX(VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar, double bottom, double top, double left, double right) { float flWidth = 2.0f * flZNear * 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((2.0f * flZNear) / (flRight - flLeft), 0.0f, (flLeft + flRight) / (flRight - flLeft), 0.0f, 0.0f, 2.0f * flZNear / (flTop - flBottom), (flTop + flBottom) / (flTop - flBottom), 0.0f, 0.0f, 0.0f, flZFar / (flZNear - flZFar), flZNear * flZFar / (flZNear - flZFar), 0.0f, 0.0f, -1.0f, 0.0f); } void ExtractClipPlanesFromNonTransposedMatrix(const VMatrix& viewProjMatrix, VPlane* pPlanesOut, bool bD3DClippingRange) { // Left Vector4D vPlane = MatrixGetRowAsVector4D(viewProjMatrix, 0) + MatrixGetRowAsVector4D(viewProjMatrix, 3); pPlanesOut[FRUSTUM_LEFT].Init(vPlane.AsVector3D(), -vPlane.w); // Right vPlane = -MatrixGetRowAsVector4D(viewProjMatrix, 0) + MatrixGetRowAsVector4D(viewProjMatrix, 3); pPlanesOut[FRUSTUM_RIGHT].Init(vPlane.AsVector3D(), -vPlane.w); // Bottom vPlane = MatrixGetRowAsVector4D(viewProjMatrix, 1) + MatrixGetRowAsVector4D(viewProjMatrix, 3); pPlanesOut[FRUSTUM_BOTTOM].Init(vPlane.AsVector3D(), -vPlane.w); // Top vPlane = -MatrixGetRowAsVector4D(viewProjMatrix, 1) + MatrixGetRowAsVector4D(viewProjMatrix, 3); pPlanesOut[FRUSTUM_TOP].Init(vPlane.AsVector3D(), -vPlane.w); // Near if (bD3DClippingRange) { // [0,1] Z clipping range (D3D-style) vPlane = MatrixGetRowAsVector4D(viewProjMatrix, 2); } else { // [-1,1] Z clipping range (OpenGL-style) vPlane = MatrixGetRowAsVector4D(viewProjMatrix, 2) + MatrixGetRowAsVector4D(viewProjMatrix, 3); } pPlanesOut[FRUSTUM_NEARZ].Init(vPlane.AsVector3D(), -vPlane.w); // Far vPlane = -MatrixGetRowAsVector4D(viewProjMatrix, 2) + MatrixGetRowAsVector4D(viewProjMatrix, 3); pPlanesOut[FRUSTUM_FARZ].Init(vPlane.AsVector3D(), -vPlane.w); for (uint i = 0; i < FRUSTUM_NUMPLANES; ++i) { float flLen2 = pPlanesOut[i].m_Normal.x * pPlanesOut[i].m_Normal.x + pPlanesOut[i].m_Normal.y * pPlanesOut[i].m_Normal.y + pPlanesOut[i].m_Normal.z * pPlanesOut[i].m_Normal.z; if (flLen2 != 0.0f) { float flScale = 1.0f / sqrt(flLen2); pPlanesOut[i].m_Normal *= flScale; pPlanesOut[i].m_Dist *= flScale; } } } #endif // !_STATIC_LINKED || _SHARED_LIB