From 39ed87570bdb2f86969d4be821c94b722dc71179 Mon Sep 17 00:00:00 2001 From: Joe Ludwig Date: Wed, 26 Jun 2013 15:22:04 -0700 Subject: First version of the SOurce SDK 2013 --- mp/src/public/bone_setup.cpp | 5948 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 5948 insertions(+) create mode 100644 mp/src/public/bone_setup.cpp (limited to 'mp/src/public/bone_setup.cpp') diff --git a/mp/src/public/bone_setup.cpp b/mp/src/public/bone_setup.cpp new file mode 100644 index 00000000..b8033c49 --- /dev/null +++ b/mp/src/public/bone_setup.cpp @@ -0,0 +1,5948 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//===========================================================================// + +#include "tier0/dbg.h" +#include "mathlib/mathlib.h" +#include "bone_setup.h" +#include + +#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 +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 g_QaternionPool; +CBoneSetupMemoryPool g_VectorPool; +CBoneSetupMemoryPool g_MatrixPool; + +// ----------------------------------------------------------------- +CBoneCache *CBoneCache::CreateResource( const bonecacheparams_t ¶ms ) +{ + 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 ¶ms ) +{ + // 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 ¶ms, 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 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 ¶ms ) +{ + 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 ¤t, 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 ¤t ) +{ + 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 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 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; +} -- cgit v1.2.3