From a0c29e7dd67abb15c74c85f07741784877edfdcd Mon Sep 17 00:00:00 2001 From: Joe Ludwig Date: Mon, 2 Sep 2013 11:39:10 -0700 Subject: General: * Fixed a variety of server browser issues with mods based on this SDK * Fixed many warnings on various platforms * Added source code for fgdlib and raytrace * Updated many source files with the latest shared source from TF2. OSX: * Added support for Xcode 4.6 * Switched OSX builds to use Xcode instead of makefiles * Moved libs from src/lib/osx32 to src/lib/public/osx32 or src/lib/common/osx32 to match windows better. Linux: * Moved libs from src/lib/linux32 to src/lib/public/linux32 or src/lib/common/linux32 to match windows better. --- mp/src/public/bone_setup.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'mp/src/public/bone_setup.cpp') diff --git a/mp/src/public/bone_setup.cpp b/mp/src/public/bone_setup.cpp index b8033c49..31fb4965 100644 --- a/mp/src/public/bone_setup.cpp +++ b/mp/src/public/bone_setup.cpp @@ -57,7 +57,9 @@ public: { p = new T[MAXSTUDIOBONES]; if ( ((size_t)p) % TSLIST_NODE_ALIGNMENT != 0 ) + { DebuggerBreak(); + } } return p; -- cgit v1.2.3 From f56bb35301836e56582a575a75864392a0177875 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B8rgen=20P=2E=20Tjern=C3=B8?= Date: Mon, 2 Dec 2013 19:31:46 -0800 Subject: Fix line endings. WHAMMY. --- mp/src/public/bone_setup.cpp | 11900 ++++++++++++++++++++--------------------- 1 file changed, 5950 insertions(+), 5950 deletions(-) (limited to 'mp/src/public/bone_setup.cpp') diff --git a/mp/src/public/bone_setup.cpp b/mp/src/public/bone_setup.cpp index 31fb4965..0cc81c67 100644 --- a/mp/src/public/bone_setup.cpp +++ b/mp/src/public/bone_setup.cpp @@ -1,5950 +1,5950 @@ -//========= Copyright Valve Corporation, All rights reserved. ============// -// -// Purpose: -// -// $NoKeywords: $ -// -//===========================================================================// - -#include "tier0/dbg.h" -#include "mathlib/mathlib.h" -#include "bone_setup.h" -#include - -#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; -} +//========= 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