mirror of
https://github.com/Mauler125/r5sdk.git
synced 2025-02-09 19:15:03 +01:00
f120354e96
* All libraries have been isolated from each other, and build into separate artifacts. * Project has been restructured to support isolating libraries. * CCrashHandler now calls a callback on crash (setup from core/dllmain.cpp, this can be setup in any way for any project. This callback is getting called when the apllication crashes. Useful for flushing buffers before closing handles to logging files for example). * Tier0 'CoreMsgV' function now calls a callback sink, which could be set by the user (currently setup to the SDK's internal logger in core/dllmain.cpp). TODO: * Add a batch file to autogenerate all projects. * Add support for dedicated server. * Add support for client dll. Bugs: * Game crashes on the title screen after the UI script compiler has finished (root cause unknown). * Curl error messages are getting logged twice for the dedicated server due to the removal of all "DEDICATED" preprocessor directives to support isolating projects. This has to be fixed properly!
1349 lines
43 KiB
C++
1349 lines
43 KiB
C++
//========= Copyright (c) 1996-2005, Valve Corporation, All rights reserved. ============//
|
|
//
|
|
// Purpose:
|
|
//
|
|
// $NoKeywords: $
|
|
//
|
|
//=============================================================================//
|
|
#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 * (float(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.0f / 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.0f / flDenom);
|
|
return true;
|
|
}
|
|
#else // old slow inaccurate 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<const Vector3D>(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<const Vector3D>(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<const Vector3D>(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<const Vector3D>(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<const Vector3D>(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 / float(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 * (float(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 * (float)tan(fovX * (M_PI / 180.0f) * 0.5f);
|
|
float height = 2 * zNear * (float)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 / vec_t(right - left), 0.0f, 0.0f, vec_t((left + right) / (left - right)),
|
|
0.0f, 2.0f / vec_t(bottom - top), 0.0f, vec_t((bottom + top) / (top - bottom)),
|
|
0.0f, 0.0f, 1.0f / vec_t(zNear - zFar), vec_t(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 * float(flZNear) * tanf(float(flFovX * M_PI) / 360.0f);
|
|
float flHeight = flWidth / float(flAspect);
|
|
dst.Init(2.0f * float(flZNear) / flWidth, 0.0f, 0.0f, 0.0f,
|
|
0.0f, 2.0f * float(flZNear) / flHeight, 0.0f, 0.0f,
|
|
0.0f, 0.0f, float(flZFar / (flZNear - flZFar)), float(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 * float(flZNear) * tanf(float(flFovX * M_PI) / 360.0f);
|
|
float flHeight = flWidth / float(flAspect);
|
|
|
|
// bottom, top, left, right are 0..1 so convert to -<val>/2..<val>/2
|
|
float flLeft = -(flWidth / 2.0f) * (1.0f - float(left)) + float(left) * (flWidth / 2.0f);
|
|
float flRight = -(flWidth / 2.0f) * (1.0f - float(right)) + float(right) * (flWidth / 2.0f);
|
|
float flBottom = -(flHeight / 2.0f) * (1.0f - float(bottom)) + float(bottom) * (flHeight / 2.0f);
|
|
float flTop = -(flHeight / 2.0f) * (1.0f - float(top)) + float(top) * (flHeight / 2.0f);
|
|
|
|
dst.Init((2.0f * float(flZNear)) / (flRight - flLeft), 0.0f, (flLeft + flRight) / (flRight - flLeft), 0.0f,
|
|
0.0f, 2.0f * float(flZNear) / (flTop - flBottom), (flTop + flBottom) / (flTop - flBottom), 0.0f,
|
|
0.0f, 0.0f, float(flZFar) / float(flZNear - flZFar), float(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
|