//==== Copyright (c) 1996-2011, Valve Corporation, All rights reserved. =====//
//
// Purpose: 
//
// $NoKeywords: $
//
//===========================================================================//

#if !defined(_STATIC_LINKED) || defined(_SHARED_LIB)

#include "mathlib/transform.h"
#include "mathlib/mathlib.h"

// memdbgon must be the last include file in a .cpp file!!!
#include "tier0/memdbgon.h"

const CTransform g_TransformIdentity(Vector3D(0.0f, 0.0f, 0.0f), Quaternion(0.0f, 0.0f, 0.0f, 1.0f));

void SetIdentityTransform(CTransform& out)
{
	out.m_vPosition = vec3_origin;
	out.m_orientation = quat_identity;
}

void ConcatTransforms(const CTransform& in1, const CTransform& in2, CTransform& out)
{
	// Store in temp to avoid problems if out == in1 or out == in2
	CTransform result;
	QuaternionMult(in1.m_orientation, in2.m_orientation, result.m_orientation);
	QuaternionMultiply(in1.m_orientation, in2.m_vPosition, result.m_vPosition);
	result.m_vPosition += in1.m_vPosition;
	out = result;
}

void VectorIRotate(const Vector3D& v, const CTransform& t, Vector3D& out)
{
	// FIXME: Make work directly with the transform
	matrix3x4_t m;
	TransformMatrix(t, m);
	VectorIRotate(v, m, out);
}

void VectorITransform(const Vector3D& v, const CTransform& t, Vector3D& out)
{
	// FIXME: Make work directly with the transform
	matrix3x4_t m;
	TransformMatrix(t, m);
	VectorITransform(v, m, out);
}

void TransformSlerp(const CTransform& p, const CTransform& q, float t, CTransform& qt)
{
	QuaternionSlerp(p.m_orientation, q.m_orientation, t, qt.m_orientation);
	VectorLerp(p.m_vPosition, q.m_vPosition, t, qt.m_vPosition);
}

void TransformLerp(const CTransform& p, const CTransform& q, float t, CTransform& qt)
{
	QuaternionBlend(p.m_orientation, q.m_orientation, t, qt.m_orientation);
	VectorLerp(p.m_vPosition, q.m_vPosition, t, qt.m_vPosition);
}

void TransformMatrix(const CTransform& in, matrix3x4_t& out)
{
	QuaternionMatrix(in.m_orientation, in.m_vPosition, out);
}

void TransformMatrix(const CTransformUnaligned& in, matrix3x4_t& out)
{
	QuaternionMatrix(in.m_orientation, in.m_vPosition, out);
}

void TransformMatrix(const CTransform& in, const Vector3D& vScaleIn, matrix3x4_t& out)
{
	QuaternionMatrix(in.m_orientation, in.m_vPosition, vScaleIn, out);
}

void MatrixTransform(const matrix3x4_t& in, CTransformUnaligned& out)
{
	MatrixQuaternion(in, out.m_orientation);
	MatrixGetColumn(in, ORIGIN, out.m_vPosition);
}

void MatrixTransform(const matrix3x4_t& in, CTransform& out)
{
	MatrixQuaternion(in, out.m_orientation);
	MatrixGetColumn(in, ORIGIN, out.m_vPosition);
}

void MatrixTransform(const matrix3x4_t& in, CTransform& out, Vector3D& vScaleOut)
{
	matrix3x4_t norm;
	vScaleOut = MatrixNormalize(in, norm);
	MatrixTransform(norm, out);
}

void AngleTransform(const QAngle& angles, const Vector3D& origin, CTransform& out)
{
	AngleQuaternion(angles, out.m_orientation);
	out.m_vPosition = origin;
}

void TransformInvert(const CTransform& in, CTransform& out)
{
	QuaternionInvert(in.m_orientation, out.m_orientation);
	QuaternionMultiply(out.m_orientation, in.m_vPosition, out.m_vPosition);
	out.m_vPosition *= -1.0f;
}

void AxisAngleTransform(const Vector3D& vecAxis, float flAngleDegrees, CTransform& out)
{
	AxisAngleQuaternion(vecAxis, flAngleDegrees, out.m_orientation);
	out.m_vPosition = vec3_origin;
}

void TransformVectorsFLU(const CTransform& in, Vector3D* pForward, Vector3D* pLeft, Vector3D* pUp)
{
	QuaternionVectorsFLU(in.m_orientation, pForward, pLeft, pUp);
}

void TransformVectorsForward(const CTransform& in, Vector3D* pForward)
{
	QuaternionVectorsForward(in.m_orientation, pForward);
}

bool TransformsAreEqual(const CTransform& src1, const CTransform& src2, float flPosTolerance, float flRotTolerance)
{
	if (!VectorsAreEqual(src1.m_vPosition, src2.m_vPosition, flPosTolerance))
		return false;
	return QuaternionsAreEqual(src1.m_orientation, src2.m_orientation, flRotTolerance);
}

// FIXME: optimize this with simd goodness
void TransformToWorldSpace(int nRootTransformCount, int nTransformCount, const int* pParentIndices, CTransform* pTransforms)
{
#ifdef _DEBUG
	for (int i = 0; i < nRootTransformCount; ++i)
	{
		Assert(pParentIndices[i] < 0);
	}
#endif

	for (int i = nRootTransformCount; i < nTransformCount; ++i)
	{
		int nParentBone = pParentIndices[i];
		Assert(nParentBone >= 0 && nParentBone < i);
		ConcatTransforms(pTransforms[nParentBone], pTransforms[i], pTransforms[i]);
	}
}

// FIXME: optimize this with simd goodness
void TransformToParentSpace(int nRootTransformCount, int nTransformCount, const int* pParentIndices, CTransform* pTransforms)
{
#ifdef _DEBUG
	for (int i = 0; i < nRootTransformCount; ++i)
	{
		Assert(pParentIndices[i] < 0);
	}
#endif

	bool* pComputedParentTransform = (bool*)stackalloc(nTransformCount * sizeof(bool));
	memset(pComputedParentTransform, 0, nTransformCount * sizeof(bool));
	CTransform* pWorldToParentTransforms = (CTransform*)stackalloc(nTransformCount * sizeof(CTransform));

	for (int b = nTransformCount; --b >= nRootTransformCount; )
	{
		int nParentBone = pParentIndices[b];
		if (!pComputedParentTransform[nParentBone])
		{
			TransformInvert(pTransforms[nParentBone], pWorldToParentTransforms[nParentBone]);
			pComputedParentTransform[nParentBone] = true;
		}
		ConcatTransforms(pWorldToParentTransforms[nParentBone], pTransforms[b], pTransforms[b]);
	}
}

#endif // !_STATIC_LINKED || _SHARED_LIB