mirror of
https://github.com/alliedmodders/hl2sdk.git
synced 2024-12-22 09:38:56 +08:00
6054 lines
173 KiB
C++
6054 lines
173 KiB
C++
//========= Copyright Valve Corporation, All rights reserved. ============//
|
||
//
|
||
// Purpose:
|
||
//
|
||
// $NoKeywords: $
|
||
//
|
||
//===========================================================================//
|
||
|
||
#include "tier0/dbg.h"
|
||
#include "mathlib/mathlib.h"
|
||
#include "bone_setup.h"
|
||
#include <string.h>
|
||
|
||
#include "collisionutils.h"
|
||
#include "vstdlib/random.h"
|
||
#include "tier0/vprof.h"
|
||
#include "bone_accessor.h"
|
||
#include "mathlib/ssequaternion.h"
|
||
#include "bitvec.h"
|
||
#include "datamanager.h"
|
||
#include "convar.h"
|
||
#include "tier0/tslist.h"
|
||
#include "vphysics_interface.h"
|
||
#ifdef CLIENT_DLL
|
||
#include "posedebugger.h"
|
||
#endif
|
||
|
||
// memdbgon must be the last include file in a .cpp file!!!
|
||
#include "tier0/memdbgon.h"
|
||
|
||
class CBoneSetup
|
||
{
|
||
public:
|
||
CBoneSetup( const CStudioHdr *pStudioHdr, int boneMask, const float poseParameter[], IPoseDebugger *pPoseDebugger = NULL );
|
||
void InitPose( Vector pos[], Quaternion q[] );
|
||
void AccumulatePose( Vector pos[], Quaternion q[], int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );
|
||
void CalcAutoplaySequences( Vector pos[], Quaternion q[], float flRealTime, CIKContext *pIKContext );
|
||
private:
|
||
void AddSequenceLayers( Vector pos[], Quaternion q[], mstudioseqdesc_t &seqdesc, int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );
|
||
void AddLocalLayers( Vector pos[], Quaternion q[], mstudioseqdesc_t &seqdesc, int sequence, float cycle, float flWeight, float flTime, CIKContext *pIKContext );
|
||
public:
|
||
const CStudioHdr *m_pStudioHdr;
|
||
int m_boneMask;
|
||
const float *m_flPoseParameter;
|
||
IPoseDebugger *m_pPoseDebugger;
|
||
};
|
||
|
||
// -----------------------------------------------------------------
|
||
template <typename T>
|
||
class CBoneSetupMemoryPool
|
||
{
|
||
public:
|
||
T *Alloc()
|
||
{
|
||
T *p = (T *)m_FreeBlocks.Pop();
|
||
if ( !p )
|
||
{
|
||
p = new T[MAXSTUDIOBONES];
|
||
if ( ((size_t)p) % TSLIST_NODE_ALIGNMENT != 0 )
|
||
{
|
||
DebuggerBreak();
|
||
}
|
||
}
|
||
|
||
return p;
|
||
}
|
||
|
||
void Free( T *p )
|
||
{
|
||
m_FreeBlocks.Push( (TSLNodeBase_t *)p );
|
||
}
|
||
|
||
private:
|
||
CTSListBase m_FreeBlocks;
|
||
};
|
||
|
||
CBoneSetupMemoryPool<Quaternion> g_QaternionPool;
|
||
CBoneSetupMemoryPool<Vector> g_VectorPool;
|
||
CBoneSetupMemoryPool<matrix3x4_t> g_MatrixPool;
|
||
|
||
// -----------------------------------------------------------------
|
||
CBoneCache *CBoneCache::CreateResource( const bonecacheparams_t ¶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<CBoneCache, bonecacheparams_t, CBoneCache *, CThreadFastMutex> g_StudioBoneCache( 128 * 1024L );
|
||
|
||
CBoneCache *Studio_GetBoneCache( memhandle_t cacheHandle )
|
||
{
|
||
AUTO_LOCK( g_StudioBoneCache.AccessMutex() );
|
||
return g_StudioBoneCache.GetResource_NoLock( cacheHandle );
|
||
}
|
||
|
||
memhandle_t Studio_CreateBoneCache( bonecacheparams_t ¶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
|
||
)
|
||
{
|
||
#ifdef STAGING_ONLY
|
||
Assert( iNewParent == -1 || (iNewParent >= 0 && iNewParent < MAXSTUDIOBONES) );
|
||
Assert( iBone > 0 );
|
||
Assert( iBone < MAXSTUDIOBONES );
|
||
#endif // STAGING_ONLY
|
||
|
||
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 );
|
||
|
||
matrix3x4_t localXform;
|
||
AngleMatrix( localQ, localPos, localXform );
|
||
|
||
if ( iNewParent != -1 )
|
||
{
|
||
BuildBoneChain( pStudioHdr, rootXform, pos, q, iNewParent, boneToWorld, boneComputed );
|
||
ConcatTransforms( boneToWorld[iNewParent], localXform, boneToWorld[iBone] );
|
||
}
|
||
else
|
||
{
|
||
boneToWorld[iBone] = localXform;
|
||
}
|
||
|
||
// 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))
|
||
{
|
||
if ( pHierarchy->iNewParent != -1 )
|
||
{
|
||
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 );
|
||
}
|
||
}
|
||
else
|
||
{
|
||
CalcLocalHierarchyAnimation( pStudioHdr, boneToWorld, boneComputed, pos, q, pbone, pHierarchy, iBone, -1, 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 <aimconstraint> Helper Bones" );
|
||
|
||
*/
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: Generate a procedural transformation so that one bone points at
|
||
// another point on the model
|
||
//-----------------------------------------------------------------------------
|
||
void DoAimAtBone(
|
||
mstudiobone_t *pBones,
|
||
int iBone,
|
||
CBoneAccessor &bonetoworld,
|
||
const CStudioHdr *pStudioHdr
|
||
)
|
||
{
|
||
mstudioaimatbone_t *pProc = (mstudioaimatbone_t *)pBones[iBone].pProcedure();
|
||
|
||
if ( !pProc )
|
||
{
|
||
return;
|
||
}
|
||
|
||
/*
|
||
* Uncomment this if the ConVar above is uncommented
|
||
*
|
||
|
||
if ( !aim_constraint.GetBool() )
|
||
{
|
||
// If the aim constraint is turned off then just copy the parent transform
|
||
// plus the offset value
|
||
|
||
matrix3x4_t boneToWorldSpace;
|
||
MatrixCopy ( bonetoworld.GetBone( pProc->parent ), boneToWorldSpace );
|
||
Vector boneWorldPosition;
|
||
VectorTransform( pProc->basepos, boneToWorldSpace, boneWorldPosition );
|
||
MatrixSetColumn( boneWorldPosition, 3, boneToWorldSpace );
|
||
MatrixCopy( boneToWorldSpace, bonetoworld.GetBoneForWrite( iBone ) );
|
||
|
||
return;
|
||
}
|
||
|
||
*/
|
||
|
||
// The world matrix of the bone to change
|
||
matrix3x4_t boneMatrix;
|
||
|
||
// Guaranteed to be unit length
|
||
const Vector &userAimVector( pProc->aimvector );
|
||
|
||
// Guaranteed to be unit length
|
||
const Vector &userUpVector( pProc->upvector );
|
||
|
||
// Get to get position of bone but also for up reference
|
||
matrix3x4_t parentSpace;
|
||
MatrixCopy ( bonetoworld.GetBone( pProc->parent ), parentSpace );
|
||
|
||
// World space position of the bone to aim
|
||
Vector aimWorldPosition;
|
||
VectorTransform( pProc->basepos, parentSpace, aimWorldPosition );
|
||
|
||
// The worldspace matrix of the bone to aim at
|
||
matrix3x4_t aimAtSpace;
|
||
if ( pStudioHdr )
|
||
{
|
||
// This means it's AIMATATTACH
|
||
const mstudioattachment_t &attachment( ((CStudioHdr *)pStudioHdr)->pAttachment( pProc->aim ) );
|
||
ConcatTransforms(
|
||
bonetoworld.GetBone( attachment.localbone ),
|
||
attachment.local,
|
||
aimAtSpace );
|
||
}
|
||
else
|
||
{
|
||
MatrixCopy( bonetoworld.GetBone( pProc->aim ), aimAtSpace );
|
||
}
|
||
|
||
Vector aimAtWorldPosition;
|
||
MatrixGetColumn( aimAtSpace, 3, aimAtWorldPosition );
|
||
|
||
// make sure the redundant parent info is correct
|
||
Assert( pProc->parent == pBones[iBone].parent );
|
||
// make sure the redundant position info is correct
|
||
Assert( pProc->basepos.DistToSqr( pBones[iBone].pos ) < 0.1 );
|
||
|
||
// The aim and up data is relative to this bone, not the parent bone
|
||
matrix3x4_t bonematrix, boneLocalToWorld;
|
||
AngleMatrix( pBones[iBone].quat, pProc->basepos, bonematrix );
|
||
ConcatTransforms( bonetoworld.GetBone( pProc->parent ), bonematrix, boneLocalToWorld );
|
||
|
||
Vector aimVector;
|
||
VectorSubtract( aimAtWorldPosition, aimWorldPosition, aimVector );
|
||
VectorNormalizeFast( aimVector );
|
||
|
||
Vector axis;
|
||
CrossProduct( userAimVector, aimVector, axis );
|
||
VectorNormalizeFast( axis );
|
||
Assert( 1.0f - fabs( DotProduct( userAimVector, aimVector ) ) > FLT_EPSILON );
|
||
float angle( acosf( DotProduct( userAimVector, aimVector ) ) );
|
||
Quaternion aimRotation;
|
||
AxisAngleQuaternion( axis, RAD2DEG( angle ), aimRotation );
|
||
|
||
if ( ( 1.0f - fabs( DotProduct( userUpVector, userAimVector ) ) ) > FLT_EPSILON )
|
||
{
|
||
matrix3x4_t aimRotationMatrix;
|
||
QuaternionMatrix( aimRotation, aimRotationMatrix );
|
||
|
||
Vector tmpV;
|
||
|
||
Vector tmp_pUp;
|
||
VectorRotate( userUpVector, aimRotationMatrix, tmp_pUp );
|
||
VectorScale( aimVector, DotProduct( aimVector, tmp_pUp ), tmpV );
|
||
Vector pUp;
|
||
VectorSubtract( tmp_pUp, tmpV, pUp );
|
||
VectorNormalizeFast( pUp );
|
||
|
||
Vector tmp_pParentUp;
|
||
VectorRotate( userUpVector, boneLocalToWorld, tmp_pParentUp );
|
||
VectorScale( aimVector, DotProduct( aimVector, tmp_pParentUp ), tmpV );
|
||
Vector pParentUp;
|
||
VectorSubtract( tmp_pParentUp, tmpV, pParentUp );
|
||
VectorNormalizeFast( pParentUp );
|
||
|
||
Quaternion upRotation;
|
||
//Assert( 1.0f - fabs( DotProduct( pUp, pParentUp ) ) > FLT_EPSILON );
|
||
if( 1.0f - fabs( DotProduct( pUp, pParentUp ) ) > FLT_EPSILON )
|
||
{
|
||
angle = acos( DotProduct( pUp, pParentUp ) );
|
||
CrossProduct( pUp, pParentUp, axis );
|
||
}
|
||
else
|
||
{
|
||
angle = 0;
|
||
axis = pUp;
|
||
}
|
||
|
||
VectorNormalizeFast( axis );
|
||
AxisAngleQuaternion( axis, RAD2DEG( angle ), upRotation );
|
||
|
||
Quaternion boneRotation;
|
||
QuaternionMult( upRotation, aimRotation, boneRotation );
|
||
QuaternionMatrix( boneRotation, aimWorldPosition, boneMatrix );
|
||
}
|
||
else
|
||
{
|
||
QuaternionMatrix( aimRotation, aimWorldPosition, boneMatrix );
|
||
}
|
||
|
||
MatrixCopy( boneMatrix, bonetoworld.GetBoneForWrite( iBone ) );
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose:
|
||
//-----------------------------------------------------------------------------
|
||
|
||
bool CalcProceduralBone(
|
||
const CStudioHdr *pStudioHdr,
|
||
int iBone,
|
||
CBoneAccessor &bonetoworld
|
||
)
|
||
{
|
||
mstudiobone_t *pbones = pStudioHdr->pBone( 0 );
|
||
|
||
if ( pStudioHdr->boneFlags(iBone) & BONE_ALWAYS_PROCEDURAL )
|
||
{
|
||
switch( pbones[iBone].proctype )
|
||
{
|
||
case STUDIO_PROC_AXISINTERP:
|
||
DoAxisInterpBone( pbones, iBone, bonetoworld );
|
||
return true;
|
||
|
||
case STUDIO_PROC_QUATINTERP:
|
||
DoQuatInterpBone( pbones, iBone, bonetoworld );
|
||
return true;
|
||
|
||
case STUDIO_PROC_AIMATBONE:
|
||
DoAimAtBone( pbones, iBone, bonetoworld, NULL );
|
||
return true;
|
||
|
||
case STUDIO_PROC_AIMATATTACH:
|
||
DoAimAtBone( pbones, iBone, bonetoworld, pStudioHdr );
|
||
return true;
|
||
|
||
default:
|
||
return false;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: Lookup a bone controller
|
||
//-----------------------------------------------------------------------------
|
||
|
||
|
||
|
||
static mstudiobonecontroller_t* FindController( const CStudioHdr *pStudioHdr, int iController)
|
||
{
|
||
// find first controller that matches the index
|
||
for (int i = 0; i < pStudioHdr->numbonecontrollers(); i++)
|
||
{
|
||
if (pStudioHdr->pBonecontroller( i )->inputfield == iController)
|
||
return pStudioHdr->pBonecontroller( i );
|
||
}
|
||
|
||
return NULL;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: converts a ranged bone controller value into a 0..1 encoded value
|
||
// Output: ctlValue contains 0..1 encoding.
|
||
// returns clamped ranged value
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_SetController( const CStudioHdr *pStudioHdr, int iController, float flValue, float &ctlValue )
|
||
{
|
||
if (! pStudioHdr)
|
||
return flValue;
|
||
|
||
mstudiobonecontroller_t *pbonecontroller = FindController(pStudioHdr, iController);
|
||
if(!pbonecontroller)
|
||
{
|
||
ctlValue = 0;
|
||
return flValue;
|
||
}
|
||
|
||
// wrap 0..360 if it's a rotational controller
|
||
if (pbonecontroller->type & (STUDIO_XR | STUDIO_YR | STUDIO_ZR))
|
||
{
|
||
// ugly hack, invert value if end < start
|
||
if (pbonecontroller->end < pbonecontroller->start)
|
||
flValue = -flValue;
|
||
|
||
// does the controller not wrap?
|
||
if (pbonecontroller->start + 359.0 >= pbonecontroller->end)
|
||
{
|
||
if (flValue > ((pbonecontroller->start + pbonecontroller->end) / 2.0) + 180)
|
||
flValue = flValue - 360;
|
||
if (flValue < ((pbonecontroller->start + pbonecontroller->end) / 2.0) - 180)
|
||
flValue = flValue + 360;
|
||
}
|
||
else
|
||
{
|
||
if (flValue > 360)
|
||
flValue = flValue - (int)(flValue / 360.0) * 360.0;
|
||
else if (flValue < 0)
|
||
flValue = flValue + (int)((flValue / -360.0) + 1) * 360.0;
|
||
}
|
||
}
|
||
|
||
ctlValue = (flValue - pbonecontroller->start) / (pbonecontroller->end - pbonecontroller->start);
|
||
if (ctlValue < 0) ctlValue = 0;
|
||
if (ctlValue > 1) ctlValue = 1;
|
||
|
||
float flReturnVal = ((1.0 - ctlValue)*pbonecontroller->start + ctlValue *pbonecontroller->end);
|
||
|
||
// ugly hack, invert value if a rotational controller and end < start
|
||
if (pbonecontroller->type & (STUDIO_XR | STUDIO_YR | STUDIO_ZR) &&
|
||
pbonecontroller->end < pbonecontroller->start )
|
||
{
|
||
flReturnVal *= -1;
|
||
}
|
||
|
||
return flReturnVal;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: converts a 0..1 encoded bone controller value into a ranged value
|
||
// Output: returns ranged value
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_GetController( const CStudioHdr *pStudioHdr, int iController, float ctlValue )
|
||
{
|
||
if (!pStudioHdr)
|
||
return 0.0;
|
||
|
||
mstudiobonecontroller_t *pbonecontroller = FindController(pStudioHdr, iController);
|
||
if(!pbonecontroller)
|
||
return 0;
|
||
|
||
return ctlValue * (pbonecontroller->end - pbonecontroller->start) + pbonecontroller->start;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: Calculates default values for the pose parameters
|
||
// Output: fills in an array
|
||
//-----------------------------------------------------------------------------
|
||
|
||
void Studio_CalcDefaultPoseParameters( const CStudioHdr *pStudioHdr, float flPoseParameter[], int nCount )
|
||
{
|
||
int nPoseCount = pStudioHdr->GetNumPoseParameters();
|
||
int nNumParams = MIN( nCount, MAXSTUDIOPOSEPARAM );
|
||
|
||
for ( int i = 0; i < nNumParams; ++i )
|
||
{
|
||
// Default to middle of the pose parameter range
|
||
flPoseParameter[ i ] = 0.5f;
|
||
if ( i < nPoseCount )
|
||
{
|
||
const mstudioposeparamdesc_t &Pose = ((CStudioHdr *)pStudioHdr)->pPoseParameter( i );
|
||
|
||
// Want to try for a zero state. If one doesn't exist set it to .5 by default.
|
||
if ( Pose.start < 0.0f && Pose.end > 0.0f )
|
||
{
|
||
float flPoseDelta = Pose.end - Pose.start;
|
||
flPoseParameter[i] = -Pose.start / flPoseDelta;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: converts a ranged pose parameter value into a 0..1 encoded value
|
||
// Output: ctlValue contains 0..1 encoding.
|
||
// returns clamped ranged value
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_SetPoseParameter( const CStudioHdr *pStudioHdr, int iParameter, float flValue, float &ctlValue )
|
||
{
|
||
if (iParameter < 0 || iParameter >= pStudioHdr->GetNumPoseParameters())
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
const mstudioposeparamdesc_t &PoseParam = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iParameter );
|
||
|
||
Assert( IsFinite( flValue ) );
|
||
|
||
if (PoseParam.loop)
|
||
{
|
||
float wrap = (PoseParam.start + PoseParam.end) / 2.0 + PoseParam.loop / 2.0;
|
||
float shift = PoseParam.loop - wrap;
|
||
|
||
flValue = flValue - PoseParam.loop * floor((flValue + shift) / PoseParam.loop);
|
||
}
|
||
|
||
ctlValue = (flValue - PoseParam.start) / (PoseParam.end - PoseParam.start);
|
||
|
||
if (ctlValue < 0) ctlValue = 0;
|
||
if (ctlValue > 1) ctlValue = 1;
|
||
|
||
Assert( IsFinite( ctlValue ) );
|
||
|
||
return ctlValue * (PoseParam.end - PoseParam.start) + PoseParam.start;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: converts a 0..1 encoded pose parameter value into a ranged value
|
||
// Output: returns ranged value
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_GetPoseParameter( const CStudioHdr *pStudioHdr, int iParameter, float ctlValue )
|
||
{
|
||
if (iParameter < 0 || iParameter >= pStudioHdr->GetNumPoseParameters())
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
const mstudioposeparamdesc_t &PoseParam = ((CStudioHdr *)pStudioHdr)->pPoseParameter( iParameter );
|
||
|
||
return ctlValue * (PoseParam.end - PoseParam.start) + PoseParam.start;
|
||
}
|
||
|
||
#ifdef _WIN32
|
||
#pragma warning (disable : 4701)
|
||
#endif
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// 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;
|
||
}
|
||
|
||
#ifdef _WIN32
|
||
#pragma warning (default : 4701)
|
||
#endif
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose:
|
||
//-----------------------------------------------------------------------------
|
||
bool SweepBoxToStudio( IPhysicsSurfaceProps *pProps, const Ray_t& ray, CStudioHdr *pStudioHdr, mstudiohitboxset_t *set,
|
||
matrix3x4_t **hitboxbones, int fContentsMask, trace_t &tr )
|
||
{
|
||
tr.fraction = 1.0;
|
||
tr.startsolid = false;
|
||
|
||
// OPTIMIZE: Partition these?
|
||
Ray_t clippedRay = ray;
|
||
int hitbox = -1;
|
||
for ( int i = 0; i < set->numhitboxes; i++ )
|
||
{
|
||
mstudiobbox_t *pbox = set->pHitbox(i);
|
||
|
||
// Filter based on contents mask
|
||
int fBoneContents = pStudioHdr->pBone( pbox->bone )->contents;
|
||
if ( ( fBoneContents & fContentsMask ) == 0 )
|
||
continue;
|
||
|
||
//FIXME: Won't work with scaling!
|
||
trace_t obbTrace;
|
||
if ( IntersectRayWithOBB( clippedRay, *hitboxbones[pbox->bone], pbox->bbmin, pbox->bbmax, 0.0f, &obbTrace ) )
|
||
{
|
||
tr.startpos = obbTrace.startpos;
|
||
tr.endpos = obbTrace.endpos;
|
||
tr.plane = obbTrace.plane;
|
||
tr.startsolid = obbTrace.startsolid;
|
||
tr.allsolid = obbTrace.allsolid;
|
||
|
||
// This logic here is to shorten the ray each time to get more early outs
|
||
tr.fraction *= obbTrace.fraction;
|
||
clippedRay.m_Delta *= obbTrace.fraction;
|
||
hitbox = i;
|
||
if (tr.startsolid)
|
||
break;
|
||
}
|
||
}
|
||
|
||
if ( hitbox >= 0 )
|
||
{
|
||
tr.hitgroup = set->pHitbox(hitbox)->group;
|
||
tr.hitbox = hitbox;
|
||
const mstudiobone_t *pBone = pStudioHdr->pBone( set->pHitbox(hitbox)->bone );
|
||
tr.contents = pBone->contents | CONTENTS_HITBOX;
|
||
tr.physicsbone = pBone->physicsbone;
|
||
tr.surface.name = "**studio**";
|
||
tr.surface.flags = SURF_HITBOX;
|
||
tr.surface.surfaceProps = pProps->GetSurfaceIndex( pBone->pszSurfaceProp() );
|
||
|
||
Assert( tr.physicsbone >= 0 );
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose:
|
||
//-----------------------------------------------------------------------------
|
||
bool TraceToStudio( IPhysicsSurfaceProps *pProps, const Ray_t& ray, CStudioHdr *pStudioHdr, mstudiohitboxset_t *set,
|
||
matrix3x4_t **hitboxbones, int fContentsMask, const Vector &vecOrigin, float flScale, trace_t &tr )
|
||
{
|
||
if ( !ray.m_IsRay )
|
||
{
|
||
return SweepBoxToStudio( pProps, ray, pStudioHdr, set, hitboxbones, fContentsMask, tr );
|
||
}
|
||
|
||
tr.fraction = 1.0;
|
||
tr.startsolid = false;
|
||
|
||
// no hit yet
|
||
int hitbox = -1;
|
||
int hitside = -1;
|
||
|
||
// OPTIMIZE: Partition these?
|
||
for ( int i = 0; i < set->numhitboxes; i++ )
|
||
{
|
||
mstudiobbox_t *pbox = set->pHitbox(i);
|
||
|
||
// Filter based on contents mask
|
||
int fBoneContents = pStudioHdr->pBone( pbox->bone )->contents;
|
||
if ( ( fBoneContents & fContentsMask ) == 0 )
|
||
continue;
|
||
|
||
// columns are axes of the bones in world space, translation is in world space
|
||
matrix3x4_t& matrix = *hitboxbones[pbox->bone];
|
||
|
||
// Because we're sending in a matrix with scale data, and because the matrix inversion in the hitbox
|
||
// code does not handle that case, we pre-scale the bones and ray down here and do our collision checks
|
||
// in unscaled space. We can then rescale the results afterwards.
|
||
|
||
int side = -1;
|
||
if ( flScale < 1.0f-FLT_EPSILON || flScale > 1.0f+FLT_EPSILON )
|
||
{
|
||
matrix3x4_t matScaled;
|
||
MatrixCopy( matrix, matScaled );
|
||
|
||
float invScale = 1.0f / flScale;
|
||
|
||
Vector vecBoneOrigin;
|
||
MatrixGetColumn( matScaled, 3, vecBoneOrigin );
|
||
|
||
// Pre-scale the origin down
|
||
Vector vecNewOrigin = vecBoneOrigin - vecOrigin;
|
||
vecNewOrigin *= invScale;
|
||
vecNewOrigin += vecOrigin;
|
||
MatrixSetColumn( vecNewOrigin, 3, matScaled );
|
||
|
||
// Scale it uniformly
|
||
VectorScale( matScaled[0], invScale, matScaled[0] );
|
||
VectorScale( matScaled[1], invScale, matScaled[1] );
|
||
VectorScale( matScaled[2], invScale, matScaled[2] );
|
||
|
||
// Pre-scale our ray as well
|
||
Vector vecRayStart = ray.m_Start - vecOrigin;
|
||
vecRayStart *= invScale;
|
||
vecRayStart += vecOrigin;
|
||
|
||
Vector vecRayDelta = ray.m_Delta * invScale;
|
||
|
||
Ray_t newRay;
|
||
newRay.Init( vecRayStart, vecRayStart + vecRayDelta );
|
||
|
||
side = ClipRayToHitbox( newRay, pbox, matScaled, tr );
|
||
}
|
||
else
|
||
{
|
||
side = ClipRayToHitbox( ray, pbox, matrix, tr );
|
||
}
|
||
|
||
if ( side >= 0 )
|
||
{
|
||
hitbox = i;
|
||
hitside = side;
|
||
}
|
||
}
|
||
|
||
if ( hitbox >= 0 )
|
||
{
|
||
mstudiobbox_t *pbox = set->pHitbox(hitbox);
|
||
VectorMA( ray.m_Start, tr.fraction, ray.m_Delta, tr.endpos );
|
||
tr.hitgroup = set->pHitbox(hitbox)->group;
|
||
tr.hitbox = hitbox;
|
||
const mstudiobone_t *pBone = pStudioHdr->pBone( pbox->bone );
|
||
tr.contents = pBone->contents | CONTENTS_HITBOX;
|
||
tr.physicsbone = pBone->physicsbone;
|
||
tr.surface.name = "**studio**";
|
||
tr.surface.flags = SURF_HITBOX;
|
||
tr.surface.surfaceProps = pProps->GetSurfaceIndex( pBone->pszSurfaceProp() );
|
||
|
||
Assert( tr.physicsbone >= 0 );
|
||
matrix3x4_t& matrix = *hitboxbones[pbox->bone];
|
||
if ( hitside >= 3 )
|
||
{
|
||
hitside -= 3;
|
||
tr.plane.normal[0] = matrix[0][hitside];
|
||
tr.plane.normal[1] = matrix[1][hitside];
|
||
tr.plane.normal[2] = matrix[2][hitside];
|
||
//tr.plane.dist = DotProduct( tr.plane.normal, Vector(matrix[0][3], matrix[1][3], matrix[2][3] ) ) + pbox->bbmax[hitside];
|
||
}
|
||
else
|
||
{
|
||
tr.plane.normal[0] = -matrix[0][hitside];
|
||
tr.plane.normal[1] = -matrix[1][hitside];
|
||
tr.plane.normal[2] = -matrix[2][hitside];
|
||
//tr.plane.dist = DotProduct( tr.plane.normal, Vector(matrix[0][3], matrix[1][3], matrix[2][3] ) ) - pbox->bbmin[hitside];
|
||
}
|
||
// simpler plane constant equation
|
||
tr.plane.dist = DotProduct( tr.endpos, tr.plane.normal );
|
||
tr.plane.type = 3;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: returns array of animations and weightings for a sequence based on current pose parameters
|
||
//-----------------------------------------------------------------------------
|
||
|
||
void Studio_SeqAnims( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, const float poseParameter[], mstudioanimdesc_t *panim[4], float *weight )
|
||
{
|
||
#if _DEBUG
|
||
VPROF_INCREMENT_COUNTER("SEQ_ANIMS",1);
|
||
#endif
|
||
if (!pStudioHdr || iSequence >= pStudioHdr->GetNumSeq())
|
||
{
|
||
weight[0] = weight[1] = weight[2] = weight[3] = 0.0;
|
||
return;
|
||
}
|
||
|
||
int i0 = 0, i1 = 0;
|
||
float s0 = 0, s1 = 0;
|
||
|
||
Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, iSequence, 0, s0, i0 );
|
||
Studio_LocalPoseParameter( pStudioHdr, poseParameter, seqdesc, iSequence, 1, s1, i1 );
|
||
|
||
panim[0] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0 , i1 ) ) );
|
||
weight[0] = (1 - s0) * (1 - s1);
|
||
|
||
panim[1] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0+1, i1 ) ) );
|
||
weight[1] = (s0) * (1 - s1);
|
||
|
||
panim[2] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0 , i1+1 ) ) );
|
||
weight[2] = (1 - s0) * (s1);
|
||
|
||
panim[3] = &((CStudioHdr *)pStudioHdr)->pAnimdesc( pStudioHdr->iRelativeAnim( iSequence, seqdesc.anim( i0+1, i1+1 ) ) );
|
||
weight[3] = (s0) * (s1);
|
||
|
||
Assert( weight[0] >= 0.0f && weight[1] >= 0.0f && weight[2] >= 0.0f && weight[3] >= 0.0f );
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: returns max frame number for a sequence
|
||
//-----------------------------------------------------------------------------
|
||
|
||
int Studio_MaxFrame( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )
|
||
{
|
||
mstudioanimdesc_t *panim[4];
|
||
float weight[4];
|
||
|
||
mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
|
||
Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
|
||
|
||
float maxFrame = 0;
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
if (weight[i] > 0)
|
||
{
|
||
maxFrame += panim[i]->numframes * weight[i];
|
||
}
|
||
}
|
||
|
||
if ( maxFrame > 1 )
|
||
maxFrame -= 1;
|
||
|
||
|
||
// FIXME: why does the weights sometimes not exactly add it 1.0 and this sometimes rounds down?
|
||
return (maxFrame + 0.01);
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: returns frames per second of a sequence
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_FPS( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )
|
||
{
|
||
mstudioanimdesc_t *panim[4];
|
||
float weight[4];
|
||
|
||
mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
|
||
Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
|
||
|
||
float t = 0;
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
if (weight[i] > 0)
|
||
{
|
||
t += panim[i]->fps * weight[i];
|
||
}
|
||
}
|
||
return t;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: returns cycles per second of a sequence (cycles/second)
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_CPS( const CStudioHdr *pStudioHdr, mstudioseqdesc_t &seqdesc, int iSequence, const float poseParameter[] )
|
||
{
|
||
mstudioanimdesc_t *panim[4];
|
||
float weight[4];
|
||
|
||
Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
|
||
|
||
float t = 0;
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
if (weight[i] > 0 && panim[i]->numframes > 1)
|
||
{
|
||
t += (panim[i]->fps / (panim[i]->numframes - 1)) * weight[i];
|
||
}
|
||
}
|
||
return t;
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: returns length (in seconds) of a sequence (seconds/cycle)
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_Duration( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[] )
|
||
{
|
||
mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
|
||
float cps = Studio_CPS( pStudioHdr, seqdesc, iSequence, poseParameter );
|
||
|
||
if( cps == 0 )
|
||
return 0.0f;
|
||
|
||
return 1.0f/cps;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: calculate changes in position and angle relative to the start of an animations cycle
|
||
// Output: updated position and angle, relative to the origin
|
||
// returns false if animation is not a movement animation
|
||
//-----------------------------------------------------------------------------
|
||
|
||
bool Studio_AnimPosition( mstudioanimdesc_t *panim, float flCycle, Vector &vecPos, QAngle &vecAngle )
|
||
{
|
||
float prevframe = 0;
|
||
vecPos.Init( );
|
||
vecAngle.Init( );
|
||
|
||
if (panim->nummovements == 0)
|
||
return false;
|
||
|
||
int iLoops = 0;
|
||
if (flCycle > 1.0)
|
||
{
|
||
iLoops = (int)flCycle;
|
||
}
|
||
else if (flCycle < 0.0)
|
||
{
|
||
iLoops = (int)flCycle - 1;
|
||
}
|
||
flCycle = flCycle - iLoops;
|
||
|
||
float flFrame = flCycle * (panim->numframes - 1);
|
||
|
||
for (int i = 0; i < panim->nummovements; i++)
|
||
{
|
||
mstudiomovement_t *pmove = panim->pMovement( i );
|
||
|
||
if (pmove->endframe >= flFrame)
|
||
{
|
||
float f = (flFrame - prevframe) / (pmove->endframe - prevframe);
|
||
|
||
float d = pmove->v0 * f + 0.5 * (pmove->v1 - pmove->v0) * f * f;
|
||
|
||
vecPos = vecPos + d * pmove->vector;
|
||
vecAngle.y = vecAngle.y * (1 - f) + pmove->angle * f;
|
||
if (iLoops != 0)
|
||
{
|
||
mstudiomovement_t *pmove = panim->pMovement( panim->nummovements - 1 );
|
||
vecPos = vecPos + iLoops * pmove->position;
|
||
vecAngle.y = vecAngle.y + iLoops * pmove->angle;
|
||
}
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
prevframe = pmove->endframe;
|
||
vecPos = pmove->position;
|
||
vecAngle.y = pmove->angle;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: calculate instantaneous velocity in ips at a given point
|
||
// in the animations cycle
|
||
// Output: velocity vector, relative to identity orientation
|
||
// returns false if animation is not a movement animation
|
||
//-----------------------------------------------------------------------------
|
||
|
||
bool Studio_AnimVelocity( mstudioanimdesc_t *panim, float flCycle, Vector &vecVelocity )
|
||
{
|
||
float prevframe = 0;
|
||
|
||
float flFrame = flCycle * (panim->numframes - 1);
|
||
flFrame = flFrame - (int)(flFrame / (panim->numframes - 1));
|
||
|
||
for (int i = 0; i < panim->nummovements; i++)
|
||
{
|
||
mstudiomovement_t *pmove = panim->pMovement( i );
|
||
|
||
if (pmove->endframe >= flFrame)
|
||
{
|
||
float f = (flFrame - prevframe) / (pmove->endframe - prevframe);
|
||
|
||
float vel = pmove->v0 * (1 - f) + pmove->v1 * f;
|
||
// scale from per block to per sec velocity
|
||
vel = vel * panim->fps / (pmove->endframe - prevframe);
|
||
|
||
vecVelocity = pmove->vector * vel;
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
prevframe = pmove->endframe;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: calculate changes in position and angle between two points in an animation cycle
|
||
// Output: updated position and angle, relative to CycleFrom being at the origin
|
||
// returns false if animation is not a movement animation
|
||
//-----------------------------------------------------------------------------
|
||
|
||
bool Studio_AnimMovement( mstudioanimdesc_t *panim, float flCycleFrom, float flCycleTo, Vector &deltaPos, QAngle &deltaAngle )
|
||
{
|
||
if (panim->nummovements == 0)
|
||
return false;
|
||
|
||
Vector startPos;
|
||
QAngle startA;
|
||
Studio_AnimPosition( panim, flCycleFrom, startPos, startA );
|
||
|
||
Vector endPos;
|
||
QAngle endA;
|
||
Studio_AnimPosition( panim, flCycleTo, endPos, endA );
|
||
|
||
Vector tmp = endPos - startPos;
|
||
deltaAngle.y = endA.y - startA.y;
|
||
VectorYawRotate( tmp, -startA.y, deltaPos );
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: finds how much of an animation to play to move given linear distance
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_FindAnimDistance( mstudioanimdesc_t *panim, float flDist )
|
||
{
|
||
float prevframe = 0;
|
||
|
||
if (flDist <= 0)
|
||
return 0.0;
|
||
|
||
for (int i = 0; i < panim->nummovements; i++)
|
||
{
|
||
mstudiomovement_t *pmove = panim->pMovement( i );
|
||
|
||
float flMove = (pmove->v0 + pmove->v1) * 0.5;
|
||
|
||
if (flMove >= flDist)
|
||
{
|
||
float root1, root2;
|
||
|
||
// d = V0 * t + 1/2 (V1-V0) * t^2
|
||
if (SolveQuadratic( 0.5 * (pmove->v1 - pmove->v0), pmove->v0, -flDist, root1, root2 ))
|
||
{
|
||
float cpf = 1.0 / (panim->numframes - 1); // cycles per frame
|
||
|
||
return (prevframe + root1 * (pmove->endframe - prevframe)) * cpf;
|
||
}
|
||
return 0.0;
|
||
}
|
||
else
|
||
{
|
||
flDist -= flMove;
|
||
prevframe = pmove->endframe;
|
||
}
|
||
}
|
||
return 1.0;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: calculate changes in position and angle between two points in a sequences cycle
|
||
// Output: updated position and angle, relative to CycleFrom being at the origin
|
||
// returns false if sequence is not a movement sequence
|
||
//-----------------------------------------------------------------------------
|
||
|
||
bool Studio_SeqMovement( const CStudioHdr *pStudioHdr, int iSequence, float flCycleFrom, float flCycleTo, const float poseParameter[], Vector &deltaPos, QAngle &deltaAngles )
|
||
{
|
||
mstudioanimdesc_t *panim[4];
|
||
float weight[4];
|
||
|
||
mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
|
||
|
||
Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
|
||
|
||
deltaPos.Init( );
|
||
deltaAngles.Init( );
|
||
|
||
bool found = false;
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
if (weight[i])
|
||
{
|
||
Vector localPos;
|
||
QAngle localAngles;
|
||
|
||
localPos.Init();
|
||
localAngles.Init();
|
||
|
||
if (Studio_AnimMovement( panim[i], flCycleFrom, flCycleTo, localPos, localAngles ))
|
||
{
|
||
found = true;
|
||
deltaPos = deltaPos + localPos * weight[i];
|
||
// FIXME: this makes no sense
|
||
deltaAngles = deltaAngles + localAngles * weight[i];
|
||
}
|
||
else if (!(panim[i]->flags & STUDIO_DELTA) && panim[i]->nummovements == 0 && seqdesc.weight(0) > 0.0)
|
||
{
|
||
found = true;
|
||
}
|
||
}
|
||
}
|
||
return found;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: calculate instantaneous velocity in ips at a given point in the sequence's cycle
|
||
// Output: velocity vector, relative to identity orientation
|
||
// returns false if sequence is not a movement sequence
|
||
//-----------------------------------------------------------------------------
|
||
|
||
bool Studio_SeqVelocity( const CStudioHdr *pStudioHdr, int iSequence, float flCycle, const float poseParameter[], Vector &vecVelocity )
|
||
{
|
||
mstudioanimdesc_t *panim[4];
|
||
float weight[4];
|
||
|
||
mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
|
||
Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
|
||
|
||
vecVelocity.Init( );
|
||
|
||
bool found = false;
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
if (weight[i])
|
||
{
|
||
Vector vecLocalVelocity;
|
||
|
||
if (Studio_AnimVelocity( panim[i], flCycle, vecLocalVelocity ))
|
||
{
|
||
vecVelocity = vecVelocity + vecLocalVelocity * weight[i];
|
||
found = true;
|
||
}
|
||
}
|
||
}
|
||
return found;
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: finds how much of an sequence to play to move given linear distance
|
||
//-----------------------------------------------------------------------------
|
||
|
||
float Studio_FindSeqDistance( const CStudioHdr *pStudioHdr, int iSequence, const float poseParameter[], float flDist )
|
||
{
|
||
mstudioanimdesc_t *panim[4];
|
||
float weight[4];
|
||
|
||
mstudioseqdesc_t &seqdesc = ((CStudioHdr *)pStudioHdr)->pSeqdesc( iSequence );
|
||
Studio_SeqAnims( pStudioHdr, seqdesc, iSequence, poseParameter, panim, weight );
|
||
|
||
float flCycle = 0;
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
if (weight[i])
|
||
{
|
||
float flLocalCycle = Studio_FindAnimDistance( panim[i], flDist );
|
||
flCycle = flCycle + flLocalCycle * weight[i];
|
||
}
|
||
}
|
||
return flCycle;
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: lookup attachment by name
|
||
//-----------------------------------------------------------------------------
|
||
|
||
int Studio_FindAttachment( const CStudioHdr *pStudioHdr, const char *pAttachmentName )
|
||
{
|
||
if ( pStudioHdr && pStudioHdr->SequencesAvailable() )
|
||
{
|
||
// Extract the bone index from the name
|
||
for (int i = 0; i < pStudioHdr->GetNumAttachments(); i++)
|
||
{
|
||
if (!V_stricmp(pAttachmentName,((CStudioHdr *)pStudioHdr)->pAttachment(i).pszName( )))
|
||
{
|
||
return i;
|
||
}
|
||
}
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: lookup attachments by substring. Randomly return one of the matching attachments.
|
||
//-----------------------------------------------------------------------------
|
||
|
||
int Studio_FindRandomAttachment( const CStudioHdr *pStudioHdr, const char *pAttachmentName )
|
||
{
|
||
if ( pStudioHdr )
|
||
{
|
||
// First move them all matching attachments into a list
|
||
CUtlVector<int> matchingAttachments;
|
||
|
||
// Extract the bone index from the name
|
||
for (int i = 0; i < pStudioHdr->GetNumAttachments(); i++)
|
||
{
|
||
if ( strstr( ((CStudioHdr *)pStudioHdr)->pAttachment(i).pszName(), pAttachmentName ) )
|
||
{
|
||
matchingAttachments.AddToTail(i);
|
||
}
|
||
}
|
||
|
||
// Then randomly return one of the attachments
|
||
if ( matchingAttachments.Size() > 0 )
|
||
return matchingAttachments[ RandomInt( 0, matchingAttachments.Size()-1 ) ];
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: lookup bone by name
|
||
//-----------------------------------------------------------------------------
|
||
|
||
int Studio_BoneIndexByName( const CStudioHdr *pStudioHdr, const char *pName )
|
||
{
|
||
if ( pStudioHdr )
|
||
{
|
||
// 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;
|
||
}
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: Drive a flex controller from a component of a bone
|
||
//-----------------------------------------------------------------------------
|
||
void Studio_RunBoneFlexDrivers( float *pflFlexControllerWeights, const CStudioHdr *pStudioHdr, const Vector *pvPositions, const matrix3x4_t *pBoneToWorld, const matrix3x4_t &mRootToWorld )
|
||
{
|
||
bool bRootToWorldInvComputed = false;
|
||
matrix3x4_t mRootToWorldInv;
|
||
matrix3x4_t mParentInv;
|
||
matrix3x4_t mBoneLocal;
|
||
|
||
const int nBoneFlexDriverCount = pStudioHdr->BoneFlexDriverCount();
|
||
|
||
for ( int i = 0; i < nBoneFlexDriverCount; ++i )
|
||
{
|
||
const mstudioboneflexdriver_t *pBoneFlexDriver = pStudioHdr->BoneFlexDriver( i );
|
||
const mstudiobone_t *pStudioBone = pStudioHdr->pBone( pBoneFlexDriver->m_nBoneIndex );
|
||
|
||
const int nControllerCount = pBoneFlexDriver->m_nControlCount;
|
||
|
||
if ( pStudioBone->flags & BONE_USED_BY_BONE_MERGE )
|
||
{
|
||
// The local space version of the bone is not available if this is a bonemerged bone
|
||
// so do the slow computation of the local version of the bone from boneToWorld
|
||
|
||
if ( pStudioBone->parent < 0 )
|
||
{
|
||
if ( !bRootToWorldInvComputed )
|
||
{
|
||
MatrixInvert( mRootToWorld, mRootToWorldInv );
|
||
bRootToWorldInvComputed = true;
|
||
}
|
||
|
||
MatrixMultiply( mRootToWorldInv, pBoneToWorld[ pBoneFlexDriver->m_nBoneIndex ], mBoneLocal );
|
||
}
|
||
else
|
||
{
|
||
MatrixInvert( pBoneToWorld[ pStudioBone->parent ], mParentInv );
|
||
MatrixMultiply( mParentInv, pBoneToWorld[ pBoneFlexDriver->m_nBoneIndex ], mBoneLocal );
|
||
}
|
||
|
||
for ( int j = 0; j < nControllerCount; ++j )
|
||
{
|
||
const mstudioboneflexdrivercontrol_t *pController = pBoneFlexDriver->pBoneFlexDriverControl( j );
|
||
const mstudioflexcontroller_t *pFlexController = pStudioHdr->pFlexcontroller( static_cast< LocalFlexController_t >( pController->m_nFlexControllerIndex ) );
|
||
|
||
if ( pFlexController->localToGlobal < 0 )
|
||
continue;
|
||
|
||
Assert( pController->m_nFlexControllerIndex >= 0 && pController->m_nFlexControllerIndex < pStudioHdr->numflexcontrollers() );
|
||
Assert( pController->m_nBoneComponent >= 0 && pController->m_nBoneComponent <= 2 );
|
||
pflFlexControllerWeights[pFlexController->localToGlobal] =
|
||
RemapValClamped( mBoneLocal[pController->m_nBoneComponent][3], pController->m_flMin, pController->m_flMax, 0.0f, 1.0f );
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Use the local space version of the bone directly for non-bonemerged bones
|
||
|
||
const Vector &position = pvPositions[ pBoneFlexDriver->m_nBoneIndex ];
|
||
|
||
for ( int j = 0; j < nControllerCount; ++j )
|
||
{
|
||
const mstudioboneflexdrivercontrol_t *pController = pBoneFlexDriver->pBoneFlexDriverControl( j );
|
||
const mstudioflexcontroller_t *pFlexController = pStudioHdr->pFlexcontroller( static_cast< LocalFlexController_t >( pController->m_nFlexControllerIndex ) );
|
||
|
||
if ( pFlexController->localToGlobal < 0 )
|
||
continue;
|
||
|
||
Assert( pController->m_nFlexControllerIndex >= 0 && pController->m_nFlexControllerIndex < pStudioHdr->numflexcontrollers() );
|
||
Assert( pController->m_nBoneComponent >= 0 && pController->m_nBoneComponent <= 2 );
|
||
pflFlexControllerWeights[pFlexController->localToGlobal] =
|
||
RemapValClamped( position[pController->m_nBoneComponent], pController->m_flMin, pController->m_flMax, 0.0f, 1.0f );
|
||
}
|
||
}
|
||
}
|
||
}
|