r5sdk/r5dev/mathlib/vmatrix.cpp

//========= 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