uid issue

2021-07-24 21:11:47 -07:00
commit c2130ba4e9
13850 changed files with 6241419 additions and 0 deletions
--- a/mathlib/transform.cpp
+++ b/mathlib/transform.cpp
@ -0,0 +1,178 @@
+//==== 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( Vector( 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 Vector &v, const CTransform &t, Vector &out )
+{
+	// FIXME: Make work directly with the transform
+	matrix3x4_t m;
+	TransformMatrix( t, m );
+	VectorIRotate( v, m, out );
+}
+
+void VectorITransform( const Vector &v, const CTransform &t, Vector &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 Vector &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, Vector &vScaleOut )
+{
+	matrix3x4_t norm;
+	vScaleOut = MatrixNormalize( in, norm );
+	MatrixTransform( norm, out );
+}
+
+void AngleTransform( const QAngle &angles, const Vector &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 Vector &vecAxis, float flAngleDegrees, CTransform &out )
+{
+	AxisAngleQuaternion( vecAxis, flAngleDegrees, out.m_orientation );
+	out.m_vPosition = vec3_origin;
+}
+
+void TransformVectorsFLU( const CTransform &in, Vector* pForward, Vector *pLeft, Vector *pUp )
+{
+	QuaternionVectorsFLU( in.m_orientation, pForward, pLeft, pUp );
+}
+
+void TransformVectorsForward( const CTransform &in, Vector* 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
+