Fix line endings. WHAMMY.

1 files changed, 5950 insertions, 5950 deletions

diff --git a/mp/src/public/bone_setup.cpp b/mp/src/public/bone_setup.cpp
index 31fb4965..0cc81c67 100644
--- a/mp/src/public/bone_setup.cpp
+++ b/mp/src/public/bone_setup.cpp
@@ -1,5950 +1,5950 @@
-//========= Copyright Valve Corporation, All rights reserved. ============//

-//

-// Purpose: 

-//

-// $NoKeywords: $

-//

-//===========================================================================//

-

-#include "tier0/dbg.h"

-#include "mathlib/mathlib.h"

-#include "bone_setup.h"

-#include <string.h>

-

-#include "collisionutils.h"

-#include "vstdlib/random.h"

-#include "tier0/vprof.h"

-#include "bone_accessor.h"

-#include "mathlib/ssequaternion.h"

-#include "bitvec.h"

-#include "datamanager.h"

-#include "convar.h"

-#include "tier0/tslist.h"

-#include "vphysics_interface.h"

-#ifdef CLIENT_DLL

-	#include "posedebugger.h"

-#endif

-

-// memdbgon must be the last include file in a .cpp file!!!

-#include "tier0/memdbgon.h"

-

-class CBoneSetup

-{

-public:

-	CBoneSetup( const CStudioHdr *pStudioHdr, int boneMask, const float poseParameter[], IPoseDebugger *pPoseDebugger = NULL );

-	void InitPose( Vector pos[], Quaternion q[] );

-	void AccumulatePose( Vector pos[], Quaternion q[], int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );

-	void CalcAutoplaySequences(	Vector pos[], Quaternion q[], float flRealTime, CIKContext *pIKContext );

-private:

-	void AddSequenceLayers( Vector pos[], Quaternion q[], mstudioseqdesc_t &seqdesc, int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );

-	void AddLocalLayers( Vector pos[], Quaternion q[], mstudioseqdesc_t &seqdesc, int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );

-public:

-	const CStudioHdr *m_pStudioHdr;

-	int m_boneMask;

-	const float *m_flPoseParameter;

-	IPoseDebugger *m_pPoseDebugger;

-};

-

-// -----------------------------------------------------------------

-template <typename T>

-class CBoneSetupMemoryPool

-{

-public:

-	T *Alloc()

-	{

-		T *p = (T *)m_FreeBlocks.Pop();

-		if ( !p )

-		{

-			p = new T[MAXSTUDIOBONES];

-			if ( ((size_t)p) % TSLIST_NODE_ALIGNMENT != 0 )

-			{

-				DebuggerBreak();

-			}

-		}

-

-		return p;

-	}

-

-	void Free( T *p )

-	{

-		m_FreeBlocks.Push( (TSLNodeBase_t *)p );

-	}

-

-private:

-	CTSListBase m_FreeBlocks;

-};

-

-CBoneSetupMemoryPool<Quaternion> g_QaternionPool;

-CBoneSetupMemoryPool<Vector> g_VectorPool;

-CBoneSetupMemoryPool<matrix3x4_t> g_MatrixPool;

-

-// -----------------------------------------------------------------

-CBoneCache *CBoneCache::CreateResource( const bonecacheparams_t &params )

-{

-	short studioToCachedIndex[MAXSTUDIOBONES];

-	short cachedToStudioIndex[MAXSTUDIOBONES];

-	int cachedBoneCount = 0;

-	for ( int i = 0; i < params.pStudioHdr->numbones(); i++ )

-	{

-		// skip bones that aren't part of the boneMask (and aren't the root bone)

-		if (i != 0 && !(params.pStudioHdr->boneFlags(i) & params.boneMask))

-		{

-			studioToCachedIndex[i] = -1;

-			continue;

-		}

-		studioToCachedIndex[i] = cachedBoneCount;

-		cachedToStudioIndex[cachedBoneCount] = i;

-		cachedBoneCount++;

-	}

-	int tableSizeStudio = sizeof(short) * params.pStudioHdr->numbones();

-	int tableSizeCached = sizeof(short) * cachedBoneCount;

-	int matrixSize = sizeof(matrix3x4_t) * cachedBoneCount;

-	int size = ( sizeof(CBoneCache) + tableSizeStudio + tableSizeCached + matrixSize + 3 ) & ~3;

-	

-	CBoneCache *pMem = (CBoneCache *)malloc( size );

-	Construct( pMem );

-	pMem->Init( params, size, studioToCachedIndex, cachedToStudioIndex, cachedBoneCount );

-	return pMem;

-}

-

-unsigned int CBoneCache::EstimatedSize( const bonecacheparams_t &params )

-{

-	// conservative estimate - max size

-	return ( params.pStudioHdr->numbones() * (sizeof(short) + sizeof(short) + sizeof(matrix3x4_t)) + 3 ) & ~3;

-}

-

-void CBoneCache::DestroyResource()

-{

-	free( this );

-}

-

-

-CBoneCache::CBoneCache()

-{

-	m_size = 0;

-	m_cachedBoneCount = 0;

-}

-

-void CBoneCache::Init( const bonecacheparams_t &params, unsigned int size, short *pStudioToCached, short *pCachedToStudio, int cachedBoneCount ) 

-{

-	m_cachedBoneCount = cachedBoneCount;

-	m_size = size;

-	m_timeValid = params.curtime;

-	m_boneMask = params.boneMask;

-

-	int studioTableSize = params.pStudioHdr->numbones() * sizeof(short);

-	m_cachedToStudioOffset = studioTableSize;

-	memcpy( StudioToCached(), pStudioToCached, studioTableSize );

-

-	int cachedTableSize = cachedBoneCount * sizeof(short);

-	memcpy( CachedToStudio(), pCachedToStudio, cachedTableSize );

-

-	m_matrixOffset = ( m_cachedToStudioOffset + cachedTableSize + 3 ) & ~3;

-	

-	UpdateBones( params.pBoneToWorld, params.pStudioHdr->numbones(), params.curtime );

-}

-

-void CBoneCache::UpdateBones( const matrix3x4_t *pBoneToWorld, int numbones, float curtime )

-{

-	matrix3x4_t *pBones = BoneArray();

-	const short *pCachedToStudio = CachedToStudio();

-

-	for ( int i = 0; i < m_cachedBoneCount; i++ )

-	{

-		int index = pCachedToStudio[i];

-		MatrixCopy( pBoneToWorld[index], pBones[i] );

-	}

-	m_timeValid = curtime;

-}

-

-matrix3x4_t *CBoneCache::GetCachedBone( int studioIndex )

-{

-	int cachedIndex = StudioToCached()[studioIndex];

-	if ( cachedIndex >= 0 )

-	{

-		return BoneArray() + cachedIndex;

-	}

-	return NULL;

-}

-

-void CBoneCache::ReadCachedBones( matrix3x4_t *pBoneToWorld )

-{

-	matrix3x4_t *pBones = BoneArray();

-	const short *pCachedToStudio = CachedToStudio();

-	for ( int i = 0; i < m_cachedBoneCount; i++ )

-	{

-		MatrixCopy( pBones[i], pBoneToWorld[pCachedToStudio[i]] );

-	}

-}

-

-void CBoneCache::ReadCachedBonePointers( matrix3x4_t **bones, int numbones )

-{

-	memset( bones, 0, sizeof(matrix3x4_t *) * numbones );

-	matrix3x4_t *pBones = BoneArray();

-	const short *pCachedToStudio = CachedToStudio();

-	for ( int i = 0; i < m_cachedBoneCount; i++ )

-	{

-		bones[pCachedToStudio[i]] = pBones + i;

-	}

-}

-

-bool CBoneCache::IsValid( float curtime, float dt )

-{

-	if ( curtime - m_timeValid <= dt )

-		return true;

-	return false;

-}

-

-

-// private functions

-matrix3x4_t *CBoneCache::BoneArray()

-{

-	return (matrix3x4_t *)( (char *)(this+1) + m_matrixOffset );

-}

-

-short *CBoneCache::StudioToCached()

-{

-	return (short *)( (char *)(this+1) );

-}

-

-short *CBoneCache::CachedToStudio()

-{

-	return (short *)( (char *)(this+1) + m_cachedToStudioOffset );

-}

-

-// Construct a singleton

-static CDataManager<CBoneCache, bonecacheparams_t, CBoneCache *, CThreadFastMutex> g_StudioBoneCache( 128 * 1024L );

-

-CBoneCache *Studio_GetBoneCache( memhandle_t cacheHandle )

-{

-	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );

-	return g_StudioBoneCache.GetResource_NoLock( cacheHandle );

-}

-

-memhandle_t Studio_CreateBoneCache( bonecacheparams_t &params )

-{

-	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );

-	return g_StudioBoneCache.CreateResource( params );

-}

-

-void Studio_DestroyBoneCache( memhandle_t cacheHandle )

-{

-	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );

-	g_StudioBoneCache.DestroyResource( cacheHandle );

-}

-

-void Studio_InvalidateBoneCache( memhandle_t cacheHandle )

-{

-	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );

-	CBoneCache *pCache = g_StudioBoneCache.GetResource_NoLock( cacheHandle );

-	if ( pCache )

-	{

-		pCache->m_timeValid = -1.0f;

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-

-void BuildBoneChain(

-	const CStudioHdr *pStudioHdr,

-	const matrix3x4_t &rootxform,

-	const Vector pos[], 

-	const Quaternion q[], 

-	int	iBone,

-	matrix3x4_t *pBoneToWorld )

-{

-	CBoneBitList boneComputed;

-	BuildBoneChain( pStudioHdr, rootxform, pos, q, iBone, pBoneToWorld, boneComputed );

-	return;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: return a sub frame rotation for a single bone

-//-----------------------------------------------------------------------------

-void ExtractAnimValue( int frame, mstudioanimvalue_t *panimvalue, float scale, float &v1, float &v2 )

-{

-	if ( !panimvalue )

-	{

-		v1 = v2 = 0;

-		return;

-	}

-

-	// Avoids a crash reading off the end of the data

-	// There is probably a better long-term solution; Ken is going to look into it.

-	if ( ( panimvalue->num.total == 1 ) && ( panimvalue->num.valid == 1 ) )

-	{

-		v1 = v2 = panimvalue[1].value * scale;

-		return;

-	}

-

-	int k = frame;

-

-	// find the data list that has the frame

-	while (panimvalue->num.total <= k)

-	{

-		k -= panimvalue->num.total;

-		panimvalue += panimvalue->num.valid + 1;

-		if ( panimvalue->num.total == 0 )

-		{

-			Assert( 0 ); // running off the end of the animation stream is bad

-			v1 = v2 = 0;

-			return;

-		}

-	}

-	if (panimvalue->num.valid > k)

-	{

-		// has valid animation data

-		v1 = panimvalue[k+1].value * scale;

-

-		if (panimvalue->num.valid > k + 1)

-		{

-			// has valid animation blend data

-			v2 = panimvalue[k+2].value * scale;

-		}

-		else

-		{

-			if (panimvalue->num.total > k + 1)

-			{

-				// data repeats, no blend

-				v2 = v1;

-			}

-			else

-			{

-				// pull blend from first data block in next list

-				v2 = panimvalue[panimvalue->num.valid+2].value * scale;

-			}

-		}

-	}

-	else

-	{

-		// get last valid data block

-		v1 = panimvalue[panimvalue->num.valid].value * scale;

-		if (panimvalue->num.total > k + 1)

-		{

-			// data repeats, no blend

-			v2 = v1;

-		}

-		else

-		{

-			// pull blend from first data block in next list

-			v2 = panimvalue[panimvalue->num.valid + 2].value * scale;

-		}

-	}

-}

-

-

-void ExtractAnimValue( int frame, mstudioanimvalue_t *panimvalue, float scale, float &v1 )

-{

-	if ( !panimvalue )

-	{

-		v1 = 0;

-		return;

-	}

-

-	int k = frame;

-

-	while (panimvalue->num.total <= k)

-	{

-		k -= panimvalue->num.total;

-		panimvalue += panimvalue->num.valid + 1;

-		if ( panimvalue->num.total == 0 )

-		{

-			Assert( 0 ); // running off the end of the animation stream is bad

-			v1 = 0;

-			return;

-		}

-	}

-	if (panimvalue->num.valid > k)

-	{

-		v1 = panimvalue[k+1].value * scale;

-	}

-	else

-	{

-		// get last valid data block

-		v1 = panimvalue[panimvalue->num.valid].value * scale;

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: return a sub frame rotation for a single bone

-//-----------------------------------------------------------------------------

-void CalcBoneQuaternion( int frame, float s, 

-						const Quaternion &baseQuat, const RadianEuler &baseRot, const Vector &baseRotScale, 

-						int iBaseFlags, const Quaternion &baseAlignment, 

-						const mstudioanim_t *panim, Quaternion &q )

-{

-	if ( panim->flags & STUDIO_ANIM_RAWROT )

-	{

-		q = *(panim->pQuat48());

-		Assert( q.IsValid() );

-		return;

-	} 

-	

-	if ( panim->flags & STUDIO_ANIM_RAWROT2 )

-	{

-		q = *(panim->pQuat64());

-		Assert( q.IsValid() );

-		return;

-	}

-

-	if ( !(panim->flags & STUDIO_ANIM_ANIMROT) )

-	{

-		if (panim->flags & STUDIO_ANIM_DELTA)

-		{

-			q.Init( 0.0f, 0.0f, 0.0f, 1.0f );

-		}

-		else

-		{

-			q = baseQuat;

-		}

-		return;

-	}

-

-	mstudioanim_valueptr_t *pValuesPtr = panim->pRotV();

-

-	if (s > 0.001f)

-	{

-		QuaternionAligned	q1, q2;

-		RadianEuler			angle1, angle2;

-

-		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 0 ), baseRotScale.x, angle1.x, angle2.x );

-		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 1 ), baseRotScale.y, angle1.y, angle2.y );

-		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 2 ), baseRotScale.z, angle1.z, angle2.z );

-

-		if (!(panim->flags & STUDIO_ANIM_DELTA))

-		{

-			angle1.x = angle1.x + baseRot.x;

-			angle1.y = angle1.y + baseRot.y;

-			angle1.z = angle1.z + baseRot.z;

-			angle2.x = angle2.x + baseRot.x;

-			angle2.y = angle2.y + baseRot.y;

-			angle2.z = angle2.z + baseRot.z;

-		}

-

-		Assert( angle1.IsValid() && angle2.IsValid() );

-		if (angle1.x != angle2.x || angle1.y != angle2.y || angle1.z != angle2.z)

-		{

-			AngleQuaternion( angle1, q1 );

-			AngleQuaternion( angle2, q2 );

-

-	#ifdef _X360

-			fltx4 q1simd, q2simd, qsimd;

-			q1simd = LoadAlignedSIMD( q1 );

-			q2simd = LoadAlignedSIMD( q2 );

-			qsimd = QuaternionBlendSIMD( q1simd, q2simd, s );

-			StoreUnalignedSIMD( q.Base(), qsimd );

-	#else

-			QuaternionBlend( q1, q2, s, q );

-	#endif

-		}

-		else

-		{

-			AngleQuaternion( angle1, q );

-		}

-	}

-	else

-	{

-		RadianEuler			angle;

-

-		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 0 ), baseRotScale.x, angle.x );

-		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 1 ), baseRotScale.y, angle.y );

-		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 2 ), baseRotScale.z, angle.z );

-

-		if (!(panim->flags & STUDIO_ANIM_DELTA))

-		{

-			angle.x = angle.x + baseRot.x;

-			angle.y = angle.y + baseRot.y;

-			angle.z = angle.z + baseRot.z;

-		}

-

-		Assert( angle.IsValid() );

-		AngleQuaternion( angle, q );

-	}

-

-	Assert( q.IsValid() );

-

-	// align to unified bone

-	if (!(panim->flags & STUDIO_ANIM_DELTA) && (iBaseFlags & BONE_FIXED_ALIGNMENT))

-	{

-		QuaternionAlign( baseAlignment, q, q );

-	}

-}

-

-inline void CalcBoneQuaternion( int frame, float s, 

-						const mstudiobone_t *pBone,

-						const mstudiolinearbone_t *pLinearBones,

-						const mstudioanim_t *panim, Quaternion &q )

-{

-	if (pLinearBones)

-	{

-		CalcBoneQuaternion( frame, s, pLinearBones->quat(panim->bone), pLinearBones->rot(panim->bone), pLinearBones->rotscale(panim->bone), pLinearBones->flags(panim->bone), pLinearBones->qalignment(panim->bone), panim, q );

-	}

-	else

-	{

-		CalcBoneQuaternion( frame, s, pBone->quat, pBone->rot, pBone->rotscale, pBone->flags, pBone->qAlignment, panim, q );

-	}

-}

-

-						

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: return a sub frame position for a single bone

-//-----------------------------------------------------------------------------

-void CalcBonePosition(	int frame, float s,

-						const Vector &basePos, const Vector &baseBoneScale, 

-						const mstudioanim_t *panim, Vector &pos	)

-{

-	if (panim->flags & STUDIO_ANIM_RAWPOS)

-	{

-		pos = *(panim->pPos());

-		Assert( pos.IsValid() );

-

-		return;

-	}

-	else if (!(panim->flags & STUDIO_ANIM_ANIMPOS))

-	{

-		if (panim->flags & STUDIO_ANIM_DELTA)

-		{

-			pos.Init( 0.0f, 0.0f, 0.0f );

-		}

-		else

-		{

-			pos = basePos;

-		}

-		return;

-	}

-

-	mstudioanim_valueptr_t *pPosV = panim->pPosV();

-	int					j;

-

-	if (s > 0.001f)

-	{

-		float v1, v2;

-		for (j = 0; j < 3; j++)

-		{

-			ExtractAnimValue( frame, pPosV->pAnimvalue( j ), baseBoneScale[j], v1, v2 );

-			pos[j] = v1 * (1.0 - s) + v2 * s;

-		}

-	}

-	else

-	{

-		for (j = 0; j < 3; j++)

-		{

-			ExtractAnimValue( frame, pPosV->pAnimvalue( j ), baseBoneScale[j], pos[j] );

-		}

-	}

-

-	if (!(panim->flags & STUDIO_ANIM_DELTA))

-	{

-		pos.x = pos.x + basePos.x;

-		pos.y = pos.y + basePos.y;

-		pos.z = pos.z + basePos.z;

-	}

-

-	Assert( pos.IsValid() );

-}

-

-

-inline void CalcBonePosition( int frame, float s, 

-						const mstudiobone_t *pBone,

-						const mstudiolinearbone_t *pLinearBones,

-						const mstudioanim_t *panim, Vector &pos )

-{

-	if (pLinearBones)

-	{

-		CalcBonePosition( frame, s, pLinearBones->pos(panim->bone), pLinearBones->posscale(panim->bone), panim, pos );

-	}

-	else

-	{

-		CalcBonePosition( frame, s, pBone->pos, pBone->posscale, panim, pos );

-	}

-}

-

-

-

-void SetupSingleBoneMatrix( 

-	CStudioHdr *pOwnerHdr, 

-	int nSequence, 

-	int iFrame,

-	int iBone, 

-	matrix3x4_t &mBoneLocal )

-{

-	mstudioseqdesc_t &seqdesc = pOwnerHdr->pSeqdesc( nSequence );

-	mstudioanimdesc_t &animdesc = pOwnerHdr->pAnimdesc( seqdesc.anim( 0, 0 ) );

-	int iLocalFrame = iFrame;

-	mstudioanim_t *panim = animdesc.pAnim( &iLocalFrame );

-	float s = 0;

-	mstudiobone_t *pbone = pOwnerHdr->pBone( iBone );

-

-	Quaternion boneQuat;

-	Vector bonePos;

-

-	// search for bone

-	while (panim && panim->bone != iBone)

-	{

-		panim = panim->pNext();

-	}

-

-	// look up animation if found, if not, initialize

-	if (panim && seqdesc.weight(iBone) > 0)

-	{

-		CalcBoneQuaternion( iLocalFrame, s, pbone, NULL, panim, boneQuat );

-		CalcBonePosition  ( iLocalFrame, s, pbone, NULL, panim, bonePos );

-	}

-	else if (animdesc.flags & STUDIO_DELTA)

-	{

-		boneQuat.Init( 0.0f, 0.0f, 0.0f, 1.0f );

-		bonePos.Init( 0.0f, 0.0f, 0.0f );

-	}

-	else

-	{

-		boneQuat = pbone->quat;

-		bonePos = pbone->pos;

-	}

-

-	QuaternionMatrix( boneQuat, bonePos, mBoneLocal );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-static void CalcDecompressedAnimation( const mstudiocompressedikerror_t *pCompressed, int iFrame, float fraq, Vector &pos, Quaternion &q )

-{

-	if (fraq > 0.0001f)

-	{

-		Vector p1, p2;

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 0 ), pCompressed->scale[0], p1.x, p2.x );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 1 ), pCompressed->scale[1], p1.y, p2.y );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 2 ), pCompressed->scale[2], p1.z, p2.z );

-		pos = p1 * (1 - fraq) + p2 * fraq;

-

-		Quaternion			q1, q2;

-		RadianEuler			angle1, angle2;

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 3 ), pCompressed->scale[3], angle1.x, angle2.x );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 4 ), pCompressed->scale[4], angle1.y, angle2.y );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 5 ), pCompressed->scale[5], angle1.z, angle2.z );

-

-		if (angle1.x != angle2.x || angle1.y != angle2.y || angle1.z != angle2.z)

-		{

-			AngleQuaternion( angle1, q1 );

-			AngleQuaternion( angle2, q2 );

-			QuaternionBlend( q1, q2, fraq, q );

-		}

-		else

-		{

-			AngleQuaternion( angle1, q );

-		}

-	}

-	else

-	{

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 0 ), pCompressed->scale[0], pos.x );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 1 ), pCompressed->scale[1], pos.y );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 2 ), pCompressed->scale[2], pos.z );

-

-		RadianEuler			angle;

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 3 ), pCompressed->scale[3], angle.x );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 4 ), pCompressed->scale[4], angle.y );

-		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 5 ), pCompressed->scale[5], angle.z );

-

-		AngleQuaternion( angle, q );

-	}

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: translate animations done in a non-standard parent space

-//-----------------------------------------------------------------------------

-static void CalcLocalHierarchyAnimation( 

-	const CStudioHdr *pStudioHdr,

-	matrix3x4_t *boneToWorld,

-	CBoneBitList &boneComputed,

-	Vector *pos, 

-	Quaternion *q,

-	//const mstudioanimdesc_t &animdesc,

-	const mstudiobone_t *pbone,

-	mstudiolocalhierarchy_t *pHierarchy,

-	int iBone,

-	int iNewParent,

-	float cycle, 

-	int iFrame,

-	float flFraq,

-	int boneMask

-	)

-{

-	Vector localPos;

-	Quaternion localQ;

-

-	// make fake root transform

-	static ALIGN16 matrix3x4_t rootXform ALIGN16_POST ( 1.0f, 0, 0, 0,   0, 1.0f, 0, 0,  0, 0, 1.0f, 0 );

-

-	// FIXME: missing check to see if seq has a weight for this bone

-	float weight = 1.0f;

-

-	// check to see if there's a ramp on the influence

-	if ( pHierarchy->tail - pHierarchy->peak < 1.0f  )

-	{

-		float index = cycle;

-

-		if (pHierarchy->end > 1.0f && index < pHierarchy->start)

-			index += 1.0f;

-

-		if (index < pHierarchy->start)

-			return;

-		if (index >= pHierarchy->end)

-			return;

-

-		if (index < pHierarchy->peak && pHierarchy->start != pHierarchy->peak)

-		{

-			weight = (index - pHierarchy->start) / (pHierarchy->peak - pHierarchy->start);

-		}

-		else if (index > pHierarchy->tail && pHierarchy->end != pHierarchy->tail)

-		{

-			weight = (pHierarchy->end - index) / (pHierarchy->end - pHierarchy->tail);

-		}

-

-		weight = SimpleSpline( weight );

-	}

-

-	CalcDecompressedAnimation( pHierarchy->pLocalAnim(), iFrame - pHierarchy->iStart, flFraq, localPos, localQ );

-

-	BuildBoneChain( pStudioHdr, rootXform, pos, q, iBone, boneToWorld, boneComputed );

-	BuildBoneChain( pStudioHdr, rootXform, pos, q, iNewParent, boneToWorld, boneComputed );

-

-	matrix3x4_t localXform;

-	AngleMatrix( localQ, localPos, localXform );

-

-	ConcatTransforms( boneToWorld[iNewParent], localXform, boneToWorld[iBone] );

-

-	// back solve

-	Vector p1;

-	Quaternion q1;

-	int n = pbone[iBone].parent;

-	if (n == -1)

-	{

-		if (weight == 1.0f)

-		{

-			MatrixAngles( boneToWorld[iBone], q[iBone], pos[iBone] );

-		}

-		else

-		{

-			MatrixAngles( boneToWorld[iBone], q1, p1 );

-			QuaternionSlerp( q[iBone], q1, weight, q[iBone] );

-			pos[iBone] = Lerp( weight, p1, pos[iBone] );

-		}

-	}

-	else

-	{

-		matrix3x4_t worldToBone;

-		MatrixInvert( boneToWorld[n], worldToBone );

-

-		matrix3x4_t local;

-		ConcatTransforms( worldToBone, boneToWorld[iBone], local );

-		if (weight == 1.0f)

-		{

-			MatrixAngles( local, q[iBone], pos[iBone] );

-		}

-		else

-		{

-			MatrixAngles( local, q1, p1 );

-			QuaternionSlerp( q[iBone], q1, weight, q[iBone] );

-			pos[iBone] = Lerp( weight, p1, pos[iBone] );

-		}

-	}

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Calc Zeroframe Data

-//-----------------------------------------------------------------------------

-

-static void CalcZeroframeData( const CStudioHdr *pStudioHdr, const studiohdr_t *pAnimStudioHdr, const virtualgroup_t *pAnimGroup, const mstudiobone_t *pAnimbone, mstudioanimdesc_t &animdesc, float fFrame, Vector *pos, Quaternion *q, int boneMask, float flWeight )

-{

-	byte *pData = animdesc.pZeroFrameData();

-

-	if (!pData)

-		return;

-

-	int i, j;

-

-	// Msg("zeroframe %s\n", animdesc.pszName() );

-	if (animdesc.zeroframecount == 1)

-	{

-		for (j = 0; j < pAnimStudioHdr->numbones; j++)

-		{

-			if (pAnimGroup)

-				i = pAnimGroup->masterBone[j];

-			else

-				i = j;

-

-			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_POS)

-			{

-				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))

-				{

-					Vector p = *(Vector48 *)pData;

-					pos[i] = pos[i] * (1.0f - flWeight) + p * flWeight;

-					Assert( pos[i].IsValid() );

-				}

-				pData += sizeof( Vector48 );

-			}

-			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_ROT)

-			{

-				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))

-				{

-					Quaternion q0 = *(Quaternion64 *)pData;

-					QuaternionBlend( q[i], q0, flWeight, q[i] );

-					Assert( q[i].IsValid() );

-				}

-				pData += sizeof( Quaternion64 );

-			}

-		}

-	}

-	else

-	{

-		float s1;

-		int index = fFrame / animdesc.zeroframespan;

-		if (index >= animdesc.zeroframecount - 1)

-		{

-			index = animdesc.zeroframecount - 2;

-			s1 = 1.0f;

-		}

-		else

-		{

-			s1 = clamp( (fFrame - index * animdesc.zeroframespan) / animdesc.zeroframespan, 0.0f, 1.0f );

-		}

-		int i0 = max( index - 1, 0 );

-		int i1 = index;

-		int i2 = min( index + 1, animdesc.zeroframecount - 1 );

-		for (j = 0; j < pAnimStudioHdr->numbones; j++)

-		{

-			if (pAnimGroup)

-				i = pAnimGroup->masterBone[j];

-			else

-				i = j;

-

-			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_POS)

-			{

-				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))

-				{

-					Vector p0 = *(((Vector48 *)pData) + i0);

-					Vector p1 = *(((Vector48 *)pData) + i1);

-					Vector p2 = *(((Vector48 *)pData) + i2);

-					Vector p3;

-					Hermite_Spline( p0, p1, p2, s1, p3 );

-					pos[i] = pos[i] * (1.0f - flWeight) + p3 * flWeight;

-					Assert( pos[i].IsValid() );

-				}

-				pData += sizeof( Vector48 ) * animdesc.zeroframecount;

-			}

-			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_ROT)

-			{

-				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))

-				{

-					Quaternion q0 = *(((Quaternion64 *)pData) + i0);

-					Quaternion q1 = *(((Quaternion64 *)pData) + i1);

-					Quaternion q2 = *(((Quaternion64 *)pData) + i2);

-					if (flWeight == 1.0f)

-					{

-						Hermite_Spline( q0, q1, q2, s1, q[i] );

-					}

-					else

-					{

-						Quaternion q3;

-						Hermite_Spline( q0, q1, q2, s1, q3 );

-						QuaternionBlend( q[i], q3, flWeight, q[i] );

-					}

-					Assert( q[i].IsValid() );

-				}

-				pData += sizeof( Quaternion64 ) * animdesc.zeroframecount;

-			}

-		}

-	}

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Find and decode a sub-frame of animation, remapping the skeleton bone indexes

-//-----------------------------------------------------------------------------

-static void CalcVirtualAnimation( virtualmodel_t *pVModel, const CStudioHdr *pStudioHdr, Vector *pos, Quaternion *q, 

-	mstudioseqdesc_t &seqdesc, int sequence, int animation,

-	float cycle, int boneMask )

-{

-	int	i, j, k;

-

-	const mstudiobone_t *pbone;

-	const virtualgroup_t *pSeqGroup;

-	const studiohdr_t *pSeqStudioHdr;

-	const mstudiolinearbone_t *pSeqLinearBones;

-	const mstudiobone_t *pSeqbone;

-	const mstudioanim_t *panim;

-	const studiohdr_t *pAnimStudioHdr;

-	const mstudiolinearbone_t *pAnimLinearBones;

-	const mstudiobone_t *pAnimbone;

-	const virtualgroup_t *pAnimGroup;

-

-	pSeqGroup = pVModel->pSeqGroup( sequence );

-	int baseanimation = pStudioHdr->iRelativeAnim( sequence, animation );

-	mstudioanimdesc_t &animdesc = ((CStudioHdr *)pStudioHdr)->pAnimdesc( baseanimation );

-	pSeqStudioHdr = ((CStudioHdr *)pStudioHdr)->pSeqStudioHdr( sequence );

-	pSeqLinearBones = pSeqStudioHdr->pLinearBones();

-	pSeqbone = pSeqStudioHdr->pBone( 0 );

-	pAnimGroup = pVModel->pAnimGroup( baseanimation );

-	pAnimStudioHdr = ((CStudioHdr *)pStudioHdr)->pAnimStudioHdr( baseanimation );

-	pAnimLinearBones = pAnimStudioHdr->pLinearBones();

-	pAnimbone = pAnimStudioHdr->pBone( 0 );

-

-	int					iFrame;

-	float				s;

-

-	float fFrame = cycle * (animdesc.numframes - 1);

-

-	iFrame = (int)fFrame;

-	s = (fFrame - iFrame);

-

-	int iLocalFrame = iFrame;

-	float flStall;

-	panim = animdesc.pAnim( &iLocalFrame, flStall );

-

-	float *pweight = seqdesc.pBoneweight( 0 );

-	pbone = pStudioHdr->pBone( 0 );

-

-	for (i = 0; i < pStudioHdr->numbones(); i++)

-	{

-		if (pStudioHdr->boneFlags(i) & boneMask)

-		{

-			int j = pSeqGroup->boneMap[i];

-			if (j >= 0 && pweight[j] > 0.0f)

-			{

-				if (animdesc.flags & STUDIO_DELTA)

-				{

-					q[i].Init( 0.0f, 0.0f, 0.0f, 1.0f );

-					pos[i].Init( 0.0f, 0.0f, 0.0f );

-				}

-				else if (pSeqLinearBones)

-				{

-					q[i] = pSeqLinearBones->quat(j);

-					pos[i] = pSeqLinearBones->pos(j);

-				}

-				else 

-				{

-					q[i] = pSeqbone[j].quat;

-					pos[i] = pSeqbone[j].pos;

-				}

-#ifdef STUDIO_ENABLE_PERF_COUNTERS

-				pStudioHdr->m_nPerfUsedBones++;

-#endif

-			}

-		}

-	}

-

-	// if the animation isn't available, look for the zero frame cache

-	if (!panim)

-	{

-		CalcZeroframeData( ((CStudioHdr *)pStudioHdr), pAnimStudioHdr, pAnimGroup, pAnimbone, animdesc, fFrame, pos, q, boneMask, 1.0 );

-		return;

-	}

-

-	// FIXME: change encoding so that bone -1 is never the case

-	while (panim && panim->bone < 255)

-	{

-		j = pAnimGroup->masterBone[panim->bone];

-		if ( j >= 0 && ( pStudioHdr->boneFlags(j) & boneMask ) )

-		{

-			k = pSeqGroup->boneMap[j];

-

-			if (k >= 0 && pweight[k] > 0.0f)

-			{

-				CalcBoneQuaternion( iLocalFrame, s, &pAnimbone[panim->bone], pAnimLinearBones, panim, q[j] );

-				CalcBonePosition  ( iLocalFrame, s, &pAnimbone[panim->bone], pAnimLinearBones, panim, pos[j] );

-#ifdef STUDIO_ENABLE_PERF_COUNTERS

-				pStudioHdr->m_nPerfAnimatedBones++;

-#endif

-			}

-		}

-		panim = panim->pNext();

-	}

-

-	// cross fade in previous zeroframe data

-	if (flStall > 0.0f)

-	{

-		CalcZeroframeData( pStudioHdr, pAnimStudioHdr, pAnimGroup, pAnimbone, animdesc, fFrame, pos, q, boneMask, flStall );

-	}

-

-	// calculate a local hierarchy override

-	if (animdesc.numlocalhierarchy)

-	{

-		matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-		CBoneBitList boneComputed;

-

-		int i;

-		for (i = 0; i < animdesc.numlocalhierarchy; i++)

-		{

-			mstudiolocalhierarchy_t *pHierarchy = animdesc.pHierarchy( i );

-

-			if ( !pHierarchy )

-				break;

-

-			int iBone = pAnimGroup->masterBone[pHierarchy->iBone];

-			if (iBone >= 0 && (pStudioHdr->boneFlags(iBone) & boneMask))

-			{

-				int iNewParent = pAnimGroup->masterBone[pHierarchy->iNewParent];

-				if (iNewParent >= 0 && (pStudioHdr->boneFlags(iNewParent) & boneMask))

-				{

-					CalcLocalHierarchyAnimation( pStudioHdr, boneToWorld, boneComputed, pos, q, pbone, pHierarchy, iBone, iNewParent, cycle, iFrame, s, boneMask );

-				}

-			}

-		}

-

-		g_MatrixPool.Free( boneToWorld );

-	}

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Find and decode a sub-frame of animation

-//-----------------------------------------------------------------------------

-

-static void CalcAnimation( const CStudioHdr *pStudioHdr,	Vector *pos, Quaternion *q, 

-	mstudioseqdesc_t &seqdesc,

-	int sequence, int animation,

-	float cycle, int boneMask )

-{

-#ifdef STUDIO_ENABLE_PERF_COUNTERS

-	pStudioHdr->m_nPerfAnimationLayers++;

-#endif

-

-	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();

-

-	if (pVModel)

-	{

-		CalcVirtualAnimation( pVModel, pStudioHdr, pos, q, seqdesc, sequence, animation, cycle, boneMask );

-		return;

-	}

-

-	mstudioanimdesc_t &animdesc = ((CStudioHdr *)pStudioHdr)->pAnimdesc( animation );

-	mstudiobone_t *pbone = pStudioHdr->pBone( 0 );

-	const mstudiolinearbone_t *pLinearBones = pStudioHdr->pLinearBones();

-

-	int					i;

-	int					iFrame;

-	float				s;

-

-	float fFrame = cycle * (animdesc.numframes - 1);

-

-	iFrame = (int)fFrame;

-	s = (fFrame - iFrame);

-

-	int iLocalFrame = iFrame;

-	float flStall;

-	mstudioanim_t *panim = animdesc.pAnim( &iLocalFrame, flStall );

-

-	float *pweight = seqdesc.pBoneweight( 0 );

-

-	// if the animation isn't available, look for the zero frame cache

-	if (!panim)

-	{

-		// Msg("zeroframe %s\n", animdesc.pszName() );

-		// pre initialize

-		for (i = 0; i < pStudioHdr->numbones(); i++, pbone++, pweight++)

-		{

-			if (*pweight > 0 && (pStudioHdr->boneFlags(i) & boneMask))

-			{

-				if (animdesc.flags & STUDIO_DELTA)

-				{

-					q[i].Init( 0.0f, 0.0f, 0.0f, 1.0f );

-					pos[i].Init( 0.0f, 0.0f, 0.0f );

-				}

-				else

-				{

-					q[i] = pbone->quat;

-					pos[i] = pbone->pos;

-				}

-			}

-		}

-

-		CalcZeroframeData( pStudioHdr, pStudioHdr->GetRenderHdr(), NULL, pStudioHdr->pBone( 0 ), animdesc, fFrame, pos, q, boneMask, 1.0 );

-

-		return;

-	}

-

-	// BUGBUG: the sequence, the anim, and the model can have all different bone mappings.

-	for (i = 0; i < pStudioHdr->numbones(); i++, pbone++, pweight++)

-	{

-		if (panim && panim->bone == i)

-		{

-			if (*pweight > 0 && (pStudioHdr->boneFlags(i) & boneMask))

-			{

-				CalcBoneQuaternion( iLocalFrame, s, pbone, pLinearBones, panim, q[i] );

-				CalcBonePosition  ( iLocalFrame, s, pbone, pLinearBones, panim, pos[i] );

-#ifdef STUDIO_ENABLE_PERF_COUNTERS

-				pStudioHdr->m_nPerfAnimatedBones++;

-				pStudioHdr->m_nPerfUsedBones++;

-#endif

-			}

-			panim = panim->pNext();

-		}

-		else if (*pweight > 0 && (pStudioHdr->boneFlags(i) & boneMask))

-		{

-			if (animdesc.flags & STUDIO_DELTA)

-			{

-				q[i].Init( 0.0f, 0.0f, 0.0f, 1.0f );

-				pos[i].Init( 0.0f, 0.0f, 0.0f );

-			}

-			else

-			{

-				q[i] = pbone->quat;

-				pos[i] = pbone->pos;

-			}

-#ifdef STUDIO_ENABLE_PERF_COUNTERS

-			pStudioHdr->m_nPerfUsedBones++;

-#endif

-		}

-	}

-

-	// cross fade in previous zeroframe data

-	if (flStall > 0.0f)

-	{

-		CalcZeroframeData( pStudioHdr, pStudioHdr->GetRenderHdr(), NULL, pStudioHdr->pBone( 0 ), animdesc, fFrame, pos, q, boneMask, flStall );

-	}

-

-	if (animdesc.numlocalhierarchy)

-	{

-		matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-		CBoneBitList boneComputed;

-

-		int i;

-		for (i = 0; i < animdesc.numlocalhierarchy; i++)

-		{

-			mstudiolocalhierarchy_t *pHierarchy = animdesc.pHierarchy( i );

-

-			if ( !pHierarchy )

-				break;

-

-			if (pStudioHdr->boneFlags(pHierarchy->iBone) & boneMask)

-			{

-				if (pStudioHdr->boneFlags(pHierarchy->iNewParent) & boneMask)

-				{

-					CalcLocalHierarchyAnimation( pStudioHdr, boneToWorld, boneComputed, pos, q, pbone, pHierarchy, pHierarchy->iBone, pHierarchy->iNewParent, cycle, iFrame, s, boneMask );

-				}

-			}

-		}

-

-		g_MatrixPool.Free( boneToWorld );

-	}

-

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: qt = ( s * p ) * q

-//-----------------------------------------------------------------------------

-void QuaternionSM( float s, const Quaternion &p, const Quaternion &q, Quaternion &qt )

-{

-	Quaternion		p1, q1;

-

-	QuaternionScale( p, s, p1 );

-	QuaternionMult( p1, q, q1 );

-	QuaternionNormalize( q1 );

-	qt[0] = q1[0];

-	qt[1] = q1[1];

-	qt[2] = q1[2];

-	qt[3] = q1[3];

-}

-

-#if ALLOW_SIMD_QUATERNION_MATH

-FORCEINLINE fltx4 QuaternionSMSIMD( float s, const fltx4 &p, const fltx4 &q )

-{

-	fltx4 p1, q1, result;

-	p1 = QuaternionScaleSIMD( p, s );

-	q1 = QuaternionMultSIMD( p1, q );

-	result = QuaternionNormalizeSIMD( q1 );

-	return result;

-}

-#endif

-

-//-----------------------------------------------------------------------------

-// Purpose: qt = p * ( s * q )

-//-----------------------------------------------------------------------------

-void QuaternionMA( const Quaternion &p, float s, const Quaternion &q, Quaternion &qt )

-{

-	Quaternion p1, q1;

-

-	QuaternionScale( q, s, q1 );

-	QuaternionMult( p, q1, p1 );

-	QuaternionNormalize( p1 );

-	qt[0] = p1[0];

-	qt[1] = p1[1];

-	qt[2] = p1[2];

-	qt[3] = p1[3];

-}

-

-#if ALLOW_SIMD_QUATERNION_MATH

-FORCEINLINE fltx4 QuaternionMASIMD( const fltx4 &p, float s, const fltx4 &q )

-{

-	fltx4 p1, q1, result;

-	q1 = QuaternionScaleSIMD( q, s );

-	p1 = QuaternionMultSIMD( p, q1 );

-	result = QuaternionNormalizeSIMD( p1 );

-	return result;

-}

-#endif

-

-

-//-----------------------------------------------------------------------------

-// Purpose: qt = p + s * q

-//-----------------------------------------------------------------------------

-void QuaternionAccumulate( const Quaternion &p, float s, const Quaternion &q, Quaternion &qt )

-{

-	Quaternion q2;

-	QuaternionAlign( p, q, q2 );

-

-	qt[0] = p[0] + s * q2[0];

-	qt[1] = p[1] + s * q2[1];

-	qt[2] = p[2] + s * q2[2];

-	qt[3] = p[3] + s * q2[3];

-}

-

-#if ALLOW_SIMD_QUATERNION_MATH

-FORCEINLINE fltx4 QuaternionAccumulateSIMD( const fltx4 &p, float s, const fltx4 &q )

-{

-	fltx4 q2, s4, result;

-	q2 = QuaternionAlignSIMD( p, q );

-	s4 = ReplicateX4( s );

-	result = MaddSIMD( s4, q2, p );

-	return result;

-}

-#endif

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: blend together in world space q1,pos1 with q2,pos2.  Return result in q1,pos1.  

-//			0 returns q1, pos1.  1 returns q2, pos2

-//-----------------------------------------------------------------------------

-

-void WorldSpaceSlerp(

-	const CStudioHdr *pStudioHdr,

-	Quaternion q1[MAXSTUDIOBONES], 

-	Vector pos1[MAXSTUDIOBONES], 

-	mstudioseqdesc_t &seqdesc, 

-	int sequence, 

-	const Quaternion q2[MAXSTUDIOBONES], 

-	const Vector pos2[MAXSTUDIOBONES], 

-	float s,

-	int boneMask )

-{

-	int			i, j;

-	float		s1; // weight of parent for q2, pos2

-	float		s2; // weight for q2, pos2

-

-	// make fake root transform

-	matrix3x4_t rootXform;

-	SetIdentityMatrix( rootXform );

-

-	// matrices for q2, pos2

-	matrix3x4_t *srcBoneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList srcBoneComputed;

-

-	matrix3x4_t *destBoneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList destBoneComputed;

-

-	matrix3x4_t *targetBoneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList targetBoneComputed;

-

-	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();

-	const virtualgroup_t *pSeqGroup = NULL;

-	if (pVModel)

-	{

-		pSeqGroup = pVModel->pSeqGroup( sequence );

-	}

-

-	mstudiobone_t *pbone = pStudioHdr->pBone( 0 );

-

-	for (i = 0; i < pStudioHdr->numbones(); i++)

-	{

-		// skip unused bones

-		if (!(pStudioHdr->boneFlags(i) & boneMask))

-		{

-			continue;

-		}

-

-		int n = pbone[i].parent;

-		s1 = 0.0;

-		if (pSeqGroup)

-		{

-			j = pSeqGroup->boneMap[i];

-			if (j >= 0)

-			{

-				s2 = s * seqdesc.weight( j );	// blend in based on this bones weight

-				if (n != -1)

-				{

-					s1 = s * seqdesc.weight( pSeqGroup->boneMap[n] );

-				}

-			}

-			else

-			{

-				s2 = 0.0;

-			}

-		}

-		else

-		{

-			s2 = s * seqdesc.weight( i );	// blend in based on this bones weight

-			if (n != -1)

-			{

-				s1 = s * seqdesc.weight( n );

-			}

-		}

-

-		if (s1 == 1.0 && s2 == 1.0)

-		{

-			pos1[i] = pos2[i];

-			q1[i] = q2[i];

-		}

-		else if (s2 > 0.0)

-		{

-			Quaternion srcQ, destQ;

-			Vector srcPos, destPos;

-			Quaternion targetQ;

-			Vector targetPos;

-			Vector tmp;

-

-			BuildBoneChain( pStudioHdr, rootXform, pos1, q1, i, destBoneToWorld, destBoneComputed );

-			BuildBoneChain( pStudioHdr, rootXform, pos2, q2, i, srcBoneToWorld, srcBoneComputed );

-

-			MatrixAngles( destBoneToWorld[i], destQ, destPos );

-			MatrixAngles( srcBoneToWorld[i], srcQ, srcPos );

-

-			QuaternionSlerp( destQ, srcQ, s2, targetQ );

-			AngleMatrix( targetQ, destPos, targetBoneToWorld[i] );

-

-			// back solve

-			if (n == -1)

-			{

-				MatrixAngles( targetBoneToWorld[i], q1[i], tmp );

-			}

-			else

-			{

-				matrix3x4_t worldToBone;

-				MatrixInvert( targetBoneToWorld[n], worldToBone );

-

-				matrix3x4_t local;

-				ConcatTransforms( worldToBone, targetBoneToWorld[i], local );

-				MatrixAngles( local, q1[i], tmp );

-

-				// blend bone lengths (local space)

-				pos1[i] = Lerp( s2, pos1[i], pos2[i] );

-			}

-		}

-	}

-	g_MatrixPool.Free( srcBoneToWorld );

-	g_MatrixPool.Free( destBoneToWorld );

-	g_MatrixPool.Free( targetBoneToWorld );

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: blend together q1,pos1 with q2,pos2.  Return result in q1,pos1.  

-//			0 returns q1, pos1.  1 returns q2, pos2

-//-----------------------------------------------------------------------------

-void SlerpBones( 

-	const CStudioHdr *pStudioHdr,

-	Quaternion q1[MAXSTUDIOBONES], 

-	Vector pos1[MAXSTUDIOBONES], 

-	mstudioseqdesc_t &seqdesc,  // source of q2 and pos2

-	int sequence, 

-	const QuaternionAligned q2[MAXSTUDIOBONES], 

-	const Vector pos2[MAXSTUDIOBONES], 

-	float s,

-	int boneMask )

-{

-	if (s <= 0.0f) 

-		return;

-	if (s > 1.0f)

-	{

-		s = 1.0f;		

-	}

-

-	if (seqdesc.flags & STUDIO_WORLD)

-	{

-		WorldSpaceSlerp( pStudioHdr, q1, pos1, seqdesc, sequence, q2, pos2, s, boneMask );

-		return;

-	}

-

-	int			i, j;

-	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();

-	const virtualgroup_t *pSeqGroup = NULL;

-	if (pVModel)

-	{

-		pSeqGroup = pVModel->pSeqGroup( sequence );

-	}

-

-	// Build weightlist for all bones

-	int nBoneCount = pStudioHdr->numbones();

-	float *pS2 = (float*)stackalloc( nBoneCount * sizeof(float) );

-	for (i = 0; i < nBoneCount; i++)

-	{

-		// skip unused bones

-		if (!(pStudioHdr->boneFlags(i) & boneMask))

-		{

-			pS2[i] = 0.0f;

-			continue;

-		}

-

-		if ( !pSeqGroup )

-		{

-			pS2[i] = s * seqdesc.weight( i );	// blend in based on this bones weight

-			continue;

-		}

-

-		j = pSeqGroup->boneMap[i];

-		if ( j >= 0 )

-		{

-			pS2[i] = s * seqdesc.weight( j );	// blend in based on this bones weight

-		}

-		else

-		{

-			pS2[i] = 0.0;

-		}

-	}

-

-	float s1, s2;

-	if ( seqdesc.flags & STUDIO_DELTA )

-	{

-		for ( i = 0; i < nBoneCount; i++ )

-		{

-			s2 = pS2[i];

-			if ( s2 <= 0.0f )

-				continue;

-

-			if ( seqdesc.flags & STUDIO_POST )

-			{

-#ifndef _X360

-				QuaternionMA( q1[i], s2, q2[i], q1[i] );

-#else

-				fltx4 q1simd = LoadUnalignedSIMD( q1[i].Base() );

-				fltx4 q2simd = LoadAlignedSIMD( q2[i] );

-				fltx4 result = QuaternionMASIMD( q1simd, s2, q2simd );

-				StoreUnalignedSIMD( q1[i].Base(), result );

-#endif

-				// FIXME: are these correct?

-				pos1[i][0] = pos1[i][0] + pos2[i][0] * s2;

-				pos1[i][1] = pos1[i][1] + pos2[i][1] * s2;

-				pos1[i][2] = pos1[i][2] + pos2[i][2] * s2;

-			}

-			else

-			{

-#ifndef _X360

-				QuaternionSM( s2, q2[i], q1[i], q1[i] );

-#else

-				fltx4 q1simd = LoadUnalignedSIMD( q1[i].Base() );

-				fltx4 q2simd = LoadAlignedSIMD( q2[i] );

-				fltx4 result = QuaternionSMSIMD( s2, q2simd, q1simd );

-				StoreUnalignedSIMD( q1[i].Base(), result );

-#endif

-

-				// FIXME: are these correct?

-				pos1[i][0] = pos1[i][0] + pos2[i][0] * s2;

-				pos1[i][1] = pos1[i][1] + pos2[i][1] * s2;

-				pos1[i][2] = pos1[i][2] + pos2[i][2] * s2;

-			}

-		}

-		return;

-	}

-

-	QuaternionAligned q3;

-	for (i = 0; i < nBoneCount; i++)

-	{

-		s2 = pS2[i];

-		if ( s2 <= 0.0f )

-			continue;

-

-		s1 = 1.0 - s2;

-

-#ifdef _X360

-		fltx4  q1simd, q2simd, result;

-		q1simd = LoadUnalignedSIMD( q1[i].Base() );

-		q2simd = LoadAlignedSIMD( q2[i] );

-#endif

-		if ( pStudioHdr->boneFlags(i) & BONE_FIXED_ALIGNMENT )

-		{

-#ifndef _X360

-			QuaternionSlerpNoAlign( q2[i], q1[i], s1, q3 );

-#else

-			result = QuaternionSlerpNoAlignSIMD( q2simd, q1simd, s1 );

-#endif

-		}

-		else

-		{

-#ifndef _X360

-			QuaternionSlerp( q2[i], q1[i], s1, q3 );

-#else

-			result = QuaternionSlerpSIMD( q2simd, q1simd, s1 );

-#endif

-		}

-

-#ifndef _X360

-		q1[i][0] = q3[0];

-		q1[i][1] = q3[1];

-		q1[i][2] = q3[2];

-		q1[i][3] = q3[3];

-#else

-		StoreUnalignedSIMD( q1[i].Base(), result );

-#endif

-

-		pos1[i][0] = pos1[i][0] * s1 + pos2[i][0] * s2;

-		pos1[i][1] = pos1[i][1] * s1 + pos2[i][1] * s2;

-		pos1[i][2] = pos1[i][2] * s1 + pos2[i][2] * s2;

-	}

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Inter-animation blend.  Assumes both types are identical.

-//			blend together q1,pos1 with q2,pos2.  Return result in q1,pos1.  

-//			0 returns q1, pos1.  1 returns q2, pos2

-//-----------------------------------------------------------------------------

-void BlendBones( 

-	const CStudioHdr *pStudioHdr,

-	Quaternion q1[MAXSTUDIOBONES], 

-	Vector pos1[MAXSTUDIOBONES], 

-	mstudioseqdesc_t &seqdesc, 

-	int sequence,

-	const Quaternion q2[MAXSTUDIOBONES], 

-	const Vector pos2[MAXSTUDIOBONES], 

-	float s,

-	int boneMask )

-{

-	int			i, j;

-	Quaternion		q3;

-

-	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();

-	const virtualgroup_t *pSeqGroup = NULL;

-	if (pVModel)

-	{

-		pSeqGroup = pVModel->pSeqGroup( sequence );

-	}

-

-	if (s <= 0)

-	{

-		Assert(0); // shouldn't have been called

-		return;

-	}

-	else if (s >= 1.0)

-	{

-		Assert(0); // shouldn't have been called

-		for (i = 0; i < pStudioHdr->numbones(); i++)

-		{

-			// skip unused bones

-			if (!(pStudioHdr->boneFlags(i) & boneMask))

-			{

-				continue;

-			}

-

-			if (pSeqGroup)

-			{

-				j = pSeqGroup->boneMap[i];

-			}

-			else

-			{

-				j = i;

-			}

-

-			if (j >= 0 && seqdesc.weight( j ) > 0.0)

-			{

-				q1[i] = q2[i];

-				pos1[i] = pos2[i];

-			}

-		}

-		return;

-	}

-

-	float s2 = s;

-	float s1 = 1.0 - s2;

-

-	for (i = 0; i < pStudioHdr->numbones(); i++)

-	{

-		// skip unused bones

-		if (!(pStudioHdr->boneFlags(i) & boneMask))

-		{

-			continue;

-		}

-

-		if (pSeqGroup)

-		{

-			j = pSeqGroup->boneMap[i];

-		}

-		else

-		{

-			j = i;

-		}

-

-		if (j >= 0 && seqdesc.weight( j ) > 0.0)

-		{

-			if (pStudioHdr->boneFlags(i) & BONE_FIXED_ALIGNMENT)

-			{

-				QuaternionBlendNoAlign( q2[i], q1[i], s1, q3 );

-			}

-			else

-			{

-				QuaternionBlend( q2[i], q1[i], s1, q3 );

-			}

-			q1[i][0] = q3[0];

-			q1[i][1] = q3[1];

-			q1[i][2] = q3[2];

-			q1[i][3] = q3[3];

-			pos1[i][0] = pos1[i][0] * s1 + pos2[i][0] * s2;

-			pos1[i][1] = pos1[i][1] * s1 + pos2[i][1] * s2;

-			pos1[i][2] = pos1[i][2] * s1 + pos2[i][2] * s2;

-		}

-	}

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Scale a set of bones.  Must be of type delta

-//-----------------------------------------------------------------------------

-void ScaleBones( 

-	const CStudioHdr *pStudioHdr,

-	Quaternion q1[MAXSTUDIOBONES], 

-	Vector pos1[MAXSTUDIOBONES], 

-	int sequence,

-	float s,

-	int boneMask )

-{

-	int			i, j;

-	Quaternion		q3;

-

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( sequence );

-

-	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();

-	const virtualgroup_t *pSeqGroup = NULL;

-	if (pVModel)

-	{

-		pSeqGroup = pVModel->pSeqGroup( sequence );

-	}

-

-	float s2 = s;

-	float s1 = 1.0 - s2;

-

-	for (i = 0; i < pStudioHdr->numbones(); i++)

-	{

-		// skip unused bones

-		if (!(pStudioHdr->boneFlags(i) & boneMask))

-		{

-			continue;

-		}

-

-		if (pSeqGroup)

-		{

-			j = pSeqGroup->boneMap[i];

-		}

-		else

-		{

-			j = i;

-		}

-

-		if (j >= 0 && seqdesc.weight( j ) > 0.0)

-		{

-			QuaternionIdentityBlend( q1[i], s1, q1[i] );

-			VectorScale( pos1[i], s2, pos1[i] );

-		}

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: resolve a global pose parameter to the specific setting for this sequence

-//-----------------------------------------------------------------------------

-void Studio_LocalPoseParameter( const CStudioHdr *pStudioHdr, const float poseParameter[], mstudioseqdesc_t &seqdesc, int iSequence, int iLocalIndex, float &flSetting, int &index )

-{

-	int iPose = pStudioHdr->GetSharedPoseParameter( iSequence, seqdesc.paramindex[iLocalIndex] );

-

-	if (iPose == -1)

-	{

-		flSetting = 0;

-		index = 0;

-		return;

-	}

-

-	const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iPose );

-

-	float flValue = poseParameter[iPose];

-

-	if (Pose.loop)

-	{

-		float wrap = (Pose.start + Pose.end) / 2.0 + Pose.loop / 2.0;

-		float shift = Pose.loop - wrap;

-

-		flValue = flValue - Pose.loop * floor((flValue + shift) / Pose.loop);

-	}

-

-	if (seqdesc.posekeyindex == 0)

-	{

-		float flLocalStart	= ((float)seqdesc.paramstart[iLocalIndex] - Pose.start) / (Pose.end - Pose.start);

-		float flLocalEnd	= ((float)seqdesc.paramend[iLocalIndex] - Pose.start) / (Pose.end - Pose.start);

-

-		// convert into local range

-		flSetting = (flValue - flLocalStart) / (flLocalEnd - flLocalStart);

-

-		// clamp.  This shouldn't ever need to happen if it's looping.

-		if (flSetting < 0)

-			flSetting = 0;

-		if (flSetting > 1)

-			flSetting = 1;

-

-		index = 0;

-		if (seqdesc.groupsize[iLocalIndex] > 2 )

-		{

-			// estimate index

-			index = (int)(flSetting * (seqdesc.groupsize[iLocalIndex] - 1));

-			if (index == seqdesc.groupsize[iLocalIndex] - 1) index = seqdesc.groupsize[iLocalIndex] - 2;

-			flSetting = flSetting * (seqdesc.groupsize[iLocalIndex] - 1) - index;

-		}

-	}

-	else

-	{

-		flValue = flValue * (Pose.end - Pose.start) + Pose.start;

-		index = 0;

-			

-		// FIXME: this needs to be 2D

-		// FIXME: this shouldn't be a linear search

-

-		while (1)

-		{

-			flSetting = (flValue - seqdesc.poseKey( iLocalIndex, index )) / (seqdesc.poseKey( iLocalIndex, index + 1 ) - seqdesc.poseKey( iLocalIndex, index ));

-			/*

-			if (index > 0 && flSetting < 0.0)

-			{

-				index--;

-				continue;

-			}

-			else 

-			*/

-			if (index < seqdesc.groupsize[iLocalIndex] - 2 && flSetting > 1.0)

-			{

-				index++;

-				continue;

-			}

-			break;

-		}

-

-		// clamp.

-		if (flSetting < 0.0f)

-			flSetting = 0.0f;

-		if (flSetting > 1.0f)

-			flSetting = 1.0f;

-	}

-}

-

-void Studio_CalcBoneToBoneTransform( const CStudioHdr *pStudioHdr, int inputBoneIndex, int outputBoneIndex, matrix3x4_t& matrixOut )

-{

-	mstudiobone_t *pbone = pStudioHdr->pBone( inputBoneIndex );

-

-	matrix3x4_t inputToPose;

-	MatrixInvert( pbone->poseToBone, inputToPose );

-	ConcatTransforms( pStudioHdr->pBone( outputBoneIndex )->poseToBone, inputToPose, matrixOut );

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate a pose for a single sequence

-//-----------------------------------------------------------------------------

-void InitPose(

-	const CStudioHdr *pStudioHdr,

-	Vector pos[], 

-	Quaternion q[],

-	int boneMask 

-	)

-{

-	if (!pStudioHdr->pLinearBones())

-	{

-		for (int i = 0; i < pStudioHdr->numbones(); i++)

-		{

-			if (pStudioHdr->boneFlags(  i ) & boneMask ) 

-			{

-				mstudiobone_t *pbone = pStudioHdr->pBone( i );

-				pos[i] = pbone->pos;

-				q[i] = pbone->quat;

-			}

-		}

-	}

-	else

-	{

-		mstudiolinearbone_t *pLinearBones = pStudioHdr->pLinearBones();

-		for (int i = 0; i < pStudioHdr->numbones(); i++)

-		{

-			if (pStudioHdr->boneFlags(  i ) & boneMask ) 

-			{

-				pos[i] = pLinearBones->pos(i);

-				q[i] = pLinearBones->quat(i);

-			}

-		}

-	}

-}

-	

-

-inline bool PoseIsAllZeros( 

-	const CStudioHdr *pStudioHdr,

-	int sequence, 

-	mstudioseqdesc_t	&seqdesc,

-	int i0,

-	int i1

-	)

-{

-	int baseanim;

-		

-	// remove "zero" positional blends

-	baseanim = pStudioHdr->iRelativeAnim( sequence, seqdesc.anim(i0  ,i1 ) );

-	mstudioanimdesc_t		&anim = ((CStudioHdr *)pStudioHdr)->pAnimdesc( baseanim );

-	return (anim.flags & STUDIO_ALLZEROS) != 0;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: turn a 2x2 blend into a 3 way triangle blend

-// Returns: returns the animination indices and barycentric coordinates of a triangle

-//			the triangle is a right triangle, and the diagonal is between elements [0] and [2]

-//-----------------------------------------------------------------------------

-

-static ConVar anim_3wayblend( "anim_3wayblend", "1", FCVAR_REPLICATED, "Toggle the 3-way animation blending code." );

-

-void Calc3WayBlendIndices( int i0, int i1, float s0, float s1, const mstudioseqdesc_t &seqdesc, int *pAnimIndices, float *pWeight )

-{

-	// Figure out which bi-section direction we are using to make triangles.

-	bool bEven = ( ( ( i0 + i1 ) & 0x1 ) == 0 );

-

-	int x1, y1;

-	int x2, y2;

-	int x3, y3;

-

-	// diagonal is between elements 1 & 3

-	// TL to BR

-	if ( bEven )

-	{

-		if ( s0 > s1 )

-		{

-			// B

-			x1 = 0; y1 = 0;

-			x2 = 1; y2 = 0;

-			x3 = 1; y3 = 1;

-			pWeight[0] = (1.0f - s0);

-			pWeight[1] = s0 - s1;

-		}

-		else

-		{

-			// C

-			x1 = 1; y1 = 1;

-			x2 = 0; y2 = 1;

-			x3 = 0; y3 = 0;

-			pWeight[0] = s0;

-			pWeight[1] = s1 - s0;

-		}

-	}

-	// BL to TR

-	else

-	{

-		float flTotal = s0 + s1;

-

-		if( flTotal > 1.0f )

-		{

-			// D

-			x1 = 1; y1 = 0;

-			x2 = 1; y2 = 1;

-			x3 = 0; y3 = 1;

-			pWeight[0] = (1.0f - s1);

-			pWeight[1] = s0 - 1.0f + s1;

-		}

-		else

-		{

-			// A

-			x1 = 0; y1 = 1;

-			x2 = 0; y2 = 0;

-			x3 = 1; y3 = 0;

-			pWeight[0] = s1;

-			pWeight[1] = 1.0f - s0 - s1;

-		}

-	}

-

-	pAnimIndices[0] = seqdesc.anim( i0 + x1, i1 + y1 );

-	pAnimIndices[1] = seqdesc.anim( i0 + x2, i1 + y2 );

-	pAnimIndices[2] = seqdesc.anim( i0 + x3, i1 + y3 );

-

-	/*

-	float w0 = ((x2-x3)*(y3-s1) - (x3-s0)*(y2-y3)) / ((x1-x3)*(y2-y3) - (x2-x3)*(y1-y3));

-	float w1 = ((x1-x3)*(y3-s1) - (x3-s0)*(y1-y3)) / ((x2-x3)*(y1-y3) - (x1-x3)*(y2-y3));

-	Assert( pWeight[0] == w0 && pWeight[1] == w1 );

-	*/

-

-	// clamp the diagonal

-	if (pWeight[1] < 0.001f)

-		pWeight[1] = 0.0f;

-	pWeight[2] = 1.0f - pWeight[0] - pWeight[1];

-

-	Assert( pWeight[0] >= 0.0f && pWeight[0] <= 1.0f );

-	Assert( pWeight[1] >= 0.0f && pWeight[1] <= 1.0f );

-	Assert( pWeight[2] >= 0.0f && pWeight[2] <= 1.0f );

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate a pose for a single sequence

-//-----------------------------------------------------------------------------

-bool CalcPoseSingle(

-	const CStudioHdr *pStudioHdr,

-	Vector pos[], 

-	Quaternion q[], 

-	mstudioseqdesc_t &seqdesc,

-	int sequence, 

-	float cycle,

-	const float poseParameter[],

-	int boneMask,

-	float flTime

-	)

-{

-	bool bResult = true;

-	

-	Vector		*pos2 = g_VectorPool.Alloc();

-	Quaternion	*q2 = g_QaternionPool.Alloc();

-	Vector		*pos3= g_VectorPool.Alloc();

-	Quaternion	*q3 = g_QaternionPool.Alloc();

-

-	if (sequence >= pStudioHdr->GetNumSeq()) 

-	{

-		sequence = 0;

-		seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( sequence );

-	}

-

-

-	int i0 = 0, i1 = 0;

-	float s0 = 0, s1 = 0;

-

-	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, sequence, 0, s0, i0 );

-	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, sequence, 1, s1, i1 );

-

-

-	if (seqdesc.flags & STUDIO_REALTIME)

-	{

-		float cps = Studio_CPS( pStudioHdr, seqdesc, sequence, poseParameter );

-		cycle = flTime * cps;

-		cycle = cycle - (int)cycle;

-	}

-	else if (seqdesc.flags & STUDIO_CYCLEPOSE)

-	{

-		int iPose = pStudioHdr->GetSharedPoseParameter( sequence, seqdesc.cycleposeindex );

-		if (iPose != -1)

-		{

-			/*

-			const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iPose );

-			cycle = poseParameter[ iPose ] * (Pose.end - Pose.start) + Pose.start;

-			*/

-			cycle = poseParameter[ iPose ];

-		}

-		else

-		{

-			cycle = 0.0f;

-		}

-	}

-	else if (cycle < 0 || cycle >= 1)

-	{

-		if (seqdesc.flags & STUDIO_LOOPING)

-		{

-			cycle = cycle - (int)cycle;

-			if (cycle < 0) cycle += 1;

-		}

-		else

-		{

-			cycle = clamp( cycle, 0.0f, 1.0f );

-		}

-	}

-

-	if (s0 < 0.001)

-	{

-		if (s1 < 0.001)

-		{

-			if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0, i1 ))

-			{

-				bResult = false;

-			}

-			else

-			{

-				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  , i1   ), cycle, boneMask );

-			}

-		}

-		else if (s1 > 0.999)

-		{

-			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  , i1+1 ), cycle, boneMask );

-		}

-		else

-		{

-			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  , i1   ), cycle, boneMask );

-			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0  , i1+1 ), cycle, boneMask );

-			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s1, boneMask );

-		}

-	}

-	else if (s0 > 0.999)

-	{

-		if (s1 < 0.001)

-		{

-			if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0+1, i1 ))

-			{

-				bResult = false;

-			}

-			else

-			{

-				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1, i1   ), cycle, boneMask );

-			}

-		}

-		else if (s1 > 0.999)

-		{

-			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1, i1+1 ), cycle, boneMask );

-		}

-		else

-		{

-			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1, i1   ), cycle, boneMask );

-			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1, i1+1 ), cycle, boneMask );

-			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s1, boneMask );

-		}

-	}

-	else

-	{

-		if (s1 < 0.001)

-		{

-			if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0+1, i1 ))

-			{

-				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1  ), cycle, boneMask );

-				ScaleBones( pStudioHdr, q, pos, sequence, 1.0 - s0, boneMask );

-			}

-			else if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0, i1 ))

-			{

-				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1  ,i1  ), cycle, boneMask );

-				ScaleBones( pStudioHdr, q, pos, sequence, s0, boneMask );

-			}

-			else

-			{

-				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1  ), cycle, boneMask );

-				CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1,i1  ), cycle, boneMask );

-

-				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s0, boneMask );

-			}

-		}

-		else if (s1 > 0.999)

-		{

-			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1+1  ), cycle, boneMask );

-			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1,i1+1  ), cycle, boneMask );

-			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s0, boneMask );

-		}

-		else if ( !anim_3wayblend.GetBool() )

-		{

-			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1  ), cycle, boneMask );

-			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1,i1  ), cycle, boneMask );

-			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s0, boneMask );

-

-			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0  , i1+1), cycle, boneMask );

-			CalcAnimation( pStudioHdr, pos3, q3, seqdesc, sequence, seqdesc.anim( i0+1, i1+1), cycle, boneMask );

-			BlendBones( pStudioHdr, q2, pos2, seqdesc, sequence, q3, pos3, s0, boneMask );

-

-			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s1, boneMask );

-		}

-		else

-		{

-			int		iAnimIndices[3];

-			float	weight[3];

-

-			Calc3WayBlendIndices( i0, i1, s0, s1, seqdesc, iAnimIndices, weight );

-

-			/*

-			char buf[256];

-			sprintf( buf, "%d %6.2f  %d %6.2f : %6.2f %6.2f %6.2f\n", i0, s0, i1, s1, weight[0], weight[1], weight[2] );

-			OutputDebugString( buf );

-			*/

-

-			if (weight[1] < 0.001)

-			{

-				// on diagonal

-				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, iAnimIndices[0], cycle, boneMask );

-				CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, iAnimIndices[2], cycle, boneMask );

-				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, weight[2] / (weight[0] + weight[2]), boneMask );

-			}

-			else

-			{

-				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, iAnimIndices[0], cycle, boneMask );

-				CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, iAnimIndices[1], cycle, boneMask );

-				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, weight[1] / (weight[0] + weight[1]), boneMask );

-

-				CalcAnimation( pStudioHdr, pos3, q3, seqdesc, sequence, iAnimIndices[2], cycle, boneMask );

-				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q3, pos3, weight[2], boneMask );

-			}

-		}

-	}

-

-	g_VectorPool.Free( pos2 );

-	g_QaternionPool.Free( q2 );

-	g_VectorPool.Free( pos3 );

-	g_QaternionPool.Free( q3 );

-

-	return bResult;

-}

-

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate a pose for a single sequence

-//			adds autolayers, runs local ik rukes

-//-----------------------------------------------------------------------------

-void CBoneSetup::AddSequenceLayers(

-   Vector pos[], 

-   Quaternion q[], 

-   mstudioseqdesc_t &seqdesc,

-   int sequence, 

-   float cycle,

-   float flWeight,

-   float flTime,

-   CIKContext *pIKContext

-   )

-{

-	for (int i = 0; i < seqdesc.numautolayers; i++)

-	{

-		mstudioautolayer_t *pLayer = seqdesc.pAutolayer( i );

-

-		if (pLayer->flags & STUDIO_AL_LOCAL)

-			continue;

-

-		float layerCycle = cycle;

-		float layerWeight = flWeight;

-

-		if (pLayer->start != pLayer->end)

-		{

-			float s = 1.0;

-			float index;

-

-			if (!(pLayer->flags & STUDIO_AL_POSE))

-			{

-				index = cycle;

-			}

-			else

-			{

-				int iSequence = m_pStudioHdr->iRelativeSeq( sequence, pLayer->iSequence );

-				int iPose = m_pStudioHdr->GetSharedPoseParameter( iSequence, pLayer->iPose );

-				if (iPose != -1)

-				{

-					const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)m_pStudioHdr)->pPoseParameter( iPose );

-					index = m_flPoseParameter[ iPose ] * (Pose.end - Pose.start) + Pose.start;

-				}

-				else

-				{

-					index = 0;

-				}

-			}

-

-			if (index < pLayer->start)

-				continue;

-			if (index >= pLayer->end)

-				continue;

-

-			if (index < pLayer->peak && pLayer->start != pLayer->peak)

-			{

-				s = (index - pLayer->start) / (pLayer->peak - pLayer->start);

-			}

-			else if (index > pLayer->tail && pLayer->end != pLayer->tail)

-			{

-				s = (pLayer->end - index) / (pLayer->end - pLayer->tail);

-			}

-

-			if (pLayer->flags & STUDIO_AL_SPLINE)

-			{

-				s = SimpleSpline( s );

-			}

-

-			if ((pLayer->flags & STUDIO_AL_XFADE) && (index > pLayer->tail))

-			{

-				layerWeight = ( s * flWeight ) / ( 1 - flWeight + s * flWeight );

-			}

-			else if (pLayer->flags & STUDIO_AL_NOBLEND)

-			{

-				layerWeight = s;

-			}

-			else

-			{

-				layerWeight = flWeight * s;

-			}

-

-			if (!(pLayer->flags & STUDIO_AL_POSE))

-			{

-				layerCycle = (cycle - pLayer->start) / (pLayer->end - pLayer->start);

-			}

-		}

-

-		int iSequence = m_pStudioHdr->iRelativeSeq( sequence, pLayer->iSequence );

-		AccumulatePose( pos, q, iSequence, layerCycle, layerWeight, flTime, pIKContext );

-	}

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate a pose for a single sequence

-//			adds autolayers, runs local ik rukes

-//-----------------------------------------------------------------------------

-void CBoneSetup::AddLocalLayers(

-	Vector pos[], 

-	Quaternion q[], 

-	mstudioseqdesc_t &seqdesc,

-	int sequence, 

-	float cycle,

-	float flWeight,

-	float flTime,

-	CIKContext *pIKContext

-	)

-{

-	if (!(seqdesc.flags & STUDIO_LOCAL))

-	{

-		return;

-	}

-

-	for (int i = 0; i < seqdesc.numautolayers; i++)

-	{

-		mstudioautolayer_t *pLayer = seqdesc.pAutolayer( i );

-

-		if (!(pLayer->flags & STUDIO_AL_LOCAL))

-			continue;

-

-		float layerCycle = cycle;

-		float layerWeight = flWeight;

-

-		if (pLayer->start != pLayer->end)

-		{

-			float s = 1.0;

-

-			if (cycle < pLayer->start)

-				continue;

-			if (cycle >= pLayer->end)

-				continue;

-

-			if (cycle < pLayer->peak && pLayer->start != pLayer->peak)

-			{

-				s = (cycle - pLayer->start) / (pLayer->peak - pLayer->start);

-			}

-			else if (cycle > pLayer->tail && pLayer->end != pLayer->tail)

-			{

-				s = (pLayer->end - cycle) / (pLayer->end - pLayer->tail);

-			}

-

-			if (pLayer->flags & STUDIO_AL_SPLINE)

-			{

-				s = SimpleSpline( s );

-			}

-

-			if ((pLayer->flags & STUDIO_AL_XFADE) && (cycle > pLayer->tail))

-			{

-				layerWeight = ( s * flWeight ) / ( 1 - flWeight + s * flWeight );

-			}

-			else if (pLayer->flags & STUDIO_AL_NOBLEND)

-			{

-				layerWeight = s;

-			}

-			else

-			{

-				layerWeight = flWeight * s;

-			}

-

-			layerCycle = (cycle - pLayer->start) / (pLayer->end - pLayer->start);

-		}

-

-		int iSequence = m_pStudioHdr->iRelativeSeq( sequence, pLayer->iSequence );

-		AccumulatePose( pos, q, iSequence, layerCycle, layerWeight, flTime, pIKContext );

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: my sleezy attempt at an interface only class

-//-----------------------------------------------------------------------------

-

-IBoneSetup::IBoneSetup( const CStudioHdr *pStudioHdr, int boneMask, const float poseParameter[], IPoseDebugger *pPoseDebugger )

-{

-	m_pBoneSetup = new CBoneSetup( pStudioHdr, boneMask, poseParameter, pPoseDebugger );

-}

-

-IBoneSetup::~IBoneSetup( void )

-{

-	if ( m_pBoneSetup )

-	{

-		delete m_pBoneSetup;

-	}

-}

-

-void IBoneSetup::InitPose( Vector pos[], Quaternion q[] )

-{

-	::InitPose( m_pBoneSetup->m_pStudioHdr, pos, q, m_pBoneSetup->m_boneMask );

-}

-

-void IBoneSetup::AccumulatePose( Vector pos[], Quaternion q[], int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext )

-{

-	m_pBoneSetup->AccumulatePose( pos, q, sequence, cycle, flWeight, flTime, pIKContext );

-}

-

-void IBoneSetup::CalcAutoplaySequences(	Vector pos[], Quaternion q[], float flRealTime, CIKContext *pIKContext )

-{

-	m_pBoneSetup->CalcAutoplaySequences( pos, q, flRealTime, pIKContext );

-}

-

-void CalcBoneAdj( const CStudioHdr *pStudioHdr, Vector pos[], Quaternion q[], const float controllers[], int boneMask );

-

-// takes a "controllers[]" array normalized to 0..1 and adds in the adjustments to pos[], and q[].

-void IBoneSetup::CalcBoneAdj( Vector pos[], Quaternion q[], const float controllers[] )

-{

-	::CalcBoneAdj( m_pBoneSetup->m_pStudioHdr, pos, q, controllers, m_pBoneSetup->m_boneMask );

-}

-

-CStudioHdr *IBoneSetup::GetStudioHdr()

-{

-	return (CStudioHdr *)m_pBoneSetup->m_pStudioHdr;

-}

-

-CBoneSetup::CBoneSetup( const CStudioHdr *pStudioHdr, int boneMask, const float poseParameter[], IPoseDebugger *pPoseDebugger )

-{

-	m_pStudioHdr = pStudioHdr;

-	m_boneMask = boneMask;

-	m_flPoseParameter = poseParameter;

-	m_pPoseDebugger = pPoseDebugger;

-}

-

-#if 0

-//-----------------------------------------------------------------------------

-// Purpose: calculate a pose for a single sequence

-//			adds autolayers, runs local ik rukes

-//-----------------------------------------------------------------------------

-void CalcPose(

-	const CStudioHdr *pStudioHdr,

-	CIKContext *pIKContext,

-	Vector pos[], 

-	Quaternion q[], 

-	int sequence, 

-	float cycle,

-	const float poseParameter[],

-	int boneMask,

-	float flWeight,

-	float flTime

-	)

-{

-	mstudioseqdesc_t	&seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( sequence );

-

-	Assert( flWeight >= 0.0f && flWeight <= 1.0f );

-	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up

-	flWeight = clamp( flWeight, 0.0f, 1.0f );

-

-	// add any IK locks to prevent numautolayers from moving extremities 

-	CIKContext seq_ik;

-	if (seqdesc.numiklocks)

-	{

-		seq_ik.Init( pStudioHdr, vec3_angle, vec3_origin, 0.0, 0, boneMask ); // local space relative so absolute position doesn't mater

-		seq_ik.AddSequenceLocks( seqdesc, pos, q );

-	}

-

-	CalcPoseSingle( pStudioHdr, pos, q, seqdesc, sequence, cycle, poseParameter, boneMask, flTime );

-

-	if ( pIKContext )

-	{

-		pIKContext->AddDependencies( seqdesc, sequence, cycle, poseParameter, flWeight );

-	}

-	

-	AddSequenceLayers( pStudioHdr, pIKContext, pos, q, seqdesc, sequence, cycle, poseParameter, boneMask, flWeight, flTime );

-

-	if (seqdesc.numiklocks)

-	{

-		seq_ik.SolveSequenceLocks( seqdesc, pos, q );

-	}

-}

-#endif

-

-//-----------------------------------------------------------------------------

-// Purpose: accumulate a pose for a single sequence on top of existing animation

-//			adds autolayers, runs local ik rukes

-//-----------------------------------------------------------------------------

-void CBoneSetup::AccumulatePose(

-	Vector pos[], 

-	Quaternion q[], 

-	int sequence, 

-	float cycle,

-	float flWeight,

-	float flTime,

-	CIKContext *pIKContext

-	)

-{

-	Vector		pos2[MAXSTUDIOBONES];

-	QuaternionAligned	q2[MAXSTUDIOBONES];

-

-	Assert( flWeight >= 0.0f && flWeight <= 1.0f );

-	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up

-	flWeight = clamp( flWeight, 0.0f, 1.0f );

-

-	if ( sequence < 0 )

-		return;

-

-#ifdef CLIENT_DLL

-	// Trigger pose debugger

-	if (m_pPoseDebugger)

-	{

-		m_pPoseDebugger->AccumulatePose( m_pStudioHdr, pIKContext, pos, q, sequence, cycle, m_flPoseParameter, m_boneMask, flWeight, flTime );

-	}

-#endif

-

-	mstudioseqdesc_t	&seqdesc = ((CStudioHdr *)m_pStudioHdr)->pSeqdesc( sequence );

-

-	// add any IK locks to prevent extremities from moving

-	CIKContext seq_ik;

-	if (seqdesc.numiklocks)

-	{

-		seq_ik.Init( m_pStudioHdr, vec3_angle, vec3_origin, 0.0, 0, m_boneMask );  // local space relative so absolute position doesn't mater

-		seq_ik.AddSequenceLocks( seqdesc, pos, q );

-	}

-

-	if (seqdesc.flags & STUDIO_LOCAL)

-	{

-		::InitPose( m_pStudioHdr, pos2, q2, m_boneMask );

-	}

-

-	if (CalcPoseSingle( m_pStudioHdr, pos2, q2, seqdesc, sequence, cycle, m_flPoseParameter, m_boneMask, flTime ))

-	{

-		// this weight is wrong, the IK rules won't composite at the correct intensity

-		AddLocalLayers( pos2, q2, seqdesc, sequence, cycle, 1.0, flTime, pIKContext );

-		SlerpBones( m_pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, flWeight, m_boneMask );

-	}

-

-

-	if ( pIKContext )

-	{

-		pIKContext->AddDependencies( seqdesc, sequence, cycle, m_flPoseParameter, flWeight );

-	}

-

-	AddSequenceLayers( pos, q, seqdesc, sequence, cycle, flWeight, flTime, pIKContext );

-

-	if (seqdesc.numiklocks)

-	{

-		seq_ik.SolveSequenceLocks( seqdesc, pos, q );

-	}

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: blend together q1,pos1 with q2,pos2.  Return result in q1,pos1.  

-//			0 returns q1, pos1.  1 returns q2, pos2

-//-----------------------------------------------------------------------------

-void CalcBoneAdj(

-	const CStudioHdr *pStudioHdr,

-	Vector pos[], 

-	Quaternion q[], 

-	const float controllers[],

-	int boneMask

-	)

-{

-	int					i, j, k;

-	float				value;

-	mstudiobonecontroller_t *pbonecontroller;

-	Vector p0;

-	RadianEuler a0;

-	Quaternion q0;

-	

-	for (j = 0; j < pStudioHdr->numbonecontrollers(); j++)

-	{

-		pbonecontroller = pStudioHdr->pBonecontroller( j );

-		k = pbonecontroller->bone;

-

-		if (pStudioHdr->boneFlags( k ) & boneMask)

-		{

-			i = pbonecontroller->inputfield;

-			value = controllers[i];

-			if (value < 0) value = 0;

-			if (value > 1.0) value = 1.0;

-			value = (1.0 - value) * pbonecontroller->start + value * pbonecontroller->end;

-

-			switch(pbonecontroller->type & STUDIO_TYPES)

-			{

-			case STUDIO_XR: 

-				a0.Init( value * (M_PI / 180.0), 0, 0 ); 

-				AngleQuaternion( a0, q0 );

-				QuaternionSM( 1.0, q0, q[k], q[k] );

-				break;

-			case STUDIO_YR: 

-				a0.Init( 0, value * (M_PI / 180.0), 0 ); 

-				AngleQuaternion( a0, q0 );

-				QuaternionSM( 1.0, q0, q[k], q[k] );

-				break;

-			case STUDIO_ZR: 

-				a0.Init( 0, 0, value * (M_PI / 180.0) ); 

-				AngleQuaternion( a0, q0 );

-				QuaternionSM( 1.0, q0, q[k], q[k] );

-				break;

-			case STUDIO_X:	

-				pos[k].x += value;

-				break;

-			case STUDIO_Y:

-				pos[k].y += value;

-				break;

-			case STUDIO_Z:

-				pos[k].z += value;

-				break;

-			}

-		}

-	}

-}

-

-

-void CalcBoneDerivatives( Vector &velocity, AngularImpulse &angVel, const matrix3x4_t &prev, const matrix3x4_t &current, float dt )

-{

-	float scale = 1.0;

-	if ( dt > 0 )

-	{

-		scale = 1.0 / dt;

-	}

-	

-	Vector endPosition, startPosition, deltaAxis;

-	QAngle endAngles, startAngles;

-	float deltaAngle;

-

-	MatrixAngles( prev, startAngles, startPosition );

-	MatrixAngles( current, endAngles, endPosition );

-

-	velocity.x = (endPosition.x - startPosition.x) * scale;

-	velocity.y = (endPosition.y - startPosition.y) * scale;

-	velocity.z = (endPosition.z - startPosition.z) * scale;

-	RotationDeltaAxisAngle( startAngles, endAngles, deltaAxis, deltaAngle );

-	VectorScale( deltaAxis, (deltaAngle * scale), angVel );

-}

-

-void CalcBoneVelocityFromDerivative( const QAngle &vecAngles, Vector &velocity, AngularImpulse &angVel, const matrix3x4_t &current )

-{

-	Vector vecLocalVelocity;

-	AngularImpulse LocalAngVel;

-	Quaternion q;

-	float angle;

-	MatrixAngles( current, q, vecLocalVelocity );

-	QuaternionAxisAngle( q, LocalAngVel, angle );

-	LocalAngVel *= angle;

-

-	matrix3x4_t matAngles;

-	AngleMatrix( vecAngles, matAngles );

-	VectorTransform( vecLocalVelocity, matAngles, velocity );

-	VectorTransform( LocalAngVel, matAngles, angVel );

-}

-

-

-

-

-class CIKSolver

-{

-public:

-//-------- SOLVE TWO LINK INVERSE KINEMATICS -------------

-// Author: Ken Perlin

-//

-// Given a two link joint from [0,0,0] to end effector position P,

-// let link lengths be a and b, and let norm |P| = c.  Clearly a+b <= c.

-//

-// Problem: find a "knee" position Q such that |Q| = a and |P-Q| = b.

-//

-// In the case of a point on the x axis R = [c,0,0], there is a

-// closed form solution S = [d,e,0], where |S| = a and |R-S| = b:

-//

-//    d2+e2 = a2                  -- because |S| = a

-//    (c-d)2+e2 = b2              -- because |R-S| = b

-//

-//    c2-2cd+d2+e2 = b2           -- combine the two equations

-//    c2-2cd = b2 - a2

-//    c-2d = (b2-a2)/c

-//    d - c/2 = (a2-b2)/c / 2

-//

-//    d = (c + (a2-b2/c) / 2      -- to solve for d and e.

-//    e = sqrt(a2-d2)

-

-   static float findD(float a, float b, float c) {

-      return (c + (a*a-b*b)/c) / 2;

-   }

-   static float findE(float a, float d) { return sqrt(a*a-d*d); } 

-

-// This leads to a solution to the more general problem:

-//

-//   (1) R = Mfwd(P)         -- rotate P onto the x axis

-//   (2) Solve for S

-//   (3) Q = Minv(S)         -- rotate back again

-

-   float Mfwd[3][3];

-   float Minv[3][3];

-

-   bool solve(float A, float B, float const P[], float const D[], float Q[]) {

-      float R[3];

-      defineM(P,D);

-      rot(Minv,P,R);

-	  float r = length(R);

-      float d = findD(A,B,r);

-      float e = findE(A,d);

-      float S[3] = {d,e,0};

-      rot(Mfwd,S,Q);

-      return d > (r - B) && d < A;

-   }

-

-// If "knee" position Q needs to be as close as possible to some point D,

-// then choose M such that M(D) is in the y>0 half of the z=0 plane.

-//

-// Given that constraint, define the forward and inverse of M as follows:

-

-   void defineM(float const P[], float const D[]) {

-      float *X = Minv[0], *Y = Minv[1], *Z = Minv[2];

-

-// Minv defines a coordinate system whose x axis contains P, so X = unit(P).

-	  int i;

-      for (i = 0 ; i < 3 ; i++)

-         X[i] = P[i];

-      normalize(X);

-

-// Its y axis is perpendicular to P, so Y = unit( E - X(E�X) ).

-

-      float dDOTx = dot(D,X);

-      for (i = 0 ; i < 3 ; i++)

-         Y[i] = D[i] - dDOTx * X[i];

-      normalize(Y);

-

-// Its z axis is perpendicular to both X and Y, so Z = X�Y.

-

-      cross(X,Y,Z);

-

-// Mfwd = (Minv)T, since transposing inverts a rotation matrix.

-

-      for (i = 0 ; i < 3 ; i++) {

-         Mfwd[i][0] = Minv[0][i];

-         Mfwd[i][1] = Minv[1][i];

-         Mfwd[i][2] = Minv[2][i];

-      }

-   }

-

-//------------ GENERAL VECTOR MATH SUPPORT -----------

-

-   static float dot(float const a[], float const b[]) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; }

-

-   static float length(float const v[]) { return sqrt( dot(v,v) ); }

-

-   static void normalize(float v[]) {

-      float norm = length(v);

-      for (int i = 0 ; i < 3 ; i++)

-         v[i] /= norm;

-   }

-

-   static void cross(float const a[], float const b[], float c[]) {

-      c[0] = a[1] * b[2] - a[2] * b[1];

-      c[1] = a[2] * b[0] - a[0] * b[2];

-      c[2] = a[0] * b[1] - a[1] * b[0];

-   }

-

-   static void rot(float const M[3][3], float const src[], float dst[]) {

-      for (int i = 0 ; i < 3 ; i++)

-         dst[i] = dot(M[i],src);

-   }

-};

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: visual debugging code

-//-----------------------------------------------------------------------------

-#if 1

-inline void debugLine(const Vector& origin, const Vector& dest, int r, int g, int b, bool noDepthTest, float duration) { };

-#else

-extern void drawLine( const Vector &p1, const Vector &p2, int r = 0, int g = 0, int b = 1, bool noDepthTest = true, float duration = 0.1 );

-void debugLine(const Vector& origin, const Vector& dest, int r, int g, int b, bool noDepthTest, float duration)

-{

-	drawLine( origin, dest, r, g, b, noDepthTest, duration );

-}

-#endif

-

-

-//-----------------------------------------------------------------------------

-// Purpose: for a 2 bone chain, find the IK solution and reset the matrices

-//-----------------------------------------------------------------------------

-bool Studio_SolveIK( mstudioikchain_t *pikchain, Vector &targetFoot, matrix3x4_t *pBoneToWorld )

-{

-	if (pikchain->pLink(0)->kneeDir.LengthSqr() > 0.0)

-	{

-		Vector targetKneeDir, targetKneePos;

-		// FIXME: knee length should be as long as the legs

-		Vector tmp = pikchain->pLink( 0 )->kneeDir;

-		VectorRotate( tmp, pBoneToWorld[ pikchain->pLink( 0 )->bone ], targetKneeDir );

-		MatrixPosition( pBoneToWorld[ pikchain->pLink( 1 )->bone ], targetKneePos );

-		return Studio_SolveIK( pikchain->pLink( 0 )->bone, pikchain->pLink( 1 )->bone, pikchain->pLink( 2 )->bone, targetFoot, targetKneePos, targetKneeDir, pBoneToWorld );

-	}

-	else

-	{

-		return Studio_SolveIK( pikchain->pLink( 0 )->bone, pikchain->pLink( 1 )->bone, pikchain->pLink( 2 )->bone, targetFoot, pBoneToWorld );

-	}

-}

-

-

-#define KNEEMAX_EPSILON 0.9998 // (0.9998 is about 1 degree)

-

-//-----------------------------------------------------------------------------

-// Purpose: Solve Knee position for a known hip and foot location, but no specific knee direction preference

-//-----------------------------------------------------------------------------

-

-bool Studio_SolveIK( int iThigh, int iKnee, int iFoot, Vector &targetFoot, matrix3x4_t *pBoneToWorld )

-{

-	Vector worldFoot, worldKnee, worldThigh;

-

-	MatrixPosition( pBoneToWorld[ iThigh ], worldThigh );

-	MatrixPosition( pBoneToWorld[ iKnee ], worldKnee );

-	MatrixPosition( pBoneToWorld[ iFoot ], worldFoot );

-

-	//debugLine( worldThigh, worldKnee, 0, 0, 255, true, 0 );

-	//debugLine( worldKnee, worldFoot, 0, 0, 255, true, 0 );

-

-	Vector ikFoot, ikKnee;

-

-	ikFoot = targetFoot - worldThigh;

-	ikKnee = worldKnee - worldThigh;

-

-	float l1 = (worldKnee-worldThigh).Length();

-	float l2 = (worldFoot-worldKnee).Length();

-	float l3 = (worldFoot-worldThigh).Length();

-

-	// leg too straight to figure out knee?

-	if (l3 > (l1 + l2) * KNEEMAX_EPSILON)

-	{

-		return false;

-	}

-

-	Vector ikHalf = (worldFoot-worldThigh) * (l1 / l3);

-

-	// FIXME: what to do when the knee completely straight?

-	Vector ikKneeDir = ikKnee - ikHalf;

-	VectorNormalize( ikKneeDir );

-

-	return Studio_SolveIK( iThigh, iKnee, iFoot, targetFoot, worldKnee, ikKneeDir, pBoneToWorld );

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: Realign the matrix so that its X axis points along the desired axis.

-//-----------------------------------------------------------------------------

-void Studio_AlignIKMatrix( matrix3x4_t &mMat, const Vector &vAlignTo )

-{

-	Vector tmp1, tmp2, tmp3;

-

-	// Column 0 (X) becomes the vector.

-	tmp1 = vAlignTo;

-	VectorNormalize( tmp1 );

-	MatrixSetColumn( tmp1, 0, mMat );

-

-	// Column 1 (Y) is the cross of the vector and column 2 (Z).

-	MatrixGetColumn( mMat, 2, tmp3 );

-	tmp2 = tmp3.Cross( tmp1 );

-	VectorNormalize( tmp2 );

-	// FIXME: check for X being too near to Z

-	MatrixSetColumn( tmp2, 1, mMat );

-

-	// Column 2 (Z) is the cross of columns 0 (X) and 1 (Y).

-	tmp3 = tmp1.Cross( tmp2 );

-	MatrixSetColumn( tmp3, 2, mMat );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Solve Knee position for a known hip and foot location, and a known knee direction

-//-----------------------------------------------------------------------------

-

-bool Studio_SolveIK( int iThigh, int iKnee, int iFoot, Vector &targetFoot, Vector &targetKneePos, Vector &targetKneeDir, matrix3x4_t *pBoneToWorld )

-{

-	Vector worldFoot, worldKnee, worldThigh;

-

-	MatrixPosition( pBoneToWorld[ iThigh ], worldThigh );

-	MatrixPosition( pBoneToWorld[ iKnee ], worldKnee );

-	MatrixPosition( pBoneToWorld[ iFoot ], worldFoot );

-

-	//debugLine( worldThigh, worldKnee, 0, 0, 255, true, 0 );

-	//debugLine( worldThigh, worldThigh + targetKneeDir, 0, 0, 255, true, 0 );

-	// debugLine( worldKnee, targetKnee, 0, 0, 255, true, 0 );

-

-	Vector ikFoot, ikTargetKnee, ikKnee;

-

-	ikFoot = targetFoot - worldThigh;

-	ikKnee = targetKneePos - worldThigh;

-

-	float l1 = (worldKnee-worldThigh).Length();

-	float l2 = (worldFoot-worldKnee).Length();

-

-	// exaggerate knee targets for legs that are nearly straight

-	// FIXME: should be configurable, and the ikKnee should be from the original animation, not modifed

-	float d = (targetFoot-worldThigh).Length() - min( l1, l2 );

-	d = max( l1 + l2, d );

-	// FIXME: too short knee directions cause trouble

-	d = d * 100;

-

-	ikTargetKnee = ikKnee + targetKneeDir * d;

-

-	// debugLine( worldKnee, worldThigh + ikTargetKnee, 0, 0, 255, true, 0 );

-

-	int color[3] = { 0, 255, 0 };

-

-	// too far away? (0.9998 is about 1 degree)

-	if (ikFoot.Length() > (l1 + l2) * KNEEMAX_EPSILON)

-	{

-		VectorNormalize( ikFoot );

-		VectorScale( ikFoot, (l1 + l2) * KNEEMAX_EPSILON, ikFoot );

-		color[0] = 255; color[1] = 0; color[2] = 0;

-	}

-

-	// too close?

-	// limit distance to about an 80 degree knee bend

-	float minDist = max( fabs(l1 - l2) * 1.15, min( l1, l2 ) * 0.15 );

-	if (ikFoot.Length() < minDist)

-	{

-		// too close to get an accurate vector, just use original vector

-		ikFoot = (worldFoot - worldThigh);

-		VectorNormalize( ikFoot );

-		VectorScale( ikFoot, minDist, ikFoot );

-	}

-

-	CIKSolver ik;

-	if (ik.solve( l1, l2, ikFoot.Base(), ikTargetKnee.Base(), ikKnee.Base() ))

-	{

-		matrix3x4_t& mWorldThigh = pBoneToWorld[ iThigh ];

-		matrix3x4_t& mWorldKnee = pBoneToWorld[ iKnee ];

-		matrix3x4_t& mWorldFoot = pBoneToWorld[ iFoot ];

-

-		//debugLine( worldThigh, ikKnee + worldThigh, 255, 0, 0, true, 0 );

-		//debugLine( ikKnee + worldThigh, ikFoot + worldThigh, 255, 0, 0, true,0 );

-

-		// debugLine( worldThigh, ikKnee + worldThigh, color[0], color[1], color[2], true, 0 );

-		// debugLine( ikKnee + worldThigh, ikFoot + worldThigh, color[0], color[1], color[2], true,0 );

-

-

-		// build transformation matrix for thigh

-		Studio_AlignIKMatrix( mWorldThigh, ikKnee );

-		Studio_AlignIKMatrix( mWorldKnee, ikFoot - ikKnee );

-

-

-		mWorldKnee[0][3] = ikKnee.x + worldThigh.x;

-		mWorldKnee[1][3] = ikKnee.y + worldThigh.y;

-		mWorldKnee[2][3] = ikKnee.z + worldThigh.z;

-

-		mWorldFoot[0][3] = ikFoot.x + worldThigh.x;

-		mWorldFoot[1][3] = ikFoot.y + worldThigh.y;

-		mWorldFoot[2][3] = ikFoot.z + worldThigh.z;

-

-		return true;

-	}

-	else

-	{

-		/*

-		debugLine( worldThigh, worldThigh + ikKnee, 255, 0, 0, true, 0 );

-		debugLine( worldThigh + ikKnee, worldThigh + ikFoot, 255, 0, 0, true, 0 );

-		debugLine( worldThigh + ikFoot, worldThigh, 255, 0, 0, true, 0 );

-		debugLine( worldThigh + ikKnee, worldThigh + ikTargetKnee, 255, 0, 0, true, 0 );

-		*/

-		return false;

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-float Studio_IKRuleWeight( mstudioikrule_t &ikRule, const mstudioanimdesc_t *panim, float flCycle, int &iFrame, float &fraq )

-{

-	if (ikRule.end > 1.0f && flCycle < ikRule.start)

-	{

-		flCycle = flCycle + 1.0f;

-	}

-

-	float value = 0.0f;

-	fraq = (panim->numframes - 1) * (flCycle - ikRule.start) + ikRule.iStart;

-	iFrame = (int)fraq;

-	fraq = fraq - iFrame;

-

-	if (flCycle < ikRule.start)

-	{

-		iFrame = ikRule.iStart;

-		fraq = 0.0f;

-		return 0.0f;

-	}

-	else if (flCycle < ikRule.peak )

-	{

-		value = (flCycle - ikRule.start) / (ikRule.peak - ikRule.start);

-	}

-	else if (flCycle < ikRule.tail )

-	{

-		return 1.0f;

-	}

-	else if (flCycle < ikRule.end )

-	{

-		value = 1.0f - ((flCycle - ikRule.tail) / (ikRule.end - ikRule.tail));

-	}

-	else

-	{

-		fraq = (panim->numframes - 1) * (ikRule.end - ikRule.start) + ikRule.iStart;

-		iFrame = (int)fraq;

-		fraq = fraq - iFrame;

-	}

-	return SimpleSpline( value );

-}

-

-

-float Studio_IKRuleWeight( ikcontextikrule_t &ikRule, float flCycle )

-{

-	if (ikRule.end > 1.0f && flCycle < ikRule.start)

-	{

-		flCycle = flCycle + 1.0f;

-	}

-

-	float value = 0.0f;

-	if (flCycle < ikRule.start)

-	{

-		return 0.0f;

-	}

-	else if (flCycle < ikRule.peak )

-	{

-		value = (flCycle - ikRule.start) / (ikRule.peak - ikRule.start);

-	}

-	else if (flCycle < ikRule.tail )

-	{

-		return 1.0f;

-	}

-	else if (flCycle < ikRule.end )

-	{

-		value = 1.0f - ((flCycle - ikRule.tail) / (ikRule.end - ikRule.tail));

-	}

-	return 3.0f * value * value - 2.0f * value * value * value;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-bool Studio_IKShouldLatch( ikcontextikrule_t &ikRule, float flCycle )

-{

-	if (ikRule.end > 1.0f && flCycle < ikRule.start)

-	{

-		flCycle = flCycle + 1.0f;

-	}

-

-	if (flCycle < ikRule.peak )

-	{

-		return false;

-	}

-	else if (flCycle < ikRule.end )

-	{

-		return true;

-	}

-	return false;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-float Studio_IKTail( ikcontextikrule_t &ikRule, float flCycle )

-{

-	if (ikRule.end > 1.0f && flCycle < ikRule.start)

-	{

-		flCycle = flCycle + 1.0f;

-	}

-

-	if (flCycle <= ikRule.tail )

-	{

-		return 0.0f;

-	}

-	else if (flCycle < ikRule.end )

-	{

-		return ((flCycle - ikRule.tail) / (ikRule.end - ikRule.tail));

-	}

-	return 0.0;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-

-bool Studio_IKAnimationError( const CStudioHdr *pStudioHdr, mstudioikrule_t *pRule, const mstudioanimdesc_t *panim, float flCycle, Vector &pos, Quaternion &q, float &flWeight )

-{

-	float fraq;

-	int iFrame;

-

-	flWeight = Studio_IKRuleWeight( *pRule, panim, flCycle, iFrame, fraq );

-	Assert( fraq >= 0.0 && fraq < 1.0 );

-	Assert( flWeight >= 0.0f && flWeight <= 1.0f );

-

-	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up

-	flWeight = clamp( flWeight, 0.0f, 1.0f );

-

-	if (pRule->type != IK_GROUND && flWeight < 0.0001)

-		return false;

-

-	mstudioikerror_t *pError = pRule->pError( iFrame );

-	if (pError != NULL)

-	{

-		if (fraq < 0.001)

-		{

-			q = pError[0].q;

-			pos = pError[0].pos;

-		}

-		else

-		{

-			QuaternionBlend( pError[0].q, pError[1].q, fraq, q );

-			pos = pError[0].pos * (1.0f - fraq) + pError[1].pos * fraq;

-		}

-		return true;

-	}

-

-	mstudiocompressedikerror_t *pCompressed = pRule->pCompressedError();

-	if (pCompressed != NULL)

-	{

-		CalcDecompressedAnimation( pCompressed, iFrame - pRule->iStart, fraq, pos, q );

-		return true;

-	}

-	// no data, disable IK rule

-	Assert( 0 );

-	flWeight = 0.0f;

-	return false;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: For a specific sequence:rule, find where it starts, stops, and what 

-//			the estimated offset from the connection point is.

-//			return true if the rule is within bounds.

-//-----------------------------------------------------------------------------

-

-bool Studio_IKSequenceError( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, float flCycle, int iRule, const float poseParameter[], mstudioanimdesc_t *panim[4], float weight[4], ikcontextikrule_t &ikRule )

-{

-	int i;

-

-	memset( &ikRule, 0, sizeof(ikRule) );

-	ikRule.start = ikRule.peak = ikRule.tail = ikRule.end = 0;

-

-

-	mstudioikrule_t *prevRule = NULL;

-

-	// find overall influence

-	for (i = 0; i < 4; i++)

-	{

-		if (weight[i])

-		{

-			if (iRule >= panim[i]->numikrules || panim[i]->numikrules != panim[0]->numikrules)

-			{

-				Assert( 0 );

-				return false;

-			}

-

-			mstudioikrule_t *pRule = panim[i]->pIKRule( iRule );

-			if (pRule == NULL)

-				return false;

-

-			float dt = 0.0;

-			if (prevRule != NULL)

-			{

-				if (pRule->start - prevRule->start > 0.5)

-				{

-					dt = -1.0;

-				}

-				else if (pRule->start - prevRule->start < -0.5)

-				{

-					dt = 1.0;

-				}

-			}

-			else

-			{

-				prevRule = pRule;

-			}

-

-			ikRule.start += (pRule->start + dt) * weight[i];

-			ikRule.peak += (pRule->peak + dt) * weight[i];

-			ikRule.tail += (pRule->tail + dt) * weight[i];

-			ikRule.end += (pRule->end + dt) * weight[i];

-		}

-	}

-	if (ikRule.start > 1.0)

-	{

-		ikRule.start -= 1.0;

-		ikRule.peak -= 1.0;

-		ikRule.tail -= 1.0;

-		ikRule.end -= 1.0;

-	}

-	else if (ikRule.start < 0.0)

-	{

-		ikRule.start += 1.0;

-		ikRule.peak += 1.0;

-		ikRule.tail += 1.0;

-		ikRule.end += 1.0;

-	}

-

-	ikRule.flWeight = Studio_IKRuleWeight( ikRule, flCycle );

-	if (ikRule.flWeight <= 0.001f)

-	{

-		// go ahead and allow IK_GROUND rules a virtual looping section

-		if ( panim[0]->pIKRule( iRule ) == NULL ) 

-			return false;

-		if ((panim[0]->flags & STUDIO_LOOPING) && panim[0]->pIKRule( iRule )->type == IK_GROUND && ikRule.end - ikRule.start > 0.75 )

-		{

-			ikRule.flWeight = 0.001;

-			flCycle = ikRule.end - 0.001;

-		}

-		else

-		{

-			return false;

-		}

-	}

-

-	Assert( ikRule.flWeight > 0.0f );

-

-	ikRule.pos.Init();

-	ikRule.q.Init();

-

-	// find target error

-	float total = 0.0f;

-	for (i = 0; i < 4; i++)

-	{

-		if (weight[i])

-		{

-			Vector pos1;

-			Quaternion q1;

-			float w;

-

-			mstudioikrule_t *pRule = panim[i]->pIKRule( iRule );

-			if (pRule == NULL)

-				return false;

-

-			ikRule.chain = pRule->chain;	// FIXME: this is anim local

-			ikRule.bone = pRule->bone;		// FIXME: this is anim local

-			ikRule.type = pRule->type;

-			ikRule.slot = pRule->slot;

-

-			ikRule.height += pRule->height * weight[i];

-			ikRule.floor += pRule->floor * weight[i];

-			ikRule.radius += pRule->radius * weight[i];

-			ikRule.drop += pRule->drop * weight[i];

-			ikRule.top += pRule->top * weight[i];

-

-			// keep track of tail condition

-			ikRule.release += Studio_IKTail( ikRule, flCycle ) * weight[i];

-

-			// only check rules with error values

-			switch( ikRule.type )

-			{

-			case IK_SELF:

-			case IK_WORLD:

-			case IK_GROUND:

-			case IK_ATTACHMENT:

-				{

-					int bResult = Studio_IKAnimationError( pStudioHdr, pRule, panim[i], flCycle, pos1, q1, w );

-

-					if (bResult)

-					{

-						ikRule.pos = ikRule.pos + pos1 * weight[i];

-						QuaternionAccumulate( ikRule.q, weight[i], q1, ikRule.q );

-						total += weight[i];

-					}

-				}

-				break;

-			default:

-				total += weight[i];

-				break;

-			}

-

-			ikRule.latched = Studio_IKShouldLatch( ikRule, flCycle ) * ikRule.flWeight;

-

-			if (ikRule.type == IK_ATTACHMENT)

-			{

-				ikRule.szLabel = pRule->pszAttachment();

-			}

-		}

-	}

-

-	if (total <= 0.0001f)

-	{

-		return false;

-	}

-

-	if (total < 0.999f)

-	{

-		VectorScale( ikRule.pos, 1.0f / total, ikRule.pos );

-		QuaternionScale( ikRule.q, 1.0f / total, ikRule.q );

-	}

-

-	if (ikRule.type == IK_SELF && ikRule.bone != -1)

-	{

-		// FIXME: this is anim local, not seq local!

-		ikRule.bone = pStudioHdr->RemapSeqBone( iSequence, ikRule.bone );

-		if (ikRule.bone == -1)

-			return false;

-	}

-

-	QuaternionNormalize( ikRule.q );

-	return true;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-

-CIKContext::CIKContext()

-{

-	m_target.EnsureCapacity( 12 ); // FIXME: this sucks, shouldn't it be grown?

-	m_iFramecounter = -1;

-	m_pStudioHdr = NULL;

-	m_flTime = -1.0f;

-	m_target.SetSize( 0 );

-}

-

-

-void CIKContext::Init( const CStudioHdr *pStudioHdr, const QAngle &angles, const Vector &pos, float flTime, int iFramecounter, int boneMask )

-{

-	m_pStudioHdr = pStudioHdr;

-	m_ikChainRule.RemoveAll(); // m_numikrules = 0;

-	if (pStudioHdr->numikchains())

-	{

-		m_ikChainRule.SetSize( pStudioHdr->numikchains() );

-

-		// FIXME: Brutal hackery to prevent a crash

-		if (m_target.Count() == 0)

-		{

-			m_target.SetSize(12);

-			memset( m_target.Base(), 0, sizeof(m_target[0])*m_target.Count() );

-			ClearTargets();

-		}

-

-	}

-	else

-	{

-		m_target.SetSize( 0 );

-	}

-	AngleMatrix( angles, pos, m_rootxform );

-	m_iFramecounter = iFramecounter;

-	m_flTime = flTime;

-	m_boneMask = boneMask;

-}

-

-void CIKContext::AddDependencies( mstudioseqdesc_t &seqdesc, int iSequence, float flCycle, const float poseParameters[], float flWeight )

-{

-	int i;

-

-	if ( m_pStudioHdr->numikchains() == 0)

-		return;

-

-	if (seqdesc.numikrules == 0)

-		return;

-

-	ikcontextikrule_t ikrule;

-

-	Assert( flWeight >= 0.0f && flWeight <= 1.0f );

-	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up

-	flWeight = clamp( flWeight, 0.0f, 1.0f );

-

-	// unify this

-	if (seqdesc.flags & STUDIO_REALTIME)

-	{

-		float cps = Studio_CPS( m_pStudioHdr, seqdesc, iSequence, poseParameters );

-		flCycle = m_flTime * cps;

-		flCycle = flCycle - (int)flCycle;

-	}

-	else if (flCycle < 0 || flCycle >= 1)

-	{

-		if (seqdesc.flags & STUDIO_LOOPING)

-		{

-			flCycle = flCycle - (int)flCycle;

-			if (flCycle < 0) flCycle += 1;

-		}

-		else

-		{

-			flCycle = max( 0.0, min( flCycle, 0.9999 ) );

-		}

-	}

-

-	mstudioanimdesc_t *panim[4];

-	float	weight[4];

-

-	Studio_SeqAnims( m_pStudioHdr, seqdesc, iSequence, poseParameters, panim, weight );

-

-	// FIXME: add proper number of rules!!!

-	for (i = 0; i < seqdesc.numikrules; i++)

-	{

-		if ( !Studio_IKSequenceError( m_pStudioHdr, seqdesc, iSequence, flCycle, i, poseParameters, panim, weight, ikrule ) )

-			continue;

-

-		// don't add rule if the bone isn't going to be calculated

-		int bone = m_pStudioHdr->pIKChain( ikrule.chain )->pLink( 2 )->bone;

-		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))

-			continue;

-

-		// or if its relative bone isn't going to be calculated

-		if ( ikrule.bone >= 0 && !(m_pStudioHdr->boneFlags( ikrule.bone ) & m_boneMask))

-			continue;

-

-		// FIXME: Brutal hackery to prevent a crash

-		if (m_target.Count() == 0)

-		{

-			m_target.SetSize(12);

-			memset( m_target.Base(), 0, sizeof(m_target[0])*m_target.Count() );

-			ClearTargets();

-		}

-

-		ikrule.flRuleWeight = flWeight;

-

-		if (ikrule.flRuleWeight * ikrule.flWeight > 0.999)

-		{

-			if ( ikrule.type != IK_UNLATCH)

-			{

-				// clear out chain if rule is 100%

-				m_ikChainRule.Element( ikrule.chain ).RemoveAll( );

-				if ( ikrule.type == IK_RELEASE)

-				{

-					continue;

-				}

-			}

-		}

-

- 		int nIndex = m_ikChainRule.Element( ikrule.chain ).AddToTail( );

-  		m_ikChainRule.Element( ikrule.chain ).Element( nIndex ) = ikrule;

-	}

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKContext::AddAutoplayLocks( Vector pos[], Quaternion q[] )

-{

-	// skip all array access if no autoplay locks.

-	if (m_pStudioHdr->GetNumIKAutoplayLocks() == 0)

-	{

-		return;

-	}

-

-	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList boneComputed;

-

-	int ikOffset = m_ikLock.AddMultipleToTail( m_pStudioHdr->GetNumIKAutoplayLocks() );

-	memset( &m_ikLock[ikOffset], 0, sizeof(ikcontextikrule_t)*m_pStudioHdr->GetNumIKAutoplayLocks() );

-

-	for (int i = 0; i < m_pStudioHdr->GetNumIKAutoplayLocks(); i++)

-	{

-		const mstudioiklock_t &lock = ((CStudioHdr *)m_pStudioHdr)->pIKAutoplayLock( i );

-		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( lock.chain );

-		int bone = pchain->pLink( 2 )->bone;

-

-		// don't bother with iklock if the bone isn't going to be calculated

-		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))

-			continue;

-

-		// eval current ik'd bone

-		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );

-

-		ikcontextikrule_t &ikrule = m_ikLock[ i + ikOffset ];

-

-		ikrule.chain = lock.chain;

-		ikrule.slot = i;

-		ikrule.type = IK_WORLD;

-

-		MatrixAngles( boneToWorld[bone], ikrule.q, ikrule.pos );

-

-		// save off current knee direction

-		if (pchain->pLink(0)->kneeDir.LengthSqr() > 0.0)

-		{

-			Vector tmp = pchain->pLink( 0 )->kneeDir;

-			VectorRotate( pchain->pLink( 0 )->kneeDir, boneToWorld[ pchain->pLink( 0 )->bone ], ikrule.kneeDir );

-			MatrixPosition( boneToWorld[ pchain->pLink( 1 )->bone ], ikrule.kneePos ); 

-		}

-		else

-		{

-			ikrule.kneeDir.Init( );

-		}

-	}

-	g_MatrixPool.Free( boneToWorld );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKContext::AddSequenceLocks( mstudioseqdesc_t &seqdesc, Vector pos[], Quaternion q[] )

-{

-	if ( m_pStudioHdr->numikchains() == 0)

-	{

-		return;

-	}

-

-	if ( seqdesc.numiklocks == 0 )

-	{

-		return;

-	}

-

-	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList boneComputed;

-

-	int ikOffset = m_ikLock.AddMultipleToTail( seqdesc.numiklocks );

-	memset( &m_ikLock[ikOffset], 0, sizeof(ikcontextikrule_t) * seqdesc.numiklocks );

-

-	for (int i = 0; i < seqdesc.numiklocks; i++)

-	{

-		mstudioiklock_t *plock = seqdesc.pIKLock( i );

-		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( plock->chain );

-		int bone = pchain->pLink( 2 )->bone;

-

-		// don't bother with iklock if the bone isn't going to be calculated

-		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))

-			continue;

-

-		// eval current ik'd bone

-		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );

-

-		ikcontextikrule_t &ikrule = m_ikLock[i+ikOffset];

-		ikrule.chain = i;

-		ikrule.slot = i;

-		ikrule.type = IK_WORLD;

-

-		MatrixAngles( boneToWorld[bone], ikrule.q, ikrule.pos );

-

-		// save off current knee direction

-		if (pchain->pLink(0)->kneeDir.LengthSqr() > 0.0)

-		{

-			VectorRotate( pchain->pLink( 0 )->kneeDir, boneToWorld[ pchain->pLink( 0 )->bone ], ikrule.kneeDir );

-		}

-		else

-		{

-			ikrule.kneeDir.Init( );

-		}

-	}

-	g_MatrixPool.Free( boneToWorld );

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: build boneToWorld transforms for a specific bone

-//-----------------------------------------------------------------------------

-void CIKContext::BuildBoneChain(

-	const Vector pos[], 

-	const Quaternion q[], 

-	int	iBone,

-	matrix3x4_t *pBoneToWorld,

-	CBoneBitList &boneComputed )

-{

-	Assert( m_pStudioHdr->boneFlags( iBone ) & m_boneMask );

-	::BuildBoneChain( m_pStudioHdr, m_rootxform, pos, q, iBone, pBoneToWorld, boneComputed );

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: build boneToWorld transforms for a specific bone

-//-----------------------------------------------------------------------------

-void BuildBoneChain(

-	const CStudioHdr *pStudioHdr,

-	const matrix3x4_t &rootxform,

-	const Vector pos[], 

-	const Quaternion q[], 

-	int	iBone,

-	matrix3x4_t *pBoneToWorld,

-	CBoneBitList &boneComputed )

-{

-	if ( boneComputed.IsBoneMarked(iBone) )

-		return;

-

-	matrix3x4_t bonematrix;

-	QuaternionMatrix( q[iBone], pos[iBone], bonematrix );

-

-	int parent = pStudioHdr->boneParent( iBone );

-	if (parent == -1) 

-	{

-		ConcatTransforms( rootxform, bonematrix, pBoneToWorld[iBone] );

-	}

-	else

-	{

-		// evil recursive!!!

-		BuildBoneChain( pStudioHdr, rootxform, pos, q, parent, pBoneToWorld, boneComputed );

-		ConcatTransforms( pBoneToWorld[parent], bonematrix, pBoneToWorld[iBone]);

-	}

-	boneComputed.MarkBone(iBone);

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: turn a specific bones boneToWorld transform into a pos and q in parents bonespace

-//-----------------------------------------------------------------------------

-void SolveBone( 

-	const CStudioHdr *pStudioHdr,

-	int	iBone,

-	matrix3x4_t *pBoneToWorld,

-	Vector pos[], 

-	Quaternion q[]

-	)

-{

-	int iParent = pStudioHdr->boneParent( iBone );

-

-	matrix3x4_t worldToBone;

-	MatrixInvert( pBoneToWorld[iParent], worldToBone );

-

-	matrix3x4_t local;

-	ConcatTransforms( worldToBone, pBoneToWorld[iBone], local );

-

-	MatrixAngles( local, q[iBone], pos[iBone] );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-

-void CIKTarget::SetOwner( int entindex, const Vector &pos, const QAngle &angles )

-{

-	latched.owner = entindex;

-	latched.absOrigin = pos;

-	latched.absAngles = angles;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-

-void CIKTarget::ClearOwner( void )

-{

-	latched.owner = -1;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-

-int CIKTarget::GetOwner( void )

-{

-	return latched.owner;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: update the latched IK values that are in a moving frame of reference

-//-----------------------------------------------------------------------------

-

-void CIKTarget::UpdateOwner( int entindex, const Vector &pos, const QAngle &angles )

-{

-	if (pos == latched.absOrigin && angles == latched.absAngles)

-		return;

-

-	matrix3x4_t in, out;

-	AngleMatrix( angles, pos, in );

-	AngleIMatrix( latched.absAngles, latched.absOrigin, out );

-

-	matrix3x4_t tmp1, tmp2;

-	QuaternionMatrix( latched.q, latched.pos, tmp1 );

-	ConcatTransforms( out, tmp1, tmp2 );

-	ConcatTransforms( in, tmp2, tmp1 );

-	MatrixAngles( tmp1, latched.q, latched.pos );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: sets the ground position of an ik target

-//-----------------------------------------------------------------------------

-

-void CIKTarget::SetPos( const Vector &pos )

-{

-	est.pos = pos;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: sets the ground "identity" orientation of an ik target

-//-----------------------------------------------------------------------------

-

-void CIKTarget::SetAngles( const QAngle &angles )

-{

-	AngleQuaternion( angles, est.q );

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: sets the ground "identity" orientation of an ik target

-//-----------------------------------------------------------------------------

-

-void CIKTarget::SetQuaternion( const Quaternion &q )

-{

-	est.q = q;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: calculates a ground "identity" orientation based on the surface

-//			normal of the ground and the desired ground identity orientation

-//-----------------------------------------------------------------------------

-

-void CIKTarget::SetNormal( const Vector &normal )

-{

-	// recalculate foot angle based on slope of surface

-	matrix3x4_t m1;

-	Vector forward, right;

-	QuaternionMatrix( est.q, m1 );

-

-	MatrixGetColumn( m1, 1, right );

-	forward = CrossProduct( right, normal );

-	right = CrossProduct( normal, forward );

-	MatrixSetColumn( forward, 0, m1 );

-	MatrixSetColumn( right, 1, m1 );

-	MatrixSetColumn( normal, 2, m1 );

-	QAngle a1;

-	Vector p1;

-	MatrixAngles( m1, est.q, p1 );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: estimates the ground impact at the center location assuming a the edge of 

-//			an Z axis aligned disc collided with it the surface.

-//-----------------------------------------------------------------------------

-

-void CIKTarget::SetPosWithNormalOffset( const Vector &pos, const Vector &normal )

-{

-	// assume it's a disc edge intersecting with the floor, so try to estimate the z location of the center

-	est.pos = pos;

-	if (normal.z > 0.9999)

-	{

-		return;

-	}

-	// clamp at 45 degrees

-	else if (normal.z > 0.707)

-	{

-		// tan == sin / cos

-		float tan = sqrt( 1 - normal.z * normal.z ) / normal.z;

-		est.pos.z = est.pos.z - est.radius * tan;

-	}

-	else

-	{

-		est.pos.z = est.pos.z - est.radius;

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKTarget::SetOnWorld( bool bOnWorld )

-{

-	est.onWorld = bOnWorld;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-bool CIKTarget::IsActive()

-{ 

-	return (est.flWeight > 0.0f);

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKTarget::IKFailed( void )

-{

-	latched.deltaPos.Init();

-	latched.deltaQ.Init();

-	latched.pos = ideal.pos;

-	latched.q = ideal.q;

-	est.latched = 0.0;

-	est.flWeight = 0.0;

-	est.onWorld = false;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKTarget::MoveReferenceFrame( Vector &deltaPos, QAngle &deltaAngles )

-{

-	est.pos -= deltaPos;

-	latched.pos -= deltaPos;

-	offset.pos -= deltaPos;

-	ideal.pos -= deltaPos;

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Invalidate any IK locks.

-//-----------------------------------------------------------------------------

-

-void CIKContext::ClearTargets( void )

-{

-	int i;

-	for (i = 0; i < m_target.Count(); i++)

-	{

-		m_target[i].latched.iFramecounter = -9999;

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: Run through the rules that survived and turn a specific bones boneToWorld 

-//			transform into a pos and q in parents bonespace

-//-----------------------------------------------------------------------------

-

-void CIKContext::UpdateTargets( Vector pos[], Quaternion q[], matrix3x4_t boneToWorld[], CBoneBitList &boneComputed )

-{

-	int i, j;

-

-	for (i = 0; i < m_target.Count(); i++)

-	{

-		m_target[i].est.flWeight = 0.0f;

-		m_target[i].est.latched = 1.0f;

-		m_target[i].est.release = 1.0f;

-		m_target[i].est.height = 0.0f;

-		m_target[i].est.floor = 0.0f;

-		m_target[i].est.radius = 0.0f;

-		m_target[i].offset.pos.Init();

-		m_target[i].offset.q.Init();

-	}

-

-	AutoIKRelease( );

-

-	for (j = 0; j < m_ikChainRule.Count(); j++)

-	{

-		for (i = 0; i < m_ikChainRule.Element( j ).Count(); i++)

-		{

-			ikcontextikrule_t *pRule = &m_ikChainRule.Element( j ).Element( i );

-

-			// ikchainresult_t *pChainRule = &chainRule[ m_ikRule[i].chain ];

-

-			switch( pRule->type )

-			{

-			case IK_ATTACHMENT:

-			case IK_GROUND:

-			// case IK_SELF:

-				{

-					matrix3x4_t footTarget;

-					CIKTarget *pTarget = &m_target[pRule->slot];

-					pTarget->chain = pRule->chain;

-					pTarget->type = pRule->type;

-

-					if (pRule->type == IK_ATTACHMENT)

-					{

-						pTarget->offset.pAttachmentName = pRule->szLabel;

-					}

-					else

-					{

-						pTarget->offset.pAttachmentName = NULL;

-					}

-

-					if (pRule->flRuleWeight == 1.0f || pTarget->est.flWeight == 0.0f)

-					{

-						pTarget->offset.q = pRule->q;

-						pTarget->offset.pos = pRule->pos;

-						pTarget->est.height = pRule->height;

-						pTarget->est.floor = pRule->floor;

-						pTarget->est.radius = pRule->radius;

-						pTarget->est.latched = pRule->latched * pRule->flRuleWeight;

-						pTarget->est.release = pRule->release;

-						pTarget->est.flWeight = pRule->flWeight * pRule->flRuleWeight;

-					}

-					else

-					{

-						QuaternionSlerp( pTarget->offset.q, pRule->q, pRule->flRuleWeight, pTarget->offset.q );

-						pTarget->offset.pos = Lerp( pRule->flRuleWeight, pTarget->offset.pos, pRule->pos );

-						pTarget->est.height = Lerp( pRule->flRuleWeight, pTarget->est.height, pRule->height );

-						pTarget->est.floor = Lerp( pRule->flRuleWeight, pTarget->est.floor, pRule->floor );

-						pTarget->est.radius = Lerp( pRule->flRuleWeight, pTarget->est.radius, pRule->radius );

-						//pTarget->est.latched = Lerp( pRule->flRuleWeight, pTarget->est.latched, pRule->latched );

-						pTarget->est.latched = min( pTarget->est.latched, pRule->latched );

-						pTarget->est.release = Lerp( pRule->flRuleWeight, pTarget->est.release, pRule->release );

-						pTarget->est.flWeight = Lerp( pRule->flRuleWeight, pTarget->est.flWeight, pRule->flWeight );

-					}

-

-					if ( pRule->type == IK_GROUND )

-					{

-						pTarget->latched.deltaPos.z = 0;

-						pTarget->est.pos.z = pTarget->est.floor + m_rootxform[2][3];

-					}

-				}

-			break;

-			case IK_UNLATCH:

-				{

-					CIKTarget *pTarget = &m_target[pRule->slot];

-					if (pRule->latched > 0.0)

-						pTarget->est.latched = 0.0;

-					else

-						pTarget->est.latched = min( pTarget->est.latched, 1.0f - pRule->flWeight );

-				}

-				break;

-			case IK_RELEASE:

-				{

-					CIKTarget *pTarget = &m_target[pRule->slot];

-					if (pRule->latched > 0.0)

-						pTarget->est.latched = 0.0;

-					else

-						pTarget->est.latched = min( pTarget->est.latched, 1.0f - pRule->flWeight );

-

-					pTarget->est.flWeight = (pTarget->est.flWeight) * (1 - pRule->flWeight * pRule->flRuleWeight);

-				}

-				break;

-			}

-		}

-	}

-

-	for (i = 0; i < m_target.Count(); i++)

-	{

-		CIKTarget *pTarget = &m_target[i];

-		if (pTarget->est.flWeight > 0.0)

-		{

-			mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( pTarget->chain );

-			// ikchainresult_t *pChainRule = &chainRule[ i ];

-			int bone = pchain->pLink( 2 )->bone;

-

-			// eval current ik'd bone

-			BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );

-

-			// xform IK target error into world space

-			matrix3x4_t local;

-			matrix3x4_t worldFootpad;

-			QuaternionMatrix( pTarget->offset.q, pTarget->offset.pos, local );

-			MatrixInvert( local, local );

-			ConcatTransforms( boneToWorld[bone], local, worldFootpad );

-

-			if (pTarget->est.latched == 1.0)

-			{

-				pTarget->latched.bNeedsLatch = true;

-			}

-			else

-			{

-				pTarget->latched.bNeedsLatch = false;

-			}

-

-			// disable latched position if it looks invalid

-			if (m_iFramecounter < 0 || pTarget->latched.iFramecounter < m_iFramecounter - 1 || pTarget->latched.iFramecounter > m_iFramecounter)

-			{

-				pTarget->latched.bHasLatch = false;

-				pTarget->latched.influence = 0.0;

-			}

-			pTarget->latched.iFramecounter = m_iFramecounter;

-

-			// find ideal contact position

-			MatrixAngles( worldFootpad, pTarget->ideal.q, pTarget->ideal.pos );

-			pTarget->est.q = pTarget->ideal.q;

-			pTarget->est.pos = pTarget->ideal.pos;

-

-			float latched = pTarget->est.latched;

-

-			if (pTarget->latched.bHasLatch)

-			{

-				if (pTarget->est.latched == 1.0)

-				{

-					// keep track of latch position error from ideal contact position

-					pTarget->latched.deltaPos = pTarget->latched.pos - pTarget->est.pos;

-					QuaternionSM( -1, pTarget->est.q, pTarget->latched.q, pTarget->latched.deltaQ );

-					pTarget->est.q = pTarget->latched.q;

-					pTarget->est.pos = pTarget->latched.pos;

-				}

-				else if (pTarget->est.latched > 0.0)

-				{

-					// ramp out latch differences during decay phase of rule

-					if (latched > 0 && latched < pTarget->latched.influence)

-					{

-						// latching has decreased

-						float dt = pTarget->latched.influence - latched;

-						if (pTarget->latched.influence > 0.0)

-							dt = dt / pTarget->latched.influence;

-

-						VectorScale( pTarget->latched.deltaPos, (1-dt), pTarget->latched.deltaPos );

-						QuaternionScale( pTarget->latched.deltaQ, (1-dt), pTarget->latched.deltaQ );

-					}

-

-					// move ideal contact position by latched error factor

-					pTarget->est.pos = pTarget->est.pos + pTarget->latched.deltaPos;

-					QuaternionMA( pTarget->est.q, 1, pTarget->latched.deltaQ, pTarget->est.q );

-					pTarget->latched.q = pTarget->est.q;

-					pTarget->latched.pos = pTarget->est.pos;

-				}

-				else

-				{

-					pTarget->latched.bHasLatch = false;

-					pTarget->latched.q = pTarget->est.q;

-					pTarget->latched.pos = pTarget->est.pos;

-					pTarget->latched.deltaPos.Init();

-					pTarget->latched.deltaQ.Init();

-				}

-				pTarget->latched.influence = latched;

-			}

-

-			// check for illegal requests

-			Vector p1, p2, p3;

-			MatrixPosition( boneToWorld[pchain->pLink( 0 )->bone], p1 ); // hip

-			MatrixPosition( boneToWorld[pchain->pLink( 1 )->bone], p2 ); // knee

-			MatrixPosition( boneToWorld[pchain->pLink( 2 )->bone], p3 ); // foot

-

-			float d1 = (p2 - p1).Length();

-			float d2 = (p3 - p2).Length();

-

-			if (pTarget->latched.bHasLatch)

-			{

-				//float d3 = (p3 - p1).Length();

-				float d4 = (p3 + pTarget->latched.deltaPos - p1).Length();

-

-				// unstick feet when distance is too great

-				if ((d4 < fabs( d1 - d2 ) || d4 * 0.95 > d1 + d2) && pTarget->est.latched > 0.2)

-				{

-					pTarget->error.flTime = m_flTime;

-				}

-

-				// unstick feet when angle is too great

-				if (pTarget->est.latched > 0.2)

-				{

-					float d = fabs( pTarget->latched.deltaQ.w ) * 2.0f - 1.0f; // QuaternionDotProduct( pTarget->latched.q, pTarget->est.q );

-

-					// FIXME: cos(45), make property of chain

-					if (d < 0.707)

-					{

-						pTarget->error.flTime = m_flTime;

-					}

-				}

-			}

-

-			Vector dt = pTarget->est.pos - p1;

-			pTarget->trace.hipToFoot = VectorNormalize( dt );

-			pTarget->trace.hipToKnee = d1;

-			pTarget->trace.kneeToFoot = d2;

-			pTarget->trace.hip = p1;

-			pTarget->trace.knee = p2;

-			pTarget->trace.closest = p1 + dt * (fabs( d1 - d2 ) * 1.01);

-			pTarget->trace.farthest = p1 + dt * (d1 + d2) * 0.99;

-			pTarget->trace.lowest = p1 + Vector( 0, 0, -1 ) * (d1 + d2) * 0.99;

-			// pTarget->trace.endpos = pTarget->est.pos;

-		}

-	}

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: insert release rules if the ik rules were in error

-//-----------------------------------------------------------------------------

-

-void CIKContext::AutoIKRelease( void )

-{

-	int i;

-

-	for (i = 0; i < m_target.Count(); i++)

-	{

-		CIKTarget *pTarget = &m_target[i];

-

-		float dt = m_flTime - pTarget->error.flTime;

-		if (pTarget->error.bInError || dt < 0.5)

-		{

-			if (!pTarget->error.bInError)

-			{

-				pTarget->error.ramp = 0.0; 

-				pTarget->error.flErrorTime = pTarget->error.flTime;

-				pTarget->error.bInError = true;

-			}

-

-			float ft = m_flTime - pTarget->error.flErrorTime;

-			if (dt < 0.25)

-			{

-				pTarget->error.ramp = min( pTarget->error.ramp + ft * 4.0, 1.0 );

-			}

-			else

-			{

-				pTarget->error.ramp = max( pTarget->error.ramp - ft * 4.0, 0.0 );

-			}

-			if (pTarget->error.ramp > 0.0)

-			{

-				ikcontextikrule_t ikrule;

-

-				ikrule.chain = pTarget->chain;

-				ikrule.bone = 0;

-				ikrule.type = IK_RELEASE;

-				ikrule.slot = i;

-				ikrule.flWeight = SimpleSpline( pTarget->error.ramp );

-				ikrule.flRuleWeight = 1.0;

-				ikrule.latched = dt < 0.25 ? 0.0 : ikrule.flWeight;

-

-				// don't bother with AutoIKRelease if the bone isn't going to be calculated

-				// this code is crashing for some unknown reason.

-				if ( pTarget->chain >= 0 && pTarget->chain < m_pStudioHdr->numikchains())

-				{

-					mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( pTarget->chain );

-					if (pchain != NULL)

-					{

-						int bone = pchain->pLink( 2 )->bone;

-						if (bone >= 0 && bone < m_pStudioHdr->numbones())

-						{

-							mstudiobone_t *pBone = m_pStudioHdr->pBone( bone );

-							if (pBone != NULL)

-							{

-								if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))

-								{

-									pTarget->error.bInError = false;

-									continue;

-								}

-								/*

-								char buf[256];

-								sprintf( buf, "dt %.4f ft %.4f weight %.4f latched %.4f\n", dt, ft, ikrule.flWeight, ikrule.latched );

-								OutputDebugString( buf );

-								*/

-

-								int nIndex = m_ikChainRule.Element( ikrule.chain ).AddToTail( );

-  								m_ikChainRule.Element( ikrule.chain ).Element( nIndex ) = ikrule;

-							}

-							else

-							{

-								DevWarning( 1, "AutoIKRelease (%s) got a NULL pBone %d\n", m_pStudioHdr->pszName(), bone );

-							}

-						}

-						else

-						{

-							DevWarning( 1, "AutoIKRelease (%s) got an out of range bone %d (%d)\n", m_pStudioHdr->pszName(), bone, m_pStudioHdr->numbones() );

-						}

-					}

-					else

-					{

-						DevWarning( 1, "AutoIKRelease (%s) got a NULL pchain %d\n", m_pStudioHdr->pszName(), pTarget->chain );

-					}

-				}

-				else

-				{

-					DevWarning( 1, "AutoIKRelease (%s) got an out of range chain %d (%d)\n", m_pStudioHdr->pszName(), pTarget->chain, m_pStudioHdr->numikchains());

-				}

-			}

-			else

-			{

-				pTarget->error.bInError = false;

-			}

-			pTarget->error.flErrorTime = m_flTime;

-		}

-	}

-}

-

-

-

-void CIKContext::SolveDependencies( Vector pos[], Quaternion q[], matrix3x4_t boneToWorld[], CBoneBitList &boneComputed	)

-{

-//	ASSERT_NO_REENTRY();

-	

-	matrix3x4_t worldTarget;

-	int i, j;

-

-	ikchainresult_t chainResult[32]; // allocate!!!

-

-	// init chain rules

-	for (i = 0; i < m_pStudioHdr->numikchains(); i++)

-	{

-		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( i );

-		ikchainresult_t *pChainResult = &chainResult[ i ];

-		int bone = pchain->pLink( 2 )->bone;

-

-		pChainResult->target = -1;

-		pChainResult->flWeight = 0.0;

-

-		// don't bother with chain if the bone isn't going to be calculated

-		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))

-			continue;

-

-		// eval current ik'd bone

-		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );

-

-		MatrixAngles( boneToWorld[bone], pChainResult->q, pChainResult->pos );

-	}

-

-	for (j = 0; j < m_ikChainRule.Count(); j++)

-	{

-		for (i = 0; i < m_ikChainRule.Element( j ).Count(); i++)

-		{

-			ikcontextikrule_t *pRule = &m_ikChainRule.Element( j ).Element( i );

-			ikchainresult_t *pChainResult = &chainResult[ pRule->chain ];

-			pChainResult->target = -1;

-

-

-			switch( pRule->type )

-			{

-			case IK_SELF:

-				{

-					// xform IK target error into world space

-					matrix3x4_t local;

-					QuaternionMatrix( pRule->q, pRule->pos, local );

-					// eval target bone space

-					if (pRule->bone != -1)

-					{

-						BuildBoneChain( pos, q, pRule->bone, boneToWorld, boneComputed );

-						ConcatTransforms( boneToWorld[pRule->bone], local, worldTarget );

-					}

-					else

-					{

-						ConcatTransforms( m_rootxform, local, worldTarget );

-					}

-			

-					float flWeight = pRule->flWeight * pRule->flRuleWeight;

-					pChainResult->flWeight = pChainResult->flWeight * (1 - flWeight) + flWeight;

-

-					Vector p2;

-					Quaternion q2;

-					

-					// target p and q

-					MatrixAngles( worldTarget, q2, p2 );

-

-					// debugLine( pChainResult->pos, p2, 0, 0, 255, true, 0.1 );

-

-					// blend in position and angles

-					pChainResult->pos = pChainResult->pos * (1.0 - flWeight) + p2 * flWeight;

-					QuaternionSlerp( pChainResult->q, q2, flWeight, pChainResult->q );

-				}

-				break;

-			case IK_WORLD:

-				Assert( 0 );

-				break;

-

-			case IK_ATTACHMENT:

-				break;

-

-			case IK_GROUND:

-				break;

-

-			case IK_RELEASE:

-				{

-					// move target back towards original location

-					float flWeight = pRule->flWeight * pRule->flRuleWeight;

-					mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( pRule->chain );

-					int bone = pchain->pLink( 2 )->bone;

-

-					Vector p2;

-					Quaternion q2;

-					

-					BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );

-					MatrixAngles( boneToWorld[bone], q2, p2 );

-

-					// blend in position and angles

-					pChainResult->pos = pChainResult->pos * (1.0 - flWeight) + p2 * flWeight;

-					QuaternionSlerp( pChainResult->q, q2, flWeight, pChainResult->q );

-				}

-				break;

-			case IK_UNLATCH:

-				{

-					/*

-					pChainResult->flWeight = pChainResult->flWeight * (1 - pRule->flWeight) + pRule->flWeight;

-

-					pChainResult->pos = pChainResult->pos * (1.0 - pRule->flWeight ) + pChainResult->local.pos * pRule->flWeight;

-					QuaternionSlerp( pChainResult->q, pChainResult->local.q, pRule->flWeight, pChainResult->q );

-					*/

-				}

-				break;

-			}

-		}

-	}

-

-	for (i = 0; i < m_target.Count(); i++)

-	{

-		CIKTarget *pTarget = &m_target[i];

-

-		if (m_target[i].est.flWeight > 0.0)

-		{

-			matrix3x4_t worldFootpad;

-			matrix3x4_t local;

-			//mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( m_target[i].chain );

-			ikchainresult_t *pChainResult = &chainResult[ pTarget->chain ];

-

-			AngleMatrix(pTarget->offset.q, pTarget->offset.pos, local );

-

-			AngleMatrix( pTarget->est.q, pTarget->est.pos, worldFootpad );

-

-			ConcatTransforms( worldFootpad, local, worldTarget );

-

-			Vector p2;

-			Quaternion q2;

-			// target p and q

-			MatrixAngles( worldTarget, q2, p2 );

-			// MatrixAngles( worldTarget, pChainResult->q, pChainResult->pos );

-

-			// blend in position and angles

-			pChainResult->flWeight = pTarget->est.flWeight;

-			pChainResult->pos = pChainResult->pos * (1.0 - pChainResult->flWeight ) + p2 * pChainResult->flWeight;

-			QuaternionSlerp( pChainResult->q, q2, pChainResult->flWeight, pChainResult->q );

-		}

-

-		if (pTarget->latched.bNeedsLatch)

-		{

-			// keep track of latch position

-			pTarget->latched.bHasLatch = true;

-			pTarget->latched.q = pTarget->est.q;

-			pTarget->latched.pos = pTarget->est.pos;

-		}

-	}

-

-	for (i = 0; i < m_pStudioHdr->numikchains(); i++)

-	{

-		ikchainresult_t *pChainResult = &chainResult[ i ];

-		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( i );

-

-		if (pChainResult->flWeight > 0.0)

-		{

-			Vector tmp;

-			MatrixPosition( boneToWorld[pchain->pLink( 2 )->bone], tmp );

-			// debugLine( pChainResult->pos, tmp, 255, 255, 255, true, 0.1 );

-

-			// do exact IK solution

-			// FIXME: once per link!

-			if (Studio_SolveIK(pchain, pChainResult->pos, boneToWorld ))

-			{

-				Vector p3;

-				MatrixGetColumn( boneToWorld[pchain->pLink( 2 )->bone], 3, p3 );

-				QuaternionMatrix( pChainResult->q, p3, boneToWorld[pchain->pLink( 2 )->bone] );

-

-				// rebuild chain

-				// FIXME: is this needed if everyone past this uses the boneToWorld array?

-				SolveBone( m_pStudioHdr, pchain->pLink( 2 )->bone, boneToWorld, pos, q );

-				SolveBone( m_pStudioHdr, pchain->pLink( 1 )->bone, boneToWorld, pos, q );

-				SolveBone( m_pStudioHdr, pchain->pLink( 0 )->bone, boneToWorld, pos, q );

-			}

-			else

-			{

-				// FIXME: need to invalidate the targets that forced this...

-				if (pChainResult->target != -1)

-				{

-					CIKTarget *pTarget = &m_target[pChainResult->target];

-					VectorScale( pTarget->latched.deltaPos, 0.8, pTarget->latched.deltaPos );

-					QuaternionScale( pTarget->latched.deltaQ, 0.8, pTarget->latched.deltaQ );

-				}

-			}

-		}

-	}

-

-#if 0

-		Vector p1, p2, p3;

-		Quaternion q1, q2, q3;

-

-		// current p and q

-		MatrixAngles( boneToWorld[bone], q1, p1 );

-

-		

-		// target p and q

-		MatrixAngles( worldTarget, q2, p2 );

-

-		// blend in position and angles

-		p3 = p1 * (1.0 - m_ikRule[i].flWeight ) + p2 * m_ikRule[i].flWeight;

-

-		// do exact IK solution

-		// FIXME: once per link!

-		Studio_SolveIK(pchain, p3, boneToWorld );

-

-		// force angle (bad?)

-		QuaternionSlerp( q1, q2, m_ikRule[i].flWeight, q3 );

-		MatrixGetColumn( boneToWorld[bone], 3, p3 );

-		QuaternionMatrix( q3, p3, boneToWorld[bone] );

-

-		// rebuild chain

-		SolveBone( m_pStudioHdr, pchain->pLink( 2 )->bone, boneToWorld, pos, q );

-		SolveBone( m_pStudioHdr, pchain->pLink( 1 )->bone, boneToWorld, pos, q );

-		SolveBone( m_pStudioHdr, pchain->pLink( 0 )->bone, boneToWorld, pos, q );

-#endif

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKContext::SolveAutoplayLocks(

-	Vector pos[], 

-	Quaternion q[]

-	)

-{

-	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList boneComputed;

-	int i;

-

-	for (i = 0; i < m_ikLock.Count(); i++)

-	{

-		const mstudioiklock_t &lock = ((CStudioHdr *)m_pStudioHdr)->pIKAutoplayLock( i );

-		SolveLock( &lock, i, pos, q, boneToWorld, boneComputed );

-	}

-	g_MatrixPool.Free( boneToWorld );

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKContext::SolveSequenceLocks(

-	mstudioseqdesc_t &seqdesc,

-	Vector pos[], 

-	Quaternion q[]

-	)

-{

-	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList boneComputed;

-	int i;

-

-	for (i = 0; i < m_ikLock.Count(); i++)

-	{

-		mstudioiklock_t *plock = seqdesc.pIKLock( i );

-		SolveLock( plock, i, pos, q, boneToWorld, boneComputed );

-	}

-	g_MatrixPool.Free( boneToWorld );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-void CIKContext::AddAllLocks( Vector pos[], Quaternion q[] )

-{

-	// skip all array access if no autoplay locks.

-	if (m_pStudioHdr->GetNumIKChains() == 0)

-	{

-		return;

-	}

-

-	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList boneComputed;

-

-	int ikOffset = m_ikLock.AddMultipleToTail( m_pStudioHdr->GetNumIKChains() );

-	memset( &m_ikLock[ikOffset], 0, sizeof(ikcontextikrule_t)*m_pStudioHdr->GetNumIKChains() );

-

-	for (int i = 0; i < m_pStudioHdr->GetNumIKChains(); i++)

-	{

-		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( i );

-		int bone = pchain->pLink( 2 )->bone;

-

-		// don't bother with iklock if the bone isn't going to be calculated

-		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))

-			continue;

-

-		// eval current ik'd bone

-		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );

-

-		ikcontextikrule_t &ikrule = m_ikLock[ i + ikOffset ];

-

-		ikrule.chain = i;

-		ikrule.slot = i;

-		ikrule.type = IK_WORLD;

-

-		MatrixAngles( boneToWorld[bone], ikrule.q, ikrule.pos );

-

-		// save off current knee direction

-		if (pchain->pLink(0)->kneeDir.LengthSqr() > 0.0)

-		{

-			Vector tmp = pchain->pLink( 0 )->kneeDir;

-			VectorRotate( pchain->pLink( 0 )->kneeDir, boneToWorld[ pchain->pLink( 0 )->bone ], ikrule.kneeDir );

-			MatrixPosition( boneToWorld[ pchain->pLink( 1 )->bone ], ikrule.kneePos ); 

-		}

-		else

-		{

-			ikrule.kneeDir.Init( );

-		}

-	}

-	g_MatrixPool.Free( boneToWorld );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-

-void CIKContext::SolveAllLocks(

-	Vector pos[], 

-	Quaternion q[]

-	)

-{

-	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();

-	CBoneBitList boneComputed;

-	int i;

-

-	mstudioiklock_t lock;

-

-	for (i = 0; i < m_ikLock.Count(); i++)

-	{

-		lock.chain = i;

-		lock.flPosWeight = 1.0;

-		lock.flLocalQWeight = 0.0;

-		lock.flags = 0;

-

-		SolveLock( &lock, i, pos, q, boneToWorld, boneComputed );

-	}

-	g_MatrixPool.Free( boneToWorld );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: 

-//-----------------------------------------------------------------------------

-

-

-void CIKContext::SolveLock(

-	const mstudioiklock_t *plock,

-	int i,

-	Vector pos[], 

-	Quaternion q[],

-	matrix3x4_t boneToWorld[], 

-	CBoneBitList &boneComputed

-	)

-{

-	mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( plock->chain );

-	int bone = pchain->pLink( 2 )->bone;

-

-	// don't bother with iklock if the bone isn't going to be calculated

-	if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))

-		return;

-

-	// eval current ik'd bone

-	BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );

-

-	Vector p1, p2, p3;

-	Quaternion q2, q3;

-

-	// current p and q

-	MatrixPosition( boneToWorld[bone], p1 );

-

-	// blend in position

-	p3 = p1 * (1.0 - plock->flPosWeight ) + m_ikLock[i].pos * plock->flPosWeight;

-

-	// do exact IK solution

-	if (m_ikLock[i].kneeDir.LengthSqr() > 0)

-	{

-		Studio_SolveIK(pchain->pLink( 0 )->bone, pchain->pLink( 1 )->bone, pchain->pLink( 2 )->bone, p3, m_ikLock[i].kneePos, m_ikLock[i].kneeDir, boneToWorld );

-	}

-	else

-	{

-		Studio_SolveIK(pchain, p3, boneToWorld );

-	}

-

-	// slam orientation

-	MatrixPosition( boneToWorld[bone], p3 );

-	QuaternionMatrix( m_ikLock[i].q, p3, boneToWorld[bone] );

-

-	// rebuild chain

-	q2 = q[ bone ];

-	SolveBone( m_pStudioHdr, pchain->pLink( 2 )->bone, boneToWorld, pos, q );

-	QuaternionSlerp( q[bone], q2, plock->flLocalQWeight, q[bone] );

-

-	SolveBone( m_pStudioHdr, pchain->pLink( 1 )->bone, boneToWorld, pos, q );

-	SolveBone( m_pStudioHdr, pchain->pLink( 0 )->bone, boneToWorld, pos, q );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: run all animations that automatically play and are driven off of poseParameters

-//-----------------------------------------------------------------------------

-void CBoneSetup::CalcAutoplaySequences(

-   Vector pos[], 

-   Quaternion q[], 

-   float flRealTime,

-   CIKContext *pIKContext

-   )

-{

-	//	ASSERT_NO_REENTRY();

-

-	int			i;

-	if ( pIKContext )

-	{

-		pIKContext->AddAutoplayLocks( pos, q );

-	}

-

-	unsigned short *pList = NULL;

-	int count = m_pStudioHdr->GetAutoplayList( &pList );

-	for (i = 0; i < count; i++)

-	{

-		int sequenceIndex = pList[i];

-		mstudioseqdesc_t &seqdesc = ((CStudioHdr *)m_pStudioHdr)->pSeqdesc( sequenceIndex );

-		if (seqdesc.flags & STUDIO_AUTOPLAY)

-		{

-			float cycle = 0;

-			float cps = Studio_CPS( m_pStudioHdr, seqdesc, sequenceIndex, m_flPoseParameter );

-			cycle = flRealTime * cps;

-			cycle = cycle - (int)cycle;

-

-			AccumulatePose( pos, q, sequenceIndex, cycle, 1.0, flRealTime, pIKContext );

-		}

-	}

-

-	if ( pIKContext )

-	{

-		pIKContext->SolveAutoplayLocks( pos, q );

-	}

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-void Studio_BuildMatrices(

-	const CStudioHdr *pStudioHdr,

-	const QAngle& angles, 

-	const Vector& origin, 

-	const Vector pos[],

-	const Quaternion q[],

-	int iBone,

-	float flScale,

-	matrix3x4_t bonetoworld[MAXSTUDIOBONES],

-	int boneMask

-	)

-{

-	int i, j;

-

-	int					chain[MAXSTUDIOBONES] = {};

-	int					chainlength = 0;

-

-	if (iBone < -1 || iBone >= pStudioHdr->numbones())

-		iBone = 0;

-

-	// build list of what bones to use

-	if (iBone == -1)

-	{

-		// all bones

-		chainlength = pStudioHdr->numbones();

-		for (i = 0; i < pStudioHdr->numbones(); i++)

-		{

-			chain[chainlength - i - 1] = i;

-		}

-	}

-	else

-	{

-		// only the parent bones

-		i = iBone;

-		while (i != -1)

-		{

-			chain[chainlength++] = i;

-			i = pStudioHdr->boneParent( i );

-		}

-	}

-

-	matrix3x4_t bonematrix;

-	matrix3x4_t rotationmatrix; // model to world transformation

-	AngleMatrix( angles, origin, rotationmatrix );

-

-	// Account for a change in scale

-	if ( flScale < 1.0f-FLT_EPSILON || flScale > 1.0f+FLT_EPSILON )

-	{

-		Vector vecOffset;

-		MatrixGetColumn( rotationmatrix, 3, vecOffset );

-		vecOffset -= origin;

-		vecOffset *= flScale;

-		vecOffset += origin;

-		MatrixSetColumn( vecOffset, 3, rotationmatrix );

-

-		// Scale it uniformly

-		VectorScale( rotationmatrix[0], flScale, rotationmatrix[0] );

-		VectorScale( rotationmatrix[1], flScale, rotationmatrix[1] );

-		VectorScale( rotationmatrix[2], flScale, rotationmatrix[2] );

-	}

-

-	for (j = chainlength - 1; j >= 0; j--)

-	{

-		i = chain[j];

-		if (pStudioHdr->boneFlags(i) & boneMask)

-		{

-			QuaternionMatrix( q[i], pos[i], bonematrix );

-

-			if (pStudioHdr->boneParent(i) == -1) 

-			{

-				ConcatTransforms (rotationmatrix, bonematrix, bonetoworld[i]);

-			} 

-			else 

-			{

-				ConcatTransforms (bonetoworld[pStudioHdr->boneParent(i)], bonematrix, bonetoworld[i]);

-			}

-		}

-	}

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: look at single column vector of another bones local transformation 

-//			and generate a procedural transformation based on how that column 

-//			points down the 6 cardinal axis (all negative weights are clamped to 0).

-//-----------------------------------------------------------------------------

-

-void DoAxisInterpBone(

-	mstudiobone_t		*pbones,

-	int	ibone,

-	CBoneAccessor &bonetoworld

-	)

-{

-	matrix3x4_t			bonematrix;

-	Vector				control;

-

-	mstudioaxisinterpbone_t *pProc = (mstudioaxisinterpbone_t *)pbones[ibone].pProcedure( );

-	const matrix3x4_t &controlBone = bonetoworld.GetBone( pProc->control );

-	if (pProc && pbones[pProc->control].parent != -1)

-	{

-		Vector tmp;

-		// pull out the control column

-		tmp.x = controlBone[0][pProc->axis];

-		tmp.y = controlBone[1][pProc->axis];

-		tmp.z = controlBone[2][pProc->axis];

-

-		// invert it back into parent's space.

-		VectorIRotate( tmp, bonetoworld.GetBone( pbones[pProc->control].parent ), control );

-#if 0

-		matrix3x4_t	tmpmatrix;

-		matrix3x4_t	controlmatrix;

-		MatrixInvert( bonetoworld.GetBone( pbones[pProc->control].parent ), tmpmatrix );

-		ConcatTransforms( tmpmatrix, bonetoworld.GetBone( pProc->control ), controlmatrix );

-

-		// pull out the control column

-		control.x = controlmatrix[0][pProc->axis];

-		control.y = controlmatrix[1][pProc->axis];

-		control.z = controlmatrix[2][pProc->axis];

-#endif

-	}

-	else

-	{

-		// pull out the control column

-		control.x = controlBone[0][pProc->axis];

-		control.y = controlBone[1][pProc->axis];

-		control.z = controlBone[2][pProc->axis];

-	}

-

-	Quaternion *q1, *q2, *q3;

-	Vector *p1, *p2, *p3;

-

-	// find axial control inputs

-	float a1 = control.x;

-	float a2 = control.y;

-	float a3 = control.z;

-	if (a1 >= 0) 

-	{ 

-		q1 = &pProc->quat[0];

-		p1 = &pProc->pos[0];

-	} 

-	else 

-	{ 

-		a1 = -a1; 

-		q1 = &pProc->quat[1];

-		p1 = &pProc->pos[1];

-	}

-

-	if (a2 >= 0) 

-	{ 

-		q2 = &pProc->quat[2]; 

-		p2 = &pProc->pos[2];

-	} 

-	else 

-	{ 

-		a2 = -a2; 

-		q2 = &pProc->quat[3]; 

-		p2 = &pProc->pos[3];

-	}

-

-	if (a3 >= 0) 

-	{ 

-		q3 = &pProc->quat[4]; 

-		p3 = &pProc->pos[4];

-	} 

-	else 

-	{ 

-		a3 = -a3; 

-		q3 = &pProc->quat[5]; 

-		p3 = &pProc->pos[5];

-	}

-

-	// do a three-way blend

-	Vector p;

-	Quaternion v, tmp;

-	if (a1 + a2 > 0)

-	{

-		float t = 1.0 / (a1 + a2 + a3);

-		// FIXME: do a proper 3-way Quat blend!

-		QuaternionSlerp( *q2, *q1, a1 / (a1 + a2), tmp );

-		QuaternionSlerp( tmp, *q3, a3 * t, v );

-		VectorScale( *p1, a1 * t, p );

-		VectorMA( p, a2 * t, *p2, p );

-		VectorMA( p, a3 * t, *p3, p );

-	}

-	else

-	{

-		QuaternionSlerp( *q3, *q3, 0, v ); // ??? no quat copy?

-		p = *p3;

-	}

-

-	QuaternionMatrix( v, p, bonematrix );

-

-	ConcatTransforms (bonetoworld.GetBone( pbones[ibone].parent ), bonematrix, bonetoworld.GetBoneForWrite( ibone ));

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Generate a procedural transformation based on how that another bones 

-//			local transformation matches a set of target orientations.

-//-----------------------------------------------------------------------------

-void DoQuatInterpBone(

-	mstudiobone_t		*pbones,

-	int	ibone,

-	CBoneAccessor &bonetoworld

-	)

-{

-	matrix3x4_t			bonematrix;

-	Vector				control;

-

-	mstudioquatinterpbone_t *pProc = (mstudioquatinterpbone_t *)pbones[ibone].pProcedure( );

-	if (pProc && pbones[pProc->control].parent != -1)

-	{

-		Quaternion	src;

-		float		weight[32];

-		float		scale = 0.0;

-		Quaternion	quat;

-		Vector		pos;

-

-		matrix3x4_t	tmpmatrix;

-		matrix3x4_t	controlmatrix;

-		MatrixInvert( bonetoworld.GetBone( pbones[pProc->control].parent), tmpmatrix );

-		ConcatTransforms( tmpmatrix, bonetoworld.GetBone( pProc->control ), controlmatrix );

-

-		MatrixAngles( controlmatrix, src, pos ); // FIXME: make a version without pos

-

-		int i;

-		for (i = 0; i < pProc->numtriggers; i++)

-		{

-			float dot = fabs( QuaternionDotProduct( pProc->pTrigger( i )->trigger, src ) );

-			// FIXME: a fast acos should be acceptable

-			dot = clamp( dot, -1.f, 1.f );

-			weight[i] = 1 - (2 * acos( dot ) * pProc->pTrigger( i )->inv_tolerance );

-			weight[i] = max( 0, weight[i] );

-			scale += weight[i];

-		}

-

-		if (scale <= 0.001)  // EPSILON?

-		{

-			AngleMatrix( pProc->pTrigger( 0 )->quat, pProc->pTrigger( 0 )->pos, bonematrix );

-			ConcatTransforms ( bonetoworld.GetBone( pbones[ibone].parent ), bonematrix, bonetoworld.GetBoneForWrite( ibone ) );

-			return;

-		}

-

-		scale = 1.0 / scale;

-

-		quat.Init( 0, 0, 0, 0);

-		pos.Init( );

-

-		for (i = 0; i < pProc->numtriggers; i++)

-		{

-			if (weight[i])

-			{

-				float s = weight[i] * scale;

-				mstudioquatinterpinfo_t *pTrigger = pProc->pTrigger( i );

-

-				QuaternionAlign( pTrigger->quat, quat, quat );

-

-				quat.x = quat.x + s * pTrigger->quat.x;

-				quat.y = quat.y + s * pTrigger->quat.y;

-				quat.z = quat.z + s * pTrigger->quat.z;

-				quat.w = quat.w + s * pTrigger->quat.w;

-				pos.x = pos.x + s * pTrigger->pos.x;

-				pos.y = pos.y + s * pTrigger->pos.y;

-				pos.z = pos.z + s * pTrigger->pos.z;

-			}

-		}

-		Assert( QuaternionNormalize( quat ) != 0);

-		QuaternionMatrix( quat, pos, bonematrix );

-	}

-

-	ConcatTransforms (bonetoworld.GetBone( pbones[ibone].parent ), bonematrix, bonetoworld.GetBoneForWrite( ibone ));

-}

-

-/*

- * This is for DoAimAtBone below, was just for testing, not needed in general

- * but to turn it back on, uncomment this and the section in DoAimAtBone() below

- *

-

-static ConVar aim_constraint( "aim_constraint", "1", FCVAR_REPLICATED, "Toggle <aimconstraint> Helper Bones" );

-

-*/

-

-//-----------------------------------------------------------------------------

-// Purpose: Generate a procedural transformation so that one bone points at

-//			another point on the model

-//-----------------------------------------------------------------------------

-void DoAimAtBone(

-	mstudiobone_t *pBones,

-	int	iBone,

-	CBoneAccessor &bonetoworld,

-	const CStudioHdr *pStudioHdr

-	)

-{

-	mstudioaimatbone_t *pProc = (mstudioaimatbone_t *)pBones[iBone].pProcedure();

-

-	if ( !pProc )

-	{

-		return;

-	}

-

-	/*

-	 * Uncomment this if the ConVar above is uncommented

-	 *

-

-	if ( !aim_constraint.GetBool() )

-	{

-		// If the aim constraint is turned off then just copy the parent transform

-		// plus the offset value

-

-		matrix3x4_t boneToWorldSpace;

-		MatrixCopy ( bonetoworld.GetBone( pProc->parent ), boneToWorldSpace );

-		Vector boneWorldPosition;

-		VectorTransform( pProc->basepos, boneToWorldSpace, boneWorldPosition );

-		MatrixSetColumn( boneWorldPosition, 3, boneToWorldSpace );

-		MatrixCopy( boneToWorldSpace, bonetoworld.GetBoneForWrite( iBone ) );

-

-		return;

-	}

-

-	*/

-

-	// The world matrix of the bone to change

-	matrix3x4_t boneMatrix;

-

-	// Guaranteed to be unit length

-	const Vector &userAimVector( pProc->aimvector );

-

-	// Guaranteed to be unit length

-	const Vector &userUpVector( pProc->upvector );

-

-	// Get to get position of bone but also for up reference

-	matrix3x4_t parentSpace;

-	MatrixCopy ( bonetoworld.GetBone( pProc->parent ), parentSpace );

-

-	// World space position of the bone to aim

-	Vector aimWorldPosition;

-	VectorTransform( pProc->basepos, parentSpace, aimWorldPosition );

-

-	// The worldspace matrix of the bone to aim at

-	matrix3x4_t aimAtSpace;

-	if ( pStudioHdr )

-	{

-		// This means it's AIMATATTACH

-		const mstudioattachment_t &attachment( ((CStudioHdr *)pStudioHdr)->pAttachment( pProc->aim ) );

-		ConcatTransforms(

-			bonetoworld.GetBone( attachment.localbone ),

-			attachment.local,

-			aimAtSpace );

-	}

-	else

-	{

-		MatrixCopy( bonetoworld.GetBone( pProc->aim ), aimAtSpace );

-	}

-

-	Vector aimAtWorldPosition;

-	MatrixGetColumn( aimAtSpace, 3, aimAtWorldPosition );

-

-	// make sure the redundant parent info is correct

-	Assert( pProc->parent == pBones[iBone].parent );

-	// make sure the redundant position info is correct

-	Assert( pProc->basepos.DistToSqr( pBones[iBone].pos ) < 0.1 );

-

-	// The aim and up data is relative to this bone, not the parent bone

-	matrix3x4_t bonematrix, boneLocalToWorld;

-	AngleMatrix( pBones[iBone].quat, pProc->basepos, bonematrix );

-	ConcatTransforms( bonetoworld.GetBone( pProc->parent ), bonematrix, boneLocalToWorld );

-

-	Vector aimVector;

-	VectorSubtract( aimAtWorldPosition, aimWorldPosition, aimVector );

-	VectorNormalizeFast( aimVector );

-

-	Vector axis;

-	CrossProduct( userAimVector, aimVector, axis );

-	VectorNormalizeFast( axis );

-	Assert( 1.0f - fabs( DotProduct( userAimVector, aimVector ) ) > FLT_EPSILON );

-	float angle( acosf( DotProduct( userAimVector, aimVector ) ) );

-	Quaternion aimRotation;

-	AxisAngleQuaternion( axis, RAD2DEG( angle ), aimRotation );

-

-	if ( ( 1.0f - fabs( DotProduct( userUpVector, userAimVector ) ) ) > FLT_EPSILON )

-	{

-		matrix3x4_t aimRotationMatrix;

-		QuaternionMatrix( aimRotation, aimRotationMatrix );

-

-		Vector tmpV;

-

-		Vector tmp_pUp;

-		VectorRotate( userUpVector, aimRotationMatrix, tmp_pUp );

-		VectorScale( aimVector, DotProduct( aimVector, tmp_pUp ), tmpV );

-		Vector pUp;

-		VectorSubtract( tmp_pUp, tmpV, pUp );

-		VectorNormalizeFast( pUp );

-

-		Vector tmp_pParentUp;

-		VectorRotate( userUpVector, boneLocalToWorld, tmp_pParentUp );

-		VectorScale( aimVector, DotProduct( aimVector, tmp_pParentUp ), tmpV );

-		Vector pParentUp;

-		VectorSubtract( tmp_pParentUp, tmpV, pParentUp );

-		VectorNormalizeFast( pParentUp );

-

-		Quaternion upRotation;

-		//Assert( 1.0f - fabs( DotProduct( pUp, pParentUp ) ) > FLT_EPSILON );

-		if( 1.0f - fabs( DotProduct( pUp, pParentUp ) ) > FLT_EPSILON )

-		{

-			angle = acos( DotProduct( pUp, pParentUp ) );

-			CrossProduct( pUp, pParentUp, axis );			

-		}

-		else

-		{

-			angle = 0;

-			axis = pUp;

-		}

-

-		VectorNormalizeFast( axis );

-		AxisAngleQuaternion( axis, RAD2DEG( angle ), upRotation );

-

-		Quaternion boneRotation;

-		QuaternionMult( upRotation, aimRotation, boneRotation );

-		QuaternionMatrix( boneRotation, aimWorldPosition, boneMatrix );

-	}

-	else

-	{

-		QuaternionMatrix( aimRotation, aimWorldPosition, boneMatrix );

-	}

-

-	MatrixCopy( boneMatrix, bonetoworld.GetBoneForWrite( iBone ) );

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-

-bool CalcProceduralBone(

-	const CStudioHdr *pStudioHdr,

-	int iBone,

-	CBoneAccessor &bonetoworld

-	)

-{

-	mstudiobone_t		*pbones = pStudioHdr->pBone( 0 );

-

-	if ( pStudioHdr->boneFlags(iBone) & BONE_ALWAYS_PROCEDURAL )

-	{

-		switch( pbones[iBone].proctype )

-		{

-		case STUDIO_PROC_AXISINTERP:

-			DoAxisInterpBone( pbones, iBone, bonetoworld );

-			return true;

-

-		case STUDIO_PROC_QUATINTERP:

-			DoQuatInterpBone( pbones, iBone, bonetoworld );

-			return true;

-

-		case STUDIO_PROC_AIMATBONE:

-			DoAimAtBone( pbones, iBone, bonetoworld, NULL );

-			return true;

-

-		case STUDIO_PROC_AIMATATTACH:

-			DoAimAtBone( pbones, iBone, bonetoworld, pStudioHdr );

-			return true;

-

-		default:

-			return false;

-		}

-	}

-	return false;

-}

-

-

-

-//-----------------------------------------------------------------------------

-// Purpose:  Lookup a bone controller

-//-----------------------------------------------------------------------------

-

-

-

-static mstudiobonecontroller_t* FindController( const CStudioHdr *pStudioHdr, int iController)

-{

-	// find first controller that matches the index

-	for (int i = 0; i < pStudioHdr->numbonecontrollers(); i++)

-	{

-		if (pStudioHdr->pBonecontroller( i )->inputfield == iController)

-			return pStudioHdr->pBonecontroller( i );

-	}

-

-	return NULL;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: converts a ranged bone controller value into a 0..1 encoded value

-// Output: 	ctlValue contains 0..1 encoding.

-//			returns clamped ranged value

-//-----------------------------------------------------------------------------

-

-float Studio_SetController( const CStudioHdr *pStudioHdr, int iController, float flValue, float &ctlValue )

-{

-	if (! pStudioHdr)

-		return flValue;

-

-	mstudiobonecontroller_t *pbonecontroller = FindController(pStudioHdr, iController);

-	if(!pbonecontroller)

-	{

-		ctlValue = 0;

-		return flValue;

-	}

-

-	// wrap 0..360 if it's a rotational controller

-	if (pbonecontroller->type & (STUDIO_XR | STUDIO_YR | STUDIO_ZR))

-	{

-		// ugly hack, invert value if end < start

-		if (pbonecontroller->end < pbonecontroller->start)

-			flValue = -flValue;

-

-		// does the controller not wrap?

-		if (pbonecontroller->start + 359.0 >= pbonecontroller->end)

-		{

-			if (flValue > ((pbonecontroller->start + pbonecontroller->end) / 2.0) + 180)

-				flValue = flValue - 360;

-			if (flValue < ((pbonecontroller->start + pbonecontroller->end) / 2.0) - 180)

-				flValue = flValue + 360;

-		}

-		else

-		{

-			if (flValue > 360)

-				flValue = flValue - (int)(flValue / 360.0) * 360.0;

-			else if (flValue < 0)

-				flValue = flValue + (int)((flValue / -360.0) + 1) * 360.0;

-		}

-	}

-

-	ctlValue = (flValue - pbonecontroller->start) / (pbonecontroller->end - pbonecontroller->start);

-	if (ctlValue < 0) ctlValue = 0;

-	if (ctlValue > 1) ctlValue = 1;

-

-	float flReturnVal = ((1.0 - ctlValue)*pbonecontroller->start + ctlValue *pbonecontroller->end);

-

-	// ugly hack, invert value if a rotational controller and end < start

-	if (pbonecontroller->type & (STUDIO_XR | STUDIO_YR | STUDIO_ZR) &&

-		pbonecontroller->end < pbonecontroller->start				)

-	{

-		flReturnVal *= -1;

-	}

-	

-	return flReturnVal;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: converts a 0..1 encoded bone controller value into a ranged value

-// Output: 	returns ranged value

-//-----------------------------------------------------------------------------

-

-float Studio_GetController( const CStudioHdr *pStudioHdr, int iController, float ctlValue )

-{

-	if (!pStudioHdr)

-		return 0.0;

-

-	mstudiobonecontroller_t *pbonecontroller = FindController(pStudioHdr, iController);

-	if(!pbonecontroller)

-		return 0;

-

-	return ctlValue * (pbonecontroller->end - pbonecontroller->start) + pbonecontroller->start;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: Calculates default values for the pose parameters

-// Output: 	fills in an array

-//-----------------------------------------------------------------------------

-

-void Studio_CalcDefaultPoseParameters( const CStudioHdr *pStudioHdr, float flPoseParameter[], int nCount )

-{

-	int nPoseCount = pStudioHdr->GetNumPoseParameters();

-	int nNumParams = MIN( nCount, MAXSTUDIOPOSEPARAM );

-

-	for ( int i = 0; i < nNumParams; ++i )

-	{

-		// Default to middle of the pose parameter range

-		flPoseParameter[ i ] = 0.5f;

-		if ( i < nPoseCount )

-		{

-			const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)pStudioHdr)->pPoseParameter( i );

-

-			// Want to try for a zero state.  If one doesn't exist set it to .5 by default.

-			if ( Pose.start < 0.0f && Pose.end > 0.0f )

-			{

-				float flPoseDelta = Pose.end - Pose.start;

-				flPoseParameter[i] = -Pose.start / flPoseDelta;

-			}

-		}

-	}

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: converts a ranged pose parameter value into a 0..1 encoded value

-// Output: 	ctlValue contains 0..1 encoding.

-//			returns clamped ranged value

-//-----------------------------------------------------------------------------

-

-float Studio_SetPoseParameter( const CStudioHdr *pStudioHdr, int iParameter, float flValue, float &ctlValue )

-{

-	if (iParameter < 0 || iParameter >= pStudioHdr->GetNumPoseParameters())

-	{

-		return 0;

-	}

-

-	const mstudioposeparamdesc_t &PoseParam = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iParameter );

-

-	Assert( IsFinite( flValue ) );

-

-	if (PoseParam.loop)

-	{

-		float wrap = (PoseParam.start + PoseParam.end) / 2.0 + PoseParam.loop / 2.0;

-		float shift = PoseParam.loop - wrap;

-

-		flValue = flValue - PoseParam.loop * floor((flValue + shift) / PoseParam.loop);

-	}

-

-	ctlValue = (flValue - PoseParam.start) / (PoseParam.end - PoseParam.start);

-

-	if (ctlValue < 0) ctlValue = 0;

-	if (ctlValue > 1) ctlValue = 1;

-

-	Assert( IsFinite( ctlValue ) );

-

-	return ctlValue * (PoseParam.end - PoseParam.start) + PoseParam.start;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: converts a 0..1 encoded pose parameter value into a ranged value

-// Output: 	returns ranged value

-//-----------------------------------------------------------------------------

-

-float Studio_GetPoseParameter( const CStudioHdr *pStudioHdr, int iParameter, float ctlValue )

-{

-	if (iParameter < 0 || iParameter >= pStudioHdr->GetNumPoseParameters())

-	{

-		return 0;

-	}

-

-	const mstudioposeparamdesc_t &PoseParam = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iParameter );

-

-	return ctlValue * (PoseParam.end - PoseParam.start) + PoseParam.start;

-}

-

-

-#pragma warning (disable : 4701)

-

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-static int ClipRayToHitbox( const Ray_t &ray, mstudiobbox_t *pbox, matrix3x4_t& matrix, trace_t &tr )

-{

-	const float flProjEpsilon = 0.01f;

-	// scale by current t so hits shorten the ray and increase the likelihood of early outs

-	Vector delta2;

-	VectorScale( ray.m_Delta, (0.5f * tr.fraction), delta2 );

-

-	// OPTIMIZE: Store this in the box instead of computing it here

-	// compute center in local space

-	Vector boxextents;

-	boxextents.x = (pbox->bbmin.x + pbox->bbmax.x) * 0.5; 

-	boxextents.y = (pbox->bbmin.y + pbox->bbmax.y) * 0.5; 

-	boxextents.z = (pbox->bbmin.z + pbox->bbmax.z) * 0.5; 

-	Vector boxCenter;

-	// transform to world space

-	VectorTransform( boxextents, matrix, boxCenter );

-	// calc extents from local center

-	boxextents.x = pbox->bbmax.x - boxextents.x;

-	boxextents.y = pbox->bbmax.y - boxextents.y;

-	boxextents.z = pbox->bbmax.z - boxextents.z;

-	// OPTIMIZE: This is optimized for world space.  If the transform is fast enough, it may make more

-	// sense to just xform and call UTIL_ClipToBox() instead.  MEASURE THIS.

-

-	// save the extents of the ray along 

-	Vector extent, uextent;

-	Vector segmentCenter;

-	segmentCenter.x = ray.m_Start.x + delta2.x - boxCenter.x;

-	segmentCenter.y = ray.m_Start.y + delta2.y - boxCenter.y;

-	segmentCenter.z = ray.m_Start.z + delta2.z - boxCenter.z;

-

-	extent.Init();

-

-	// check box axes for separation

-	for ( int j = 0; j < 3; j++ )

-	{

-		extent[j] = delta2.x * matrix[0][j] + delta2.y * matrix[1][j] +	delta2.z * matrix[2][j];

-		uextent[j] = fabsf(extent[j]);

-		float coord = segmentCenter.x * matrix[0][j] + segmentCenter.y * matrix[1][j] +	segmentCenter.z * matrix[2][j];

-		coord = fabsf(coord);

-

-		if ( coord > (boxextents[j] + uextent[j]) )

-			return -1;

-	}

-

-	// now check cross axes for separation

-	float tmp, tmpfix, cextent;

-	Vector cross;

-	CrossProduct( delta2, segmentCenter, cross );

-	cextent = cross.x * matrix[0][0] + cross.y * matrix[1][0] + cross.z * matrix[2][0];

-	cextent = fabsf(cextent);

-	tmp = boxextents[1]*uextent[2] + boxextents[2]*uextent[1];

-	tmpfix = MAX(tmp, flProjEpsilon);

-	if ( cextent > tmpfix )

-		return -1;

-	

-//	if ( cextent > tmp && cextent <= tmpfix )

-//		DevWarning( "ClipRayToHitbox trace precision error case\n" );

-

-	cextent = cross.x * matrix[0][1] + cross.y * matrix[1][1] + cross.z * matrix[2][1];

-	cextent = fabsf(cextent);

-	tmp = boxextents[0]*uextent[2] + boxextents[2]*uextent[0];

-	tmpfix = MAX(tmp, flProjEpsilon);

-	if ( cextent > tmpfix )

-		return -1;

-

-//	if ( cextent > tmp && cextent <= tmpfix )

-//		DevWarning( "ClipRayToHitbox trace precision error case\n" );

-

-	cextent = cross.x * matrix[0][2] + cross.y * matrix[1][2] + cross.z * matrix[2][2];

-	cextent = fabsf(cextent);

-	tmp = boxextents[0]*uextent[1] + boxextents[1]*uextent[0];

-	tmpfix = MAX(tmp, flProjEpsilon);

-	if ( cextent > tmpfix )

-		return -1;

-

-//	if ( cextent > tmp && cextent <= tmpfix )

-//		DevWarning( "ClipRayToHitbox trace precision error case\n" );

-

-	// !!! We hit this box !!! compute intersection point and return

-	Vector start;

-

-	// Compute ray start in bone space

-	VectorITransform( ray.m_Start, matrix, start );

-	// extent is delta2 in bone space, recompute delta in bone space

-	VectorScale( extent, 2, extent );

-

-	// delta was prescaled by the current t, so no need to see if this intersection

-	// is closer

-	trace_t boxTrace;

-	if ( !IntersectRayWithBox( start, extent, pbox->bbmin, pbox->bbmax, 0.0f, &boxTrace ) )

-		return -1;

-

-	Assert( IsFinite(boxTrace.fraction) );

-	tr.fraction *= boxTrace.fraction;

-	tr.startsolid = boxTrace.startsolid;

-	int hitside = boxTrace.plane.type;

-	if ( boxTrace.plane.normal[hitside] >= 0 )

-	{

-		hitside += 3;

-	}

-	return hitside;

-}

-

-#pragma warning (default : 4701)

-

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-bool SweepBoxToStudio( IPhysicsSurfaceProps *pProps, const Ray_t& ray, CStudioHdr *pStudioHdr, mstudiohitboxset_t *set, 

-				   matrix3x4_t **hitboxbones, int fContentsMask, trace_t &tr )

-{

-	tr.fraction = 1.0;

-	tr.startsolid = false;

-

-	// OPTIMIZE: Partition these?

-	Ray_t clippedRay = ray;

-	int hitbox = -1;

-	for ( int i = 0; i < set->numhitboxes; i++ )

-	{

-		mstudiobbox_t *pbox = set->pHitbox(i);

-

-		// Filter based on contents mask

-		int fBoneContents = pStudioHdr->pBone( pbox->bone )->contents;

-		if ( ( fBoneContents & fContentsMask ) == 0 )

-			continue;

-		

-		//FIXME: Won't work with scaling!

-		trace_t obbTrace;

-		if ( IntersectRayWithOBB( clippedRay, *hitboxbones[pbox->bone], pbox->bbmin, pbox->bbmax, 0.0f, &obbTrace ) )

-		{

-			tr.startpos = obbTrace.startpos;

-			tr.endpos = obbTrace.endpos;

-			tr.plane = obbTrace.plane;

-			tr.startsolid = obbTrace.startsolid;

-			tr.allsolid = obbTrace.allsolid;

-

-			// This logic here is to shorten the ray each time to get more early outs

-			tr.fraction *= obbTrace.fraction;

-			clippedRay.m_Delta *= obbTrace.fraction;

-			hitbox = i;

-			if (tr.startsolid)

-				break;

-		}

-	}

-

-	if ( hitbox >= 0 )

-	{

-		tr.hitgroup = set->pHitbox(hitbox)->group;

-		tr.hitbox = hitbox;

-		const mstudiobone_t *pBone = pStudioHdr->pBone( set->pHitbox(hitbox)->bone );

-		tr.contents = pBone->contents | CONTENTS_HITBOX;

-		tr.physicsbone = pBone->physicsbone;

-		tr.surface.name = "**studio**";

-		tr.surface.flags = SURF_HITBOX;

-		tr.surface.surfaceProps = pProps->GetSurfaceIndex( pBone->pszSurfaceProp() );

-

-		Assert( tr.physicsbone >= 0 );

-		return true;

-	}

-	return false;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose:

-//-----------------------------------------------------------------------------

-bool TraceToStudio( IPhysicsSurfaceProps *pProps, const Ray_t& ray, CStudioHdr *pStudioHdr, mstudiohitboxset_t *set, 

-				   matrix3x4_t **hitboxbones, int fContentsMask, const Vector &vecOrigin, float flScale, trace_t &tr )

-{

-	if ( !ray.m_IsRay )

-	{

-		return SweepBoxToStudio( pProps, ray, pStudioHdr, set, hitboxbones, fContentsMask, tr );

-	}

-

-	tr.fraction = 1.0;

-	tr.startsolid = false;

-

-	// no hit yet

-	int hitbox = -1;

-	int hitside = -1;

-

-	// OPTIMIZE: Partition these?

-	for ( int i = 0; i < set->numhitboxes; i++ )

-	{

-		mstudiobbox_t *pbox = set->pHitbox(i);

-

-		// Filter based on contents mask

-		int fBoneContents = pStudioHdr->pBone( pbox->bone )->contents;

-		if ( ( fBoneContents & fContentsMask ) == 0 )

-			continue;

-		

-		// columns are axes of the bones in world space, translation is in world space

-		matrix3x4_t& matrix = *hitboxbones[pbox->bone];

-		

-		// Because we're sending in a matrix with scale data, and because the matrix inversion in the hitbox

-		// code does not handle that case, we pre-scale the bones and ray down here and do our collision checks

-		// in unscaled space.  We can then rescale the results afterwards.

-

-		int side = -1;

-		if ( flScale < 1.0f-FLT_EPSILON || flScale > 1.0f+FLT_EPSILON )

-		{

-			matrix3x4_t matScaled;

-			MatrixCopy( matrix, matScaled );

-			

-			float invScale = 1.0f / flScale;

-

-			Vector vecBoneOrigin;

-			MatrixGetColumn( matScaled, 3, vecBoneOrigin );

-			

-			// Pre-scale the origin down

-			Vector vecNewOrigin = vecBoneOrigin - vecOrigin;

-			vecNewOrigin *= invScale;

-			vecNewOrigin += vecOrigin;

-			MatrixSetColumn( vecNewOrigin, 3, matScaled );

-

-			// Scale it uniformly

-			VectorScale( matScaled[0], invScale, matScaled[0] );

-			VectorScale( matScaled[1], invScale, matScaled[1] );

-			VectorScale( matScaled[2], invScale, matScaled[2] );

-			

-			// Pre-scale our ray as well

-			Vector vecRayStart = ray.m_Start - vecOrigin;

-			vecRayStart *= invScale;

-			vecRayStart += vecOrigin;

-			

-			Vector vecRayDelta = ray.m_Delta * invScale;

-

-			Ray_t newRay;

-			newRay.Init( vecRayStart, vecRayStart + vecRayDelta );  

-			

-			side = ClipRayToHitbox( newRay, pbox, matScaled, tr );

-		}

-		else

-		{

-			side = ClipRayToHitbox( ray, pbox, matrix, tr );

-		}

-

-		if ( side >= 0 )

-		{

-			hitbox = i;

-			hitside = side;

-		}

-	}

-

-	if ( hitbox >= 0 )

-	{

-		mstudiobbox_t *pbox = set->pHitbox(hitbox);

-		VectorMA( ray.m_Start, tr.fraction, ray.m_Delta, tr.endpos );

-		tr.hitgroup = set->pHitbox(hitbox)->group;

-		tr.hitbox = hitbox;

-		const mstudiobone_t *pBone = pStudioHdr->pBone( pbox->bone );

-		tr.contents = pBone->contents | CONTENTS_HITBOX;

-		tr.physicsbone = pBone->physicsbone;

-		tr.surface.name = "**studio**";

-		tr.surface.flags = SURF_HITBOX;

-		tr.surface.surfaceProps = pProps->GetSurfaceIndex( pBone->pszSurfaceProp() );

-

-		Assert( tr.physicsbone >= 0 );

-		matrix3x4_t& matrix = *hitboxbones[pbox->bone];

-		if ( hitside >= 3 )

-		{

-			hitside -= 3;

-			tr.plane.normal[0] = matrix[0][hitside];

-			tr.plane.normal[1] = matrix[1][hitside];

-			tr.plane.normal[2] = matrix[2][hitside];

-			//tr.plane.dist = DotProduct( tr.plane.normal, Vector(matrix[0][3], matrix[1][3], matrix[2][3] ) ) + pbox->bbmax[hitside];

-		}

-		else

-		{

-			tr.plane.normal[0] = -matrix[0][hitside];

-			tr.plane.normal[1] = -matrix[1][hitside];

-			tr.plane.normal[2] = -matrix[2][hitside];

-			//tr.plane.dist = DotProduct( tr.plane.normal, Vector(matrix[0][3], matrix[1][3], matrix[2][3] ) ) - pbox->bbmin[hitside];

-		}

-		// simpler plane constant equation

-		tr.plane.dist = DotProduct( tr.endpos, tr.plane.normal );

-		tr.plane.type = 3;

-		return true;

-	}

-	return false;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: returns array of animations and weightings for a sequence based on current pose parameters

-//-----------------------------------------------------------------------------

-

-void Studio_SeqAnims( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, const float poseParameter[], mstudioanimdesc_t *panim[4], float *weight )

-{

-#if _DEBUG

-	VPROF_INCREMENT_COUNTER("SEQ_ANIMS",1);

-#endif

-	if (!pStudioHdr || iSequence >= pStudioHdr->GetNumSeq())

-	{

-		weight[0] = weight[1] = weight[2] = weight[3] = 0.0;

-		return;

-	}

-

-	int i0 = 0, i1 = 0;

-	float s0 = 0, s1 = 0;

-	

-	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, iSequence, 0, s0, i0 );

-	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, iSequence, 1, s1, i1 );

-

-	panim[0] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0  , i1 ) ) );

-	weight[0] = (1 - s0) * (1 - s1);

-

-	panim[1] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0+1, i1 ) ) );

-	weight[1] = (s0) * (1 - s1);

-

-	panim[2] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0  , i1+1 ) ) );

-	weight[2] = (1 - s0) * (s1);

-

-	panim[3] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0+1, i1+1 ) ) );

-	weight[3] = (s0) * (s1);

-

-	Assert( weight[0] >= 0.0f && weight[1] >= 0.0f && weight[2] >= 0.0f && weight[3] >= 0.0f );

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: returns max frame number for a sequence

-//-----------------------------------------------------------------------------

-

-int Studio_MaxFrame( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )

-{

-	mstudioanimdesc_t *panim[4];

-	float	weight[4];

-

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );

-	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );

-

-	float maxFrame = 0;

-	for (int i = 0; i < 4; i++)

-	{

-		if (weight[i] > 0)

-		{

-			maxFrame += panim[i]->numframes * weight[i];

-		}

-	}

-

-	if ( maxFrame > 1 )

-		maxFrame -= 1;

-	

-

-	// FIXME: why does the weights sometimes not exactly add it 1.0 and this sometimes rounds down?

-	return (maxFrame + 0.01);

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: returns frames per second of a sequence

-//-----------------------------------------------------------------------------

-

-float Studio_FPS( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )

-{

-	mstudioanimdesc_t *panim[4];

-	float	weight[4];

-

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );

-	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );

-

-	float t = 0;

-

-	for (int i = 0; i < 4; i++)

-	{

-		if (weight[i] > 0)

-		{

-			t += panim[i]->fps * weight[i];

-		}

-	}

-	return t;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: returns cycles per second of a sequence (cycles/second)

-//-----------------------------------------------------------------------------

-

-float Studio_CPS( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, const float poseParameter[] )

-{

-	mstudioanimdesc_t *panim[4];

-	float	weight[4];

-

-	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );

-

-	float t = 0;

-

-	for (int i = 0; i < 4; i++)

-	{

-		if (weight[i] > 0 && panim[i]->numframes > 1)

-		{

-			t += (panim[i]->fps / (panim[i]->numframes - 1)) * weight[i];

-		}

-	}

-	return t;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: returns length (in seconds) of a sequence (seconds/cycle)

-//-----------------------------------------------------------------------------

-

-float Studio_Duration( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )

-{

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );

-	float cps = Studio_CPS( pStudioHdr, seqdesc, iSequence, poseParameter );

-

-	if( cps == 0 )

-		return 0.0f;

-

-	return 1.0f/cps;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate changes in position and angle relative to the start of an animations cycle

-// Output:	updated position and angle, relative to the origin

-//			returns false if animation is not a movement animation

-//-----------------------------------------------------------------------------

-

-bool Studio_AnimPosition( mstudioanimdesc_t *panim, float flCycle, Vector &vecPos, QAngle &vecAngle )

-{

-	float	prevframe = 0;

-	vecPos.Init( );

-	vecAngle.Init( );

-

-	if (panim->nummovements == 0)

-		return false;

-

-	int iLoops = 0;

-	if (flCycle > 1.0)

-	{

-		iLoops = (int)flCycle;

-	}

-	else if (flCycle < 0.0)

-	{

-		iLoops = (int)flCycle - 1;

-	}

-	flCycle = flCycle - iLoops;

-

-	float	flFrame = flCycle * (panim->numframes - 1);

-

-	for (int i = 0; i < panim->nummovements; i++)

-	{

-		mstudiomovement_t *pmove = panim->pMovement( i );

-

-		if (pmove->endframe >= flFrame)

-		{

-			float f = (flFrame - prevframe) / (pmove->endframe - prevframe);

-

-			float d = pmove->v0 * f + 0.5 * (pmove->v1 - pmove->v0) * f * f;

-

-			vecPos = vecPos + d * pmove->vector;

-			vecAngle.y = vecAngle.y * (1 - f) + pmove->angle * f;

-			if (iLoops != 0)

-			{

-				mstudiomovement_t *pmove = panim->pMovement( panim->nummovements - 1 );

-				vecPos = vecPos + iLoops * pmove->position; 

-				vecAngle.y = vecAngle.y + iLoops * pmove->angle; 

-			}

-			return true;

-		}

-		else

-		{

-			prevframe = pmove->endframe;

-			vecPos = pmove->position;

-			vecAngle.y = pmove->angle;

-		}

-	}

-

-	return false;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate instantaneous velocity in ips at a given point 

-//			in the animations cycle

-// Output:	velocity vector, relative to identity orientation

-//			returns false if animation is not a movement animation

-//-----------------------------------------------------------------------------

-

-bool Studio_AnimVelocity( mstudioanimdesc_t *panim, float flCycle, Vector &vecVelocity )

-{

-	float	prevframe = 0;

-

-	float	flFrame = flCycle * (panim->numframes - 1);

-	flFrame = flFrame - (int)(flFrame / (panim->numframes - 1));

-

-	for (int i = 0; i < panim->nummovements; i++)

-	{

-		mstudiomovement_t *pmove = panim->pMovement( i );

-

-		if (pmove->endframe >= flFrame)

-		{

-			float f = (flFrame - prevframe) / (pmove->endframe - prevframe);

-

-			float vel = pmove->v0 * (1 - f) + pmove->v1 * f;

-			// scale from per block to per sec velocity

-			vel = vel * panim->fps / (pmove->endframe - prevframe);

-

-			vecVelocity = pmove->vector * vel;

-			return true;

-		}

-		else

-		{

-			prevframe = pmove->endframe;

-		}

-	}

-	return false;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate changes in position and angle between two points in an animation cycle

-// Output:	updated position and angle, relative to CycleFrom being at the origin

-//			returns false if animation is not a movement animation

-//-----------------------------------------------------------------------------

-

-bool Studio_AnimMovement( mstudioanimdesc_t *panim, float flCycleFrom, float flCycleTo, Vector &deltaPos, QAngle &deltaAngle )

-{

-	if (panim->nummovements == 0)

-		return false;

-

-	Vector startPos;

-	QAngle startA;

-	Studio_AnimPosition( panim, flCycleFrom, startPos, startA );

-

-	Vector endPos;

-	QAngle endA;

-	Studio_AnimPosition( panim, flCycleTo, endPos, endA );

-

-	Vector tmp = endPos - startPos;

-	deltaAngle.y = endA.y - startA.y;

-	VectorYawRotate( tmp, -startA.y, deltaPos );

-

-	return true;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: finds how much of an animation to play to move given linear distance

-//-----------------------------------------------------------------------------

-

-float Studio_FindAnimDistance( mstudioanimdesc_t *panim, float flDist )

-{

-	float	prevframe = 0;

-

-	if (flDist <= 0)

-		return 0.0;

-

-	for (int i = 0; i < panim->nummovements; i++)

-	{

-		mstudiomovement_t *pmove = panim->pMovement( i );

-

-		float flMove = (pmove->v0 + pmove->v1) * 0.5;

-

-		if (flMove >= flDist)

-		{

-			float root1, root2;

-

-			// d = V0 * t + 1/2 (V1-V0) * t^2

-			if (SolveQuadratic( 0.5 * (pmove->v1 - pmove->v0), pmove->v0, -flDist, root1, root2 ))

-			{

-				float cpf = 1.0 / (panim->numframes - 1);  // cycles per frame

-

-				return (prevframe + root1 * (pmove->endframe - prevframe)) * cpf;

-			}

-			return 0.0;

-		}

-		else

-		{

-			flDist -= flMove;

-			prevframe = pmove->endframe;

-		}

-	}

-	return 1.0;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate changes in position and angle between two points in a sequences cycle

-// Output:	updated position and angle, relative to CycleFrom being at the origin

-//			returns false if sequence is not a movement sequence

-//-----------------------------------------------------------------------------

-

-bool Studio_SeqMovement( const CStudioHdr *pStudioHdr, int iSequence, float flCycleFrom, float flCycleTo, const float poseParameter[], Vector &deltaPos, QAngle &deltaAngles )

-{

-	mstudioanimdesc_t *panim[4];

-	float	weight[4];

-

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );

-

-	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );

-	

-	deltaPos.Init( );

-	deltaAngles.Init( );

-

-	bool found = false;

-

-	for (int i = 0; i < 4; i++)

-	{

-		if (weight[i])

-		{

-			Vector localPos;

-			QAngle localAngles;

-

-			localPos.Init();

-			localAngles.Init();

-

-			if (Studio_AnimMovement( panim[i], flCycleFrom, flCycleTo, localPos, localAngles ))

-			{

-				found = true;

-				deltaPos = deltaPos + localPos * weight[i];

-				// FIXME: this makes no sense

-				deltaAngles = deltaAngles + localAngles * weight[i];

-			}

-			else if (!(panim[i]->flags & STUDIO_DELTA) && panim[i]->nummovements == 0 && seqdesc.weight(0) > 0.0)

-			{

-				found = true;

-			}

-		}

-	}

-	return found;

-}

-

-

-//-----------------------------------------------------------------------------

-// Purpose: calculate instantaneous velocity in ips at a given point in the sequence's cycle

-// Output:	velocity vector, relative to identity orientation

-//			returns false if sequence is not a movement sequence

-//-----------------------------------------------------------------------------

-

-bool Studio_SeqVelocity( const CStudioHdr *pStudioHdr, int iSequence, float flCycle, const float poseParameter[], Vector &vecVelocity )

-{

-	mstudioanimdesc_t *panim[4];

-	float	weight[4];

-

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );

-	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );

-	

-	vecVelocity.Init( );

-

-	bool found = false;

-

-	for (int i = 0; i < 4; i++)

-	{

-		if (weight[i])

-		{

-			Vector vecLocalVelocity;

-

-			if (Studio_AnimVelocity( panim[i], flCycle, vecLocalVelocity ))

-			{

-				vecVelocity = vecVelocity + vecLocalVelocity * weight[i];

-				found = true;

-			}

-		}

-	}

-	return found;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: finds how much of an sequence to play to move given linear distance

-//-----------------------------------------------------------------------------

-

-float Studio_FindSeqDistance( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[], float flDist )

-{

-	mstudioanimdesc_t *panim[4];

-	float	weight[4];

-

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );

-	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );

-	

-	float flCycle = 0;

-

-	for (int i = 0; i < 4; i++)

-	{

-		if (weight[i])

-		{

-			float flLocalCycle = Studio_FindAnimDistance( panim[i], flDist );

-			flCycle = flCycle + flLocalCycle * weight[i];

-		}

-	}

-	return flCycle;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: lookup attachment by name

-//-----------------------------------------------------------------------------

-

-int Studio_FindAttachment( const CStudioHdr *pStudioHdr, const char *pAttachmentName )

-{

-	if ( pStudioHdr && pStudioHdr->SequencesAvailable() )

-	{

-		// Extract the bone index from the name

-		for (int i = 0; i < pStudioHdr->GetNumAttachments(); i++)

-		{

-			if (!V_stricmp(pAttachmentName,((CStudioHdr *)pStudioHdr)->pAttachment(i).pszName( ))) 

-			{

-				return i;

-			}

-		}

-	}

-

-	return -1;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: lookup attachments by substring. Randomly return one of the matching attachments.

-//-----------------------------------------------------------------------------

-

-int Studio_FindRandomAttachment( const CStudioHdr *pStudioHdr, const char *pAttachmentName )

-{

-	if ( pStudioHdr )

-	{

-		// First move them all matching attachments into a list

-		CUtlVector<int> matchingAttachments;

-

-		// Extract the bone index from the name

-		for (int i = 0; i < pStudioHdr->GetNumAttachments(); i++)

-		{

-			if ( strstr( ((CStudioHdr *)pStudioHdr)->pAttachment(i).pszName(), pAttachmentName ) ) 

-			{

-				matchingAttachments.AddToTail(i);

-			}

-		}

-

-		// Then randomly return one of the attachments

-		if ( matchingAttachments.Size() > 0 )

-			return matchingAttachments[ RandomInt( 0, matchingAttachments.Size()-1 ) ];

-	}

-

-	return -1;

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: lookup bone by name

-//-----------------------------------------------------------------------------

-

-int Studio_BoneIndexByName( const CStudioHdr *pStudioHdr, const char *pName )

-{

-	// binary search for the bone matching pName

-	int start = 0, end = pStudioHdr->numbones()-1;

-	const byte *pBoneTable = pStudioHdr->GetBoneTableSortedByName();

-	mstudiobone_t *pbones = pStudioHdr->pBone( 0 );

-	while (start <= end)

-	{

-		int mid = (start + end) >> 1;

-		int cmp = Q_stricmp( pbones[pBoneTable[mid]].pszName(), pName );

-		

-		if ( cmp < 0 )

-		{

-			start = mid + 1;

-		}

-		else if ( cmp > 0 )

-		{

-			end = mid - 1;

-		}

-		else

-		{

-			return pBoneTable[mid];

-		}

-	}

-	return -1;

-}

-

-const char *Studio_GetDefaultSurfaceProps( CStudioHdr *pstudiohdr )

-{

-	return pstudiohdr->pszSurfaceProp();

-}

-

-float Studio_GetMass( CStudioHdr *pstudiohdr )

-{

-	return pstudiohdr->mass();

-}

-

-//-----------------------------------------------------------------------------

-// Purpose: return pointer to sequence key value buffer

-//-----------------------------------------------------------------------------

-

-const char *Studio_GetKeyValueText( const CStudioHdr *pStudioHdr, int iSequence )

-{

-	if (pStudioHdr && pStudioHdr->SequencesAvailable())

-	{

-		if (iSequence >= 0 && iSequence < pStudioHdr->GetNumSeq())

-		{

-			return ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence ).KeyValueText();

-		}

-	}

-	return NULL;

-}

-

-bool Studio_PrefetchSequence( const CStudioHdr *pStudioHdr, int iSequence )

-{

-	bool pendingload = false;

-	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );

-	int size0 = seqdesc.groupsize[ 0 ];

-	int size1 = seqdesc.groupsize[ 1 ];

-	for ( int i = 0; i < size0; ++i )

-	{

-		for ( int j = 0; j < size1; ++j )

-		{

-			mstudioanimdesc_t &animdesc = ((CStudioHdr *)pStudioHdr)->pAnimdesc( seqdesc.anim( i, j ) );

-			int iFrame = 0;

-			mstudioanim_t *panim = animdesc.pAnim( &iFrame );

-			if ( !panim )

-			{

-				pendingload = true;

-			}

-		}

-	}

-

-	// Everything for this sequence is resident?

-	return !pendingload;

-}

+//========= Copyright Valve Corporation, All rights reserved. ============//
+//
+// Purpose: 
+//
+// $NoKeywords: $
+//
+//===========================================================================//
+
+#include "tier0/dbg.h"
+#include "mathlib/mathlib.h"
+#include "bone_setup.h"
+#include <string.h>
+
+#include "collisionutils.h"
+#include "vstdlib/random.h"
+#include "tier0/vprof.h"
+#include "bone_accessor.h"
+#include "mathlib/ssequaternion.h"
+#include "bitvec.h"
+#include "datamanager.h"
+#include "convar.h"
+#include "tier0/tslist.h"
+#include "vphysics_interface.h"
+#ifdef CLIENT_DLL
+	#include "posedebugger.h"
+#endif
+
+// memdbgon must be the last include file in a .cpp file!!!
+#include "tier0/memdbgon.h"
+
+class CBoneSetup
+{
+public:
+	CBoneSetup( const CStudioHdr *pStudioHdr, int boneMask, const float poseParameter[], IPoseDebugger *pPoseDebugger = NULL );
+	void InitPose( Vector pos[], Quaternion q[] );
+	void AccumulatePose( Vector pos[], Quaternion q[], int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );
+	void CalcAutoplaySequences(	Vector pos[], Quaternion q[], float flRealTime, CIKContext *pIKContext );
+private:
+	void AddSequenceLayers( Vector pos[], Quaternion q[], mstudioseqdesc_t &seqdesc, int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );
+	void AddLocalLayers( Vector pos[], Quaternion q[], mstudioseqdesc_t &seqdesc, int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );
+public:
+	const CStudioHdr *m_pStudioHdr;
+	int m_boneMask;
+	const float *m_flPoseParameter;
+	IPoseDebugger *m_pPoseDebugger;
+};
+
+// -----------------------------------------------------------------
+template <typename T>
+class CBoneSetupMemoryPool
+{
+public:
+	T *Alloc()
+	{
+		T *p = (T *)m_FreeBlocks.Pop();
+		if ( !p )
+		{
+			p = new T[MAXSTUDIOBONES];
+			if ( ((size_t)p) % TSLIST_NODE_ALIGNMENT != 0 )
+			{
+				DebuggerBreak();
+			}
+		}
+
+		return p;
+	}
+
+	void Free( T *p )
+	{
+		m_FreeBlocks.Push( (TSLNodeBase_t *)p );
+	}
+
+private:
+	CTSListBase m_FreeBlocks;
+};
+
+CBoneSetupMemoryPool<Quaternion> g_QaternionPool;
+CBoneSetupMemoryPool<Vector> g_VectorPool;
+CBoneSetupMemoryPool<matrix3x4_t> g_MatrixPool;
+
+// -----------------------------------------------------------------
+CBoneCache *CBoneCache::CreateResource( const bonecacheparams_t &params )
+{
+	short studioToCachedIndex[MAXSTUDIOBONES];
+	short cachedToStudioIndex[MAXSTUDIOBONES];
+	int cachedBoneCount = 0;
+	for ( int i = 0; i < params.pStudioHdr->numbones(); i++ )
+	{
+		// skip bones that aren't part of the boneMask (and aren't the root bone)
+		if (i != 0 && !(params.pStudioHdr->boneFlags(i) & params.boneMask))
+		{
+			studioToCachedIndex[i] = -1;
+			continue;
+		}
+		studioToCachedIndex[i] = cachedBoneCount;
+		cachedToStudioIndex[cachedBoneCount] = i;
+		cachedBoneCount++;
+	}
+	int tableSizeStudio = sizeof(short) * params.pStudioHdr->numbones();
+	int tableSizeCached = sizeof(short) * cachedBoneCount;
+	int matrixSize = sizeof(matrix3x4_t) * cachedBoneCount;
+	int size = ( sizeof(CBoneCache) + tableSizeStudio + tableSizeCached + matrixSize + 3 ) & ~3;
+	
+	CBoneCache *pMem = (CBoneCache *)malloc( size );
+	Construct( pMem );
+	pMem->Init( params, size, studioToCachedIndex, cachedToStudioIndex, cachedBoneCount );
+	return pMem;
+}
+
+unsigned int CBoneCache::EstimatedSize( const bonecacheparams_t &params )
+{
+	// conservative estimate - max size
+	return ( params.pStudioHdr->numbones() * (sizeof(short) + sizeof(short) + sizeof(matrix3x4_t)) + 3 ) & ~3;
+}
+
+void CBoneCache::DestroyResource()
+{
+	free( this );
+}
+
+
+CBoneCache::CBoneCache()
+{
+	m_size = 0;
+	m_cachedBoneCount = 0;
+}
+
+void CBoneCache::Init( const bonecacheparams_t &params, unsigned int size, short *pStudioToCached, short *pCachedToStudio, int cachedBoneCount ) 
+{
+	m_cachedBoneCount = cachedBoneCount;
+	m_size = size;
+	m_timeValid = params.curtime;
+	m_boneMask = params.boneMask;
+
+	int studioTableSize = params.pStudioHdr->numbones() * sizeof(short);
+	m_cachedToStudioOffset = studioTableSize;
+	memcpy( StudioToCached(), pStudioToCached, studioTableSize );
+
+	int cachedTableSize = cachedBoneCount * sizeof(short);
+	memcpy( CachedToStudio(), pCachedToStudio, cachedTableSize );
+
+	m_matrixOffset = ( m_cachedToStudioOffset + cachedTableSize + 3 ) & ~3;
+	
+	UpdateBones( params.pBoneToWorld, params.pStudioHdr->numbones(), params.curtime );
+}
+
+void CBoneCache::UpdateBones( const matrix3x4_t *pBoneToWorld, int numbones, float curtime )
+{
+	matrix3x4_t *pBones = BoneArray();
+	const short *pCachedToStudio = CachedToStudio();
+
+	for ( int i = 0; i < m_cachedBoneCount; i++ )
+	{
+		int index = pCachedToStudio[i];
+		MatrixCopy( pBoneToWorld[index], pBones[i] );
+	}
+	m_timeValid = curtime;
+}
+
+matrix3x4_t *CBoneCache::GetCachedBone( int studioIndex )
+{
+	int cachedIndex = StudioToCached()[studioIndex];
+	if ( cachedIndex >= 0 )
+	{
+		return BoneArray() + cachedIndex;
+	}
+	return NULL;
+}
+
+void CBoneCache::ReadCachedBones( matrix3x4_t *pBoneToWorld )
+{
+	matrix3x4_t *pBones = BoneArray();
+	const short *pCachedToStudio = CachedToStudio();
+	for ( int i = 0; i < m_cachedBoneCount; i++ )
+	{
+		MatrixCopy( pBones[i], pBoneToWorld[pCachedToStudio[i]] );
+	}
+}
+
+void CBoneCache::ReadCachedBonePointers( matrix3x4_t **bones, int numbones )
+{
+	memset( bones, 0, sizeof(matrix3x4_t *) * numbones );
+	matrix3x4_t *pBones = BoneArray();
+	const short *pCachedToStudio = CachedToStudio();
+	for ( int i = 0; i < m_cachedBoneCount; i++ )
+	{
+		bones[pCachedToStudio[i]] = pBones + i;
+	}
+}
+
+bool CBoneCache::IsValid( float curtime, float dt )
+{
+	if ( curtime - m_timeValid <= dt )
+		return true;
+	return false;
+}
+
+
+// private functions
+matrix3x4_t *CBoneCache::BoneArray()
+{
+	return (matrix3x4_t *)( (char *)(this+1) + m_matrixOffset );
+}
+
+short *CBoneCache::StudioToCached()
+{
+	return (short *)( (char *)(this+1) );
+}
+
+short *CBoneCache::CachedToStudio()
+{
+	return (short *)( (char *)(this+1) + m_cachedToStudioOffset );
+}
+
+// Construct a singleton
+static CDataManager<CBoneCache, bonecacheparams_t, CBoneCache *, CThreadFastMutex> g_StudioBoneCache( 128 * 1024L );
+
+CBoneCache *Studio_GetBoneCache( memhandle_t cacheHandle )
+{
+	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );
+	return g_StudioBoneCache.GetResource_NoLock( cacheHandle );
+}
+
+memhandle_t Studio_CreateBoneCache( bonecacheparams_t &params )
+{
+	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );
+	return g_StudioBoneCache.CreateResource( params );
+}
+
+void Studio_DestroyBoneCache( memhandle_t cacheHandle )
+{
+	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );
+	g_StudioBoneCache.DestroyResource( cacheHandle );
+}
+
+void Studio_InvalidateBoneCache( memhandle_t cacheHandle )
+{
+	AUTO_LOCK( g_StudioBoneCache.AccessMutex() );
+	CBoneCache *pCache = g_StudioBoneCache.GetResource_NoLock( cacheHandle );
+	if ( pCache )
+	{
+		pCache->m_timeValid = -1.0f;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+
+void BuildBoneChain(
+	const CStudioHdr *pStudioHdr,
+	const matrix3x4_t &rootxform,
+	const Vector pos[], 
+	const Quaternion q[], 
+	int	iBone,
+	matrix3x4_t *pBoneToWorld )
+{
+	CBoneBitList boneComputed;
+	BuildBoneChain( pStudioHdr, rootxform, pos, q, iBone, pBoneToWorld, boneComputed );
+	return;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: return a sub frame rotation for a single bone
+//-----------------------------------------------------------------------------
+void ExtractAnimValue( int frame, mstudioanimvalue_t *panimvalue, float scale, float &v1, float &v2 )
+{
+	if ( !panimvalue )
+	{
+		v1 = v2 = 0;
+		return;
+	}
+
+	// Avoids a crash reading off the end of the data
+	// There is probably a better long-term solution; Ken is going to look into it.
+	if ( ( panimvalue->num.total == 1 ) && ( panimvalue->num.valid == 1 ) )
+	{
+		v1 = v2 = panimvalue[1].value * scale;
+		return;
+	}
+
+	int k = frame;
+
+	// find the data list that has the frame
+	while (panimvalue->num.total <= k)
+	{
+		k -= panimvalue->num.total;
+		panimvalue += panimvalue->num.valid + 1;
+		if ( panimvalue->num.total == 0 )
+		{
+			Assert( 0 ); // running off the end of the animation stream is bad
+			v1 = v2 = 0;
+			return;
+		}
+	}
+	if (panimvalue->num.valid > k)
+	{
+		// has valid animation data
+		v1 = panimvalue[k+1].value * scale;
+
+		if (panimvalue->num.valid > k + 1)
+		{
+			// has valid animation blend data
+			v2 = panimvalue[k+2].value * scale;
+		}
+		else
+		{
+			if (panimvalue->num.total > k + 1)
+			{
+				// data repeats, no blend
+				v2 = v1;
+			}
+			else
+			{
+				// pull blend from first data block in next list
+				v2 = panimvalue[panimvalue->num.valid+2].value * scale;
+			}
+		}
+	}
+	else
+	{
+		// get last valid data block
+		v1 = panimvalue[panimvalue->num.valid].value * scale;
+		if (panimvalue->num.total > k + 1)
+		{
+			// data repeats, no blend
+			v2 = v1;
+		}
+		else
+		{
+			// pull blend from first data block in next list
+			v2 = panimvalue[panimvalue->num.valid + 2].value * scale;
+		}
+	}
+}
+
+
+void ExtractAnimValue( int frame, mstudioanimvalue_t *panimvalue, float scale, float &v1 )
+{
+	if ( !panimvalue )
+	{
+		v1 = 0;
+		return;
+	}
+
+	int k = frame;
+
+	while (panimvalue->num.total <= k)
+	{
+		k -= panimvalue->num.total;
+		panimvalue += panimvalue->num.valid + 1;
+		if ( panimvalue->num.total == 0 )
+		{
+			Assert( 0 ); // running off the end of the animation stream is bad
+			v1 = 0;
+			return;
+		}
+	}
+	if (panimvalue->num.valid > k)
+	{
+		v1 = panimvalue[k+1].value * scale;
+	}
+	else
+	{
+		// get last valid data block
+		v1 = panimvalue[panimvalue->num.valid].value * scale;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: return a sub frame rotation for a single bone
+//-----------------------------------------------------------------------------
+void CalcBoneQuaternion( int frame, float s, 
+						const Quaternion &baseQuat, const RadianEuler &baseRot, const Vector &baseRotScale, 
+						int iBaseFlags, const Quaternion &baseAlignment, 
+						const mstudioanim_t *panim, Quaternion &q )
+{
+	if ( panim->flags & STUDIO_ANIM_RAWROT )
+	{
+		q = *(panim->pQuat48());
+		Assert( q.IsValid() );
+		return;
+	} 
+	
+	if ( panim->flags & STUDIO_ANIM_RAWROT2 )
+	{
+		q = *(panim->pQuat64());
+		Assert( q.IsValid() );
+		return;
+	}
+
+	if ( !(panim->flags & STUDIO_ANIM_ANIMROT) )
+	{
+		if (panim->flags & STUDIO_ANIM_DELTA)
+		{
+			q.Init( 0.0f, 0.0f, 0.0f, 1.0f );
+		}
+		else
+		{
+			q = baseQuat;
+		}
+		return;
+	}
+
+	mstudioanim_valueptr_t *pValuesPtr = panim->pRotV();
+
+	if (s > 0.001f)
+	{
+		QuaternionAligned	q1, q2;
+		RadianEuler			angle1, angle2;
+
+		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 0 ), baseRotScale.x, angle1.x, angle2.x );
+		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 1 ), baseRotScale.y, angle1.y, angle2.y );
+		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 2 ), baseRotScale.z, angle1.z, angle2.z );
+
+		if (!(panim->flags & STUDIO_ANIM_DELTA))
+		{
+			angle1.x = angle1.x + baseRot.x;
+			angle1.y = angle1.y + baseRot.y;
+			angle1.z = angle1.z + baseRot.z;
+			angle2.x = angle2.x + baseRot.x;
+			angle2.y = angle2.y + baseRot.y;
+			angle2.z = angle2.z + baseRot.z;
+		}
+
+		Assert( angle1.IsValid() && angle2.IsValid() );
+		if (angle1.x != angle2.x || angle1.y != angle2.y || angle1.z != angle2.z)
+		{
+			AngleQuaternion( angle1, q1 );
+			AngleQuaternion( angle2, q2 );
+
+	#ifdef _X360
+			fltx4 q1simd, q2simd, qsimd;
+			q1simd = LoadAlignedSIMD( q1 );
+			q2simd = LoadAlignedSIMD( q2 );
+			qsimd = QuaternionBlendSIMD( q1simd, q2simd, s );
+			StoreUnalignedSIMD( q.Base(), qsimd );
+	#else
+			QuaternionBlend( q1, q2, s, q );
+	#endif
+		}
+		else
+		{
+			AngleQuaternion( angle1, q );
+		}
+	}
+	else
+	{
+		RadianEuler			angle;
+
+		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 0 ), baseRotScale.x, angle.x );
+		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 1 ), baseRotScale.y, angle.y );
+		ExtractAnimValue( frame, pValuesPtr->pAnimvalue( 2 ), baseRotScale.z, angle.z );
+
+		if (!(panim->flags & STUDIO_ANIM_DELTA))
+		{
+			angle.x = angle.x + baseRot.x;
+			angle.y = angle.y + baseRot.y;
+			angle.z = angle.z + baseRot.z;
+		}
+
+		Assert( angle.IsValid() );
+		AngleQuaternion( angle, q );
+	}
+
+	Assert( q.IsValid() );
+
+	// align to unified bone
+	if (!(panim->flags & STUDIO_ANIM_DELTA) && (iBaseFlags & BONE_FIXED_ALIGNMENT))
+	{
+		QuaternionAlign( baseAlignment, q, q );
+	}
+}
+
+inline void CalcBoneQuaternion( int frame, float s, 
+						const mstudiobone_t *pBone,
+						const mstudiolinearbone_t *pLinearBones,
+						const mstudioanim_t *panim, Quaternion &q )
+{
+	if (pLinearBones)
+	{
+		CalcBoneQuaternion( frame, s, pLinearBones->quat(panim->bone), pLinearBones->rot(panim->bone), pLinearBones->rotscale(panim->bone), pLinearBones->flags(panim->bone), pLinearBones->qalignment(panim->bone), panim, q );
+	}
+	else
+	{
+		CalcBoneQuaternion( frame, s, pBone->quat, pBone->rot, pBone->rotscale, pBone->flags, pBone->qAlignment, panim, q );
+	}
+}
+
+						
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: return a sub frame position for a single bone
+//-----------------------------------------------------------------------------
+void CalcBonePosition(	int frame, float s,
+						const Vector &basePos, const Vector &baseBoneScale, 
+						const mstudioanim_t *panim, Vector &pos	)
+{
+	if (panim->flags & STUDIO_ANIM_RAWPOS)
+	{
+		pos = *(panim->pPos());
+		Assert( pos.IsValid() );
+
+		return;
+	}
+	else if (!(panim->flags & STUDIO_ANIM_ANIMPOS))
+	{
+		if (panim->flags & STUDIO_ANIM_DELTA)
+		{
+			pos.Init( 0.0f, 0.0f, 0.0f );
+		}
+		else
+		{
+			pos = basePos;
+		}
+		return;
+	}
+
+	mstudioanim_valueptr_t *pPosV = panim->pPosV();
+	int					j;
+
+	if (s > 0.001f)
+	{
+		float v1, v2;
+		for (j = 0; j < 3; j++)
+		{
+			ExtractAnimValue( frame, pPosV->pAnimvalue( j ), baseBoneScale[j], v1, v2 );
+			pos[j] = v1 * (1.0 - s) + v2 * s;
+		}
+	}
+	else
+	{
+		for (j = 0; j < 3; j++)
+		{
+			ExtractAnimValue( frame, pPosV->pAnimvalue( j ), baseBoneScale[j], pos[j] );
+		}
+	}
+
+	if (!(panim->flags & STUDIO_ANIM_DELTA))
+	{
+		pos.x = pos.x + basePos.x;
+		pos.y = pos.y + basePos.y;
+		pos.z = pos.z + basePos.z;
+	}
+
+	Assert( pos.IsValid() );
+}
+
+
+inline void CalcBonePosition( int frame, float s, 
+						const mstudiobone_t *pBone,
+						const mstudiolinearbone_t *pLinearBones,
+						const mstudioanim_t *panim, Vector &pos )
+{
+	if (pLinearBones)
+	{
+		CalcBonePosition( frame, s, pLinearBones->pos(panim->bone), pLinearBones->posscale(panim->bone), panim, pos );
+	}
+	else
+	{
+		CalcBonePosition( frame, s, pBone->pos, pBone->posscale, panim, pos );
+	}
+}
+
+
+
+void SetupSingleBoneMatrix( 
+	CStudioHdr *pOwnerHdr, 
+	int nSequence, 
+	int iFrame,
+	int iBone, 
+	matrix3x4_t &mBoneLocal )
+{
+	mstudioseqdesc_t &seqdesc = pOwnerHdr->pSeqdesc( nSequence );
+	mstudioanimdesc_t &animdesc = pOwnerHdr->pAnimdesc( seqdesc.anim( 0, 0 ) );
+	int iLocalFrame = iFrame;
+	mstudioanim_t *panim = animdesc.pAnim( &iLocalFrame );
+	float s = 0;
+	mstudiobone_t *pbone = pOwnerHdr->pBone( iBone );
+
+	Quaternion boneQuat;
+	Vector bonePos;
+
+	// search for bone
+	while (panim && panim->bone != iBone)
+	{
+		panim = panim->pNext();
+	}
+
+	// look up animation if found, if not, initialize
+	if (panim && seqdesc.weight(iBone) > 0)
+	{
+		CalcBoneQuaternion( iLocalFrame, s, pbone, NULL, panim, boneQuat );
+		CalcBonePosition  ( iLocalFrame, s, pbone, NULL, panim, bonePos );
+	}
+	else if (animdesc.flags & STUDIO_DELTA)
+	{
+		boneQuat.Init( 0.0f, 0.0f, 0.0f, 1.0f );
+		bonePos.Init( 0.0f, 0.0f, 0.0f );
+	}
+	else
+	{
+		boneQuat = pbone->quat;
+		bonePos = pbone->pos;
+	}
+
+	QuaternionMatrix( boneQuat, bonePos, mBoneLocal );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+static void CalcDecompressedAnimation( const mstudiocompressedikerror_t *pCompressed, int iFrame, float fraq, Vector &pos, Quaternion &q )
+{
+	if (fraq > 0.0001f)
+	{
+		Vector p1, p2;
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 0 ), pCompressed->scale[0], p1.x, p2.x );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 1 ), pCompressed->scale[1], p1.y, p2.y );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 2 ), pCompressed->scale[2], p1.z, p2.z );
+		pos = p1 * (1 - fraq) + p2 * fraq;
+
+		Quaternion			q1, q2;
+		RadianEuler			angle1, angle2;
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 3 ), pCompressed->scale[3], angle1.x, angle2.x );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 4 ), pCompressed->scale[4], angle1.y, angle2.y );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 5 ), pCompressed->scale[5], angle1.z, angle2.z );
+
+		if (angle1.x != angle2.x || angle1.y != angle2.y || angle1.z != angle2.z)
+		{
+			AngleQuaternion( angle1, q1 );
+			AngleQuaternion( angle2, q2 );
+			QuaternionBlend( q1, q2, fraq, q );
+		}
+		else
+		{
+			AngleQuaternion( angle1, q );
+		}
+	}
+	else
+	{
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 0 ), pCompressed->scale[0], pos.x );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 1 ), pCompressed->scale[1], pos.y );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 2 ), pCompressed->scale[2], pos.z );
+
+		RadianEuler			angle;
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 3 ), pCompressed->scale[3], angle.x );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 4 ), pCompressed->scale[4], angle.y );
+		ExtractAnimValue( iFrame, pCompressed->pAnimvalue( 5 ), pCompressed->scale[5], angle.z );
+
+		AngleQuaternion( angle, q );
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: translate animations done in a non-standard parent space
+//-----------------------------------------------------------------------------
+static void CalcLocalHierarchyAnimation( 
+	const CStudioHdr *pStudioHdr,
+	matrix3x4_t *boneToWorld,
+	CBoneBitList &boneComputed,
+	Vector *pos, 
+	Quaternion *q,
+	//const mstudioanimdesc_t &animdesc,
+	const mstudiobone_t *pbone,
+	mstudiolocalhierarchy_t *pHierarchy,
+	int iBone,
+	int iNewParent,
+	float cycle, 
+	int iFrame,
+	float flFraq,
+	int boneMask
+	)
+{
+	Vector localPos;
+	Quaternion localQ;
+
+	// make fake root transform
+	static ALIGN16 matrix3x4_t rootXform ALIGN16_POST ( 1.0f, 0, 0, 0,   0, 1.0f, 0, 0,  0, 0, 1.0f, 0 );
+
+	// FIXME: missing check to see if seq has a weight for this bone
+	float weight = 1.0f;
+
+	// check to see if there's a ramp on the influence
+	if ( pHierarchy->tail - pHierarchy->peak < 1.0f  )
+	{
+		float index = cycle;
+
+		if (pHierarchy->end > 1.0f && index < pHierarchy->start)
+			index += 1.0f;
+
+		if (index < pHierarchy->start)
+			return;
+		if (index >= pHierarchy->end)
+			return;
+
+		if (index < pHierarchy->peak && pHierarchy->start != pHierarchy->peak)
+		{
+			weight = (index - pHierarchy->start) / (pHierarchy->peak - pHierarchy->start);
+		}
+		else if (index > pHierarchy->tail && pHierarchy->end != pHierarchy->tail)
+		{
+			weight = (pHierarchy->end - index) / (pHierarchy->end - pHierarchy->tail);
+		}
+
+		weight = SimpleSpline( weight );
+	}
+
+	CalcDecompressedAnimation( pHierarchy->pLocalAnim(), iFrame - pHierarchy->iStart, flFraq, localPos, localQ );
+
+	BuildBoneChain( pStudioHdr, rootXform, pos, q, iBone, boneToWorld, boneComputed );
+	BuildBoneChain( pStudioHdr, rootXform, pos, q, iNewParent, boneToWorld, boneComputed );
+
+	matrix3x4_t localXform;
+	AngleMatrix( localQ, localPos, localXform );
+
+	ConcatTransforms( boneToWorld[iNewParent], localXform, boneToWorld[iBone] );
+
+	// back solve
+	Vector p1;
+	Quaternion q1;
+	int n = pbone[iBone].parent;
+	if (n == -1)
+	{
+		if (weight == 1.0f)
+		{
+			MatrixAngles( boneToWorld[iBone], q[iBone], pos[iBone] );
+		}
+		else
+		{
+			MatrixAngles( boneToWorld[iBone], q1, p1 );
+			QuaternionSlerp( q[iBone], q1, weight, q[iBone] );
+			pos[iBone] = Lerp( weight, p1, pos[iBone] );
+		}
+	}
+	else
+	{
+		matrix3x4_t worldToBone;
+		MatrixInvert( boneToWorld[n], worldToBone );
+
+		matrix3x4_t local;
+		ConcatTransforms( worldToBone, boneToWorld[iBone], local );
+		if (weight == 1.0f)
+		{
+			MatrixAngles( local, q[iBone], pos[iBone] );
+		}
+		else
+		{
+			MatrixAngles( local, q1, p1 );
+			QuaternionSlerp( q[iBone], q1, weight, q[iBone] );
+			pos[iBone] = Lerp( weight, p1, pos[iBone] );
+		}
+	}
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Calc Zeroframe Data
+//-----------------------------------------------------------------------------
+
+static void CalcZeroframeData( const CStudioHdr *pStudioHdr, const studiohdr_t *pAnimStudioHdr, const virtualgroup_t *pAnimGroup, const mstudiobone_t *pAnimbone, mstudioanimdesc_t &animdesc, float fFrame, Vector *pos, Quaternion *q, int boneMask, float flWeight )
+{
+	byte *pData = animdesc.pZeroFrameData();
+
+	if (!pData)
+		return;
+
+	int i, j;
+
+	// Msg("zeroframe %s\n", animdesc.pszName() );
+	if (animdesc.zeroframecount == 1)
+	{
+		for (j = 0; j < pAnimStudioHdr->numbones; j++)
+		{
+			if (pAnimGroup)
+				i = pAnimGroup->masterBone[j];
+			else
+				i = j;
+
+			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_POS)
+			{
+				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))
+				{
+					Vector p = *(Vector48 *)pData;
+					pos[i] = pos[i] * (1.0f - flWeight) + p * flWeight;
+					Assert( pos[i].IsValid() );
+				}
+				pData += sizeof( Vector48 );
+			}
+			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_ROT)
+			{
+				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))
+				{
+					Quaternion q0 = *(Quaternion64 *)pData;
+					QuaternionBlend( q[i], q0, flWeight, q[i] );
+					Assert( q[i].IsValid() );
+				}
+				pData += sizeof( Quaternion64 );
+			}
+		}
+	}
+	else
+	{
+		float s1;
+		int index = fFrame / animdesc.zeroframespan;
+		if (index >= animdesc.zeroframecount - 1)
+		{
+			index = animdesc.zeroframecount - 2;
+			s1 = 1.0f;
+		}
+		else
+		{
+			s1 = clamp( (fFrame - index * animdesc.zeroframespan) / animdesc.zeroframespan, 0.0f, 1.0f );
+		}
+		int i0 = max( index - 1, 0 );
+		int i1 = index;
+		int i2 = min( index + 1, animdesc.zeroframecount - 1 );
+		for (j = 0; j < pAnimStudioHdr->numbones; j++)
+		{
+			if (pAnimGroup)
+				i = pAnimGroup->masterBone[j];
+			else
+				i = j;
+
+			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_POS)
+			{
+				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))
+				{
+					Vector p0 = *(((Vector48 *)pData) + i0);
+					Vector p1 = *(((Vector48 *)pData) + i1);
+					Vector p2 = *(((Vector48 *)pData) + i2);
+					Vector p3;
+					Hermite_Spline( p0, p1, p2, s1, p3 );
+					pos[i] = pos[i] * (1.0f - flWeight) + p3 * flWeight;
+					Assert( pos[i].IsValid() );
+				}
+				pData += sizeof( Vector48 ) * animdesc.zeroframecount;
+			}
+			if (pAnimbone[j].flags & BONE_HAS_SAVEFRAME_ROT)
+			{
+				if ((i >= 0) && (pStudioHdr->boneFlags(i) & boneMask))
+				{
+					Quaternion q0 = *(((Quaternion64 *)pData) + i0);
+					Quaternion q1 = *(((Quaternion64 *)pData) + i1);
+					Quaternion q2 = *(((Quaternion64 *)pData) + i2);
+					if (flWeight == 1.0f)
+					{
+						Hermite_Spline( q0, q1, q2, s1, q[i] );
+					}
+					else
+					{
+						Quaternion q3;
+						Hermite_Spline( q0, q1, q2, s1, q3 );
+						QuaternionBlend( q[i], q3, flWeight, q[i] );
+					}
+					Assert( q[i].IsValid() );
+				}
+				pData += sizeof( Quaternion64 ) * animdesc.zeroframecount;
+			}
+		}
+	}
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Find and decode a sub-frame of animation, remapping the skeleton bone indexes
+//-----------------------------------------------------------------------------
+static void CalcVirtualAnimation( virtualmodel_t *pVModel, const CStudioHdr *pStudioHdr, Vector *pos, Quaternion *q, 
+	mstudioseqdesc_t &seqdesc, int sequence, int animation,
+	float cycle, int boneMask )
+{
+	int	i, j, k;
+
+	const mstudiobone_t *pbone;
+	const virtualgroup_t *pSeqGroup;
+	const studiohdr_t *pSeqStudioHdr;
+	const mstudiolinearbone_t *pSeqLinearBones;
+	const mstudiobone_t *pSeqbone;
+	const mstudioanim_t *panim;
+	const studiohdr_t *pAnimStudioHdr;
+	const mstudiolinearbone_t *pAnimLinearBones;
+	const mstudiobone_t *pAnimbone;
+	const virtualgroup_t *pAnimGroup;
+
+	pSeqGroup = pVModel->pSeqGroup( sequence );
+	int baseanimation = pStudioHdr->iRelativeAnim( sequence, animation );
+	mstudioanimdesc_t &animdesc = ((CStudioHdr *)pStudioHdr)->pAnimdesc( baseanimation );
+	pSeqStudioHdr = ((CStudioHdr *)pStudioHdr)->pSeqStudioHdr( sequence );
+	pSeqLinearBones = pSeqStudioHdr->pLinearBones();
+	pSeqbone = pSeqStudioHdr->pBone( 0 );
+	pAnimGroup = pVModel->pAnimGroup( baseanimation );
+	pAnimStudioHdr = ((CStudioHdr *)pStudioHdr)->pAnimStudioHdr( baseanimation );
+	pAnimLinearBones = pAnimStudioHdr->pLinearBones();
+	pAnimbone = pAnimStudioHdr->pBone( 0 );
+
+	int					iFrame;
+	float				s;
+
+	float fFrame = cycle * (animdesc.numframes - 1);
+
+	iFrame = (int)fFrame;
+	s = (fFrame - iFrame);
+
+	int iLocalFrame = iFrame;
+	float flStall;
+	panim = animdesc.pAnim( &iLocalFrame, flStall );
+
+	float *pweight = seqdesc.pBoneweight( 0 );
+	pbone = pStudioHdr->pBone( 0 );
+
+	for (i = 0; i < pStudioHdr->numbones(); i++)
+	{
+		if (pStudioHdr->boneFlags(i) & boneMask)
+		{
+			int j = pSeqGroup->boneMap[i];
+			if (j >= 0 && pweight[j] > 0.0f)
+			{
+				if (animdesc.flags & STUDIO_DELTA)
+				{
+					q[i].Init( 0.0f, 0.0f, 0.0f, 1.0f );
+					pos[i].Init( 0.0f, 0.0f, 0.0f );
+				}
+				else if (pSeqLinearBones)
+				{
+					q[i] = pSeqLinearBones->quat(j);
+					pos[i] = pSeqLinearBones->pos(j);
+				}
+				else 
+				{
+					q[i] = pSeqbone[j].quat;
+					pos[i] = pSeqbone[j].pos;
+				}
+#ifdef STUDIO_ENABLE_PERF_COUNTERS
+				pStudioHdr->m_nPerfUsedBones++;
+#endif
+			}
+		}
+	}
+
+	// if the animation isn't available, look for the zero frame cache
+	if (!panim)
+	{
+		CalcZeroframeData( ((CStudioHdr *)pStudioHdr), pAnimStudioHdr, pAnimGroup, pAnimbone, animdesc, fFrame, pos, q, boneMask, 1.0 );
+		return;
+	}
+
+	// FIXME: change encoding so that bone -1 is never the case
+	while (panim && panim->bone < 255)
+	{
+		j = pAnimGroup->masterBone[panim->bone];
+		if ( j >= 0 && ( pStudioHdr->boneFlags(j) & boneMask ) )
+		{
+			k = pSeqGroup->boneMap[j];
+
+			if (k >= 0 && pweight[k] > 0.0f)
+			{
+				CalcBoneQuaternion( iLocalFrame, s, &pAnimbone[panim->bone], pAnimLinearBones, panim, q[j] );
+				CalcBonePosition  ( iLocalFrame, s, &pAnimbone[panim->bone], pAnimLinearBones, panim, pos[j] );
+#ifdef STUDIO_ENABLE_PERF_COUNTERS
+				pStudioHdr->m_nPerfAnimatedBones++;
+#endif
+			}
+		}
+		panim = panim->pNext();
+	}
+
+	// cross fade in previous zeroframe data
+	if (flStall > 0.0f)
+	{
+		CalcZeroframeData( pStudioHdr, pAnimStudioHdr, pAnimGroup, pAnimbone, animdesc, fFrame, pos, q, boneMask, flStall );
+	}
+
+	// calculate a local hierarchy override
+	if (animdesc.numlocalhierarchy)
+	{
+		matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+		CBoneBitList boneComputed;
+
+		int i;
+		for (i = 0; i < animdesc.numlocalhierarchy; i++)
+		{
+			mstudiolocalhierarchy_t *pHierarchy = animdesc.pHierarchy( i );
+
+			if ( !pHierarchy )
+				break;
+
+			int iBone = pAnimGroup->masterBone[pHierarchy->iBone];
+			if (iBone >= 0 && (pStudioHdr->boneFlags(iBone) & boneMask))
+			{
+				int iNewParent = pAnimGroup->masterBone[pHierarchy->iNewParent];
+				if (iNewParent >= 0 && (pStudioHdr->boneFlags(iNewParent) & boneMask))
+				{
+					CalcLocalHierarchyAnimation( pStudioHdr, boneToWorld, boneComputed, pos, q, pbone, pHierarchy, iBone, iNewParent, cycle, iFrame, s, boneMask );
+				}
+			}
+		}
+
+		g_MatrixPool.Free( boneToWorld );
+	}
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Find and decode a sub-frame of animation
+//-----------------------------------------------------------------------------
+
+static void CalcAnimation( const CStudioHdr *pStudioHdr,	Vector *pos, Quaternion *q, 
+	mstudioseqdesc_t &seqdesc,
+	int sequence, int animation,
+	float cycle, int boneMask )
+{
+#ifdef STUDIO_ENABLE_PERF_COUNTERS
+	pStudioHdr->m_nPerfAnimationLayers++;
+#endif
+
+	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();
+
+	if (pVModel)
+	{
+		CalcVirtualAnimation( pVModel, pStudioHdr, pos, q, seqdesc, sequence, animation, cycle, boneMask );
+		return;
+	}
+
+	mstudioanimdesc_t &animdesc = ((CStudioHdr *)pStudioHdr)->pAnimdesc( animation );
+	mstudiobone_t *pbone = pStudioHdr->pBone( 0 );
+	const mstudiolinearbone_t *pLinearBones = pStudioHdr->pLinearBones();
+
+	int					i;
+	int					iFrame;
+	float				s;
+
+	float fFrame = cycle * (animdesc.numframes - 1);
+
+	iFrame = (int)fFrame;
+	s = (fFrame - iFrame);
+
+	int iLocalFrame = iFrame;
+	float flStall;
+	mstudioanim_t *panim = animdesc.pAnim( &iLocalFrame, flStall );
+
+	float *pweight = seqdesc.pBoneweight( 0 );
+
+	// if the animation isn't available, look for the zero frame cache
+	if (!panim)
+	{
+		// Msg("zeroframe %s\n", animdesc.pszName() );
+		// pre initialize
+		for (i = 0; i < pStudioHdr->numbones(); i++, pbone++, pweight++)
+		{
+			if (*pweight > 0 && (pStudioHdr->boneFlags(i) & boneMask))
+			{
+				if (animdesc.flags & STUDIO_DELTA)
+				{
+					q[i].Init( 0.0f, 0.0f, 0.0f, 1.0f );
+					pos[i].Init( 0.0f, 0.0f, 0.0f );
+				}
+				else
+				{
+					q[i] = pbone->quat;
+					pos[i] = pbone->pos;
+				}
+			}
+		}
+
+		CalcZeroframeData( pStudioHdr, pStudioHdr->GetRenderHdr(), NULL, pStudioHdr->pBone( 0 ), animdesc, fFrame, pos, q, boneMask, 1.0 );
+
+		return;
+	}
+
+	// BUGBUG: the sequence, the anim, and the model can have all different bone mappings.
+	for (i = 0; i < pStudioHdr->numbones(); i++, pbone++, pweight++)
+	{
+		if (panim && panim->bone == i)
+		{
+			if (*pweight > 0 && (pStudioHdr->boneFlags(i) & boneMask))
+			{
+				CalcBoneQuaternion( iLocalFrame, s, pbone, pLinearBones, panim, q[i] );
+				CalcBonePosition  ( iLocalFrame, s, pbone, pLinearBones, panim, pos[i] );
+#ifdef STUDIO_ENABLE_PERF_COUNTERS
+				pStudioHdr->m_nPerfAnimatedBones++;
+				pStudioHdr->m_nPerfUsedBones++;
+#endif
+			}
+			panim = panim->pNext();
+		}
+		else if (*pweight > 0 && (pStudioHdr->boneFlags(i) & boneMask))
+		{
+			if (animdesc.flags & STUDIO_DELTA)
+			{
+				q[i].Init( 0.0f, 0.0f, 0.0f, 1.0f );
+				pos[i].Init( 0.0f, 0.0f, 0.0f );
+			}
+			else
+			{
+				q[i] = pbone->quat;
+				pos[i] = pbone->pos;
+			}
+#ifdef STUDIO_ENABLE_PERF_COUNTERS
+			pStudioHdr->m_nPerfUsedBones++;
+#endif
+		}
+	}
+
+	// cross fade in previous zeroframe data
+	if (flStall > 0.0f)
+	{
+		CalcZeroframeData( pStudioHdr, pStudioHdr->GetRenderHdr(), NULL, pStudioHdr->pBone( 0 ), animdesc, fFrame, pos, q, boneMask, flStall );
+	}
+
+	if (animdesc.numlocalhierarchy)
+	{
+		matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+		CBoneBitList boneComputed;
+
+		int i;
+		for (i = 0; i < animdesc.numlocalhierarchy; i++)
+		{
+			mstudiolocalhierarchy_t *pHierarchy = animdesc.pHierarchy( i );
+
+			if ( !pHierarchy )
+				break;
+
+			if (pStudioHdr->boneFlags(pHierarchy->iBone) & boneMask)
+			{
+				if (pStudioHdr->boneFlags(pHierarchy->iNewParent) & boneMask)
+				{
+					CalcLocalHierarchyAnimation( pStudioHdr, boneToWorld, boneComputed, pos, q, pbone, pHierarchy, pHierarchy->iBone, pHierarchy->iNewParent, cycle, iFrame, s, boneMask );
+				}
+			}
+		}
+
+		g_MatrixPool.Free( boneToWorld );
+	}
+
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: qt = ( s * p ) * q
+//-----------------------------------------------------------------------------
+void QuaternionSM( float s, const Quaternion &p, const Quaternion &q, Quaternion &qt )
+{
+	Quaternion		p1, q1;
+
+	QuaternionScale( p, s, p1 );
+	QuaternionMult( p1, q, q1 );
+	QuaternionNormalize( q1 );
+	qt[0] = q1[0];
+	qt[1] = q1[1];
+	qt[2] = q1[2];
+	qt[3] = q1[3];
+}
+
+#if ALLOW_SIMD_QUATERNION_MATH
+FORCEINLINE fltx4 QuaternionSMSIMD( float s, const fltx4 &p, const fltx4 &q )
+{
+	fltx4 p1, q1, result;
+	p1 = QuaternionScaleSIMD( p, s );
+	q1 = QuaternionMultSIMD( p1, q );
+	result = QuaternionNormalizeSIMD( q1 );
+	return result;
+}
+#endif
+
+//-----------------------------------------------------------------------------
+// Purpose: qt = p * ( s * q )
+//-----------------------------------------------------------------------------
+void QuaternionMA( const Quaternion &p, float s, const Quaternion &q, Quaternion &qt )
+{
+	Quaternion p1, q1;
+
+	QuaternionScale( q, s, q1 );
+	QuaternionMult( p, q1, p1 );
+	QuaternionNormalize( p1 );
+	qt[0] = p1[0];
+	qt[1] = p1[1];
+	qt[2] = p1[2];
+	qt[3] = p1[3];
+}
+
+#if ALLOW_SIMD_QUATERNION_MATH
+FORCEINLINE fltx4 QuaternionMASIMD( const fltx4 &p, float s, const fltx4 &q )
+{
+	fltx4 p1, q1, result;
+	q1 = QuaternionScaleSIMD( q, s );
+	p1 = QuaternionMultSIMD( p, q1 );
+	result = QuaternionNormalizeSIMD( p1 );
+	return result;
+}
+#endif
+
+
+//-----------------------------------------------------------------------------
+// Purpose: qt = p + s * q
+//-----------------------------------------------------------------------------
+void QuaternionAccumulate( const Quaternion &p, float s, const Quaternion &q, Quaternion &qt )
+{
+	Quaternion q2;
+	QuaternionAlign( p, q, q2 );
+
+	qt[0] = p[0] + s * q2[0];
+	qt[1] = p[1] + s * q2[1];
+	qt[2] = p[2] + s * q2[2];
+	qt[3] = p[3] + s * q2[3];
+}
+
+#if ALLOW_SIMD_QUATERNION_MATH
+FORCEINLINE fltx4 QuaternionAccumulateSIMD( const fltx4 &p, float s, const fltx4 &q )
+{
+	fltx4 q2, s4, result;
+	q2 = QuaternionAlignSIMD( p, q );
+	s4 = ReplicateX4( s );
+	result = MaddSIMD( s4, q2, p );
+	return result;
+}
+#endif
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: blend together in world space q1,pos1 with q2,pos2.  Return result in q1,pos1.  
+//			0 returns q1, pos1.  1 returns q2, pos2
+//-----------------------------------------------------------------------------
+
+void WorldSpaceSlerp(
+	const CStudioHdr *pStudioHdr,
+	Quaternion q1[MAXSTUDIOBONES], 
+	Vector pos1[MAXSTUDIOBONES], 
+	mstudioseqdesc_t &seqdesc, 
+	int sequence, 
+	const Quaternion q2[MAXSTUDIOBONES], 
+	const Vector pos2[MAXSTUDIOBONES], 
+	float s,
+	int boneMask )
+{
+	int			i, j;
+	float		s1; // weight of parent for q2, pos2
+	float		s2; // weight for q2, pos2
+
+	// make fake root transform
+	matrix3x4_t rootXform;
+	SetIdentityMatrix( rootXform );
+
+	// matrices for q2, pos2
+	matrix3x4_t *srcBoneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList srcBoneComputed;
+
+	matrix3x4_t *destBoneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList destBoneComputed;
+
+	matrix3x4_t *targetBoneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList targetBoneComputed;
+
+	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();
+	const virtualgroup_t *pSeqGroup = NULL;
+	if (pVModel)
+	{
+		pSeqGroup = pVModel->pSeqGroup( sequence );
+	}
+
+	mstudiobone_t *pbone = pStudioHdr->pBone( 0 );
+
+	for (i = 0; i < pStudioHdr->numbones(); i++)
+	{
+		// skip unused bones
+		if (!(pStudioHdr->boneFlags(i) & boneMask))
+		{
+			continue;
+		}
+
+		int n = pbone[i].parent;
+		s1 = 0.0;
+		if (pSeqGroup)
+		{
+			j = pSeqGroup->boneMap[i];
+			if (j >= 0)
+			{
+				s2 = s * seqdesc.weight( j );	// blend in based on this bones weight
+				if (n != -1)
+				{
+					s1 = s * seqdesc.weight( pSeqGroup->boneMap[n] );
+				}
+			}
+			else
+			{
+				s2 = 0.0;
+			}
+		}
+		else
+		{
+			s2 = s * seqdesc.weight( i );	// blend in based on this bones weight
+			if (n != -1)
+			{
+				s1 = s * seqdesc.weight( n );
+			}
+		}
+
+		if (s1 == 1.0 && s2 == 1.0)
+		{
+			pos1[i] = pos2[i];
+			q1[i] = q2[i];
+		}
+		else if (s2 > 0.0)
+		{
+			Quaternion srcQ, destQ;
+			Vector srcPos, destPos;
+			Quaternion targetQ;
+			Vector targetPos;
+			Vector tmp;
+
+			BuildBoneChain( pStudioHdr, rootXform, pos1, q1, i, destBoneToWorld, destBoneComputed );
+			BuildBoneChain( pStudioHdr, rootXform, pos2, q2, i, srcBoneToWorld, srcBoneComputed );
+
+			MatrixAngles( destBoneToWorld[i], destQ, destPos );
+			MatrixAngles( srcBoneToWorld[i], srcQ, srcPos );
+
+			QuaternionSlerp( destQ, srcQ, s2, targetQ );
+			AngleMatrix( targetQ, destPos, targetBoneToWorld[i] );
+
+			// back solve
+			if (n == -1)
+			{
+				MatrixAngles( targetBoneToWorld[i], q1[i], tmp );
+			}
+			else
+			{
+				matrix3x4_t worldToBone;
+				MatrixInvert( targetBoneToWorld[n], worldToBone );
+
+				matrix3x4_t local;
+				ConcatTransforms( worldToBone, targetBoneToWorld[i], local );
+				MatrixAngles( local, q1[i], tmp );
+
+				// blend bone lengths (local space)
+				pos1[i] = Lerp( s2, pos1[i], pos2[i] );
+			}
+		}
+	}
+	g_MatrixPool.Free( srcBoneToWorld );
+	g_MatrixPool.Free( destBoneToWorld );
+	g_MatrixPool.Free( targetBoneToWorld );
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: blend together q1,pos1 with q2,pos2.  Return result in q1,pos1.  
+//			0 returns q1, pos1.  1 returns q2, pos2
+//-----------------------------------------------------------------------------
+void SlerpBones( 
+	const CStudioHdr *pStudioHdr,
+	Quaternion q1[MAXSTUDIOBONES], 
+	Vector pos1[MAXSTUDIOBONES], 
+	mstudioseqdesc_t &seqdesc,  // source of q2 and pos2
+	int sequence, 
+	const QuaternionAligned q2[MAXSTUDIOBONES], 
+	const Vector pos2[MAXSTUDIOBONES], 
+	float s,
+	int boneMask )
+{
+	if (s <= 0.0f) 
+		return;
+	if (s > 1.0f)
+	{
+		s = 1.0f;		
+	}
+
+	if (seqdesc.flags & STUDIO_WORLD)
+	{
+		WorldSpaceSlerp( pStudioHdr, q1, pos1, seqdesc, sequence, q2, pos2, s, boneMask );
+		return;
+	}
+
+	int			i, j;
+	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();
+	const virtualgroup_t *pSeqGroup = NULL;
+	if (pVModel)
+	{
+		pSeqGroup = pVModel->pSeqGroup( sequence );
+	}
+
+	// Build weightlist for all bones
+	int nBoneCount = pStudioHdr->numbones();
+	float *pS2 = (float*)stackalloc( nBoneCount * sizeof(float) );
+	for (i = 0; i < nBoneCount; i++)
+	{
+		// skip unused bones
+		if (!(pStudioHdr->boneFlags(i) & boneMask))
+		{
+			pS2[i] = 0.0f;
+			continue;
+		}
+
+		if ( !pSeqGroup )
+		{
+			pS2[i] = s * seqdesc.weight( i );	// blend in based on this bones weight
+			continue;
+		}
+
+		j = pSeqGroup->boneMap[i];
+		if ( j >= 0 )
+		{
+			pS2[i] = s * seqdesc.weight( j );	// blend in based on this bones weight
+		}
+		else
+		{
+			pS2[i] = 0.0;
+		}
+	}
+
+	float s1, s2;
+	if ( seqdesc.flags & STUDIO_DELTA )
+	{
+		for ( i = 0; i < nBoneCount; i++ )
+		{
+			s2 = pS2[i];
+			if ( s2 <= 0.0f )
+				continue;
+
+			if ( seqdesc.flags & STUDIO_POST )
+			{
+#ifndef _X360
+				QuaternionMA( q1[i], s2, q2[i], q1[i] );
+#else
+				fltx4 q1simd = LoadUnalignedSIMD( q1[i].Base() );
+				fltx4 q2simd = LoadAlignedSIMD( q2[i] );
+				fltx4 result = QuaternionMASIMD( q1simd, s2, q2simd );
+				StoreUnalignedSIMD( q1[i].Base(), result );
+#endif
+				// FIXME: are these correct?
+				pos1[i][0] = pos1[i][0] + pos2[i][0] * s2;
+				pos1[i][1] = pos1[i][1] + pos2[i][1] * s2;
+				pos1[i][2] = pos1[i][2] + pos2[i][2] * s2;
+			}
+			else
+			{
+#ifndef _X360
+				QuaternionSM( s2, q2[i], q1[i], q1[i] );
+#else
+				fltx4 q1simd = LoadUnalignedSIMD( q1[i].Base() );
+				fltx4 q2simd = LoadAlignedSIMD( q2[i] );
+				fltx4 result = QuaternionSMSIMD( s2, q2simd, q1simd );
+				StoreUnalignedSIMD( q1[i].Base(), result );
+#endif
+
+				// FIXME: are these correct?
+				pos1[i][0] = pos1[i][0] + pos2[i][0] * s2;
+				pos1[i][1] = pos1[i][1] + pos2[i][1] * s2;
+				pos1[i][2] = pos1[i][2] + pos2[i][2] * s2;
+			}
+		}
+		return;
+	}
+
+	QuaternionAligned q3;
+	for (i = 0; i < nBoneCount; i++)
+	{
+		s2 = pS2[i];
+		if ( s2 <= 0.0f )
+			continue;
+
+		s1 = 1.0 - s2;
+
+#ifdef _X360
+		fltx4  q1simd, q2simd, result;
+		q1simd = LoadUnalignedSIMD( q1[i].Base() );
+		q2simd = LoadAlignedSIMD( q2[i] );
+#endif
+		if ( pStudioHdr->boneFlags(i) & BONE_FIXED_ALIGNMENT )
+		{
+#ifndef _X360
+			QuaternionSlerpNoAlign( q2[i], q1[i], s1, q3 );
+#else
+			result = QuaternionSlerpNoAlignSIMD( q2simd, q1simd, s1 );
+#endif
+		}
+		else
+		{
+#ifndef _X360
+			QuaternionSlerp( q2[i], q1[i], s1, q3 );
+#else
+			result = QuaternionSlerpSIMD( q2simd, q1simd, s1 );
+#endif
+		}
+
+#ifndef _X360
+		q1[i][0] = q3[0];
+		q1[i][1] = q3[1];
+		q1[i][2] = q3[2];
+		q1[i][3] = q3[3];
+#else
+		StoreUnalignedSIMD( q1[i].Base(), result );
+#endif
+
+		pos1[i][0] = pos1[i][0] * s1 + pos2[i][0] * s2;
+		pos1[i][1] = pos1[i][1] * s1 + pos2[i][1] * s2;
+		pos1[i][2] = pos1[i][2] * s1 + pos2[i][2] * s2;
+	}
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Inter-animation blend.  Assumes both types are identical.
+//			blend together q1,pos1 with q2,pos2.  Return result in q1,pos1.  
+//			0 returns q1, pos1.  1 returns q2, pos2
+//-----------------------------------------------------------------------------
+void BlendBones( 
+	const CStudioHdr *pStudioHdr,
+	Quaternion q1[MAXSTUDIOBONES], 
+	Vector pos1[MAXSTUDIOBONES], 
+	mstudioseqdesc_t &seqdesc, 
+	int sequence,
+	const Quaternion q2[MAXSTUDIOBONES], 
+	const Vector pos2[MAXSTUDIOBONES], 
+	float s,
+	int boneMask )
+{
+	int			i, j;
+	Quaternion		q3;
+
+	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();
+	const virtualgroup_t *pSeqGroup = NULL;
+	if (pVModel)
+	{
+		pSeqGroup = pVModel->pSeqGroup( sequence );
+	}
+
+	if (s <= 0)
+	{
+		Assert(0); // shouldn't have been called
+		return;
+	}
+	else if (s >= 1.0)
+	{
+		Assert(0); // shouldn't have been called
+		for (i = 0; i < pStudioHdr->numbones(); i++)
+		{
+			// skip unused bones
+			if (!(pStudioHdr->boneFlags(i) & boneMask))
+			{
+				continue;
+			}
+
+			if (pSeqGroup)
+			{
+				j = pSeqGroup->boneMap[i];
+			}
+			else
+			{
+				j = i;
+			}
+
+			if (j >= 0 && seqdesc.weight( j ) > 0.0)
+			{
+				q1[i] = q2[i];
+				pos1[i] = pos2[i];
+			}
+		}
+		return;
+	}
+
+	float s2 = s;
+	float s1 = 1.0 - s2;
+
+	for (i = 0; i < pStudioHdr->numbones(); i++)
+	{
+		// skip unused bones
+		if (!(pStudioHdr->boneFlags(i) & boneMask))
+		{
+			continue;
+		}
+
+		if (pSeqGroup)
+		{
+			j = pSeqGroup->boneMap[i];
+		}
+		else
+		{
+			j = i;
+		}
+
+		if (j >= 0 && seqdesc.weight( j ) > 0.0)
+		{
+			if (pStudioHdr->boneFlags(i) & BONE_FIXED_ALIGNMENT)
+			{
+				QuaternionBlendNoAlign( q2[i], q1[i], s1, q3 );
+			}
+			else
+			{
+				QuaternionBlend( q2[i], q1[i], s1, q3 );
+			}
+			q1[i][0] = q3[0];
+			q1[i][1] = q3[1];
+			q1[i][2] = q3[2];
+			q1[i][3] = q3[3];
+			pos1[i][0] = pos1[i][0] * s1 + pos2[i][0] * s2;
+			pos1[i][1] = pos1[i][1] * s1 + pos2[i][1] * s2;
+			pos1[i][2] = pos1[i][2] * s1 + pos2[i][2] * s2;
+		}
+	}
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Scale a set of bones.  Must be of type delta
+//-----------------------------------------------------------------------------
+void ScaleBones( 
+	const CStudioHdr *pStudioHdr,
+	Quaternion q1[MAXSTUDIOBONES], 
+	Vector pos1[MAXSTUDIOBONES], 
+	int sequence,
+	float s,
+	int boneMask )
+{
+	int			i, j;
+	Quaternion		q3;
+
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( sequence );
+
+	virtualmodel_t *pVModel = pStudioHdr->GetVirtualModel();
+	const virtualgroup_t *pSeqGroup = NULL;
+	if (pVModel)
+	{
+		pSeqGroup = pVModel->pSeqGroup( sequence );
+	}
+
+	float s2 = s;
+	float s1 = 1.0 - s2;
+
+	for (i = 0; i < pStudioHdr->numbones(); i++)
+	{
+		// skip unused bones
+		if (!(pStudioHdr->boneFlags(i) & boneMask))
+		{
+			continue;
+		}
+
+		if (pSeqGroup)
+		{
+			j = pSeqGroup->boneMap[i];
+		}
+		else
+		{
+			j = i;
+		}
+
+		if (j >= 0 && seqdesc.weight( j ) > 0.0)
+		{
+			QuaternionIdentityBlend( q1[i], s1, q1[i] );
+			VectorScale( pos1[i], s2, pos1[i] );
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: resolve a global pose parameter to the specific setting for this sequence
+//-----------------------------------------------------------------------------
+void Studio_LocalPoseParameter( const CStudioHdr *pStudioHdr, const float poseParameter[], mstudioseqdesc_t &seqdesc, int iSequence, int iLocalIndex, float &flSetting, int &index )
+{
+	int iPose = pStudioHdr->GetSharedPoseParameter( iSequence, seqdesc.paramindex[iLocalIndex] );
+
+	if (iPose == -1)
+	{
+		flSetting = 0;
+		index = 0;
+		return;
+	}
+
+	const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iPose );
+
+	float flValue = poseParameter[iPose];
+
+	if (Pose.loop)
+	{
+		float wrap = (Pose.start + Pose.end) / 2.0 + Pose.loop / 2.0;
+		float shift = Pose.loop - wrap;
+
+		flValue = flValue - Pose.loop * floor((flValue + shift) / Pose.loop);
+	}
+
+	if (seqdesc.posekeyindex == 0)
+	{
+		float flLocalStart	= ((float)seqdesc.paramstart[iLocalIndex] - Pose.start) / (Pose.end - Pose.start);
+		float flLocalEnd	= ((float)seqdesc.paramend[iLocalIndex] - Pose.start) / (Pose.end - Pose.start);
+
+		// convert into local range
+		flSetting = (flValue - flLocalStart) / (flLocalEnd - flLocalStart);
+
+		// clamp.  This shouldn't ever need to happen if it's looping.
+		if (flSetting < 0)
+			flSetting = 0;
+		if (flSetting > 1)
+			flSetting = 1;
+
+		index = 0;
+		if (seqdesc.groupsize[iLocalIndex] > 2 )
+		{
+			// estimate index
+			index = (int)(flSetting * (seqdesc.groupsize[iLocalIndex] - 1));
+			if (index == seqdesc.groupsize[iLocalIndex] - 1) index = seqdesc.groupsize[iLocalIndex] - 2;
+			flSetting = flSetting * (seqdesc.groupsize[iLocalIndex] - 1) - index;
+		}
+	}
+	else
+	{
+		flValue = flValue * (Pose.end - Pose.start) + Pose.start;
+		index = 0;
+			
+		// FIXME: this needs to be 2D
+		// FIXME: this shouldn't be a linear search
+
+		while (1)
+		{
+			flSetting = (flValue - seqdesc.poseKey( iLocalIndex, index )) / (seqdesc.poseKey( iLocalIndex, index + 1 ) - seqdesc.poseKey( iLocalIndex, index ));
+			/*
+			if (index > 0 && flSetting < 0.0)
+			{
+				index--;
+				continue;
+			}
+			else 
+			*/
+			if (index < seqdesc.groupsize[iLocalIndex] - 2 && flSetting > 1.0)
+			{
+				index++;
+				continue;
+			}
+			break;
+		}
+
+		// clamp.
+		if (flSetting < 0.0f)
+			flSetting = 0.0f;
+		if (flSetting > 1.0f)
+			flSetting = 1.0f;
+	}
+}
+
+void Studio_CalcBoneToBoneTransform( const CStudioHdr *pStudioHdr, int inputBoneIndex, int outputBoneIndex, matrix3x4_t& matrixOut )
+{
+	mstudiobone_t *pbone = pStudioHdr->pBone( inputBoneIndex );
+
+	matrix3x4_t inputToPose;
+	MatrixInvert( pbone->poseToBone, inputToPose );
+	ConcatTransforms( pStudioHdr->pBone( outputBoneIndex )->poseToBone, inputToPose, matrixOut );
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate a pose for a single sequence
+//-----------------------------------------------------------------------------
+void InitPose(
+	const CStudioHdr *pStudioHdr,
+	Vector pos[], 
+	Quaternion q[],
+	int boneMask 
+	)
+{
+	if (!pStudioHdr->pLinearBones())
+	{
+		for (int i = 0; i < pStudioHdr->numbones(); i++)
+		{
+			if (pStudioHdr->boneFlags(  i ) & boneMask ) 
+			{
+				mstudiobone_t *pbone = pStudioHdr->pBone( i );
+				pos[i] = pbone->pos;
+				q[i] = pbone->quat;
+			}
+		}
+	}
+	else
+	{
+		mstudiolinearbone_t *pLinearBones = pStudioHdr->pLinearBones();
+		for (int i = 0; i < pStudioHdr->numbones(); i++)
+		{
+			if (pStudioHdr->boneFlags(  i ) & boneMask ) 
+			{
+				pos[i] = pLinearBones->pos(i);
+				q[i] = pLinearBones->quat(i);
+			}
+		}
+	}
+}
+	
+
+inline bool PoseIsAllZeros( 
+	const CStudioHdr *pStudioHdr,
+	int sequence, 
+	mstudioseqdesc_t	&seqdesc,
+	int i0,
+	int i1
+	)
+{
+	int baseanim;
+		
+	// remove "zero" positional blends
+	baseanim = pStudioHdr->iRelativeAnim( sequence, seqdesc.anim(i0  ,i1 ) );
+	mstudioanimdesc_t		&anim = ((CStudioHdr *)pStudioHdr)->pAnimdesc( baseanim );
+	return (anim.flags & STUDIO_ALLZEROS) != 0;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: turn a 2x2 blend into a 3 way triangle blend
+// Returns: returns the animination indices and barycentric coordinates of a triangle
+//			the triangle is a right triangle, and the diagonal is between elements [0] and [2]
+//-----------------------------------------------------------------------------
+
+static ConVar anim_3wayblend( "anim_3wayblend", "1", FCVAR_REPLICATED, "Toggle the 3-way animation blending code." );
+
+void Calc3WayBlendIndices( int i0, int i1, float s0, float s1, const mstudioseqdesc_t &seqdesc, int *pAnimIndices, float *pWeight )
+{
+	// Figure out which bi-section direction we are using to make triangles.
+	bool bEven = ( ( ( i0 + i1 ) & 0x1 ) == 0 );
+
+	int x1, y1;
+	int x2, y2;
+	int x3, y3;
+
+	// diagonal is between elements 1 & 3
+	// TL to BR
+	if ( bEven )
+	{
+		if ( s0 > s1 )
+		{
+			// B
+			x1 = 0; y1 = 0;
+			x2 = 1; y2 = 0;
+			x3 = 1; y3 = 1;
+			pWeight[0] = (1.0f - s0);
+			pWeight[1] = s0 - s1;
+		}
+		else
+		{
+			// C
+			x1 = 1; y1 = 1;
+			x2 = 0; y2 = 1;
+			x3 = 0; y3 = 0;
+			pWeight[0] = s0;
+			pWeight[1] = s1 - s0;
+		}
+	}
+	// BL to TR
+	else
+	{
+		float flTotal = s0 + s1;
+
+		if( flTotal > 1.0f )
+		{
+			// D
+			x1 = 1; y1 = 0;
+			x2 = 1; y2 = 1;
+			x3 = 0; y3 = 1;
+			pWeight[0] = (1.0f - s1);
+			pWeight[1] = s0 - 1.0f + s1;
+		}
+		else
+		{
+			// A
+			x1 = 0; y1 = 1;
+			x2 = 0; y2 = 0;
+			x3 = 1; y3 = 0;
+			pWeight[0] = s1;
+			pWeight[1] = 1.0f - s0 - s1;
+		}
+	}
+
+	pAnimIndices[0] = seqdesc.anim( i0 + x1, i1 + y1 );
+	pAnimIndices[1] = seqdesc.anim( i0 + x2, i1 + y2 );
+	pAnimIndices[2] = seqdesc.anim( i0 + x3, i1 + y3 );
+
+	/*
+	float w0 = ((x2-x3)*(y3-s1) - (x3-s0)*(y2-y3)) / ((x1-x3)*(y2-y3) - (x2-x3)*(y1-y3));
+	float w1 = ((x1-x3)*(y3-s1) - (x3-s0)*(y1-y3)) / ((x2-x3)*(y1-y3) - (x1-x3)*(y2-y3));
+	Assert( pWeight[0] == w0 && pWeight[1] == w1 );
+	*/
+
+	// clamp the diagonal
+	if (pWeight[1] < 0.001f)
+		pWeight[1] = 0.0f;
+	pWeight[2] = 1.0f - pWeight[0] - pWeight[1];
+
+	Assert( pWeight[0] >= 0.0f && pWeight[0] <= 1.0f );
+	Assert( pWeight[1] >= 0.0f && pWeight[1] <= 1.0f );
+	Assert( pWeight[2] >= 0.0f && pWeight[2] <= 1.0f );
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate a pose for a single sequence
+//-----------------------------------------------------------------------------
+bool CalcPoseSingle(
+	const CStudioHdr *pStudioHdr,
+	Vector pos[], 
+	Quaternion q[], 
+	mstudioseqdesc_t &seqdesc,
+	int sequence, 
+	float cycle,
+	const float poseParameter[],
+	int boneMask,
+	float flTime
+	)
+{
+	bool bResult = true;
+	
+	Vector		*pos2 = g_VectorPool.Alloc();
+	Quaternion	*q2 = g_QaternionPool.Alloc();
+	Vector		*pos3= g_VectorPool.Alloc();
+	Quaternion	*q3 = g_QaternionPool.Alloc();
+
+	if (sequence >= pStudioHdr->GetNumSeq()) 
+	{
+		sequence = 0;
+		seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( sequence );
+	}
+
+
+	int i0 = 0, i1 = 0;
+	float s0 = 0, s1 = 0;
+
+	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, sequence, 0, s0, i0 );
+	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, sequence, 1, s1, i1 );
+
+
+	if (seqdesc.flags & STUDIO_REALTIME)
+	{
+		float cps = Studio_CPS( pStudioHdr, seqdesc, sequence, poseParameter );
+		cycle = flTime * cps;
+		cycle = cycle - (int)cycle;
+	}
+	else if (seqdesc.flags & STUDIO_CYCLEPOSE)
+	{
+		int iPose = pStudioHdr->GetSharedPoseParameter( sequence, seqdesc.cycleposeindex );
+		if (iPose != -1)
+		{
+			/*
+			const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iPose );
+			cycle = poseParameter[ iPose ] * (Pose.end - Pose.start) + Pose.start;
+			*/
+			cycle = poseParameter[ iPose ];
+		}
+		else
+		{
+			cycle = 0.0f;
+		}
+	}
+	else if (cycle < 0 || cycle >= 1)
+	{
+		if (seqdesc.flags & STUDIO_LOOPING)
+		{
+			cycle = cycle - (int)cycle;
+			if (cycle < 0) cycle += 1;
+		}
+		else
+		{
+			cycle = clamp( cycle, 0.0f, 1.0f );
+		}
+	}
+
+	if (s0 < 0.001)
+	{
+		if (s1 < 0.001)
+		{
+			if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0, i1 ))
+			{
+				bResult = false;
+			}
+			else
+			{
+				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  , i1   ), cycle, boneMask );
+			}
+		}
+		else if (s1 > 0.999)
+		{
+			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  , i1+1 ), cycle, boneMask );
+		}
+		else
+		{
+			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  , i1   ), cycle, boneMask );
+			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0  , i1+1 ), cycle, boneMask );
+			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s1, boneMask );
+		}
+	}
+	else if (s0 > 0.999)
+	{
+		if (s1 < 0.001)
+		{
+			if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0+1, i1 ))
+			{
+				bResult = false;
+			}
+			else
+			{
+				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1, i1   ), cycle, boneMask );
+			}
+		}
+		else if (s1 > 0.999)
+		{
+			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1, i1+1 ), cycle, boneMask );
+		}
+		else
+		{
+			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1, i1   ), cycle, boneMask );
+			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1, i1+1 ), cycle, boneMask );
+			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s1, boneMask );
+		}
+	}
+	else
+	{
+		if (s1 < 0.001)
+		{
+			if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0+1, i1 ))
+			{
+				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1  ), cycle, boneMask );
+				ScaleBones( pStudioHdr, q, pos, sequence, 1.0 - s0, boneMask );
+			}
+			else if (PoseIsAllZeros( pStudioHdr, sequence, seqdesc, i0, i1 ))
+			{
+				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0+1  ,i1  ), cycle, boneMask );
+				ScaleBones( pStudioHdr, q, pos, sequence, s0, boneMask );
+			}
+			else
+			{
+				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1  ), cycle, boneMask );
+				CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1,i1  ), cycle, boneMask );
+
+				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s0, boneMask );
+			}
+		}
+		else if (s1 > 0.999)
+		{
+			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1+1  ), cycle, boneMask );
+			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1,i1+1  ), cycle, boneMask );
+			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s0, boneMask );
+		}
+		else if ( !anim_3wayblend.GetBool() )
+		{
+			CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, seqdesc.anim( i0  ,i1  ), cycle, boneMask );
+			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0+1,i1  ), cycle, boneMask );
+			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s0, boneMask );
+
+			CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, seqdesc.anim( i0  , i1+1), cycle, boneMask );
+			CalcAnimation( pStudioHdr, pos3, q3, seqdesc, sequence, seqdesc.anim( i0+1, i1+1), cycle, boneMask );
+			BlendBones( pStudioHdr, q2, pos2, seqdesc, sequence, q3, pos3, s0, boneMask );
+
+			BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, s1, boneMask );
+		}
+		else
+		{
+			int		iAnimIndices[3];
+			float	weight[3];
+
+			Calc3WayBlendIndices( i0, i1, s0, s1, seqdesc, iAnimIndices, weight );
+
+			/*
+			char buf[256];
+			sprintf( buf, "%d %6.2f  %d %6.2f : %6.2f %6.2f %6.2f\n", i0, s0, i1, s1, weight[0], weight[1], weight[2] );
+			OutputDebugString( buf );
+			*/
+
+			if (weight[1] < 0.001)
+			{
+				// on diagonal
+				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, iAnimIndices[0], cycle, boneMask );
+				CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, iAnimIndices[2], cycle, boneMask );
+				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, weight[2] / (weight[0] + weight[2]), boneMask );
+			}
+			else
+			{
+				CalcAnimation( pStudioHdr, pos,  q,  seqdesc, sequence, iAnimIndices[0], cycle, boneMask );
+				CalcAnimation( pStudioHdr, pos2, q2, seqdesc, sequence, iAnimIndices[1], cycle, boneMask );
+				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, weight[1] / (weight[0] + weight[1]), boneMask );
+
+				CalcAnimation( pStudioHdr, pos3, q3, seqdesc, sequence, iAnimIndices[2], cycle, boneMask );
+				BlendBones( pStudioHdr, q, pos, seqdesc, sequence, q3, pos3, weight[2], boneMask );
+			}
+		}
+	}
+
+	g_VectorPool.Free( pos2 );
+	g_QaternionPool.Free( q2 );
+	g_VectorPool.Free( pos3 );
+	g_QaternionPool.Free( q3 );
+
+	return bResult;
+}
+
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate a pose for a single sequence
+//			adds autolayers, runs local ik rukes
+//-----------------------------------------------------------------------------
+void CBoneSetup::AddSequenceLayers(
+   Vector pos[], 
+   Quaternion q[], 
+   mstudioseqdesc_t &seqdesc,
+   int sequence, 
+   float cycle,
+   float flWeight,
+   float flTime,
+   CIKContext *pIKContext
+   )
+{
+	for (int i = 0; i < seqdesc.numautolayers; i++)
+	{
+		mstudioautolayer_t *pLayer = seqdesc.pAutolayer( i );
+
+		if (pLayer->flags & STUDIO_AL_LOCAL)
+			continue;
+
+		float layerCycle = cycle;
+		float layerWeight = flWeight;
+
+		if (pLayer->start != pLayer->end)
+		{
+			float s = 1.0;
+			float index;
+
+			if (!(pLayer->flags & STUDIO_AL_POSE))
+			{
+				index = cycle;
+			}
+			else
+			{
+				int iSequence = m_pStudioHdr->iRelativeSeq( sequence, pLayer->iSequence );
+				int iPose = m_pStudioHdr->GetSharedPoseParameter( iSequence, pLayer->iPose );
+				if (iPose != -1)
+				{
+					const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)m_pStudioHdr)->pPoseParameter( iPose );
+					index = m_flPoseParameter[ iPose ] * (Pose.end - Pose.start) + Pose.start;
+				}
+				else
+				{
+					index = 0;
+				}
+			}
+
+			if (index < pLayer->start)
+				continue;
+			if (index >= pLayer->end)
+				continue;
+
+			if (index < pLayer->peak && pLayer->start != pLayer->peak)
+			{
+				s = (index - pLayer->start) / (pLayer->peak - pLayer->start);
+			}
+			else if (index > pLayer->tail && pLayer->end != pLayer->tail)
+			{
+				s = (pLayer->end - index) / (pLayer->end - pLayer->tail);
+			}
+
+			if (pLayer->flags & STUDIO_AL_SPLINE)
+			{
+				s = SimpleSpline( s );
+			}
+
+			if ((pLayer->flags & STUDIO_AL_XFADE) && (index > pLayer->tail))
+			{
+				layerWeight = ( s * flWeight ) / ( 1 - flWeight + s * flWeight );
+			}
+			else if (pLayer->flags & STUDIO_AL_NOBLEND)
+			{
+				layerWeight = s;
+			}
+			else
+			{
+				layerWeight = flWeight * s;
+			}
+
+			if (!(pLayer->flags & STUDIO_AL_POSE))
+			{
+				layerCycle = (cycle - pLayer->start) / (pLayer->end - pLayer->start);
+			}
+		}
+
+		int iSequence = m_pStudioHdr->iRelativeSeq( sequence, pLayer->iSequence );
+		AccumulatePose( pos, q, iSequence, layerCycle, layerWeight, flTime, pIKContext );
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate a pose for a single sequence
+//			adds autolayers, runs local ik rukes
+//-----------------------------------------------------------------------------
+void CBoneSetup::AddLocalLayers(
+	Vector pos[], 
+	Quaternion q[], 
+	mstudioseqdesc_t &seqdesc,
+	int sequence, 
+	float cycle,
+	float flWeight,
+	float flTime,
+	CIKContext *pIKContext
+	)
+{
+	if (!(seqdesc.flags & STUDIO_LOCAL))
+	{
+		return;
+	}
+
+	for (int i = 0; i < seqdesc.numautolayers; i++)
+	{
+		mstudioautolayer_t *pLayer = seqdesc.pAutolayer( i );
+
+		if (!(pLayer->flags & STUDIO_AL_LOCAL))
+			continue;
+
+		float layerCycle = cycle;
+		float layerWeight = flWeight;
+
+		if (pLayer->start != pLayer->end)
+		{
+			float s = 1.0;
+
+			if (cycle < pLayer->start)
+				continue;
+			if (cycle >= pLayer->end)
+				continue;
+
+			if (cycle < pLayer->peak && pLayer->start != pLayer->peak)
+			{
+				s = (cycle - pLayer->start) / (pLayer->peak - pLayer->start);
+			}
+			else if (cycle > pLayer->tail && pLayer->end != pLayer->tail)
+			{
+				s = (pLayer->end - cycle) / (pLayer->end - pLayer->tail);
+			}
+
+			if (pLayer->flags & STUDIO_AL_SPLINE)
+			{
+				s = SimpleSpline( s );
+			}
+
+			if ((pLayer->flags & STUDIO_AL_XFADE) && (cycle > pLayer->tail))
+			{
+				layerWeight = ( s * flWeight ) / ( 1 - flWeight + s * flWeight );
+			}
+			else if (pLayer->flags & STUDIO_AL_NOBLEND)
+			{
+				layerWeight = s;
+			}
+			else
+			{
+				layerWeight = flWeight * s;
+			}
+
+			layerCycle = (cycle - pLayer->start) / (pLayer->end - pLayer->start);
+		}
+
+		int iSequence = m_pStudioHdr->iRelativeSeq( sequence, pLayer->iSequence );
+		AccumulatePose( pos, q, iSequence, layerCycle, layerWeight, flTime, pIKContext );
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: my sleezy attempt at an interface only class
+//-----------------------------------------------------------------------------
+
+IBoneSetup::IBoneSetup( const CStudioHdr *pStudioHdr, int boneMask, const float poseParameter[], IPoseDebugger *pPoseDebugger )
+{
+	m_pBoneSetup = new CBoneSetup( pStudioHdr, boneMask, poseParameter, pPoseDebugger );
+}
+
+IBoneSetup::~IBoneSetup( void )
+{
+	if ( m_pBoneSetup )
+	{
+		delete m_pBoneSetup;
+	}
+}
+
+void IBoneSetup::InitPose( Vector pos[], Quaternion q[] )
+{
+	::InitPose( m_pBoneSetup->m_pStudioHdr, pos, q, m_pBoneSetup->m_boneMask );
+}
+
+void IBoneSetup::AccumulatePose( Vector pos[], Quaternion q[], int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext )
+{
+	m_pBoneSetup->AccumulatePose( pos, q, sequence, cycle, flWeight, flTime, pIKContext );
+}
+
+void IBoneSetup::CalcAutoplaySequences(	Vector pos[], Quaternion q[], float flRealTime, CIKContext *pIKContext )
+{
+	m_pBoneSetup->CalcAutoplaySequences( pos, q, flRealTime, pIKContext );
+}
+
+void CalcBoneAdj( const CStudioHdr *pStudioHdr, Vector pos[], Quaternion q[], const float controllers[], int boneMask );
+
+// takes a "controllers[]" array normalized to 0..1 and adds in the adjustments to pos[], and q[].
+void IBoneSetup::CalcBoneAdj( Vector pos[], Quaternion q[], const float controllers[] )
+{
+	::CalcBoneAdj( m_pBoneSetup->m_pStudioHdr, pos, q, controllers, m_pBoneSetup->m_boneMask );
+}
+
+CStudioHdr *IBoneSetup::GetStudioHdr()
+{
+	return (CStudioHdr *)m_pBoneSetup->m_pStudioHdr;
+}
+
+CBoneSetup::CBoneSetup( const CStudioHdr *pStudioHdr, int boneMask, const float poseParameter[], IPoseDebugger *pPoseDebugger )
+{
+	m_pStudioHdr = pStudioHdr;
+	m_boneMask = boneMask;
+	m_flPoseParameter = poseParameter;
+	m_pPoseDebugger = pPoseDebugger;
+}
+
+#if 0
+//-----------------------------------------------------------------------------
+// Purpose: calculate a pose for a single sequence
+//			adds autolayers, runs local ik rukes
+//-----------------------------------------------------------------------------
+void CalcPose(
+	const CStudioHdr *pStudioHdr,
+	CIKContext *pIKContext,
+	Vector pos[], 
+	Quaternion q[], 
+	int sequence, 
+	float cycle,
+	const float poseParameter[],
+	int boneMask,
+	float flWeight,
+	float flTime
+	)
+{
+	mstudioseqdesc_t	&seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( sequence );
+
+	Assert( flWeight >= 0.0f && flWeight <= 1.0f );
+	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up
+	flWeight = clamp( flWeight, 0.0f, 1.0f );
+
+	// add any IK locks to prevent numautolayers from moving extremities 
+	CIKContext seq_ik;
+	if (seqdesc.numiklocks)
+	{
+		seq_ik.Init( pStudioHdr, vec3_angle, vec3_origin, 0.0, 0, boneMask ); // local space relative so absolute position doesn't mater
+		seq_ik.AddSequenceLocks( seqdesc, pos, q );
+	}
+
+	CalcPoseSingle( pStudioHdr, pos, q, seqdesc, sequence, cycle, poseParameter, boneMask, flTime );
+
+	if ( pIKContext )
+	{
+		pIKContext->AddDependencies( seqdesc, sequence, cycle, poseParameter, flWeight );
+	}
+	
+	AddSequenceLayers( pStudioHdr, pIKContext, pos, q, seqdesc, sequence, cycle, poseParameter, boneMask, flWeight, flTime );
+
+	if (seqdesc.numiklocks)
+	{
+		seq_ik.SolveSequenceLocks( seqdesc, pos, q );
+	}
+}
+#endif
+
+//-----------------------------------------------------------------------------
+// Purpose: accumulate a pose for a single sequence on top of existing animation
+//			adds autolayers, runs local ik rukes
+//-----------------------------------------------------------------------------
+void CBoneSetup::AccumulatePose(
+	Vector pos[], 
+	Quaternion q[], 
+	int sequence, 
+	float cycle,
+	float flWeight,
+	float flTime,
+	CIKContext *pIKContext
+	)
+{
+	Vector		pos2[MAXSTUDIOBONES];
+	QuaternionAligned	q2[MAXSTUDIOBONES];
+
+	Assert( flWeight >= 0.0f && flWeight <= 1.0f );
+	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up
+	flWeight = clamp( flWeight, 0.0f, 1.0f );
+
+	if ( sequence < 0 )
+		return;
+
+#ifdef CLIENT_DLL
+	// Trigger pose debugger
+	if (m_pPoseDebugger)
+	{
+		m_pPoseDebugger->AccumulatePose( m_pStudioHdr, pIKContext, pos, q, sequence, cycle, m_flPoseParameter, m_boneMask, flWeight, flTime );
+	}
+#endif
+
+	mstudioseqdesc_t	&seqdesc = ((CStudioHdr *)m_pStudioHdr)->pSeqdesc( sequence );
+
+	// add any IK locks to prevent extremities from moving
+	CIKContext seq_ik;
+	if (seqdesc.numiklocks)
+	{
+		seq_ik.Init( m_pStudioHdr, vec3_angle, vec3_origin, 0.0, 0, m_boneMask );  // local space relative so absolute position doesn't mater
+		seq_ik.AddSequenceLocks( seqdesc, pos, q );
+	}
+
+	if (seqdesc.flags & STUDIO_LOCAL)
+	{
+		::InitPose( m_pStudioHdr, pos2, q2, m_boneMask );
+	}
+
+	if (CalcPoseSingle( m_pStudioHdr, pos2, q2, seqdesc, sequence, cycle, m_flPoseParameter, m_boneMask, flTime ))
+	{
+		// this weight is wrong, the IK rules won't composite at the correct intensity
+		AddLocalLayers( pos2, q2, seqdesc, sequence, cycle, 1.0, flTime, pIKContext );
+		SlerpBones( m_pStudioHdr, q, pos, seqdesc, sequence, q2, pos2, flWeight, m_boneMask );
+	}
+
+
+	if ( pIKContext )
+	{
+		pIKContext->AddDependencies( seqdesc, sequence, cycle, m_flPoseParameter, flWeight );
+	}
+
+	AddSequenceLayers( pos, q, seqdesc, sequence, cycle, flWeight, flTime, pIKContext );
+
+	if (seqdesc.numiklocks)
+	{
+		seq_ik.SolveSequenceLocks( seqdesc, pos, q );
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: blend together q1,pos1 with q2,pos2.  Return result in q1,pos1.  
+//			0 returns q1, pos1.  1 returns q2, pos2
+//-----------------------------------------------------------------------------
+void CalcBoneAdj(
+	const CStudioHdr *pStudioHdr,
+	Vector pos[], 
+	Quaternion q[], 
+	const float controllers[],
+	int boneMask
+	)
+{
+	int					i, j, k;
+	float				value;
+	mstudiobonecontroller_t *pbonecontroller;
+	Vector p0;
+	RadianEuler a0;
+	Quaternion q0;
+	
+	for (j = 0; j < pStudioHdr->numbonecontrollers(); j++)
+	{
+		pbonecontroller = pStudioHdr->pBonecontroller( j );
+		k = pbonecontroller->bone;
+
+		if (pStudioHdr->boneFlags( k ) & boneMask)
+		{
+			i = pbonecontroller->inputfield;
+			value = controllers[i];
+			if (value < 0) value = 0;
+			if (value > 1.0) value = 1.0;
+			value = (1.0 - value) * pbonecontroller->start + value * pbonecontroller->end;
+
+			switch(pbonecontroller->type & STUDIO_TYPES)
+			{
+			case STUDIO_XR: 
+				a0.Init( value * (M_PI / 180.0), 0, 0 ); 
+				AngleQuaternion( a0, q0 );
+				QuaternionSM( 1.0, q0, q[k], q[k] );
+				break;
+			case STUDIO_YR: 
+				a0.Init( 0, value * (M_PI / 180.0), 0 ); 
+				AngleQuaternion( a0, q0 );
+				QuaternionSM( 1.0, q0, q[k], q[k] );
+				break;
+			case STUDIO_ZR: 
+				a0.Init( 0, 0, value * (M_PI / 180.0) ); 
+				AngleQuaternion( a0, q0 );
+				QuaternionSM( 1.0, q0, q[k], q[k] );
+				break;
+			case STUDIO_X:	
+				pos[k].x += value;
+				break;
+			case STUDIO_Y:
+				pos[k].y += value;
+				break;
+			case STUDIO_Z:
+				pos[k].z += value;
+				break;
+			}
+		}
+	}
+}
+
+
+void CalcBoneDerivatives( Vector &velocity, AngularImpulse &angVel, const matrix3x4_t &prev, const matrix3x4_t &current, float dt )
+{
+	float scale = 1.0;
+	if ( dt > 0 )
+	{
+		scale = 1.0 / dt;
+	}
+	
+	Vector endPosition, startPosition, deltaAxis;
+	QAngle endAngles, startAngles;
+	float deltaAngle;
+
+	MatrixAngles( prev, startAngles, startPosition );
+	MatrixAngles( current, endAngles, endPosition );
+
+	velocity.x = (endPosition.x - startPosition.x) * scale;
+	velocity.y = (endPosition.y - startPosition.y) * scale;
+	velocity.z = (endPosition.z - startPosition.z) * scale;
+	RotationDeltaAxisAngle( startAngles, endAngles, deltaAxis, deltaAngle );
+	VectorScale( deltaAxis, (deltaAngle * scale), angVel );
+}
+
+void CalcBoneVelocityFromDerivative( const QAngle &vecAngles, Vector &velocity, AngularImpulse &angVel, const matrix3x4_t &current )
+{
+	Vector vecLocalVelocity;
+	AngularImpulse LocalAngVel;
+	Quaternion q;
+	float angle;
+	MatrixAngles( current, q, vecLocalVelocity );
+	QuaternionAxisAngle( q, LocalAngVel, angle );
+	LocalAngVel *= angle;
+
+	matrix3x4_t matAngles;
+	AngleMatrix( vecAngles, matAngles );
+	VectorTransform( vecLocalVelocity, matAngles, velocity );
+	VectorTransform( LocalAngVel, matAngles, angVel );
+}
+
+
+
+
+class CIKSolver
+{
+public:
+//-------- SOLVE TWO LINK INVERSE KINEMATICS -------------
+// Author: Ken Perlin
+//
+// Given a two link joint from [0,0,0] to end effector position P,
+// let link lengths be a and b, and let norm |P| = c.  Clearly a+b <= c.
+//
+// Problem: find a "knee" position Q such that |Q| = a and |P-Q| = b.
+//
+// In the case of a point on the x axis R = [c,0,0], there is a
+// closed form solution S = [d,e,0], where |S| = a and |R-S| = b:
+//
+//    d2+e2 = a2                  -- because |S| = a
+//    (c-d)2+e2 = b2              -- because |R-S| = b
+//
+//    c2-2cd+d2+e2 = b2           -- combine the two equations
+//    c2-2cd = b2 - a2
+//    c-2d = (b2-a2)/c
+//    d - c/2 = (a2-b2)/c / 2
+//
+//    d = (c + (a2-b2/c) / 2      -- to solve for d and e.
+//    e = sqrt(a2-d2)
+
+   static float findD(float a, float b, float c) {
+      return (c + (a*a-b*b)/c) / 2;
+   }
+   static float findE(float a, float d) { return sqrt(a*a-d*d); } 
+
+// This leads to a solution to the more general problem:
+//
+//   (1) R = Mfwd(P)         -- rotate P onto the x axis
+//   (2) Solve for S
+//   (3) Q = Minv(S)         -- rotate back again
+
+   float Mfwd[3][3];
+   float Minv[3][3];
+
+   bool solve(float A, float B, float const P[], float const D[], float Q[]) {
+      float R[3];
+      defineM(P,D);
+      rot(Minv,P,R);
+	  float r = length(R);
+      float d = findD(A,B,r);
+      float e = findE(A,d);
+      float S[3] = {d,e,0};
+      rot(Mfwd,S,Q);
+      return d > (r - B) && d < A;
+   }
+
+// If "knee" position Q needs to be as close as possible to some point D,
+// then choose M such that M(D) is in the y>0 half of the z=0 plane.
+//
+// Given that constraint, define the forward and inverse of M as follows:
+
+   void defineM(float const P[], float const D[]) {
+      float *X = Minv[0], *Y = Minv[1], *Z = Minv[2];
+
+// Minv defines a coordinate system whose x axis contains P, so X = unit(P).
+	  int i;
+      for (i = 0 ; i < 3 ; i++)
+         X[i] = P[i];
+      normalize(X);
+
+// Its y axis is perpendicular to P, so Y = unit( E - X(E�X) ).
+
+      float dDOTx = dot(D,X);
+      for (i = 0 ; i < 3 ; i++)
+         Y[i] = D[i] - dDOTx * X[i];
+      normalize(Y);
+
+// Its z axis is perpendicular to both X and Y, so Z = X�Y.
+
+      cross(X,Y,Z);
+
+// Mfwd = (Minv)T, since transposing inverts a rotation matrix.
+
+      for (i = 0 ; i < 3 ; i++) {
+         Mfwd[i][0] = Minv[0][i];
+         Mfwd[i][1] = Minv[1][i];
+         Mfwd[i][2] = Minv[2][i];
+      }
+   }
+
+//------------ GENERAL VECTOR MATH SUPPORT -----------
+
+   static float dot(float const a[], float const b[]) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; }
+
+   static float length(float const v[]) { return sqrt( dot(v,v) ); }
+
+   static void normalize(float v[]) {
+      float norm = length(v);
+      for (int i = 0 ; i < 3 ; i++)
+         v[i] /= norm;
+   }
+
+   static void cross(float const a[], float const b[], float c[]) {
+      c[0] = a[1] * b[2] - a[2] * b[1];
+      c[1] = a[2] * b[0] - a[0] * b[2];
+      c[2] = a[0] * b[1] - a[1] * b[0];
+   }
+
+   static void rot(float const M[3][3], float const src[], float dst[]) {
+      for (int i = 0 ; i < 3 ; i++)
+         dst[i] = dot(M[i],src);
+   }
+};
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: visual debugging code
+//-----------------------------------------------------------------------------
+#if 1
+inline void debugLine(const Vector& origin, const Vector& dest, int r, int g, int b, bool noDepthTest, float duration) { };
+#else
+extern void drawLine( const Vector &p1, const Vector &p2, int r = 0, int g = 0, int b = 1, bool noDepthTest = true, float duration = 0.1 );
+void debugLine(const Vector& origin, const Vector& dest, int r, int g, int b, bool noDepthTest, float duration)
+{
+	drawLine( origin, dest, r, g, b, noDepthTest, duration );
+}
+#endif
+
+
+//-----------------------------------------------------------------------------
+// Purpose: for a 2 bone chain, find the IK solution and reset the matrices
+//-----------------------------------------------------------------------------
+bool Studio_SolveIK( mstudioikchain_t *pikchain, Vector &targetFoot, matrix3x4_t *pBoneToWorld )
+{
+	if (pikchain->pLink(0)->kneeDir.LengthSqr() > 0.0)
+	{
+		Vector targetKneeDir, targetKneePos;
+		// FIXME: knee length should be as long as the legs
+		Vector tmp = pikchain->pLink( 0 )->kneeDir;
+		VectorRotate( tmp, pBoneToWorld[ pikchain->pLink( 0 )->bone ], targetKneeDir );
+		MatrixPosition( pBoneToWorld[ pikchain->pLink( 1 )->bone ], targetKneePos );
+		return Studio_SolveIK( pikchain->pLink( 0 )->bone, pikchain->pLink( 1 )->bone, pikchain->pLink( 2 )->bone, targetFoot, targetKneePos, targetKneeDir, pBoneToWorld );
+	}
+	else
+	{
+		return Studio_SolveIK( pikchain->pLink( 0 )->bone, pikchain->pLink( 1 )->bone, pikchain->pLink( 2 )->bone, targetFoot, pBoneToWorld );
+	}
+}
+
+
+#define KNEEMAX_EPSILON 0.9998 // (0.9998 is about 1 degree)
+
+//-----------------------------------------------------------------------------
+// Purpose: Solve Knee position for a known hip and foot location, but no specific knee direction preference
+//-----------------------------------------------------------------------------
+
+bool Studio_SolveIK( int iThigh, int iKnee, int iFoot, Vector &targetFoot, matrix3x4_t *pBoneToWorld )
+{
+	Vector worldFoot, worldKnee, worldThigh;
+
+	MatrixPosition( pBoneToWorld[ iThigh ], worldThigh );
+	MatrixPosition( pBoneToWorld[ iKnee ], worldKnee );
+	MatrixPosition( pBoneToWorld[ iFoot ], worldFoot );
+
+	//debugLine( worldThigh, worldKnee, 0, 0, 255, true, 0 );
+	//debugLine( worldKnee, worldFoot, 0, 0, 255, true, 0 );
+
+	Vector ikFoot, ikKnee;
+
+	ikFoot = targetFoot - worldThigh;
+	ikKnee = worldKnee - worldThigh;
+
+	float l1 = (worldKnee-worldThigh).Length();
+	float l2 = (worldFoot-worldKnee).Length();
+	float l3 = (worldFoot-worldThigh).Length();
+
+	// leg too straight to figure out knee?
+	if (l3 > (l1 + l2) * KNEEMAX_EPSILON)
+	{
+		return false;
+	}
+
+	Vector ikHalf = (worldFoot-worldThigh) * (l1 / l3);
+
+	// FIXME: what to do when the knee completely straight?
+	Vector ikKneeDir = ikKnee - ikHalf;
+	VectorNormalize( ikKneeDir );
+
+	return Studio_SolveIK( iThigh, iKnee, iFoot, targetFoot, worldKnee, ikKneeDir, pBoneToWorld );
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: Realign the matrix so that its X axis points along the desired axis.
+//-----------------------------------------------------------------------------
+void Studio_AlignIKMatrix( matrix3x4_t &mMat, const Vector &vAlignTo )
+{
+	Vector tmp1, tmp2, tmp3;
+
+	// Column 0 (X) becomes the vector.
+	tmp1 = vAlignTo;
+	VectorNormalize( tmp1 );
+	MatrixSetColumn( tmp1, 0, mMat );
+
+	// Column 1 (Y) is the cross of the vector and column 2 (Z).
+	MatrixGetColumn( mMat, 2, tmp3 );
+	tmp2 = tmp3.Cross( tmp1 );
+	VectorNormalize( tmp2 );
+	// FIXME: check for X being too near to Z
+	MatrixSetColumn( tmp2, 1, mMat );
+
+	// Column 2 (Z) is the cross of columns 0 (X) and 1 (Y).
+	tmp3 = tmp1.Cross( tmp2 );
+	MatrixSetColumn( tmp3, 2, mMat );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Solve Knee position for a known hip and foot location, and a known knee direction
+//-----------------------------------------------------------------------------
+
+bool Studio_SolveIK( int iThigh, int iKnee, int iFoot, Vector &targetFoot, Vector &targetKneePos, Vector &targetKneeDir, matrix3x4_t *pBoneToWorld )
+{
+	Vector worldFoot, worldKnee, worldThigh;
+
+	MatrixPosition( pBoneToWorld[ iThigh ], worldThigh );
+	MatrixPosition( pBoneToWorld[ iKnee ], worldKnee );
+	MatrixPosition( pBoneToWorld[ iFoot ], worldFoot );
+
+	//debugLine( worldThigh, worldKnee, 0, 0, 255, true, 0 );
+	//debugLine( worldThigh, worldThigh + targetKneeDir, 0, 0, 255, true, 0 );
+	// debugLine( worldKnee, targetKnee, 0, 0, 255, true, 0 );
+
+	Vector ikFoot, ikTargetKnee, ikKnee;
+
+	ikFoot = targetFoot - worldThigh;
+	ikKnee = targetKneePos - worldThigh;
+
+	float l1 = (worldKnee-worldThigh).Length();
+	float l2 = (worldFoot-worldKnee).Length();
+
+	// exaggerate knee targets for legs that are nearly straight
+	// FIXME: should be configurable, and the ikKnee should be from the original animation, not modifed
+	float d = (targetFoot-worldThigh).Length() - min( l1, l2 );
+	d = max( l1 + l2, d );
+	// FIXME: too short knee directions cause trouble
+	d = d * 100;
+
+	ikTargetKnee = ikKnee + targetKneeDir * d;
+
+	// debugLine( worldKnee, worldThigh + ikTargetKnee, 0, 0, 255, true, 0 );
+
+	int color[3] = { 0, 255, 0 };
+
+	// too far away? (0.9998 is about 1 degree)
+	if (ikFoot.Length() > (l1 + l2) * KNEEMAX_EPSILON)
+	{
+		VectorNormalize( ikFoot );
+		VectorScale( ikFoot, (l1 + l2) * KNEEMAX_EPSILON, ikFoot );
+		color[0] = 255; color[1] = 0; color[2] = 0;
+	}
+
+	// too close?
+	// limit distance to about an 80 degree knee bend
+	float minDist = max( fabs(l1 - l2) * 1.15, min( l1, l2 ) * 0.15 );
+	if (ikFoot.Length() < minDist)
+	{
+		// too close to get an accurate vector, just use original vector
+		ikFoot = (worldFoot - worldThigh);
+		VectorNormalize( ikFoot );
+		VectorScale( ikFoot, minDist, ikFoot );
+	}
+
+	CIKSolver ik;
+	if (ik.solve( l1, l2, ikFoot.Base(), ikTargetKnee.Base(), ikKnee.Base() ))
+	{
+		matrix3x4_t& mWorldThigh = pBoneToWorld[ iThigh ];
+		matrix3x4_t& mWorldKnee = pBoneToWorld[ iKnee ];
+		matrix3x4_t& mWorldFoot = pBoneToWorld[ iFoot ];
+
+		//debugLine( worldThigh, ikKnee + worldThigh, 255, 0, 0, true, 0 );
+		//debugLine( ikKnee + worldThigh, ikFoot + worldThigh, 255, 0, 0, true,0 );
+
+		// debugLine( worldThigh, ikKnee + worldThigh, color[0], color[1], color[2], true, 0 );
+		// debugLine( ikKnee + worldThigh, ikFoot + worldThigh, color[0], color[1], color[2], true,0 );
+
+
+		// build transformation matrix for thigh
+		Studio_AlignIKMatrix( mWorldThigh, ikKnee );
+		Studio_AlignIKMatrix( mWorldKnee, ikFoot - ikKnee );
+
+
+		mWorldKnee[0][3] = ikKnee.x + worldThigh.x;
+		mWorldKnee[1][3] = ikKnee.y + worldThigh.y;
+		mWorldKnee[2][3] = ikKnee.z + worldThigh.z;
+
+		mWorldFoot[0][3] = ikFoot.x + worldThigh.x;
+		mWorldFoot[1][3] = ikFoot.y + worldThigh.y;
+		mWorldFoot[2][3] = ikFoot.z + worldThigh.z;
+
+		return true;
+	}
+	else
+	{
+		/*
+		debugLine( worldThigh, worldThigh + ikKnee, 255, 0, 0, true, 0 );
+		debugLine( worldThigh + ikKnee, worldThigh + ikFoot, 255, 0, 0, true, 0 );
+		debugLine( worldThigh + ikFoot, worldThigh, 255, 0, 0, true, 0 );
+		debugLine( worldThigh + ikKnee, worldThigh + ikTargetKnee, 255, 0, 0, true, 0 );
+		*/
+		return false;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+float Studio_IKRuleWeight( mstudioikrule_t &ikRule, const mstudioanimdesc_t *panim, float flCycle, int &iFrame, float &fraq )
+{
+	if (ikRule.end > 1.0f && flCycle < ikRule.start)
+	{
+		flCycle = flCycle + 1.0f;
+	}
+
+	float value = 0.0f;
+	fraq = (panim->numframes - 1) * (flCycle - ikRule.start) + ikRule.iStart;
+	iFrame = (int)fraq;
+	fraq = fraq - iFrame;
+
+	if (flCycle < ikRule.start)
+	{
+		iFrame = ikRule.iStart;
+		fraq = 0.0f;
+		return 0.0f;
+	}
+	else if (flCycle < ikRule.peak )
+	{
+		value = (flCycle - ikRule.start) / (ikRule.peak - ikRule.start);
+	}
+	else if (flCycle < ikRule.tail )
+	{
+		return 1.0f;
+	}
+	else if (flCycle < ikRule.end )
+	{
+		value = 1.0f - ((flCycle - ikRule.tail) / (ikRule.end - ikRule.tail));
+	}
+	else
+	{
+		fraq = (panim->numframes - 1) * (ikRule.end - ikRule.start) + ikRule.iStart;
+		iFrame = (int)fraq;
+		fraq = fraq - iFrame;
+	}
+	return SimpleSpline( value );
+}
+
+
+float Studio_IKRuleWeight( ikcontextikrule_t &ikRule, float flCycle )
+{
+	if (ikRule.end > 1.0f && flCycle < ikRule.start)
+	{
+		flCycle = flCycle + 1.0f;
+	}
+
+	float value = 0.0f;
+	if (flCycle < ikRule.start)
+	{
+		return 0.0f;
+	}
+	else if (flCycle < ikRule.peak )
+	{
+		value = (flCycle - ikRule.start) / (ikRule.peak - ikRule.start);
+	}
+	else if (flCycle < ikRule.tail )
+	{
+		return 1.0f;
+	}
+	else if (flCycle < ikRule.end )
+	{
+		value = 1.0f - ((flCycle - ikRule.tail) / (ikRule.end - ikRule.tail));
+	}
+	return 3.0f * value * value - 2.0f * value * value * value;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+bool Studio_IKShouldLatch( ikcontextikrule_t &ikRule, float flCycle )
+{
+	if (ikRule.end > 1.0f && flCycle < ikRule.start)
+	{
+		flCycle = flCycle + 1.0f;
+	}
+
+	if (flCycle < ikRule.peak )
+	{
+		return false;
+	}
+	else if (flCycle < ikRule.end )
+	{
+		return true;
+	}
+	return false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+float Studio_IKTail( ikcontextikrule_t &ikRule, float flCycle )
+{
+	if (ikRule.end > 1.0f && flCycle < ikRule.start)
+	{
+		flCycle = flCycle + 1.0f;
+	}
+
+	if (flCycle <= ikRule.tail )
+	{
+		return 0.0f;
+	}
+	else if (flCycle < ikRule.end )
+	{
+		return ((flCycle - ikRule.tail) / (ikRule.end - ikRule.tail));
+	}
+	return 0.0;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+
+bool Studio_IKAnimationError( const CStudioHdr *pStudioHdr, mstudioikrule_t *pRule, const mstudioanimdesc_t *panim, float flCycle, Vector &pos, Quaternion &q, float &flWeight )
+{
+	float fraq;
+	int iFrame;
+
+	flWeight = Studio_IKRuleWeight( *pRule, panim, flCycle, iFrame, fraq );
+	Assert( fraq >= 0.0 && fraq < 1.0 );
+	Assert( flWeight >= 0.0f && flWeight <= 1.0f );
+
+	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up
+	flWeight = clamp( flWeight, 0.0f, 1.0f );
+
+	if (pRule->type != IK_GROUND && flWeight < 0.0001)
+		return false;
+
+	mstudioikerror_t *pError = pRule->pError( iFrame );
+	if (pError != NULL)
+	{
+		if (fraq < 0.001)
+		{
+			q = pError[0].q;
+			pos = pError[0].pos;
+		}
+		else
+		{
+			QuaternionBlend( pError[0].q, pError[1].q, fraq, q );
+			pos = pError[0].pos * (1.0f - fraq) + pError[1].pos * fraq;
+		}
+		return true;
+	}
+
+	mstudiocompressedikerror_t *pCompressed = pRule->pCompressedError();
+	if (pCompressed != NULL)
+	{
+		CalcDecompressedAnimation( pCompressed, iFrame - pRule->iStart, fraq, pos, q );
+		return true;
+	}
+	// no data, disable IK rule
+	Assert( 0 );
+	flWeight = 0.0f;
+	return false;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: For a specific sequence:rule, find where it starts, stops, and what 
+//			the estimated offset from the connection point is.
+//			return true if the rule is within bounds.
+//-----------------------------------------------------------------------------
+
+bool Studio_IKSequenceError( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, float flCycle, int iRule, const float poseParameter[], mstudioanimdesc_t *panim[4], float weight[4], ikcontextikrule_t &ikRule )
+{
+	int i;
+
+	memset( &ikRule, 0, sizeof(ikRule) );
+	ikRule.start = ikRule.peak = ikRule.tail = ikRule.end = 0;
+
+
+	mstudioikrule_t *prevRule = NULL;
+
+	// find overall influence
+	for (i = 0; i < 4; i++)
+	{
+		if (weight[i])
+		{
+			if (iRule >= panim[i]->numikrules || panim[i]->numikrules != panim[0]->numikrules)
+			{
+				Assert( 0 );
+				return false;
+			}
+
+			mstudioikrule_t *pRule = panim[i]->pIKRule( iRule );
+			if (pRule == NULL)
+				return false;
+
+			float dt = 0.0;
+			if (prevRule != NULL)
+			{
+				if (pRule->start - prevRule->start > 0.5)
+				{
+					dt = -1.0;
+				}
+				else if (pRule->start - prevRule->start < -0.5)
+				{
+					dt = 1.0;
+				}
+			}
+			else
+			{
+				prevRule = pRule;
+			}
+
+			ikRule.start += (pRule->start + dt) * weight[i];
+			ikRule.peak += (pRule->peak + dt) * weight[i];
+			ikRule.tail += (pRule->tail + dt) * weight[i];
+			ikRule.end += (pRule->end + dt) * weight[i];
+		}
+	}
+	if (ikRule.start > 1.0)
+	{
+		ikRule.start -= 1.0;
+		ikRule.peak -= 1.0;
+		ikRule.tail -= 1.0;
+		ikRule.end -= 1.0;
+	}
+	else if (ikRule.start < 0.0)
+	{
+		ikRule.start += 1.0;
+		ikRule.peak += 1.0;
+		ikRule.tail += 1.0;
+		ikRule.end += 1.0;
+	}
+
+	ikRule.flWeight = Studio_IKRuleWeight( ikRule, flCycle );
+	if (ikRule.flWeight <= 0.001f)
+	{
+		// go ahead and allow IK_GROUND rules a virtual looping section
+		if ( panim[0]->pIKRule( iRule ) == NULL ) 
+			return false;
+		if ((panim[0]->flags & STUDIO_LOOPING) && panim[0]->pIKRule( iRule )->type == IK_GROUND && ikRule.end - ikRule.start > 0.75 )
+		{
+			ikRule.flWeight = 0.001;
+			flCycle = ikRule.end - 0.001;
+		}
+		else
+		{
+			return false;
+		}
+	}
+
+	Assert( ikRule.flWeight > 0.0f );
+
+	ikRule.pos.Init();
+	ikRule.q.Init();
+
+	// find target error
+	float total = 0.0f;
+	for (i = 0; i < 4; i++)
+	{
+		if (weight[i])
+		{
+			Vector pos1;
+			Quaternion q1;
+			float w;
+
+			mstudioikrule_t *pRule = panim[i]->pIKRule( iRule );
+			if (pRule == NULL)
+				return false;
+
+			ikRule.chain = pRule->chain;	// FIXME: this is anim local
+			ikRule.bone = pRule->bone;		// FIXME: this is anim local
+			ikRule.type = pRule->type;
+			ikRule.slot = pRule->slot;
+
+			ikRule.height += pRule->height * weight[i];
+			ikRule.floor += pRule->floor * weight[i];
+			ikRule.radius += pRule->radius * weight[i];
+			ikRule.drop += pRule->drop * weight[i];
+			ikRule.top += pRule->top * weight[i];
+
+			// keep track of tail condition
+			ikRule.release += Studio_IKTail( ikRule, flCycle ) * weight[i];
+
+			// only check rules with error values
+			switch( ikRule.type )
+			{
+			case IK_SELF:
+			case IK_WORLD:
+			case IK_GROUND:
+			case IK_ATTACHMENT:
+				{
+					int bResult = Studio_IKAnimationError( pStudioHdr, pRule, panim[i], flCycle, pos1, q1, w );
+
+					if (bResult)
+					{
+						ikRule.pos = ikRule.pos + pos1 * weight[i];
+						QuaternionAccumulate( ikRule.q, weight[i], q1, ikRule.q );
+						total += weight[i];
+					}
+				}
+				break;
+			default:
+				total += weight[i];
+				break;
+			}
+
+			ikRule.latched = Studio_IKShouldLatch( ikRule, flCycle ) * ikRule.flWeight;
+
+			if (ikRule.type == IK_ATTACHMENT)
+			{
+				ikRule.szLabel = pRule->pszAttachment();
+			}
+		}
+	}
+
+	if (total <= 0.0001f)
+	{
+		return false;
+	}
+
+	if (total < 0.999f)
+	{
+		VectorScale( ikRule.pos, 1.0f / total, ikRule.pos );
+		QuaternionScale( ikRule.q, 1.0f / total, ikRule.q );
+	}
+
+	if (ikRule.type == IK_SELF && ikRule.bone != -1)
+	{
+		// FIXME: this is anim local, not seq local!
+		ikRule.bone = pStudioHdr->RemapSeqBone( iSequence, ikRule.bone );
+		if (ikRule.bone == -1)
+			return false;
+	}
+
+	QuaternionNormalize( ikRule.q );
+	return true;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+
+CIKContext::CIKContext()
+{
+	m_target.EnsureCapacity( 12 ); // FIXME: this sucks, shouldn't it be grown?
+	m_iFramecounter = -1;
+	m_pStudioHdr = NULL;
+	m_flTime = -1.0f;
+	m_target.SetSize( 0 );
+}
+
+
+void CIKContext::Init( const CStudioHdr *pStudioHdr, const QAngle &angles, const Vector &pos, float flTime, int iFramecounter, int boneMask )
+{
+	m_pStudioHdr = pStudioHdr;
+	m_ikChainRule.RemoveAll(); // m_numikrules = 0;
+	if (pStudioHdr->numikchains())
+	{
+		m_ikChainRule.SetSize( pStudioHdr->numikchains() );
+
+		// FIXME: Brutal hackery to prevent a crash
+		if (m_target.Count() == 0)
+		{
+			m_target.SetSize(12);
+			memset( m_target.Base(), 0, sizeof(m_target[0])*m_target.Count() );
+			ClearTargets();
+		}
+
+	}
+	else
+	{
+		m_target.SetSize( 0 );
+	}
+	AngleMatrix( angles, pos, m_rootxform );
+	m_iFramecounter = iFramecounter;
+	m_flTime = flTime;
+	m_boneMask = boneMask;
+}
+
+void CIKContext::AddDependencies( mstudioseqdesc_t &seqdesc, int iSequence, float flCycle, const float poseParameters[], float flWeight )
+{
+	int i;
+
+	if ( m_pStudioHdr->numikchains() == 0)
+		return;
+
+	if (seqdesc.numikrules == 0)
+		return;
+
+	ikcontextikrule_t ikrule;
+
+	Assert( flWeight >= 0.0f && flWeight <= 1.0f );
+	// This shouldn't be necessary, but the Assert should help us catch whoever is screwing this up
+	flWeight = clamp( flWeight, 0.0f, 1.0f );
+
+	// unify this
+	if (seqdesc.flags & STUDIO_REALTIME)
+	{
+		float cps = Studio_CPS( m_pStudioHdr, seqdesc, iSequence, poseParameters );
+		flCycle = m_flTime * cps;
+		flCycle = flCycle - (int)flCycle;
+	}
+	else if (flCycle < 0 || flCycle >= 1)
+	{
+		if (seqdesc.flags & STUDIO_LOOPING)
+		{
+			flCycle = flCycle - (int)flCycle;
+			if (flCycle < 0) flCycle += 1;
+		}
+		else
+		{
+			flCycle = max( 0.0, min( flCycle, 0.9999 ) );
+		}
+	}
+
+	mstudioanimdesc_t *panim[4];
+	float	weight[4];
+
+	Studio_SeqAnims( m_pStudioHdr, seqdesc, iSequence, poseParameters, panim, weight );
+
+	// FIXME: add proper number of rules!!!
+	for (i = 0; i < seqdesc.numikrules; i++)
+	{
+		if ( !Studio_IKSequenceError( m_pStudioHdr, seqdesc, iSequence, flCycle, i, poseParameters, panim, weight, ikrule ) )
+			continue;
+
+		// don't add rule if the bone isn't going to be calculated
+		int bone = m_pStudioHdr->pIKChain( ikrule.chain )->pLink( 2 )->bone;
+		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))
+			continue;
+
+		// or if its relative bone isn't going to be calculated
+		if ( ikrule.bone >= 0 && !(m_pStudioHdr->boneFlags( ikrule.bone ) & m_boneMask))
+			continue;
+
+		// FIXME: Brutal hackery to prevent a crash
+		if (m_target.Count() == 0)
+		{
+			m_target.SetSize(12);
+			memset( m_target.Base(), 0, sizeof(m_target[0])*m_target.Count() );
+			ClearTargets();
+		}
+
+		ikrule.flRuleWeight = flWeight;
+
+		if (ikrule.flRuleWeight * ikrule.flWeight > 0.999)
+		{
+			if ( ikrule.type != IK_UNLATCH)
+			{
+				// clear out chain if rule is 100%
+				m_ikChainRule.Element( ikrule.chain ).RemoveAll( );
+				if ( ikrule.type == IK_RELEASE)
+				{
+					continue;
+				}
+			}
+		}
+
+ 		int nIndex = m_ikChainRule.Element( ikrule.chain ).AddToTail( );
+  		m_ikChainRule.Element( ikrule.chain ).Element( nIndex ) = ikrule;
+	}
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKContext::AddAutoplayLocks( Vector pos[], Quaternion q[] )
+{
+	// skip all array access if no autoplay locks.
+	if (m_pStudioHdr->GetNumIKAutoplayLocks() == 0)
+	{
+		return;
+	}
+
+	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList boneComputed;
+
+	int ikOffset = m_ikLock.AddMultipleToTail( m_pStudioHdr->GetNumIKAutoplayLocks() );
+	memset( &m_ikLock[ikOffset], 0, sizeof(ikcontextikrule_t)*m_pStudioHdr->GetNumIKAutoplayLocks() );
+
+	for (int i = 0; i < m_pStudioHdr->GetNumIKAutoplayLocks(); i++)
+	{
+		const mstudioiklock_t &lock = ((CStudioHdr *)m_pStudioHdr)->pIKAutoplayLock( i );
+		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( lock.chain );
+		int bone = pchain->pLink( 2 )->bone;
+
+		// don't bother with iklock if the bone isn't going to be calculated
+		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))
+			continue;
+
+		// eval current ik'd bone
+		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );
+
+		ikcontextikrule_t &ikrule = m_ikLock[ i + ikOffset ];
+
+		ikrule.chain = lock.chain;
+		ikrule.slot = i;
+		ikrule.type = IK_WORLD;
+
+		MatrixAngles( boneToWorld[bone], ikrule.q, ikrule.pos );
+
+		// save off current knee direction
+		if (pchain->pLink(0)->kneeDir.LengthSqr() > 0.0)
+		{
+			Vector tmp = pchain->pLink( 0 )->kneeDir;
+			VectorRotate( pchain->pLink( 0 )->kneeDir, boneToWorld[ pchain->pLink( 0 )->bone ], ikrule.kneeDir );
+			MatrixPosition( boneToWorld[ pchain->pLink( 1 )->bone ], ikrule.kneePos ); 
+		}
+		else
+		{
+			ikrule.kneeDir.Init( );
+		}
+	}
+	g_MatrixPool.Free( boneToWorld );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKContext::AddSequenceLocks( mstudioseqdesc_t &seqdesc, Vector pos[], Quaternion q[] )
+{
+	if ( m_pStudioHdr->numikchains() == 0)
+	{
+		return;
+	}
+
+	if ( seqdesc.numiklocks == 0 )
+	{
+		return;
+	}
+
+	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList boneComputed;
+
+	int ikOffset = m_ikLock.AddMultipleToTail( seqdesc.numiklocks );
+	memset( &m_ikLock[ikOffset], 0, sizeof(ikcontextikrule_t) * seqdesc.numiklocks );
+
+	for (int i = 0; i < seqdesc.numiklocks; i++)
+	{
+		mstudioiklock_t *plock = seqdesc.pIKLock( i );
+		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( plock->chain );
+		int bone = pchain->pLink( 2 )->bone;
+
+		// don't bother with iklock if the bone isn't going to be calculated
+		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))
+			continue;
+
+		// eval current ik'd bone
+		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );
+
+		ikcontextikrule_t &ikrule = m_ikLock[i+ikOffset];
+		ikrule.chain = i;
+		ikrule.slot = i;
+		ikrule.type = IK_WORLD;
+
+		MatrixAngles( boneToWorld[bone], ikrule.q, ikrule.pos );
+
+		// save off current knee direction
+		if (pchain->pLink(0)->kneeDir.LengthSqr() > 0.0)
+		{
+			VectorRotate( pchain->pLink( 0 )->kneeDir, boneToWorld[ pchain->pLink( 0 )->bone ], ikrule.kneeDir );
+		}
+		else
+		{
+			ikrule.kneeDir.Init( );
+		}
+	}
+	g_MatrixPool.Free( boneToWorld );
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: build boneToWorld transforms for a specific bone
+//-----------------------------------------------------------------------------
+void CIKContext::BuildBoneChain(
+	const Vector pos[], 
+	const Quaternion q[], 
+	int	iBone,
+	matrix3x4_t *pBoneToWorld,
+	CBoneBitList &boneComputed )
+{
+	Assert( m_pStudioHdr->boneFlags( iBone ) & m_boneMask );
+	::BuildBoneChain( m_pStudioHdr, m_rootxform, pos, q, iBone, pBoneToWorld, boneComputed );
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: build boneToWorld transforms for a specific bone
+//-----------------------------------------------------------------------------
+void BuildBoneChain(
+	const CStudioHdr *pStudioHdr,
+	const matrix3x4_t &rootxform,
+	const Vector pos[], 
+	const Quaternion q[], 
+	int	iBone,
+	matrix3x4_t *pBoneToWorld,
+	CBoneBitList &boneComputed )
+{
+	if ( boneComputed.IsBoneMarked(iBone) )
+		return;
+
+	matrix3x4_t bonematrix;
+	QuaternionMatrix( q[iBone], pos[iBone], bonematrix );
+
+	int parent = pStudioHdr->boneParent( iBone );
+	if (parent == -1) 
+	{
+		ConcatTransforms( rootxform, bonematrix, pBoneToWorld[iBone] );
+	}
+	else
+	{
+		// evil recursive!!!
+		BuildBoneChain( pStudioHdr, rootxform, pos, q, parent, pBoneToWorld, boneComputed );
+		ConcatTransforms( pBoneToWorld[parent], bonematrix, pBoneToWorld[iBone]);
+	}
+	boneComputed.MarkBone(iBone);
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: turn a specific bones boneToWorld transform into a pos and q in parents bonespace
+//-----------------------------------------------------------------------------
+void SolveBone( 
+	const CStudioHdr *pStudioHdr,
+	int	iBone,
+	matrix3x4_t *pBoneToWorld,
+	Vector pos[], 
+	Quaternion q[]
+	)
+{
+	int iParent = pStudioHdr->boneParent( iBone );
+
+	matrix3x4_t worldToBone;
+	MatrixInvert( pBoneToWorld[iParent], worldToBone );
+
+	matrix3x4_t local;
+	ConcatTransforms( worldToBone, pBoneToWorld[iBone], local );
+
+	MatrixAngles( local, q[iBone], pos[iBone] );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+
+void CIKTarget::SetOwner( int entindex, const Vector &pos, const QAngle &angles )
+{
+	latched.owner = entindex;
+	latched.absOrigin = pos;
+	latched.absAngles = angles;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+
+void CIKTarget::ClearOwner( void )
+{
+	latched.owner = -1;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+
+int CIKTarget::GetOwner( void )
+{
+	return latched.owner;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: update the latched IK values that are in a moving frame of reference
+//-----------------------------------------------------------------------------
+
+void CIKTarget::UpdateOwner( int entindex, const Vector &pos, const QAngle &angles )
+{
+	if (pos == latched.absOrigin && angles == latched.absAngles)
+		return;
+
+	matrix3x4_t in, out;
+	AngleMatrix( angles, pos, in );
+	AngleIMatrix( latched.absAngles, latched.absOrigin, out );
+
+	matrix3x4_t tmp1, tmp2;
+	QuaternionMatrix( latched.q, latched.pos, tmp1 );
+	ConcatTransforms( out, tmp1, tmp2 );
+	ConcatTransforms( in, tmp2, tmp1 );
+	MatrixAngles( tmp1, latched.q, latched.pos );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: sets the ground position of an ik target
+//-----------------------------------------------------------------------------
+
+void CIKTarget::SetPos( const Vector &pos )
+{
+	est.pos = pos;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: sets the ground "identity" orientation of an ik target
+//-----------------------------------------------------------------------------
+
+void CIKTarget::SetAngles( const QAngle &angles )
+{
+	AngleQuaternion( angles, est.q );
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: sets the ground "identity" orientation of an ik target
+//-----------------------------------------------------------------------------
+
+void CIKTarget::SetQuaternion( const Quaternion &q )
+{
+	est.q = q;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: calculates a ground "identity" orientation based on the surface
+//			normal of the ground and the desired ground identity orientation
+//-----------------------------------------------------------------------------
+
+void CIKTarget::SetNormal( const Vector &normal )
+{
+	// recalculate foot angle based on slope of surface
+	matrix3x4_t m1;
+	Vector forward, right;
+	QuaternionMatrix( est.q, m1 );
+
+	MatrixGetColumn( m1, 1, right );
+	forward = CrossProduct( right, normal );
+	right = CrossProduct( normal, forward );
+	MatrixSetColumn( forward, 0, m1 );
+	MatrixSetColumn( right, 1, m1 );
+	MatrixSetColumn( normal, 2, m1 );
+	QAngle a1;
+	Vector p1;
+	MatrixAngles( m1, est.q, p1 );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: estimates the ground impact at the center location assuming a the edge of 
+//			an Z axis aligned disc collided with it the surface.
+//-----------------------------------------------------------------------------
+
+void CIKTarget::SetPosWithNormalOffset( const Vector &pos, const Vector &normal )
+{
+	// assume it's a disc edge intersecting with the floor, so try to estimate the z location of the center
+	est.pos = pos;
+	if (normal.z > 0.9999)
+	{
+		return;
+	}
+	// clamp at 45 degrees
+	else if (normal.z > 0.707)
+	{
+		// tan == sin / cos
+		float tan = sqrt( 1 - normal.z * normal.z ) / normal.z;
+		est.pos.z = est.pos.z - est.radius * tan;
+	}
+	else
+	{
+		est.pos.z = est.pos.z - est.radius;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKTarget::SetOnWorld( bool bOnWorld )
+{
+	est.onWorld = bOnWorld;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+bool CIKTarget::IsActive()
+{ 
+	return (est.flWeight > 0.0f);
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKTarget::IKFailed( void )
+{
+	latched.deltaPos.Init();
+	latched.deltaQ.Init();
+	latched.pos = ideal.pos;
+	latched.q = ideal.q;
+	est.latched = 0.0;
+	est.flWeight = 0.0;
+	est.onWorld = false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKTarget::MoveReferenceFrame( Vector &deltaPos, QAngle &deltaAngles )
+{
+	est.pos -= deltaPos;
+	latched.pos -= deltaPos;
+	offset.pos -= deltaPos;
+	ideal.pos -= deltaPos;
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Invalidate any IK locks.
+//-----------------------------------------------------------------------------
+
+void CIKContext::ClearTargets( void )
+{
+	int i;
+	for (i = 0; i < m_target.Count(); i++)
+	{
+		m_target[i].latched.iFramecounter = -9999;
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: Run through the rules that survived and turn a specific bones boneToWorld 
+//			transform into a pos and q in parents bonespace
+//-----------------------------------------------------------------------------
+
+void CIKContext::UpdateTargets( Vector pos[], Quaternion q[], matrix3x4_t boneToWorld[], CBoneBitList &boneComputed )
+{
+	int i, j;
+
+	for (i = 0; i < m_target.Count(); i++)
+	{
+		m_target[i].est.flWeight = 0.0f;
+		m_target[i].est.latched = 1.0f;
+		m_target[i].est.release = 1.0f;
+		m_target[i].est.height = 0.0f;
+		m_target[i].est.floor = 0.0f;
+		m_target[i].est.radius = 0.0f;
+		m_target[i].offset.pos.Init();
+		m_target[i].offset.q.Init();
+	}
+
+	AutoIKRelease( );
+
+	for (j = 0; j < m_ikChainRule.Count(); j++)
+	{
+		for (i = 0; i < m_ikChainRule.Element( j ).Count(); i++)
+		{
+			ikcontextikrule_t *pRule = &m_ikChainRule.Element( j ).Element( i );
+
+			// ikchainresult_t *pChainRule = &chainRule[ m_ikRule[i].chain ];
+
+			switch( pRule->type )
+			{
+			case IK_ATTACHMENT:
+			case IK_GROUND:
+			// case IK_SELF:
+				{
+					matrix3x4_t footTarget;
+					CIKTarget *pTarget = &m_target[pRule->slot];
+					pTarget->chain = pRule->chain;
+					pTarget->type = pRule->type;
+
+					if (pRule->type == IK_ATTACHMENT)
+					{
+						pTarget->offset.pAttachmentName = pRule->szLabel;
+					}
+					else
+					{
+						pTarget->offset.pAttachmentName = NULL;
+					}
+
+					if (pRule->flRuleWeight == 1.0f || pTarget->est.flWeight == 0.0f)
+					{
+						pTarget->offset.q = pRule->q;
+						pTarget->offset.pos = pRule->pos;
+						pTarget->est.height = pRule->height;
+						pTarget->est.floor = pRule->floor;
+						pTarget->est.radius = pRule->radius;
+						pTarget->est.latched = pRule->latched * pRule->flRuleWeight;
+						pTarget->est.release = pRule->release;
+						pTarget->est.flWeight = pRule->flWeight * pRule->flRuleWeight;
+					}
+					else
+					{
+						QuaternionSlerp( pTarget->offset.q, pRule->q, pRule->flRuleWeight, pTarget->offset.q );
+						pTarget->offset.pos = Lerp( pRule->flRuleWeight, pTarget->offset.pos, pRule->pos );
+						pTarget->est.height = Lerp( pRule->flRuleWeight, pTarget->est.height, pRule->height );
+						pTarget->est.floor = Lerp( pRule->flRuleWeight, pTarget->est.floor, pRule->floor );
+						pTarget->est.radius = Lerp( pRule->flRuleWeight, pTarget->est.radius, pRule->radius );
+						//pTarget->est.latched = Lerp( pRule->flRuleWeight, pTarget->est.latched, pRule->latched );
+						pTarget->est.latched = min( pTarget->est.latched, pRule->latched );
+						pTarget->est.release = Lerp( pRule->flRuleWeight, pTarget->est.release, pRule->release );
+						pTarget->est.flWeight = Lerp( pRule->flRuleWeight, pTarget->est.flWeight, pRule->flWeight );
+					}
+
+					if ( pRule->type == IK_GROUND )
+					{
+						pTarget->latched.deltaPos.z = 0;
+						pTarget->est.pos.z = pTarget->est.floor + m_rootxform[2][3];
+					}
+				}
+			break;
+			case IK_UNLATCH:
+				{
+					CIKTarget *pTarget = &m_target[pRule->slot];
+					if (pRule->latched > 0.0)
+						pTarget->est.latched = 0.0;
+					else
+						pTarget->est.latched = min( pTarget->est.latched, 1.0f - pRule->flWeight );
+				}
+				break;
+			case IK_RELEASE:
+				{
+					CIKTarget *pTarget = &m_target[pRule->slot];
+					if (pRule->latched > 0.0)
+						pTarget->est.latched = 0.0;
+					else
+						pTarget->est.latched = min( pTarget->est.latched, 1.0f - pRule->flWeight );
+
+					pTarget->est.flWeight = (pTarget->est.flWeight) * (1 - pRule->flWeight * pRule->flRuleWeight);
+				}
+				break;
+			}
+		}
+	}
+
+	for (i = 0; i < m_target.Count(); i++)
+	{
+		CIKTarget *pTarget = &m_target[i];
+		if (pTarget->est.flWeight > 0.0)
+		{
+			mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( pTarget->chain );
+			// ikchainresult_t *pChainRule = &chainRule[ i ];
+			int bone = pchain->pLink( 2 )->bone;
+
+			// eval current ik'd bone
+			BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );
+
+			// xform IK target error into world space
+			matrix3x4_t local;
+			matrix3x4_t worldFootpad;
+			QuaternionMatrix( pTarget->offset.q, pTarget->offset.pos, local );
+			MatrixInvert( local, local );
+			ConcatTransforms( boneToWorld[bone], local, worldFootpad );
+
+			if (pTarget->est.latched == 1.0)
+			{
+				pTarget->latched.bNeedsLatch = true;
+			}
+			else
+			{
+				pTarget->latched.bNeedsLatch = false;
+			}
+
+			// disable latched position if it looks invalid
+			if (m_iFramecounter < 0 || pTarget->latched.iFramecounter < m_iFramecounter - 1 || pTarget->latched.iFramecounter > m_iFramecounter)
+			{
+				pTarget->latched.bHasLatch = false;
+				pTarget->latched.influence = 0.0;
+			}
+			pTarget->latched.iFramecounter = m_iFramecounter;
+
+			// find ideal contact position
+			MatrixAngles( worldFootpad, pTarget->ideal.q, pTarget->ideal.pos );
+			pTarget->est.q = pTarget->ideal.q;
+			pTarget->est.pos = pTarget->ideal.pos;
+
+			float latched = pTarget->est.latched;
+
+			if (pTarget->latched.bHasLatch)
+			{
+				if (pTarget->est.latched == 1.0)
+				{
+					// keep track of latch position error from ideal contact position
+					pTarget->latched.deltaPos = pTarget->latched.pos - pTarget->est.pos;
+					QuaternionSM( -1, pTarget->est.q, pTarget->latched.q, pTarget->latched.deltaQ );
+					pTarget->est.q = pTarget->latched.q;
+					pTarget->est.pos = pTarget->latched.pos;
+				}
+				else if (pTarget->est.latched > 0.0)
+				{
+					// ramp out latch differences during decay phase of rule
+					if (latched > 0 && latched < pTarget->latched.influence)
+					{
+						// latching has decreased
+						float dt = pTarget->latched.influence - latched;
+						if (pTarget->latched.influence > 0.0)
+							dt = dt / pTarget->latched.influence;
+
+						VectorScale( pTarget->latched.deltaPos, (1-dt), pTarget->latched.deltaPos );
+						QuaternionScale( pTarget->latched.deltaQ, (1-dt), pTarget->latched.deltaQ );
+					}
+
+					// move ideal contact position by latched error factor
+					pTarget->est.pos = pTarget->est.pos + pTarget->latched.deltaPos;
+					QuaternionMA( pTarget->est.q, 1, pTarget->latched.deltaQ, pTarget->est.q );
+					pTarget->latched.q = pTarget->est.q;
+					pTarget->latched.pos = pTarget->est.pos;
+				}
+				else
+				{
+					pTarget->latched.bHasLatch = false;
+					pTarget->latched.q = pTarget->est.q;
+					pTarget->latched.pos = pTarget->est.pos;
+					pTarget->latched.deltaPos.Init();
+					pTarget->latched.deltaQ.Init();
+				}
+				pTarget->latched.influence = latched;
+			}
+
+			// check for illegal requests
+			Vector p1, p2, p3;
+			MatrixPosition( boneToWorld[pchain->pLink( 0 )->bone], p1 ); // hip
+			MatrixPosition( boneToWorld[pchain->pLink( 1 )->bone], p2 ); // knee
+			MatrixPosition( boneToWorld[pchain->pLink( 2 )->bone], p3 ); // foot
+
+			float d1 = (p2 - p1).Length();
+			float d2 = (p3 - p2).Length();
+
+			if (pTarget->latched.bHasLatch)
+			{
+				//float d3 = (p3 - p1).Length();
+				float d4 = (p3 + pTarget->latched.deltaPos - p1).Length();
+
+				// unstick feet when distance is too great
+				if ((d4 < fabs( d1 - d2 ) || d4 * 0.95 > d1 + d2) && pTarget->est.latched > 0.2)
+				{
+					pTarget->error.flTime = m_flTime;
+				}
+
+				// unstick feet when angle is too great
+				if (pTarget->est.latched > 0.2)
+				{
+					float d = fabs( pTarget->latched.deltaQ.w ) * 2.0f - 1.0f; // QuaternionDotProduct( pTarget->latched.q, pTarget->est.q );
+
+					// FIXME: cos(45), make property of chain
+					if (d < 0.707)
+					{
+						pTarget->error.flTime = m_flTime;
+					}
+				}
+			}
+
+			Vector dt = pTarget->est.pos - p1;
+			pTarget->trace.hipToFoot = VectorNormalize( dt );
+			pTarget->trace.hipToKnee = d1;
+			pTarget->trace.kneeToFoot = d2;
+			pTarget->trace.hip = p1;
+			pTarget->trace.knee = p2;
+			pTarget->trace.closest = p1 + dt * (fabs( d1 - d2 ) * 1.01);
+			pTarget->trace.farthest = p1 + dt * (d1 + d2) * 0.99;
+			pTarget->trace.lowest = p1 + Vector( 0, 0, -1 ) * (d1 + d2) * 0.99;
+			// pTarget->trace.endpos = pTarget->est.pos;
+		}
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: insert release rules if the ik rules were in error
+//-----------------------------------------------------------------------------
+
+void CIKContext::AutoIKRelease( void )
+{
+	int i;
+
+	for (i = 0; i < m_target.Count(); i++)
+	{
+		CIKTarget *pTarget = &m_target[i];
+
+		float dt = m_flTime - pTarget->error.flTime;
+		if (pTarget->error.bInError || dt < 0.5)
+		{
+			if (!pTarget->error.bInError)
+			{
+				pTarget->error.ramp = 0.0; 
+				pTarget->error.flErrorTime = pTarget->error.flTime;
+				pTarget->error.bInError = true;
+			}
+
+			float ft = m_flTime - pTarget->error.flErrorTime;
+			if (dt < 0.25)
+			{
+				pTarget->error.ramp = min( pTarget->error.ramp + ft * 4.0, 1.0 );
+			}
+			else
+			{
+				pTarget->error.ramp = max( pTarget->error.ramp - ft * 4.0, 0.0 );
+			}
+			if (pTarget->error.ramp > 0.0)
+			{
+				ikcontextikrule_t ikrule;
+
+				ikrule.chain = pTarget->chain;
+				ikrule.bone = 0;
+				ikrule.type = IK_RELEASE;
+				ikrule.slot = i;
+				ikrule.flWeight = SimpleSpline( pTarget->error.ramp );
+				ikrule.flRuleWeight = 1.0;
+				ikrule.latched = dt < 0.25 ? 0.0 : ikrule.flWeight;
+
+				// don't bother with AutoIKRelease if the bone isn't going to be calculated
+				// this code is crashing for some unknown reason.
+				if ( pTarget->chain >= 0 && pTarget->chain < m_pStudioHdr->numikchains())
+				{
+					mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( pTarget->chain );
+					if (pchain != NULL)
+					{
+						int bone = pchain->pLink( 2 )->bone;
+						if (bone >= 0 && bone < m_pStudioHdr->numbones())
+						{
+							mstudiobone_t *pBone = m_pStudioHdr->pBone( bone );
+							if (pBone != NULL)
+							{
+								if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))
+								{
+									pTarget->error.bInError = false;
+									continue;
+								}
+								/*
+								char buf[256];
+								sprintf( buf, "dt %.4f ft %.4f weight %.4f latched %.4f\n", dt, ft, ikrule.flWeight, ikrule.latched );
+								OutputDebugString( buf );
+								*/
+
+								int nIndex = m_ikChainRule.Element( ikrule.chain ).AddToTail( );
+  								m_ikChainRule.Element( ikrule.chain ).Element( nIndex ) = ikrule;
+							}
+							else
+							{
+								DevWarning( 1, "AutoIKRelease (%s) got a NULL pBone %d\n", m_pStudioHdr->pszName(), bone );
+							}
+						}
+						else
+						{
+							DevWarning( 1, "AutoIKRelease (%s) got an out of range bone %d (%d)\n", m_pStudioHdr->pszName(), bone, m_pStudioHdr->numbones() );
+						}
+					}
+					else
+					{
+						DevWarning( 1, "AutoIKRelease (%s) got a NULL pchain %d\n", m_pStudioHdr->pszName(), pTarget->chain );
+					}
+				}
+				else
+				{
+					DevWarning( 1, "AutoIKRelease (%s) got an out of range chain %d (%d)\n", m_pStudioHdr->pszName(), pTarget->chain, m_pStudioHdr->numikchains());
+				}
+			}
+			else
+			{
+				pTarget->error.bInError = false;
+			}
+			pTarget->error.flErrorTime = m_flTime;
+		}
+	}
+}
+
+
+
+void CIKContext::SolveDependencies( Vector pos[], Quaternion q[], matrix3x4_t boneToWorld[], CBoneBitList &boneComputed	)
+{
+//	ASSERT_NO_REENTRY();
+	
+	matrix3x4_t worldTarget;
+	int i, j;
+
+	ikchainresult_t chainResult[32]; // allocate!!!
+
+	// init chain rules
+	for (i = 0; i < m_pStudioHdr->numikchains(); i++)
+	{
+		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( i );
+		ikchainresult_t *pChainResult = &chainResult[ i ];
+		int bone = pchain->pLink( 2 )->bone;
+
+		pChainResult->target = -1;
+		pChainResult->flWeight = 0.0;
+
+		// don't bother with chain if the bone isn't going to be calculated
+		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))
+			continue;
+
+		// eval current ik'd bone
+		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );
+
+		MatrixAngles( boneToWorld[bone], pChainResult->q, pChainResult->pos );
+	}
+
+	for (j = 0; j < m_ikChainRule.Count(); j++)
+	{
+		for (i = 0; i < m_ikChainRule.Element( j ).Count(); i++)
+		{
+			ikcontextikrule_t *pRule = &m_ikChainRule.Element( j ).Element( i );
+			ikchainresult_t *pChainResult = &chainResult[ pRule->chain ];
+			pChainResult->target = -1;
+
+
+			switch( pRule->type )
+			{
+			case IK_SELF:
+				{
+					// xform IK target error into world space
+					matrix3x4_t local;
+					QuaternionMatrix( pRule->q, pRule->pos, local );
+					// eval target bone space
+					if (pRule->bone != -1)
+					{
+						BuildBoneChain( pos, q, pRule->bone, boneToWorld, boneComputed );
+						ConcatTransforms( boneToWorld[pRule->bone], local, worldTarget );
+					}
+					else
+					{
+						ConcatTransforms( m_rootxform, local, worldTarget );
+					}
+			
+					float flWeight = pRule->flWeight * pRule->flRuleWeight;
+					pChainResult->flWeight = pChainResult->flWeight * (1 - flWeight) + flWeight;
+
+					Vector p2;
+					Quaternion q2;
+					
+					// target p and q
+					MatrixAngles( worldTarget, q2, p2 );
+
+					// debugLine( pChainResult->pos, p2, 0, 0, 255, true, 0.1 );
+
+					// blend in position and angles
+					pChainResult->pos = pChainResult->pos * (1.0 - flWeight) + p2 * flWeight;
+					QuaternionSlerp( pChainResult->q, q2, flWeight, pChainResult->q );
+				}
+				break;
+			case IK_WORLD:
+				Assert( 0 );
+				break;
+
+			case IK_ATTACHMENT:
+				break;
+
+			case IK_GROUND:
+				break;
+
+			case IK_RELEASE:
+				{
+					// move target back towards original location
+					float flWeight = pRule->flWeight * pRule->flRuleWeight;
+					mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( pRule->chain );
+					int bone = pchain->pLink( 2 )->bone;
+
+					Vector p2;
+					Quaternion q2;
+					
+					BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );
+					MatrixAngles( boneToWorld[bone], q2, p2 );
+
+					// blend in position and angles
+					pChainResult->pos = pChainResult->pos * (1.0 - flWeight) + p2 * flWeight;
+					QuaternionSlerp( pChainResult->q, q2, flWeight, pChainResult->q );
+				}
+				break;
+			case IK_UNLATCH:
+				{
+					/*
+					pChainResult->flWeight = pChainResult->flWeight * (1 - pRule->flWeight) + pRule->flWeight;
+
+					pChainResult->pos = pChainResult->pos * (1.0 - pRule->flWeight ) + pChainResult->local.pos * pRule->flWeight;
+					QuaternionSlerp( pChainResult->q, pChainResult->local.q, pRule->flWeight, pChainResult->q );
+					*/
+				}
+				break;
+			}
+		}
+	}
+
+	for (i = 0; i < m_target.Count(); i++)
+	{
+		CIKTarget *pTarget = &m_target[i];
+
+		if (m_target[i].est.flWeight > 0.0)
+		{
+			matrix3x4_t worldFootpad;
+			matrix3x4_t local;
+			//mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( m_target[i].chain );
+			ikchainresult_t *pChainResult = &chainResult[ pTarget->chain ];
+
+			AngleMatrix(pTarget->offset.q, pTarget->offset.pos, local );
+
+			AngleMatrix( pTarget->est.q, pTarget->est.pos, worldFootpad );
+
+			ConcatTransforms( worldFootpad, local, worldTarget );
+
+			Vector p2;
+			Quaternion q2;
+			// target p and q
+			MatrixAngles( worldTarget, q2, p2 );
+			// MatrixAngles( worldTarget, pChainResult->q, pChainResult->pos );
+
+			// blend in position and angles
+			pChainResult->flWeight = pTarget->est.flWeight;
+			pChainResult->pos = pChainResult->pos * (1.0 - pChainResult->flWeight ) + p2 * pChainResult->flWeight;
+			QuaternionSlerp( pChainResult->q, q2, pChainResult->flWeight, pChainResult->q );
+		}
+
+		if (pTarget->latched.bNeedsLatch)
+		{
+			// keep track of latch position
+			pTarget->latched.bHasLatch = true;
+			pTarget->latched.q = pTarget->est.q;
+			pTarget->latched.pos = pTarget->est.pos;
+		}
+	}
+
+	for (i = 0; i < m_pStudioHdr->numikchains(); i++)
+	{
+		ikchainresult_t *pChainResult = &chainResult[ i ];
+		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( i );
+
+		if (pChainResult->flWeight > 0.0)
+		{
+			Vector tmp;
+			MatrixPosition( boneToWorld[pchain->pLink( 2 )->bone], tmp );
+			// debugLine( pChainResult->pos, tmp, 255, 255, 255, true, 0.1 );
+
+			// do exact IK solution
+			// FIXME: once per link!
+			if (Studio_SolveIK(pchain, pChainResult->pos, boneToWorld ))
+			{
+				Vector p3;
+				MatrixGetColumn( boneToWorld[pchain->pLink( 2 )->bone], 3, p3 );
+				QuaternionMatrix( pChainResult->q, p3, boneToWorld[pchain->pLink( 2 )->bone] );
+
+				// rebuild chain
+				// FIXME: is this needed if everyone past this uses the boneToWorld array?
+				SolveBone( m_pStudioHdr, pchain->pLink( 2 )->bone, boneToWorld, pos, q );
+				SolveBone( m_pStudioHdr, pchain->pLink( 1 )->bone, boneToWorld, pos, q );
+				SolveBone( m_pStudioHdr, pchain->pLink( 0 )->bone, boneToWorld, pos, q );
+			}
+			else
+			{
+				// FIXME: need to invalidate the targets that forced this...
+				if (pChainResult->target != -1)
+				{
+					CIKTarget *pTarget = &m_target[pChainResult->target];
+					VectorScale( pTarget->latched.deltaPos, 0.8, pTarget->latched.deltaPos );
+					QuaternionScale( pTarget->latched.deltaQ, 0.8, pTarget->latched.deltaQ );
+				}
+			}
+		}
+	}
+
+#if 0
+		Vector p1, p2, p3;
+		Quaternion q1, q2, q3;
+
+		// current p and q
+		MatrixAngles( boneToWorld[bone], q1, p1 );
+
+		
+		// target p and q
+		MatrixAngles( worldTarget, q2, p2 );
+
+		// blend in position and angles
+		p3 = p1 * (1.0 - m_ikRule[i].flWeight ) + p2 * m_ikRule[i].flWeight;
+
+		// do exact IK solution
+		// FIXME: once per link!
+		Studio_SolveIK(pchain, p3, boneToWorld );
+
+		// force angle (bad?)
+		QuaternionSlerp( q1, q2, m_ikRule[i].flWeight, q3 );
+		MatrixGetColumn( boneToWorld[bone], 3, p3 );
+		QuaternionMatrix( q3, p3, boneToWorld[bone] );
+
+		// rebuild chain
+		SolveBone( m_pStudioHdr, pchain->pLink( 2 )->bone, boneToWorld, pos, q );
+		SolveBone( m_pStudioHdr, pchain->pLink( 1 )->bone, boneToWorld, pos, q );
+		SolveBone( m_pStudioHdr, pchain->pLink( 0 )->bone, boneToWorld, pos, q );
+#endif
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKContext::SolveAutoplayLocks(
+	Vector pos[], 
+	Quaternion q[]
+	)
+{
+	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList boneComputed;
+	int i;
+
+	for (i = 0; i < m_ikLock.Count(); i++)
+	{
+		const mstudioiklock_t &lock = ((CStudioHdr *)m_pStudioHdr)->pIKAutoplayLock( i );
+		SolveLock( &lock, i, pos, q, boneToWorld, boneComputed );
+	}
+	g_MatrixPool.Free( boneToWorld );
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKContext::SolveSequenceLocks(
+	mstudioseqdesc_t &seqdesc,
+	Vector pos[], 
+	Quaternion q[]
+	)
+{
+	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList boneComputed;
+	int i;
+
+	for (i = 0; i < m_ikLock.Count(); i++)
+	{
+		mstudioiklock_t *plock = seqdesc.pIKLock( i );
+		SolveLock( plock, i, pos, q, boneToWorld, boneComputed );
+	}
+	g_MatrixPool.Free( boneToWorld );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+void CIKContext::AddAllLocks( Vector pos[], Quaternion q[] )
+{
+	// skip all array access if no autoplay locks.
+	if (m_pStudioHdr->GetNumIKChains() == 0)
+	{
+		return;
+	}
+
+	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList boneComputed;
+
+	int ikOffset = m_ikLock.AddMultipleToTail( m_pStudioHdr->GetNumIKChains() );
+	memset( &m_ikLock[ikOffset], 0, sizeof(ikcontextikrule_t)*m_pStudioHdr->GetNumIKChains() );
+
+	for (int i = 0; i < m_pStudioHdr->GetNumIKChains(); i++)
+	{
+		mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( i );
+		int bone = pchain->pLink( 2 )->bone;
+
+		// don't bother with iklock if the bone isn't going to be calculated
+		if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))
+			continue;
+
+		// eval current ik'd bone
+		BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );
+
+		ikcontextikrule_t &ikrule = m_ikLock[ i + ikOffset ];
+
+		ikrule.chain = i;
+		ikrule.slot = i;
+		ikrule.type = IK_WORLD;
+
+		MatrixAngles( boneToWorld[bone], ikrule.q, ikrule.pos );
+
+		// save off current knee direction
+		if (pchain->pLink(0)->kneeDir.LengthSqr() > 0.0)
+		{
+			Vector tmp = pchain->pLink( 0 )->kneeDir;
+			VectorRotate( pchain->pLink( 0 )->kneeDir, boneToWorld[ pchain->pLink( 0 )->bone ], ikrule.kneeDir );
+			MatrixPosition( boneToWorld[ pchain->pLink( 1 )->bone ], ikrule.kneePos ); 
+		}
+		else
+		{
+			ikrule.kneeDir.Init( );
+		}
+	}
+	g_MatrixPool.Free( boneToWorld );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+
+void CIKContext::SolveAllLocks(
+	Vector pos[], 
+	Quaternion q[]
+	)
+{
+	matrix3x4_t *boneToWorld = g_MatrixPool.Alloc();
+	CBoneBitList boneComputed;
+	int i;
+
+	mstudioiklock_t lock;
+
+	for (i = 0; i < m_ikLock.Count(); i++)
+	{
+		lock.chain = i;
+		lock.flPosWeight = 1.0;
+		lock.flLocalQWeight = 0.0;
+		lock.flags = 0;
+
+		SolveLock( &lock, i, pos, q, boneToWorld, boneComputed );
+	}
+	g_MatrixPool.Free( boneToWorld );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: 
+//-----------------------------------------------------------------------------
+
+
+void CIKContext::SolveLock(
+	const mstudioiklock_t *plock,
+	int i,
+	Vector pos[], 
+	Quaternion q[],
+	matrix3x4_t boneToWorld[], 
+	CBoneBitList &boneComputed
+	)
+{
+	mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( plock->chain );
+	int bone = pchain->pLink( 2 )->bone;
+
+	// don't bother with iklock if the bone isn't going to be calculated
+	if ( !(m_pStudioHdr->boneFlags( bone ) & m_boneMask))
+		return;
+
+	// eval current ik'd bone
+	BuildBoneChain( pos, q, bone, boneToWorld, boneComputed );
+
+	Vector p1, p2, p3;
+	Quaternion q2, q3;
+
+	// current p and q
+	MatrixPosition( boneToWorld[bone], p1 );
+
+	// blend in position
+	p3 = p1 * (1.0 - plock->flPosWeight ) + m_ikLock[i].pos * plock->flPosWeight;
+
+	// do exact IK solution
+	if (m_ikLock[i].kneeDir.LengthSqr() > 0)
+	{
+		Studio_SolveIK(pchain->pLink( 0 )->bone, pchain->pLink( 1 )->bone, pchain->pLink( 2 )->bone, p3, m_ikLock[i].kneePos, m_ikLock[i].kneeDir, boneToWorld );
+	}
+	else
+	{
+		Studio_SolveIK(pchain, p3, boneToWorld );
+	}
+
+	// slam orientation
+	MatrixPosition( boneToWorld[bone], p3 );
+	QuaternionMatrix( m_ikLock[i].q, p3, boneToWorld[bone] );
+
+	// rebuild chain
+	q2 = q[ bone ];
+	SolveBone( m_pStudioHdr, pchain->pLink( 2 )->bone, boneToWorld, pos, q );
+	QuaternionSlerp( q[bone], q2, plock->flLocalQWeight, q[bone] );
+
+	SolveBone( m_pStudioHdr, pchain->pLink( 1 )->bone, boneToWorld, pos, q );
+	SolveBone( m_pStudioHdr, pchain->pLink( 0 )->bone, boneToWorld, pos, q );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: run all animations that automatically play and are driven off of poseParameters
+//-----------------------------------------------------------------------------
+void CBoneSetup::CalcAutoplaySequences(
+   Vector pos[], 
+   Quaternion q[], 
+   float flRealTime,
+   CIKContext *pIKContext
+   )
+{
+	//	ASSERT_NO_REENTRY();
+
+	int			i;
+	if ( pIKContext )
+	{
+		pIKContext->AddAutoplayLocks( pos, q );
+	}
+
+	unsigned short *pList = NULL;
+	int count = m_pStudioHdr->GetAutoplayList( &pList );
+	for (i = 0; i < count; i++)
+	{
+		int sequenceIndex = pList[i];
+		mstudioseqdesc_t &seqdesc = ((CStudioHdr *)m_pStudioHdr)->pSeqdesc( sequenceIndex );
+		if (seqdesc.flags & STUDIO_AUTOPLAY)
+		{
+			float cycle = 0;
+			float cps = Studio_CPS( m_pStudioHdr, seqdesc, sequenceIndex, m_flPoseParameter );
+			cycle = flRealTime * cps;
+			cycle = cycle - (int)cycle;
+
+			AccumulatePose( pos, q, sequenceIndex, cycle, 1.0, flRealTime, pIKContext );
+		}
+	}
+
+	if ( pIKContext )
+	{
+		pIKContext->SolveAutoplayLocks( pos, q );
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+void Studio_BuildMatrices(
+	const CStudioHdr *pStudioHdr,
+	const QAngle& angles, 
+	const Vector& origin, 
+	const Vector pos[],
+	const Quaternion q[],
+	int iBone,
+	float flScale,
+	matrix3x4_t bonetoworld[MAXSTUDIOBONES],
+	int boneMask
+	)
+{
+	int i, j;
+
+	int					chain[MAXSTUDIOBONES] = {};
+	int					chainlength = 0;
+
+	if (iBone < -1 || iBone >= pStudioHdr->numbones())
+		iBone = 0;
+
+	// build list of what bones to use
+	if (iBone == -1)
+	{
+		// all bones
+		chainlength = pStudioHdr->numbones();
+		for (i = 0; i < pStudioHdr->numbones(); i++)
+		{
+			chain[chainlength - i - 1] = i;
+		}
+	}
+	else
+	{
+		// only the parent bones
+		i = iBone;
+		while (i != -1)
+		{
+			chain[chainlength++] = i;
+			i = pStudioHdr->boneParent( i );
+		}
+	}
+
+	matrix3x4_t bonematrix;
+	matrix3x4_t rotationmatrix; // model to world transformation
+	AngleMatrix( angles, origin, rotationmatrix );
+
+	// Account for a change in scale
+	if ( flScale < 1.0f-FLT_EPSILON || flScale > 1.0f+FLT_EPSILON )
+	{
+		Vector vecOffset;
+		MatrixGetColumn( rotationmatrix, 3, vecOffset );
+		vecOffset -= origin;
+		vecOffset *= flScale;
+		vecOffset += origin;
+		MatrixSetColumn( vecOffset, 3, rotationmatrix );
+
+		// Scale it uniformly
+		VectorScale( rotationmatrix[0], flScale, rotationmatrix[0] );
+		VectorScale( rotationmatrix[1], flScale, rotationmatrix[1] );
+		VectorScale( rotationmatrix[2], flScale, rotationmatrix[2] );
+	}
+
+	for (j = chainlength - 1; j >= 0; j--)
+	{
+		i = chain[j];
+		if (pStudioHdr->boneFlags(i) & boneMask)
+		{
+			QuaternionMatrix( q[i], pos[i], bonematrix );
+
+			if (pStudioHdr->boneParent(i) == -1) 
+			{
+				ConcatTransforms (rotationmatrix, bonematrix, bonetoworld[i]);
+			} 
+			else 
+			{
+				ConcatTransforms (bonetoworld[pStudioHdr->boneParent(i)], bonematrix, bonetoworld[i]);
+			}
+		}
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: look at single column vector of another bones local transformation 
+//			and generate a procedural transformation based on how that column 
+//			points down the 6 cardinal axis (all negative weights are clamped to 0).
+//-----------------------------------------------------------------------------
+
+void DoAxisInterpBone(
+	mstudiobone_t		*pbones,
+	int	ibone,
+	CBoneAccessor &bonetoworld
+	)
+{
+	matrix3x4_t			bonematrix;
+	Vector				control;
+
+	mstudioaxisinterpbone_t *pProc = (mstudioaxisinterpbone_t *)pbones[ibone].pProcedure( );
+	const matrix3x4_t &controlBone = bonetoworld.GetBone( pProc->control );
+	if (pProc && pbones[pProc->control].parent != -1)
+	{
+		Vector tmp;
+		// pull out the control column
+		tmp.x = controlBone[0][pProc->axis];
+		tmp.y = controlBone[1][pProc->axis];
+		tmp.z = controlBone[2][pProc->axis];
+
+		// invert it back into parent's space.
+		VectorIRotate( tmp, bonetoworld.GetBone( pbones[pProc->control].parent ), control );
+#if 0
+		matrix3x4_t	tmpmatrix;
+		matrix3x4_t	controlmatrix;
+		MatrixInvert( bonetoworld.GetBone( pbones[pProc->control].parent ), tmpmatrix );
+		ConcatTransforms( tmpmatrix, bonetoworld.GetBone( pProc->control ), controlmatrix );
+
+		// pull out the control column
+		control.x = controlmatrix[0][pProc->axis];
+		control.y = controlmatrix[1][pProc->axis];
+		control.z = controlmatrix[2][pProc->axis];
+#endif
+	}
+	else
+	{
+		// pull out the control column
+		control.x = controlBone[0][pProc->axis];
+		control.y = controlBone[1][pProc->axis];
+		control.z = controlBone[2][pProc->axis];
+	}
+
+	Quaternion *q1, *q2, *q3;
+	Vector *p1, *p2, *p3;
+
+	// find axial control inputs
+	float a1 = control.x;
+	float a2 = control.y;
+	float a3 = control.z;
+	if (a1 >= 0) 
+	{ 
+		q1 = &pProc->quat[0];
+		p1 = &pProc->pos[0];
+	} 
+	else 
+	{ 
+		a1 = -a1; 
+		q1 = &pProc->quat[1];
+		p1 = &pProc->pos[1];
+	}
+
+	if (a2 >= 0) 
+	{ 
+		q2 = &pProc->quat[2]; 
+		p2 = &pProc->pos[2];
+	} 
+	else 
+	{ 
+		a2 = -a2; 
+		q2 = &pProc->quat[3]; 
+		p2 = &pProc->pos[3];
+	}
+
+	if (a3 >= 0) 
+	{ 
+		q3 = &pProc->quat[4]; 
+		p3 = &pProc->pos[4];
+	} 
+	else 
+	{ 
+		a3 = -a3; 
+		q3 = &pProc->quat[5]; 
+		p3 = &pProc->pos[5];
+	}
+
+	// do a three-way blend
+	Vector p;
+	Quaternion v, tmp;
+	if (a1 + a2 > 0)
+	{
+		float t = 1.0 / (a1 + a2 + a3);
+		// FIXME: do a proper 3-way Quat blend!
+		QuaternionSlerp( *q2, *q1, a1 / (a1 + a2), tmp );
+		QuaternionSlerp( tmp, *q3, a3 * t, v );
+		VectorScale( *p1, a1 * t, p );
+		VectorMA( p, a2 * t, *p2, p );
+		VectorMA( p, a3 * t, *p3, p );
+	}
+	else
+	{
+		QuaternionSlerp( *q3, *q3, 0, v ); // ??? no quat copy?
+		p = *p3;
+	}
+
+	QuaternionMatrix( v, p, bonematrix );
+
+	ConcatTransforms (bonetoworld.GetBone( pbones[ibone].parent ), bonematrix, bonetoworld.GetBoneForWrite( ibone ));
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Generate a procedural transformation based on how that another bones 
+//			local transformation matches a set of target orientations.
+//-----------------------------------------------------------------------------
+void DoQuatInterpBone(
+	mstudiobone_t		*pbones,
+	int	ibone,
+	CBoneAccessor &bonetoworld
+	)
+{
+	matrix3x4_t			bonematrix;
+	Vector				control;
+
+	mstudioquatinterpbone_t *pProc = (mstudioquatinterpbone_t *)pbones[ibone].pProcedure( );
+	if (pProc && pbones[pProc->control].parent != -1)
+	{
+		Quaternion	src;
+		float		weight[32];
+		float		scale = 0.0;
+		Quaternion	quat;
+		Vector		pos;
+
+		matrix3x4_t	tmpmatrix;
+		matrix3x4_t	controlmatrix;
+		MatrixInvert( bonetoworld.GetBone( pbones[pProc->control].parent), tmpmatrix );
+		ConcatTransforms( tmpmatrix, bonetoworld.GetBone( pProc->control ), controlmatrix );
+
+		MatrixAngles( controlmatrix, src, pos ); // FIXME: make a version without pos
+
+		int i;
+		for (i = 0; i < pProc->numtriggers; i++)
+		{
+			float dot = fabs( QuaternionDotProduct( pProc->pTrigger( i )->trigger, src ) );
+			// FIXME: a fast acos should be acceptable
+			dot = clamp( dot, -1.f, 1.f );
+			weight[i] = 1 - (2 * acos( dot ) * pProc->pTrigger( i )->inv_tolerance );
+			weight[i] = max( 0, weight[i] );
+			scale += weight[i];
+		}
+
+		if (scale <= 0.001)  // EPSILON?
+		{
+			AngleMatrix( pProc->pTrigger( 0 )->quat, pProc->pTrigger( 0 )->pos, bonematrix );
+			ConcatTransforms ( bonetoworld.GetBone( pbones[ibone].parent ), bonematrix, bonetoworld.GetBoneForWrite( ibone ) );
+			return;
+		}
+
+		scale = 1.0 / scale;
+
+		quat.Init( 0, 0, 0, 0);
+		pos.Init( );
+
+		for (i = 0; i < pProc->numtriggers; i++)
+		{
+			if (weight[i])
+			{
+				float s = weight[i] * scale;
+				mstudioquatinterpinfo_t *pTrigger = pProc->pTrigger( i );
+
+				QuaternionAlign( pTrigger->quat, quat, quat );
+
+				quat.x = quat.x + s * pTrigger->quat.x;
+				quat.y = quat.y + s * pTrigger->quat.y;
+				quat.z = quat.z + s * pTrigger->quat.z;
+				quat.w = quat.w + s * pTrigger->quat.w;
+				pos.x = pos.x + s * pTrigger->pos.x;
+				pos.y = pos.y + s * pTrigger->pos.y;
+				pos.z = pos.z + s * pTrigger->pos.z;
+			}
+		}
+		Assert( QuaternionNormalize( quat ) != 0);
+		QuaternionMatrix( quat, pos, bonematrix );
+	}
+
+	ConcatTransforms (bonetoworld.GetBone( pbones[ibone].parent ), bonematrix, bonetoworld.GetBoneForWrite( ibone ));
+}
+
+/*
+ * This is for DoAimAtBone below, was just for testing, not needed in general
+ * but to turn it back on, uncomment this and the section in DoAimAtBone() below
+ *
+
+static ConVar aim_constraint( "aim_constraint", "1", FCVAR_REPLICATED, "Toggle <aimconstraint> Helper Bones" );
+
+*/
+
+//-----------------------------------------------------------------------------
+// Purpose: Generate a procedural transformation so that one bone points at
+//			another point on the model
+//-----------------------------------------------------------------------------
+void DoAimAtBone(
+	mstudiobone_t *pBones,
+	int	iBone,
+	CBoneAccessor &bonetoworld,
+	const CStudioHdr *pStudioHdr
+	)
+{
+	mstudioaimatbone_t *pProc = (mstudioaimatbone_t *)pBones[iBone].pProcedure();
+
+	if ( !pProc )
+	{
+		return;
+	}
+
+	/*
+	 * Uncomment this if the ConVar above is uncommented
+	 *
+
+	if ( !aim_constraint.GetBool() )
+	{
+		// If the aim constraint is turned off then just copy the parent transform
+		// plus the offset value
+
+		matrix3x4_t boneToWorldSpace;
+		MatrixCopy ( bonetoworld.GetBone( pProc->parent ), boneToWorldSpace );
+		Vector boneWorldPosition;
+		VectorTransform( pProc->basepos, boneToWorldSpace, boneWorldPosition );
+		MatrixSetColumn( boneWorldPosition, 3, boneToWorldSpace );
+		MatrixCopy( boneToWorldSpace, bonetoworld.GetBoneForWrite( iBone ) );
+
+		return;
+	}
+
+	*/
+
+	// The world matrix of the bone to change
+	matrix3x4_t boneMatrix;
+
+	// Guaranteed to be unit length
+	const Vector &userAimVector( pProc->aimvector );
+
+	// Guaranteed to be unit length
+	const Vector &userUpVector( pProc->upvector );
+
+	// Get to get position of bone but also for up reference
+	matrix3x4_t parentSpace;
+	MatrixCopy ( bonetoworld.GetBone( pProc->parent ), parentSpace );
+
+	// World space position of the bone to aim
+	Vector aimWorldPosition;
+	VectorTransform( pProc->basepos, parentSpace, aimWorldPosition );
+
+	// The worldspace matrix of the bone to aim at
+	matrix3x4_t aimAtSpace;
+	if ( pStudioHdr )
+	{
+		// This means it's AIMATATTACH
+		const mstudioattachment_t &attachment( ((CStudioHdr *)pStudioHdr)->pAttachment( pProc->aim ) );
+		ConcatTransforms(
+			bonetoworld.GetBone( attachment.localbone ),
+			attachment.local,
+			aimAtSpace );
+	}
+	else
+	{
+		MatrixCopy( bonetoworld.GetBone( pProc->aim ), aimAtSpace );
+	}
+
+	Vector aimAtWorldPosition;
+	MatrixGetColumn( aimAtSpace, 3, aimAtWorldPosition );
+
+	// make sure the redundant parent info is correct
+	Assert( pProc->parent == pBones[iBone].parent );
+	// make sure the redundant position info is correct
+	Assert( pProc->basepos.DistToSqr( pBones[iBone].pos ) < 0.1 );
+
+	// The aim and up data is relative to this bone, not the parent bone
+	matrix3x4_t bonematrix, boneLocalToWorld;
+	AngleMatrix( pBones[iBone].quat, pProc->basepos, bonematrix );
+	ConcatTransforms( bonetoworld.GetBone( pProc->parent ), bonematrix, boneLocalToWorld );
+
+	Vector aimVector;
+	VectorSubtract( aimAtWorldPosition, aimWorldPosition, aimVector );
+	VectorNormalizeFast( aimVector );
+
+	Vector axis;
+	CrossProduct( userAimVector, aimVector, axis );
+	VectorNormalizeFast( axis );
+	Assert( 1.0f - fabs( DotProduct( userAimVector, aimVector ) ) > FLT_EPSILON );
+	float angle( acosf( DotProduct( userAimVector, aimVector ) ) );
+	Quaternion aimRotation;
+	AxisAngleQuaternion( axis, RAD2DEG( angle ), aimRotation );
+
+	if ( ( 1.0f - fabs( DotProduct( userUpVector, userAimVector ) ) ) > FLT_EPSILON )
+	{
+		matrix3x4_t aimRotationMatrix;
+		QuaternionMatrix( aimRotation, aimRotationMatrix );
+
+		Vector tmpV;
+
+		Vector tmp_pUp;
+		VectorRotate( userUpVector, aimRotationMatrix, tmp_pUp );
+		VectorScale( aimVector, DotProduct( aimVector, tmp_pUp ), tmpV );
+		Vector pUp;
+		VectorSubtract( tmp_pUp, tmpV, pUp );
+		VectorNormalizeFast( pUp );
+
+		Vector tmp_pParentUp;
+		VectorRotate( userUpVector, boneLocalToWorld, tmp_pParentUp );
+		VectorScale( aimVector, DotProduct( aimVector, tmp_pParentUp ), tmpV );
+		Vector pParentUp;
+		VectorSubtract( tmp_pParentUp, tmpV, pParentUp );
+		VectorNormalizeFast( pParentUp );
+
+		Quaternion upRotation;
+		//Assert( 1.0f - fabs( DotProduct( pUp, pParentUp ) ) > FLT_EPSILON );
+		if( 1.0f - fabs( DotProduct( pUp, pParentUp ) ) > FLT_EPSILON )
+		{
+			angle = acos( DotProduct( pUp, pParentUp ) );
+			CrossProduct( pUp, pParentUp, axis );			
+		}
+		else
+		{
+			angle = 0;
+			axis = pUp;
+		}
+
+		VectorNormalizeFast( axis );
+		AxisAngleQuaternion( axis, RAD2DEG( angle ), upRotation );
+
+		Quaternion boneRotation;
+		QuaternionMult( upRotation, aimRotation, boneRotation );
+		QuaternionMatrix( boneRotation, aimWorldPosition, boneMatrix );
+	}
+	else
+	{
+		QuaternionMatrix( aimRotation, aimWorldPosition, boneMatrix );
+	}
+
+	MatrixCopy( boneMatrix, bonetoworld.GetBoneForWrite( iBone ) );
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+
+bool CalcProceduralBone(
+	const CStudioHdr *pStudioHdr,
+	int iBone,
+	CBoneAccessor &bonetoworld
+	)
+{
+	mstudiobone_t		*pbones = pStudioHdr->pBone( 0 );
+
+	if ( pStudioHdr->boneFlags(iBone) & BONE_ALWAYS_PROCEDURAL )
+	{
+		switch( pbones[iBone].proctype )
+		{
+		case STUDIO_PROC_AXISINTERP:
+			DoAxisInterpBone( pbones, iBone, bonetoworld );
+			return true;
+
+		case STUDIO_PROC_QUATINTERP:
+			DoQuatInterpBone( pbones, iBone, bonetoworld );
+			return true;
+
+		case STUDIO_PROC_AIMATBONE:
+			DoAimAtBone( pbones, iBone, bonetoworld, NULL );
+			return true;
+
+		case STUDIO_PROC_AIMATATTACH:
+			DoAimAtBone( pbones, iBone, bonetoworld, pStudioHdr );
+			return true;
+
+		default:
+			return false;
+		}
+	}
+	return false;
+}
+
+
+
+//-----------------------------------------------------------------------------
+// Purpose:  Lookup a bone controller
+//-----------------------------------------------------------------------------
+
+
+
+static mstudiobonecontroller_t* FindController( const CStudioHdr *pStudioHdr, int iController)
+{
+	// find first controller that matches the index
+	for (int i = 0; i < pStudioHdr->numbonecontrollers(); i++)
+	{
+		if (pStudioHdr->pBonecontroller( i )->inputfield == iController)
+			return pStudioHdr->pBonecontroller( i );
+	}
+
+	return NULL;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: converts a ranged bone controller value into a 0..1 encoded value
+// Output: 	ctlValue contains 0..1 encoding.
+//			returns clamped ranged value
+//-----------------------------------------------------------------------------
+
+float Studio_SetController( const CStudioHdr *pStudioHdr, int iController, float flValue, float &ctlValue )
+{
+	if (! pStudioHdr)
+		return flValue;
+
+	mstudiobonecontroller_t *pbonecontroller = FindController(pStudioHdr, iController);
+	if(!pbonecontroller)
+	{
+		ctlValue = 0;
+		return flValue;
+	}
+
+	// wrap 0..360 if it's a rotational controller
+	if (pbonecontroller->type & (STUDIO_XR | STUDIO_YR | STUDIO_ZR))
+	{
+		// ugly hack, invert value if end < start
+		if (pbonecontroller->end < pbonecontroller->start)
+			flValue = -flValue;
+
+		// does the controller not wrap?
+		if (pbonecontroller->start + 359.0 >= pbonecontroller->end)
+		{
+			if (flValue > ((pbonecontroller->start + pbonecontroller->end) / 2.0) + 180)
+				flValue = flValue - 360;
+			if (flValue < ((pbonecontroller->start + pbonecontroller->end) / 2.0) - 180)
+				flValue = flValue + 360;
+		}
+		else
+		{
+			if (flValue > 360)
+				flValue = flValue - (int)(flValue / 360.0) * 360.0;
+			else if (flValue < 0)
+				flValue = flValue + (int)((flValue / -360.0) + 1) * 360.0;
+		}
+	}
+
+	ctlValue = (flValue - pbonecontroller->start) / (pbonecontroller->end - pbonecontroller->start);
+	if (ctlValue < 0) ctlValue = 0;
+	if (ctlValue > 1) ctlValue = 1;
+
+	float flReturnVal = ((1.0 - ctlValue)*pbonecontroller->start + ctlValue *pbonecontroller->end);
+
+	// ugly hack, invert value if a rotational controller and end < start
+	if (pbonecontroller->type & (STUDIO_XR | STUDIO_YR | STUDIO_ZR) &&
+		pbonecontroller->end < pbonecontroller->start				)
+	{
+		flReturnVal *= -1;
+	}
+	
+	return flReturnVal;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: converts a 0..1 encoded bone controller value into a ranged value
+// Output: 	returns ranged value
+//-----------------------------------------------------------------------------
+
+float Studio_GetController( const CStudioHdr *pStudioHdr, int iController, float ctlValue )
+{
+	if (!pStudioHdr)
+		return 0.0;
+
+	mstudiobonecontroller_t *pbonecontroller = FindController(pStudioHdr, iController);
+	if(!pbonecontroller)
+		return 0;
+
+	return ctlValue * (pbonecontroller->end - pbonecontroller->start) + pbonecontroller->start;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: Calculates default values for the pose parameters
+// Output: 	fills in an array
+//-----------------------------------------------------------------------------
+
+void Studio_CalcDefaultPoseParameters( const CStudioHdr *pStudioHdr, float flPoseParameter[], int nCount )
+{
+	int nPoseCount = pStudioHdr->GetNumPoseParameters();
+	int nNumParams = MIN( nCount, MAXSTUDIOPOSEPARAM );
+
+	for ( int i = 0; i < nNumParams; ++i )
+	{
+		// Default to middle of the pose parameter range
+		flPoseParameter[ i ] = 0.5f;
+		if ( i < nPoseCount )
+		{
+			const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)pStudioHdr)->pPoseParameter( i );
+
+			// Want to try for a zero state.  If one doesn't exist set it to .5 by default.
+			if ( Pose.start < 0.0f && Pose.end > 0.0f )
+			{
+				float flPoseDelta = Pose.end - Pose.start;
+				flPoseParameter[i] = -Pose.start / flPoseDelta;
+			}
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: converts a ranged pose parameter value into a 0..1 encoded value
+// Output: 	ctlValue contains 0..1 encoding.
+//			returns clamped ranged value
+//-----------------------------------------------------------------------------
+
+float Studio_SetPoseParameter( const CStudioHdr *pStudioHdr, int iParameter, float flValue, float &ctlValue )
+{
+	if (iParameter < 0 || iParameter >= pStudioHdr->GetNumPoseParameters())
+	{
+		return 0;
+	}
+
+	const mstudioposeparamdesc_t &PoseParam = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iParameter );
+
+	Assert( IsFinite( flValue ) );
+
+	if (PoseParam.loop)
+	{
+		float wrap = (PoseParam.start + PoseParam.end) / 2.0 + PoseParam.loop / 2.0;
+		float shift = PoseParam.loop - wrap;
+
+		flValue = flValue - PoseParam.loop * floor((flValue + shift) / PoseParam.loop);
+	}
+
+	ctlValue = (flValue - PoseParam.start) / (PoseParam.end - PoseParam.start);
+
+	if (ctlValue < 0) ctlValue = 0;
+	if (ctlValue > 1) ctlValue = 1;
+
+	Assert( IsFinite( ctlValue ) );
+
+	return ctlValue * (PoseParam.end - PoseParam.start) + PoseParam.start;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: converts a 0..1 encoded pose parameter value into a ranged value
+// Output: 	returns ranged value
+//-----------------------------------------------------------------------------
+
+float Studio_GetPoseParameter( const CStudioHdr *pStudioHdr, int iParameter, float ctlValue )
+{
+	if (iParameter < 0 || iParameter >= pStudioHdr->GetNumPoseParameters())
+	{
+		return 0;
+	}
+
+	const mstudioposeparamdesc_t &PoseParam = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iParameter );
+
+	return ctlValue * (PoseParam.end - PoseParam.start) + PoseParam.start;
+}
+
+
+#pragma warning (disable : 4701)
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+static int ClipRayToHitbox( const Ray_t &ray, mstudiobbox_t *pbox, matrix3x4_t& matrix, trace_t &tr )
+{
+	const float flProjEpsilon = 0.01f;
+	// scale by current t so hits shorten the ray and increase the likelihood of early outs
+	Vector delta2;
+	VectorScale( ray.m_Delta, (0.5f * tr.fraction), delta2 );
+
+	// OPTIMIZE: Store this in the box instead of computing it here
+	// compute center in local space
+	Vector boxextents;
+	boxextents.x = (pbox->bbmin.x + pbox->bbmax.x) * 0.5; 
+	boxextents.y = (pbox->bbmin.y + pbox->bbmax.y) * 0.5; 
+	boxextents.z = (pbox->bbmin.z + pbox->bbmax.z) * 0.5; 
+	Vector boxCenter;
+	// transform to world space
+	VectorTransform( boxextents, matrix, boxCenter );
+	// calc extents from local center
+	boxextents.x = pbox->bbmax.x - boxextents.x;
+	boxextents.y = pbox->bbmax.y - boxextents.y;
+	boxextents.z = pbox->bbmax.z - boxextents.z;
+	// OPTIMIZE: This is optimized for world space.  If the transform is fast enough, it may make more
+	// sense to just xform and call UTIL_ClipToBox() instead.  MEASURE THIS.
+
+	// save the extents of the ray along 
+	Vector extent, uextent;
+	Vector segmentCenter;
+	segmentCenter.x = ray.m_Start.x + delta2.x - boxCenter.x;
+	segmentCenter.y = ray.m_Start.y + delta2.y - boxCenter.y;
+	segmentCenter.z = ray.m_Start.z + delta2.z - boxCenter.z;
+
+	extent.Init();
+
+	// check box axes for separation
+	for ( int j = 0; j < 3; j++ )
+	{
+		extent[j] = delta2.x * matrix[0][j] + delta2.y * matrix[1][j] +	delta2.z * matrix[2][j];
+		uextent[j] = fabsf(extent[j]);
+		float coord = segmentCenter.x * matrix[0][j] + segmentCenter.y * matrix[1][j] +	segmentCenter.z * matrix[2][j];
+		coord = fabsf(coord);
+
+		if ( coord > (boxextents[j] + uextent[j]) )
+			return -1;
+	}
+
+	// now check cross axes for separation
+	float tmp, tmpfix, cextent;
+	Vector cross;
+	CrossProduct( delta2, segmentCenter, cross );
+	cextent = cross.x * matrix[0][0] + cross.y * matrix[1][0] + cross.z * matrix[2][0];
+	cextent = fabsf(cextent);
+	tmp = boxextents[1]*uextent[2] + boxextents[2]*uextent[1];
+	tmpfix = MAX(tmp, flProjEpsilon);
+	if ( cextent > tmpfix )
+		return -1;
+	
+//	if ( cextent > tmp && cextent <= tmpfix )
+//		DevWarning( "ClipRayToHitbox trace precision error case\n" );
+
+	cextent = cross.x * matrix[0][1] + cross.y * matrix[1][1] + cross.z * matrix[2][1];
+	cextent = fabsf(cextent);
+	tmp = boxextents[0]*uextent[2] + boxextents[2]*uextent[0];
+	tmpfix = MAX(tmp, flProjEpsilon);
+	if ( cextent > tmpfix )
+		return -1;
+
+//	if ( cextent > tmp && cextent <= tmpfix )
+//		DevWarning( "ClipRayToHitbox trace precision error case\n" );
+
+	cextent = cross.x * matrix[0][2] + cross.y * matrix[1][2] + cross.z * matrix[2][2];
+	cextent = fabsf(cextent);
+	tmp = boxextents[0]*uextent[1] + boxextents[1]*uextent[0];
+	tmpfix = MAX(tmp, flProjEpsilon);
+	if ( cextent > tmpfix )
+		return -1;
+
+//	if ( cextent > tmp && cextent <= tmpfix )
+//		DevWarning( "ClipRayToHitbox trace precision error case\n" );
+
+	// !!! We hit this box !!! compute intersection point and return
+	Vector start;
+
+	// Compute ray start in bone space
+	VectorITransform( ray.m_Start, matrix, start );
+	// extent is delta2 in bone space, recompute delta in bone space
+	VectorScale( extent, 2, extent );
+
+	// delta was prescaled by the current t, so no need to see if this intersection
+	// is closer
+	trace_t boxTrace;
+	if ( !IntersectRayWithBox( start, extent, pbox->bbmin, pbox->bbmax, 0.0f, &boxTrace ) )
+		return -1;
+
+	Assert( IsFinite(boxTrace.fraction) );
+	tr.fraction *= boxTrace.fraction;
+	tr.startsolid = boxTrace.startsolid;
+	int hitside = boxTrace.plane.type;
+	if ( boxTrace.plane.normal[hitside] >= 0 )
+	{
+		hitside += 3;
+	}
+	return hitside;
+}
+
+#pragma warning (default : 4701)
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool SweepBoxToStudio( IPhysicsSurfaceProps *pProps, const Ray_t& ray, CStudioHdr *pStudioHdr, mstudiohitboxset_t *set, 
+				   matrix3x4_t **hitboxbones, int fContentsMask, trace_t &tr )
+{
+	tr.fraction = 1.0;
+	tr.startsolid = false;
+
+	// OPTIMIZE: Partition these?
+	Ray_t clippedRay = ray;
+	int hitbox = -1;
+	for ( int i = 0; i < set->numhitboxes; i++ )
+	{
+		mstudiobbox_t *pbox = set->pHitbox(i);
+
+		// Filter based on contents mask
+		int fBoneContents = pStudioHdr->pBone( pbox->bone )->contents;
+		if ( ( fBoneContents & fContentsMask ) == 0 )
+			continue;
+		
+		//FIXME: Won't work with scaling!
+		trace_t obbTrace;
+		if ( IntersectRayWithOBB( clippedRay, *hitboxbones[pbox->bone], pbox->bbmin, pbox->bbmax, 0.0f, &obbTrace ) )
+		{
+			tr.startpos = obbTrace.startpos;
+			tr.endpos = obbTrace.endpos;
+			tr.plane = obbTrace.plane;
+			tr.startsolid = obbTrace.startsolid;
+			tr.allsolid = obbTrace.allsolid;
+
+			// This logic here is to shorten the ray each time to get more early outs
+			tr.fraction *= obbTrace.fraction;
+			clippedRay.m_Delta *= obbTrace.fraction;
+			hitbox = i;
+			if (tr.startsolid)
+				break;
+		}
+	}
+
+	if ( hitbox >= 0 )
+	{
+		tr.hitgroup = set->pHitbox(hitbox)->group;
+		tr.hitbox = hitbox;
+		const mstudiobone_t *pBone = pStudioHdr->pBone( set->pHitbox(hitbox)->bone );
+		tr.contents = pBone->contents | CONTENTS_HITBOX;
+		tr.physicsbone = pBone->physicsbone;
+		tr.surface.name = "**studio**";
+		tr.surface.flags = SURF_HITBOX;
+		tr.surface.surfaceProps = pProps->GetSurfaceIndex( pBone->pszSurfaceProp() );
+
+		Assert( tr.physicsbone >= 0 );
+		return true;
+	}
+	return false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+bool TraceToStudio( IPhysicsSurfaceProps *pProps, const Ray_t& ray, CStudioHdr *pStudioHdr, mstudiohitboxset_t *set, 
+				   matrix3x4_t **hitboxbones, int fContentsMask, const Vector &vecOrigin, float flScale, trace_t &tr )
+{
+	if ( !ray.m_IsRay )
+	{
+		return SweepBoxToStudio( pProps, ray, pStudioHdr, set, hitboxbones, fContentsMask, tr );
+	}
+
+	tr.fraction = 1.0;
+	tr.startsolid = false;
+
+	// no hit yet
+	int hitbox = -1;
+	int hitside = -1;
+
+	// OPTIMIZE: Partition these?
+	for ( int i = 0; i < set->numhitboxes; i++ )
+	{
+		mstudiobbox_t *pbox = set->pHitbox(i);
+
+		// Filter based on contents mask
+		int fBoneContents = pStudioHdr->pBone( pbox->bone )->contents;
+		if ( ( fBoneContents & fContentsMask ) == 0 )
+			continue;
+		
+		// columns are axes of the bones in world space, translation is in world space
+		matrix3x4_t& matrix = *hitboxbones[pbox->bone];
+		
+		// Because we're sending in a matrix with scale data, and because the matrix inversion in the hitbox
+		// code does not handle that case, we pre-scale the bones and ray down here and do our collision checks
+		// in unscaled space.  We can then rescale the results afterwards.
+
+		int side = -1;
+		if ( flScale < 1.0f-FLT_EPSILON || flScale > 1.0f+FLT_EPSILON )
+		{
+			matrix3x4_t matScaled;
+			MatrixCopy( matrix, matScaled );
+			
+			float invScale = 1.0f / flScale;
+
+			Vector vecBoneOrigin;
+			MatrixGetColumn( matScaled, 3, vecBoneOrigin );
+			
+			// Pre-scale the origin down
+			Vector vecNewOrigin = vecBoneOrigin - vecOrigin;
+			vecNewOrigin *= invScale;
+			vecNewOrigin += vecOrigin;
+			MatrixSetColumn( vecNewOrigin, 3, matScaled );
+
+			// Scale it uniformly
+			VectorScale( matScaled[0], invScale, matScaled[0] );
+			VectorScale( matScaled[1], invScale, matScaled[1] );
+			VectorScale( matScaled[2], invScale, matScaled[2] );
+			
+			// Pre-scale our ray as well
+			Vector vecRayStart = ray.m_Start - vecOrigin;
+			vecRayStart *= invScale;
+			vecRayStart += vecOrigin;
+			
+			Vector vecRayDelta = ray.m_Delta * invScale;
+
+			Ray_t newRay;
+			newRay.Init( vecRayStart, vecRayStart + vecRayDelta );  
+			
+			side = ClipRayToHitbox( newRay, pbox, matScaled, tr );
+		}
+		else
+		{
+			side = ClipRayToHitbox( ray, pbox, matrix, tr );
+		}
+
+		if ( side >= 0 )
+		{
+			hitbox = i;
+			hitside = side;
+		}
+	}
+
+	if ( hitbox >= 0 )
+	{
+		mstudiobbox_t *pbox = set->pHitbox(hitbox);
+		VectorMA( ray.m_Start, tr.fraction, ray.m_Delta, tr.endpos );
+		tr.hitgroup = set->pHitbox(hitbox)->group;
+		tr.hitbox = hitbox;
+		const mstudiobone_t *pBone = pStudioHdr->pBone( pbox->bone );
+		tr.contents = pBone->contents | CONTENTS_HITBOX;
+		tr.physicsbone = pBone->physicsbone;
+		tr.surface.name = "**studio**";
+		tr.surface.flags = SURF_HITBOX;
+		tr.surface.surfaceProps = pProps->GetSurfaceIndex( pBone->pszSurfaceProp() );
+
+		Assert( tr.physicsbone >= 0 );
+		matrix3x4_t& matrix = *hitboxbones[pbox->bone];
+		if ( hitside >= 3 )
+		{
+			hitside -= 3;
+			tr.plane.normal[0] = matrix[0][hitside];
+			tr.plane.normal[1] = matrix[1][hitside];
+			tr.plane.normal[2] = matrix[2][hitside];
+			//tr.plane.dist = DotProduct( tr.plane.normal, Vector(matrix[0][3], matrix[1][3], matrix[2][3] ) ) + pbox->bbmax[hitside];
+		}
+		else
+		{
+			tr.plane.normal[0] = -matrix[0][hitside];
+			tr.plane.normal[1] = -matrix[1][hitside];
+			tr.plane.normal[2] = -matrix[2][hitside];
+			//tr.plane.dist = DotProduct( tr.plane.normal, Vector(matrix[0][3], matrix[1][3], matrix[2][3] ) ) - pbox->bbmin[hitside];
+		}
+		// simpler plane constant equation
+		tr.plane.dist = DotProduct( tr.endpos, tr.plane.normal );
+		tr.plane.type = 3;
+		return true;
+	}
+	return false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: returns array of animations and weightings for a sequence based on current pose parameters
+//-----------------------------------------------------------------------------
+
+void Studio_SeqAnims( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, const float poseParameter[], mstudioanimdesc_t *panim[4], float *weight )
+{
+#if _DEBUG
+	VPROF_INCREMENT_COUNTER("SEQ_ANIMS",1);
+#endif
+	if (!pStudioHdr || iSequence >= pStudioHdr->GetNumSeq())
+	{
+		weight[0] = weight[1] = weight[2] = weight[3] = 0.0;
+		return;
+	}
+
+	int i0 = 0, i1 = 0;
+	float s0 = 0, s1 = 0;
+	
+	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, iSequence, 0, s0, i0 );
+	Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, iSequence, 1, s1, i1 );
+
+	panim[0] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0  , i1 ) ) );
+	weight[0] = (1 - s0) * (1 - s1);
+
+	panim[1] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0+1, i1 ) ) );
+	weight[1] = (s0) * (1 - s1);
+
+	panim[2] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0  , i1+1 ) ) );
+	weight[2] = (1 - s0) * (s1);
+
+	panim[3] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0+1, i1+1 ) ) );
+	weight[3] = (s0) * (s1);
+
+	Assert( weight[0] >= 0.0f && weight[1] >= 0.0f && weight[2] >= 0.0f && weight[3] >= 0.0f );
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: returns max frame number for a sequence
+//-----------------------------------------------------------------------------
+
+int Studio_MaxFrame( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )
+{
+	mstudioanimdesc_t *panim[4];
+	float	weight[4];
+
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
+	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
+
+	float maxFrame = 0;
+	for (int i = 0; i < 4; i++)
+	{
+		if (weight[i] > 0)
+		{
+			maxFrame += panim[i]->numframes * weight[i];
+		}
+	}
+
+	if ( maxFrame > 1 )
+		maxFrame -= 1;
+	
+
+	// FIXME: why does the weights sometimes not exactly add it 1.0 and this sometimes rounds down?
+	return (maxFrame + 0.01);
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: returns frames per second of a sequence
+//-----------------------------------------------------------------------------
+
+float Studio_FPS( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )
+{
+	mstudioanimdesc_t *panim[4];
+	float	weight[4];
+
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
+	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
+
+	float t = 0;
+
+	for (int i = 0; i < 4; i++)
+	{
+		if (weight[i] > 0)
+		{
+			t += panim[i]->fps * weight[i];
+		}
+	}
+	return t;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: returns cycles per second of a sequence (cycles/second)
+//-----------------------------------------------------------------------------
+
+float Studio_CPS( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, const float poseParameter[] )
+{
+	mstudioanimdesc_t *panim[4];
+	float	weight[4];
+
+	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
+
+	float t = 0;
+
+	for (int i = 0; i < 4; i++)
+	{
+		if (weight[i] > 0 && panim[i]->numframes > 1)
+		{
+			t += (panim[i]->fps / (panim[i]->numframes - 1)) * weight[i];
+		}
+	}
+	return t;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: returns length (in seconds) of a sequence (seconds/cycle)
+//-----------------------------------------------------------------------------
+
+float Studio_Duration( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )
+{
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
+	float cps = Studio_CPS( pStudioHdr, seqdesc, iSequence, poseParameter );
+
+	if( cps == 0 )
+		return 0.0f;
+
+	return 1.0f/cps;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate changes in position and angle relative to the start of an animations cycle
+// Output:	updated position and angle, relative to the origin
+//			returns false if animation is not a movement animation
+//-----------------------------------------------------------------------------
+
+bool Studio_AnimPosition( mstudioanimdesc_t *panim, float flCycle, Vector &vecPos, QAngle &vecAngle )
+{
+	float	prevframe = 0;
+	vecPos.Init( );
+	vecAngle.Init( );
+
+	if (panim->nummovements == 0)
+		return false;
+
+	int iLoops = 0;
+	if (flCycle > 1.0)
+	{
+		iLoops = (int)flCycle;
+	}
+	else if (flCycle < 0.0)
+	{
+		iLoops = (int)flCycle - 1;
+	}
+	flCycle = flCycle - iLoops;
+
+	float	flFrame = flCycle * (panim->numframes - 1);
+
+	for (int i = 0; i < panim->nummovements; i++)
+	{
+		mstudiomovement_t *pmove = panim->pMovement( i );
+
+		if (pmove->endframe >= flFrame)
+		{
+			float f = (flFrame - prevframe) / (pmove->endframe - prevframe);
+
+			float d = pmove->v0 * f + 0.5 * (pmove->v1 - pmove->v0) * f * f;
+
+			vecPos = vecPos + d * pmove->vector;
+			vecAngle.y = vecAngle.y * (1 - f) + pmove->angle * f;
+			if (iLoops != 0)
+			{
+				mstudiomovement_t *pmove = panim->pMovement( panim->nummovements - 1 );
+				vecPos = vecPos + iLoops * pmove->position; 
+				vecAngle.y = vecAngle.y + iLoops * pmove->angle; 
+			}
+			return true;
+		}
+		else
+		{
+			prevframe = pmove->endframe;
+			vecPos = pmove->position;
+			vecAngle.y = pmove->angle;
+		}
+	}
+
+	return false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate instantaneous velocity in ips at a given point 
+//			in the animations cycle
+// Output:	velocity vector, relative to identity orientation
+//			returns false if animation is not a movement animation
+//-----------------------------------------------------------------------------
+
+bool Studio_AnimVelocity( mstudioanimdesc_t *panim, float flCycle, Vector &vecVelocity )
+{
+	float	prevframe = 0;
+
+	float	flFrame = flCycle * (panim->numframes - 1);
+	flFrame = flFrame - (int)(flFrame / (panim->numframes - 1));
+
+	for (int i = 0; i < panim->nummovements; i++)
+	{
+		mstudiomovement_t *pmove = panim->pMovement( i );
+
+		if (pmove->endframe >= flFrame)
+		{
+			float f = (flFrame - prevframe) / (pmove->endframe - prevframe);
+
+			float vel = pmove->v0 * (1 - f) + pmove->v1 * f;
+			// scale from per block to per sec velocity
+			vel = vel * panim->fps / (pmove->endframe - prevframe);
+
+			vecVelocity = pmove->vector * vel;
+			return true;
+		}
+		else
+		{
+			prevframe = pmove->endframe;
+		}
+	}
+	return false;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate changes in position and angle between two points in an animation cycle
+// Output:	updated position and angle, relative to CycleFrom being at the origin
+//			returns false if animation is not a movement animation
+//-----------------------------------------------------------------------------
+
+bool Studio_AnimMovement( mstudioanimdesc_t *panim, float flCycleFrom, float flCycleTo, Vector &deltaPos, QAngle &deltaAngle )
+{
+	if (panim->nummovements == 0)
+		return false;
+
+	Vector startPos;
+	QAngle startA;
+	Studio_AnimPosition( panim, flCycleFrom, startPos, startA );
+
+	Vector endPos;
+	QAngle endA;
+	Studio_AnimPosition( panim, flCycleTo, endPos, endA );
+
+	Vector tmp = endPos - startPos;
+	deltaAngle.y = endA.y - startA.y;
+	VectorYawRotate( tmp, -startA.y, deltaPos );
+
+	return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: finds how much of an animation to play to move given linear distance
+//-----------------------------------------------------------------------------
+
+float Studio_FindAnimDistance( mstudioanimdesc_t *panim, float flDist )
+{
+	float	prevframe = 0;
+
+	if (flDist <= 0)
+		return 0.0;
+
+	for (int i = 0; i < panim->nummovements; i++)
+	{
+		mstudiomovement_t *pmove = panim->pMovement( i );
+
+		float flMove = (pmove->v0 + pmove->v1) * 0.5;
+
+		if (flMove >= flDist)
+		{
+			float root1, root2;
+
+			// d = V0 * t + 1/2 (V1-V0) * t^2
+			if (SolveQuadratic( 0.5 * (pmove->v1 - pmove->v0), pmove->v0, -flDist, root1, root2 ))
+			{
+				float cpf = 1.0 / (panim->numframes - 1);  // cycles per frame
+
+				return (prevframe + root1 * (pmove->endframe - prevframe)) * cpf;
+			}
+			return 0.0;
+		}
+		else
+		{
+			flDist -= flMove;
+			prevframe = pmove->endframe;
+		}
+	}
+	return 1.0;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate changes in position and angle between two points in a sequences cycle
+// Output:	updated position and angle, relative to CycleFrom being at the origin
+//			returns false if sequence is not a movement sequence
+//-----------------------------------------------------------------------------
+
+bool Studio_SeqMovement( const CStudioHdr *pStudioHdr, int iSequence, float flCycleFrom, float flCycleTo, const float poseParameter[], Vector &deltaPos, QAngle &deltaAngles )
+{
+	mstudioanimdesc_t *panim[4];
+	float	weight[4];
+
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
+
+	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
+	
+	deltaPos.Init( );
+	deltaAngles.Init( );
+
+	bool found = false;
+
+	for (int i = 0; i < 4; i++)
+	{
+		if (weight[i])
+		{
+			Vector localPos;
+			QAngle localAngles;
+
+			localPos.Init();
+			localAngles.Init();
+
+			if (Studio_AnimMovement( panim[i], flCycleFrom, flCycleTo, localPos, localAngles ))
+			{
+				found = true;
+				deltaPos = deltaPos + localPos * weight[i];
+				// FIXME: this makes no sense
+				deltaAngles = deltaAngles + localAngles * weight[i];
+			}
+			else if (!(panim[i]->flags & STUDIO_DELTA) && panim[i]->nummovements == 0 && seqdesc.weight(0) > 0.0)
+			{
+				found = true;
+			}
+		}
+	}
+	return found;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: calculate instantaneous velocity in ips at a given point in the sequence's cycle
+// Output:	velocity vector, relative to identity orientation
+//			returns false if sequence is not a movement sequence
+//-----------------------------------------------------------------------------
+
+bool Studio_SeqVelocity( const CStudioHdr *pStudioHdr, int iSequence, float flCycle, const float poseParameter[], Vector &vecVelocity )
+{
+	mstudioanimdesc_t *panim[4];
+	float	weight[4];
+
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
+	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
+	
+	vecVelocity.Init( );
+
+	bool found = false;
+
+	for (int i = 0; i < 4; i++)
+	{
+		if (weight[i])
+		{
+			Vector vecLocalVelocity;
+
+			if (Studio_AnimVelocity( panim[i], flCycle, vecLocalVelocity ))
+			{
+				vecVelocity = vecVelocity + vecLocalVelocity * weight[i];
+				found = true;
+			}
+		}
+	}
+	return found;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: finds how much of an sequence to play to move given linear distance
+//-----------------------------------------------------------------------------
+
+float Studio_FindSeqDistance( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[], float flDist )
+{
+	mstudioanimdesc_t *panim[4];
+	float	weight[4];
+
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
+	Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
+	
+	float flCycle = 0;
+
+	for (int i = 0; i < 4; i++)
+	{
+		if (weight[i])
+		{
+			float flLocalCycle = Studio_FindAnimDistance( panim[i], flDist );
+			flCycle = flCycle + flLocalCycle * weight[i];
+		}
+	}
+	return flCycle;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: lookup attachment by name
+//-----------------------------------------------------------------------------
+
+int Studio_FindAttachment( const CStudioHdr *pStudioHdr, const char *pAttachmentName )
+{
+	if ( pStudioHdr && pStudioHdr->SequencesAvailable() )
+	{
+		// Extract the bone index from the name
+		for (int i = 0; i < pStudioHdr->GetNumAttachments(); i++)
+		{
+			if (!V_stricmp(pAttachmentName,((CStudioHdr *)pStudioHdr)->pAttachment(i).pszName( ))) 
+			{
+				return i;
+			}
+		}
+	}
+
+	return -1;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: lookup attachments by substring. Randomly return one of the matching attachments.
+//-----------------------------------------------------------------------------
+
+int Studio_FindRandomAttachment( const CStudioHdr *pStudioHdr, const char *pAttachmentName )
+{
+	if ( pStudioHdr )
+	{
+		// First move them all matching attachments into a list
+		CUtlVector<int> matchingAttachments;
+
+		// Extract the bone index from the name
+		for (int i = 0; i < pStudioHdr->GetNumAttachments(); i++)
+		{
+			if ( strstr( ((CStudioHdr *)pStudioHdr)->pAttachment(i).pszName(), pAttachmentName ) ) 
+			{
+				matchingAttachments.AddToTail(i);
+			}
+		}
+
+		// Then randomly return one of the attachments
+		if ( matchingAttachments.Size() > 0 )
+			return matchingAttachments[ RandomInt( 0, matchingAttachments.Size()-1 ) ];
+	}
+
+	return -1;
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: lookup bone by name
+//-----------------------------------------------------------------------------
+
+int Studio_BoneIndexByName( const CStudioHdr *pStudioHdr, const char *pName )
+{
+	// binary search for the bone matching pName
+	int start = 0, end = pStudioHdr->numbones()-1;
+	const byte *pBoneTable = pStudioHdr->GetBoneTableSortedByName();
+	mstudiobone_t *pbones = pStudioHdr->pBone( 0 );
+	while (start <= end)
+	{
+		int mid = (start + end) >> 1;
+		int cmp = Q_stricmp( pbones[pBoneTable[mid]].pszName(), pName );
+		
+		if ( cmp < 0 )
+		{
+			start = mid + 1;
+		}
+		else if ( cmp > 0 )
+		{
+			end = mid - 1;
+		}
+		else
+		{
+			return pBoneTable[mid];
+		}
+	}
+	return -1;
+}
+
+const char *Studio_GetDefaultSurfaceProps( CStudioHdr *pstudiohdr )
+{
+	return pstudiohdr->pszSurfaceProp();
+}
+
+float Studio_GetMass( CStudioHdr *pstudiohdr )
+{
+	return pstudiohdr->mass();
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: return pointer to sequence key value buffer
+//-----------------------------------------------------------------------------
+
+const char *Studio_GetKeyValueText( const CStudioHdr *pStudioHdr, int iSequence )
+{
+	if (pStudioHdr && pStudioHdr->SequencesAvailable())
+	{
+		if (iSequence >= 0 && iSequence < pStudioHdr->GetNumSeq())
+		{
+			return ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence ).KeyValueText();
+		}
+	}
+	return NULL;
+}
+
+bool Studio_PrefetchSequence( const CStudioHdr *pStudioHdr, int iSequence )
+{
+	bool pendingload = false;
+	mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
+	int size0 = seqdesc.groupsize[ 0 ];
+	int size1 = seqdesc.groupsize[ 1 ];
+	for ( int i = 0; i < size0; ++i )
+	{
+		for ( int j = 0; j < size1; ++j )
+		{
+			mstudioanimdesc_t &animdesc = ((CStudioHdr *)pStudioHdr)->pAnimdesc( seqdesc.anim( i, j ) );
+			int iFrame = 0;
+			mstudioanim_t *panim = animdesc.pAnim( &iFrame );
+			if ( !panim )
+			{
+				pendingload = true;
+			}
+		}
+	}
+
+	// Everything for this sequence is resident?
+	return !pendingload;
+}