d4708
/
source-engine
mirror of https://github.com/nillerusr/source-engine.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
Modified source engine (2017) developed by valve and leaked in 2020. Not for commercial purporses
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
491 Commits
25 Branches
6 Tags
221 MiB
 
 
 
 
 
 
Tree: e3edbc2d96
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'e3edbc2d96'
${ noResults }
source-engine/public/mathlib/vmatrix.h

2055 lines
63 KiB
Raw Blame History

//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose:
//
// $NoKeywords: $
//
//=============================================================================//
//
// VMatrix always postmultiply vectors as in Ax = b.
// Given a set of basis vectors ((F)orward, (L)eft, (U)p), and a (T)ranslation,
// a matrix to transform a vector into that space looks like this:
// Fx Lx Ux Tx
// Fy Ly Uy Ty
// Fz Lz Uz Tz
// 0 0 0 1
// Note that concatenating matrices needs to multiply them in reverse order.
// ie: if I want to apply matrix A, B, then C, the equation needs to look like this:
// C * B * A * v
// ie:
// v = A * v;
// v = B * v;
// v = C * v;
//=============================================================================
#ifndef VMATRIX_H
#define VMATRIX_H
#ifdef _WIN32
#pragma once
#endif
#include <math.h>
#include <string.h>
#include "mathlib/vector.h"
#include "mathlib/vplane.h"
#include "mathlib/vector4d.h"
#include "mathlib/mathlib.h"
struct cplane_t;
#ifndef M_PI
#define M_PI 3.14159265358979323846 // matches value in gcc v2 math.h
#endif
class alignas(16) VMatrix
{
public:
VMatrix();
VMatrix(
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
);
// Creates a matrix where the X axis = forward
// the Y axis = left, and the Z axis = up
VMatrix( const Vector& forward, const Vector& left, const Vector& up );
VMatrix( const Vector& forward, const Vector& left, const Vector& up, const Vector& translation );
// Construct from a 3x4 matrix
VMatrix( const matrix3x4_t& matrix3x4 );
VMatrix( const VMatrix& ) = default;
// Set the values in the matrix.
void 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
);
// Initialize from a 3x4
void Init( const matrix3x4_t& matrix3x4 );
// array access
inline float* operator[](int i)
{
return m[i];
}
inline const float* operator[](int i) const
{
return m[i];
}
// Get a pointer to m[0][0]
inline float *Base()
{
return &m[0][0];
}
inline const float *Base() const
{
return &m[0][0];
}
void SetLeft(const Vector &vLeft);
void SetUp(const Vector &vUp);
void SetForward(const Vector &vForward);
void GetBasisVectors(Vector &vForward, Vector &vLeft, Vector &vUp) const;
void SetBasisVectors(const Vector &vForward, const Vector &vLeft, const Vector &vUp);
// Get/set the translation.
Vector & GetTranslation( Vector &vTrans ) const;
void SetTranslation(const Vector &vTrans);
void PreTranslate(const Vector &vTrans);
void PostTranslate(const Vector &vTrans);
const matrix3x4_t& As3x4() const;
void CopyFrom3x4( const matrix3x4_t &m3x4 );
void Set3x4( matrix3x4_t& matrix3x4 ) 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
// Access the basis vectors.
Vector GetLeft() const;
Vector GetUp() const;
Vector GetForward() const;
Vector GetTranslation() const;
#endif
// Matrix->vector operations.
public:
// Multiply by a 3D vector (same as operator*).
void V3Mul(const Vector &vIn, Vector &vOut) const;
// Multiply by a 4D vector.
void V4Mul(const Vector4D &vIn, Vector4D &vOut) const;
#ifndef VECTOR_NO_SLOW_OPERATIONS
// Applies the rotation (ignores translation in the matrix). (This just calls VMul3x3).
Vector ApplyRotation(const Vector &vVec) const;
// Multiply by a vector (divides by w, assumes input w is 1).
Vector operator*(const Vector &vVec) const;
// Multiply by the upper 3x3 part of the matrix (ie: only apply rotation).
Vector VMul3x3(const Vector &vVec) const;
// Apply the inverse (transposed) rotation (only works on pure rotation matrix)
Vector VMul3x3Transpose(const Vector &vVec) const;
// Multiply by the upper 3 rows.
Vector VMul4x3(const Vector &vVec) const;
// Apply the inverse (transposed) transformation (only works on pure rotation/translation)
Vector VMul4x3Transpose(const Vector &vVec) const;
#endif
// Matrix->plane operations.
public:
// Transform the plane. The matrix can only contain translation and rotation.
void TransformPlane( const VPlane &inPlane, VPlane &outPlane ) const;
#ifndef VECTOR_NO_SLOW_OPERATIONS
// Just calls TransformPlane and returns the result.
VPlane operator*(const VPlane &thePlane) const;
#endif
// Matrix->matrix operations.
public:
VMatrix& operator=(const VMatrix &mOther);
// Multiply two matrices (out = this * vm).
void MatrixMul( const VMatrix &vm, VMatrix &out ) const;
// Add two matrices.
const VMatrix& operator+=(const VMatrix &other);
#ifndef VECTOR_NO_SLOW_OPERATIONS
// Just calls MatrixMul and returns the result.
VMatrix operator*(const VMatrix &mOther) const;
// Add/Subtract two matrices.
VMatrix operator+(const VMatrix &other) const;
VMatrix operator-(const VMatrix &other) const;
// Negation.
VMatrix operator-() const;
// Return inverse matrix. Be careful because the results are undefined
// if the matrix doesn't have an inverse (ie: InverseGeneral returns false).
VMatrix operator~() const;
#endif
// Matrix operations.
public:
// Set to identity.
void Identity();
bool IsIdentity() const;
// Setup a matrix for origin and angles.
void SetupMatrixOrgAngles( const Vector &origin, const QAngle &vAngles );
// Setup a matrix for angles and no translation.
void SetupMatrixAngles( const QAngle &vAngles );
// General inverse. This may fail so check the return!
bool InverseGeneral(VMatrix &vInverse) const;
// Does a fast inverse, assuming the matrix only contains translation and rotation.
void InverseTR( VMatrix &mRet ) const;
// Usually used for debug checks. Returns true if the upper 3x3 contains
// unit vectors and they are all orthogonal.
bool IsRotationMatrix() const;
#ifndef VECTOR_NO_SLOW_OPERATIONS
// This calls the other InverseTR and returns the result.
VMatrix InverseTR() const;
// Get the scale of the matrix's basis vectors.
Vector GetScale() const;
// (Fast) multiply by a scaling matrix setup from vScale.
VMatrix Scale(const Vector &vScale);
// Normalize the basis vectors.
VMatrix NormalizeBasisVectors() const;
// Transpose.
VMatrix Transpose() const;
// Transpose upper-left 3x3.
VMatrix Transpose3x3() const;
#endif
public:
// The matrix.
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;
}
//-----------------------------------------------------------------------------
// 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];
}
//-----------------------------------------------------------------------------
// 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<const Vector&>(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];
}
inline 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;
}
static inline 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;
}
//-----------------------------------------------------------------------------
// Transpose
//-----------------------------------------------------------------------------
inline void Swap( float& a, float& b )
{
float tmp = a;
a = b;
b = tmp;
}
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.
//-----------------------------------------------------------------------------
inline 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;
}
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.
//-----------------------------------------------------------------------------
inline VMatrix::VMatrix()
{
Init(
0.f, 0.f, 0.f, 0.f,
0.f, 0.f, 0.f, 0.f,
0.f, 0.f, 0.f, 0.f,
0.f, 0.f, 0.f, 0.f
);
}
inline VMatrix::VMatrix(
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)
{
Init(
m00, m01, m02, m03,
m10, m11, m12, m13,
m20, m21, m22, m23,
m30, m31, m32, m33
);
}
inline VMatrix::VMatrix( const matrix3x4_t& matrix3x4 )
{
Init( matrix3x4 );
}
//-----------------------------------------------------------------------------
// Creates a matrix where the X axis = forward
// the Y axis = left, and the Z axis = up
//-----------------------------------------------------------------------------
inline VMatrix::VMatrix( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis )
{
Init(
xAxis.x, yAxis.x, zAxis.x, 0.0f,
xAxis.y, yAxis.y, zAxis.y, 0.0f,
xAxis.z, yAxis.z, zAxis.z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
}
inline VMatrix::VMatrix( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis, const Vector& translation )
{
Init(
xAxis.x, yAxis.x, zAxis.x, translation.x,
xAxis.y, yAxis.y, zAxis.y, translation.y,
xAxis.z, yAxis.z, zAxis.z, translation.z,
0.0f, 0.0f, 0.0f, 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]);
}
inline Vector VMatrix::GetLeft() const
{
return Vector(m[0][1], m[1][1], m[2][1]);
}
inline Vector VMatrix::GetUp() const
{
return Vector(m[0][2], m[1][2], m[2][2]);
}
#endif
inline void VMatrix::SetForward(const Vector &vForward)
{
m[0][0] = vForward.x;
m[1][0] = vForward.y;
m[2][0] = vForward.z;
}
inline void VMatrix::SetLeft(const Vector &vLeft)
{
m[0][1] = vLeft.x;
m[1][1] = vLeft.y;
m[2][1] = vLeft.z;
}
inline void VMatrix::SetUp(const Vector &vUp)
{
m[0][2] = vUp.x;
m[1][2] = vUp.y;
m[2][2] = vUp.z;
}
inline void VMatrix::GetBasisVectors(Vector &vForward, Vector &vLeft, Vector &vUp) const
{
vForward.Init( m[0][0], m[1][0], m[2][0] );
vLeft.Init( m[0][1], m[1][1], m[2][1] );
vUp.Init( m[0][2], m[1][2], m[2][2] );
}
inline void VMatrix::SetBasisVectors(const Vector &vForward, const Vector &vLeft, const Vector &vUp)
{
SetForward(vForward);
SetLeft(vLeft);
SetUp(vUp);
}
//-----------------------------------------------------------------------------
// Methods related to the translation component of the matrix
//-----------------------------------------------------------------------------
#ifndef VECTOR_NO_SLOW_OPERATIONS
inline Vector VMatrix::GetTranslation() const
{
return Vector(m[0][3], m[1][3], m[2][3]);
}
#endif
inline Vector& VMatrix::GetTranslation( Vector &vTrans ) const
{
vTrans.x = m[0][3];
vTrans.y = m[1][3];
vTrans.z = m[2][3];
return vTrans;
}
inline void VMatrix::SetTranslation(const Vector &vTrans)
{
m[0][3] = vTrans.x;
m[1][3] = vTrans.y;
m[2][3] = vTrans.z;
}
//-----------------------------------------------------------------------------
// appply translation to this matrix in the input space
//-----------------------------------------------------------------------------
inline void VMatrix::PreTranslate(const Vector &vTrans)
{
Vector tmp;
Vector3DMultiplyPosition( *this, vTrans, tmp );
m[0][3] = tmp.x;
m[1][3] = tmp.y;
m[2][3] = tmp.z;
}
//-----------------------------------------------------------------------------
// appply translation to this matrix in the output space
//-----------------------------------------------------------------------------
inline void VMatrix::PostTranslate(const Vector &vTrans)
{
m[0][3] += vTrans.x;
m[1][3] += vTrans.y;
m[2][3] += vTrans.z;
}
inline const matrix3x4_t& VMatrix::As3x4() const
{
return *((const matrix3x4_t*)this);
}
inline void VMatrix::CopyFrom3x4( const matrix3x4_t &m3x4 )
{
memcpy( m, m3x4.Base(), sizeof( matrix3x4_t ) );
m[3][0] = m[3][1] = m[3][2] = 0;
m[3][3] = 1;
}
inline void VMatrix::Set3x4( matrix3x4_t& matrix3x4 ) const
{
memcpy(matrix3x4.Base(), m, sizeof( matrix3x4_t ) );
}
#ifndef VECTOR_NO_SLOW_OPERATIONS
inline Vector VMatrix::VMul4x3(const Vector &vVec) const
{
Vector vResult;
Vector3DMultiplyPosition( *this, vVec, vResult );
return vResult;
}
inline Vector VMatrix::VMul4x3Transpose(const Vector &vVec) const
{
Vector tmp = vVec;
tmp.x -= m[0][3];
tmp.y -= m[1][3];
tmp.z -= m[2][3];
return Vector(
m[0][0]*tmp.x + m[1][0]*tmp.y + m[2][0]*tmp.z,
m[0][1]*tmp.x + m[1][1]*tmp.y + m[2][1]*tmp.z,
m[0][2]*tmp.x + m[1][2]*tmp.y + m[2][2]*tmp.z
);
}
inline Vector VMatrix::VMul3x3(const Vector &vVec) const
{
return Vector(
m[0][0]*vVec.x + m[0][1]*vVec.y + m[0][2]*vVec.z,
m[1][0]*vVec.x + m[1][1]*vVec.y + m[1][2]*vVec.z,
m[2][0]*vVec.x + m[2][1]*vVec.y + m[2][2]*vVec.z
);
}
inline Vector VMatrix::VMul3x3Transpose(const Vector &vVec) const
{
return Vector(
m[0][0]*vVec.x + m[1][0]*vVec.y + m[2][0]*vVec.z,
m[0][1]*vVec.x + m[1][1]*vVec.y + m[2][1]*vVec.z,
m[0][2]*vVec.x + m[1][2]*vVec.y + m[2][2]*vVec.z
);
}
#endif // VECTOR_NO_SLOW_OPERATIONS
inline void VMatrix::V3Mul(const Vector &vIn, Vector &vOut) const
{
vec_t rw;
rw = 1.0f / (m[3][0]*vIn.x + m[3][1]*vIn.y + m[3][2]*vIn.z + m[3][3]);
vOut.x = (m[0][0]*vIn.x + m[0][1]*vIn.y + m[0][2]*vIn.z + m[0][3]) * rw;
vOut.y = (m[1][0]*vIn.x + m[1][1]*vIn.y + m[1][2]*vIn.z + m[1][3]) * rw;
vOut.z = (m[2][0]*vIn.x + m[2][1]*vIn.y + m[2][2]*vIn.z + m[2][3]) * rw;
}
inline void VMatrix::V4Mul(const Vector4D &vIn, Vector4D &vOut) const
{
vOut[0] = m[0][0]*vIn[0] + m[0][1]*vIn[1] + m[0][2]*vIn[2] + m[0][3]*vIn[3];
vOut[1] = m[1][0]*vIn[0] + m[1][1]*vIn[1] + m[1][2]*vIn[2] + m[1][3]*vIn[3];
vOut[2] = m[2][0]*vIn[0] + m[2][1]*vIn[1] + m[2][2]*vIn[2] + m[2][3]*vIn[3];
vOut[3] = m[3][0]*vIn[0] + m[3][1]*vIn[1] + m[3][2]*vIn[2] + m[3][3]*vIn[3];
}
//-----------------------------------------------------------------------------
// Other random stuff
//-----------------------------------------------------------------------------
inline void VMatrix::Identity()
{
MatrixSetIdentity( *this );
}
inline bool VMatrix::IsIdentity() const
{
return
m[0][0] == 1.0f && m[0][1] == 0.0f && m[0][2] == 0.0f && m[0][3] == 0.0f &&
m[1][0] == 0.0f && m[1][1] == 1.0f && m[1][2] == 0.0f && m[1][3] == 0.0f &&
m[2][0] == 0.0f && m[2][1] == 0.0f && m[2][2] == 1.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;
}
#ifndef VECTOR_NO_SLOW_OPERATIONS
inline Vector VMatrix::ApplyRotation(const Vector &vVec) const
{
return VMul3x3(vVec);
}
#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;
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;
}
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<const char *>(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<const Vector&>(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<const Vector&>(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<const Vector&>(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<const Vector&>(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;
}
//-----------------------------------------------------------------------------
// Accessors
//-----------------------------------------------------------------------------
inline void MatrixGetColumn( const VMatrix &src, int nCol, Vector *pColumn )
{
Assert( (nCol >= 0) && (nCol <= 3) );
pColumn->x = src[0][nCol];
pColumn->y = src[1][nCol];
pColumn->z = src[2][nCol];
}
inline void MatrixGetRow( const VMatrix &src, int nRow, Vector *pRow )
{
Assert( (nRow >= 0) && (nRow <= 3) );
*pRow = *(Vector*)src[nRow];
}
inline void MatrixSetRow( VMatrix &dst, int nRow, const Vector &row )
{
Assert( (nRow >= 0) && (nRow <= 3) );
*(Vector*)dst[nRow] = row;
}
//-----------------------------------------------------------------------------
// Transform a plane that has an axis-aligned normal
//-----------------------------------------------------------------------------
inline void MatrixTransformAxisAlignedPlane( const VMatrix &src, int nDim, float flSign, float flDist, cplane_t &outPlane )
{
// See MatrixTransformPlane in the .cpp file for an explanation of the algorithm.
MatrixGetColumn( src, nDim, &outPlane.normal );
outPlane.normal *= flSign;
outPlane.dist = flDist * DotProduct( outPlane.normal, outPlane.normal );
// NOTE: Writing this out by hand because it doesn't inline (inline depth isn't large enough)
// This should read outPlane.dist += DotProduct( outPlane.normal, src.GetTranslation );
outPlane.dist += outPlane.normal.x * src.m[0][3] + outPlane.normal.y * src.m[1][3] + outPlane.normal.z * src.m[2][3];
}
//-----------------------------------------------------------------------------
// Matrix equality test
//-----------------------------------------------------------------------------
inline bool MatricesAreEqual( const VMatrix &src1, const VMatrix &src2, float flTolerance )
{
for ( int i = 0; i < 3; ++i )
{
for ( int j = 0; j < 3; ++j )
{
if ( fabs( src1[i][j] - src2[i][j] ) > flTolerance )
return false;
}
}
return true;
}
//-----------------------------------------------------------------------------
//
//-----------------------------------------------------------------------------
void MatrixBuildOrtho( VMatrix& dst, double left, double top, double right, double bottom, double zNear, double zFar );
void MatrixBuildPerspectiveX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar );
void MatrixBuildPerspectiveOffCenterX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar, double bottom, double top, double left, double right );
void MatrixBuildPerspectiveZRange( VMatrix& dst, double flZNear, double flZFar );
inline void MatrixOrtho( VMatrix& dst, double left, double top, double right, double bottom, double zNear, double zFar )
{
VMatrix mat;
MatrixBuildOrtho( mat, left, top, right, bottom, zNear, zFar );
VMatrix temp;
MatrixMultiply( dst, mat, temp );
dst = temp;
}
inline void MatrixPerspectiveX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar )
{
VMatrix mat;
MatrixBuildPerspectiveX( mat, flFovX, flAspect, flZNear, flZFar );
VMatrix temp;
MatrixMultiply( dst, mat, temp );
dst = temp;
}
inline void MatrixPerspectiveOffCenterX( VMatrix& dst, double flFovX, double flAspect, double flZNear, double flZFar, double bottom, double top, double left, double right )
{
VMatrix mat;
MatrixBuildPerspectiveOffCenterX( mat, flFovX, flAspect, flZNear, flZFar, bottom, top, left, right );
VMatrix temp;
MatrixMultiply( dst, mat, temp );
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 -<val>/2..<val>/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