You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1509 lines
45 KiB
1509 lines
45 KiB
//===== Copyright © 1996-2005, Valve Corporation, All rights reserved. ======//
|
|
//
|
|
// Purpose:
|
|
//
|
|
// $NoKeywords: $
|
|
//
|
|
//===========================================================================//
|
|
|
|
//#include "tier0/dbg.h"
|
|
#include "mathlib/mathlib.h"
|
|
#include "bone_setup_PS3.h"
|
|
|
|
#if !defined(__SPU__)
|
|
#include <string.h>
|
|
#include "tier0/vprof.h"
|
|
#endif
|
|
|
|
#include "bone_utils_PS3.h"
|
|
|
|
|
|
|
|
class CIKSolver_PS3
|
|
{
|
|
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: for a 2 bone chain, find the IK solution and reset the matrices
|
|
//-----------------------------------------------------------------------------
|
|
bool Studio_SolveIK_PS3( Vector &kneeDir0, int bone0, int bone1, int bone2, Vector &targetFoot, matrix3x4a_t *pBoneToWorld )
|
|
{
|
|
#if 0
|
|
// see bone_ik.cpp - something with the CS models breaks this
|
|
if( kneeDir0.LengthSqr() > 0.0f )
|
|
{
|
|
Vector targetKneeDir, targetKneePos;
|
|
// FIXME: knee length should be as long as the legs
|
|
Vector tmp = kneeDir0;
|
|
VectorRotate_PS3( tmp, pBoneToWorld[ bone0 ], targetKneeDir );
|
|
MatrixPosition_PS3( pBoneToWorld[ bone1 ], targetKneePos );
|
|
return Studio_SolveIK_PS3( bone0, bone1, bone2, targetFoot, targetKneePos, targetKneeDir, pBoneToWorld );
|
|
}
|
|
else
|
|
#endif
|
|
{
|
|
return Studio_SolveIK_PS3( bone0, bone1, bone2, targetFoot, pBoneToWorld );
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//
|
|
//-----------------------------------------------------------------------------
|
|
float Studio_IKRuleWeight_PS3( mstudioikrule_t_PS3 &ikRule, int anim_numframes, float flCycle, int &iFrame, float &fraq )
|
|
{
|
|
if( ikRule.end > 1.0f && flCycle < ikRule.start )
|
|
{
|
|
flCycle = flCycle + 1.0f;
|
|
}
|
|
|
|
float value = 0.0f;
|
|
fraq = (anim_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 = (anim_numframes - 1) * (ikRule.end - ikRule.start) + ikRule.iStart;
|
|
iFrame = (int)fraq;
|
|
fraq = fraq - iFrame;
|
|
}
|
|
return SimpleSpline_PS3( value );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//
|
|
//-----------------------------------------------------------------------------
|
|
float Studio_IKRuleWeight_PS3( ikcontextikrule_t_PS3 &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;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//
|
|
//-----------------------------------------------------------------------------
|
|
float Studio_IKTail_PS3( ikcontextikrule_t_PS3 &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.0f;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose:
|
|
//-----------------------------------------------------------------------------
|
|
|
|
bool Studio_IKShouldLatch_PS3( ikcontextikrule_t_PS3 &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;
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//
|
|
//-----------------------------------------------------------------------------
|
|
bool Studio_IKAnimationError_PS3( animData_SPU *panim, mstudioanimdesc_t_PS3 *pAnimdesc, void *pEA_IKRule, mstudioikrule_t_PS3 *pLS_IKRule, float flCycle, BoneVector &pos, BoneQuaternion &q, float &flWeight )
|
|
{
|
|
float fraq;
|
|
int iFrame;
|
|
|
|
if( !pEA_IKRule )
|
|
return false;
|
|
|
|
flWeight = Studio_IKRuleWeight_PS3( *pLS_IKRule, pAnimdesc->numframes, flCycle, iFrame, fraq );
|
|
Assert( fraq >= 0.0f && fraq < 1.0f );
|
|
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( pLS_IKRule->type != IK_GROUND && flWeight < 0.0001f )
|
|
return false;
|
|
|
|
void *pEA_Error = (void *)pLS_IKRule->pError( pEA_IKRule, iFrame );
|
|
|
|
if( pEA_Error != NULL )
|
|
{
|
|
mstudioikerror_t_PS3 error[ 3 ] ALIGN16;
|
|
mstudioikerror_t_PS3 *pLS_Error;
|
|
|
|
pLS_Error = (mstudioikerror_t_PS3 *)SPUmemcpy_UnalignedGet( error, (uint32)pEA_Error, 2*sizeof(mstudioikerror_t_PS3) );
|
|
|
|
if( fraq < 0.001f )
|
|
{
|
|
q = pLS_Error[0].q;
|
|
pos = pLS_Error[0].pos;
|
|
}
|
|
else
|
|
{
|
|
QuaternionBlend_PS3( pLS_Error[0].q, pLS_Error[1].q, fraq, q );
|
|
pos = pLS_Error[0].pos * (1.0f - fraq) + pLS_Error[1].pos * fraq;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void *pEA_Compressed = (void *)pLS_IKRule->pCompressedError( pEA_IKRule );
|
|
|
|
if( pEA_Compressed != NULL )
|
|
{
|
|
mstudiocompressedikerror_t_PS3 compressed[ 3 ] ALIGN16;
|
|
mstudiocompressedikerror_t_PS3 *pLS_Compressed;
|
|
|
|
pLS_Compressed = (mstudiocompressedikerror_t_PS3 *)SPUmemcpy_UnalignedGet( compressed, (uint32)pEA_Compressed, sizeof(mstudiocompressedikerror_t_PS3) );
|
|
|
|
CalcDecompressedAnimation_PS3( pEA_Compressed, pLS_Compressed, iFrame - pLS_IKRule->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_PS3( accumposeentry_SPU *pPoseEntry, int iRule, const float poseParameter[], ikcontextikrule_t_PS3 &ikRule )
|
|
{
|
|
int i;
|
|
|
|
memset( &ikRule, 0, sizeof(ikRule) );
|
|
ikRule.start = ikRule.peak = ikRule.tail = ikRule.end = 0.0f;
|
|
|
|
float prevStart = 0.0f;
|
|
|
|
animData_SPU *pAnim0 = NULL;
|
|
if( pPoseEntry->animIndices[0] != -1 )
|
|
{
|
|
pAnim0 = &pPoseEntry->anims[ pPoseEntry->animIndices[0] ];
|
|
}
|
|
|
|
//mstudioikrule_t_PS3 ikrule[4][2] ALIGN16;
|
|
byte ikrule[4][ ROUNDUPTONEXT16B( sizeof(mstudioikrule_t_PS3) ) ] ALIGN16;
|
|
|
|
//mstudioikrulezeroframe_t_PS3 ikrulezeroframe[4][2] ALIGN16;
|
|
byte ikrulezeroframe[4][ ROUNDUPTONEXT16B( sizeof(mstudioikrulezeroframe_t_PS3) ) ] ALIGN16;
|
|
|
|
//mstudioanimdesc_t_PS3 ls_animdesc[4][2] ALIGN16;
|
|
byte ls_animdesc[4][ ROUNDUPTONEXT16B( sizeof(mstudioanimdesc_t_PS3) ) ] ALIGN16;
|
|
|
|
|
|
mstudioikrule_t_PS3 *pLS_IKRule[4];
|
|
mstudioikrulezeroframe_t_PS3 *pLS_IKRuleZeroFrame[4];
|
|
mstudioanimdesc_t_PS3 *pLS_animdesc[4];
|
|
|
|
float flCycle = pPoseEntry->cyclePoseSingle;
|
|
|
|
|
|
// prefetch animdesc
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
int idx = pPoseEntry->animIndices[ i ];
|
|
if( idx != -1 )
|
|
{
|
|
animData_SPU *pAnim = &pPoseEntry->anims[ idx ];
|
|
pLS_animdesc[ idx ] = (mstudioanimdesc_t_PS3 *)SPUmemcpy_UnalignedGet_MustSync( ls_animdesc[ idx ], (uint32)pAnim->pEA_animdesc, sizeof(mstudioanimdesc_t_PS3), DMATAG_ANIM );
|
|
}
|
|
else
|
|
{
|
|
pLS_animdesc[ idx ] = NULL;
|
|
}
|
|
|
|
pLS_IKRule[ i ] = NULL;
|
|
pLS_IKRuleZeroFrame[ i ] = NULL;
|
|
}
|
|
|
|
SPUmemcpy_Sync( 1<<DMATAG_ANIM );
|
|
|
|
// find overall influence
|
|
for( i = 0; i < 4; i++ )
|
|
{
|
|
int idx = pPoseEntry->animIndices[ i ];
|
|
|
|
if( idx != -1 )
|
|
{
|
|
mstudioanimdesc_t_PS3 *pAnimdesc = pLS_animdesc[ idx ];
|
|
animData_SPU *pAnim = &pPoseEntry->anims[ idx ];
|
|
|
|
if( iRule >= pAnimdesc->numikrules || pAnimdesc->numikrules != pLS_animdesc[ pPoseEntry->animIndices[0] ]->numikrules )
|
|
{
|
|
Assert( 0 );
|
|
return false;
|
|
}
|
|
|
|
//mstudioikrule_t_PS3 *pRule = panim[i]->pIKRule( iRule );
|
|
if( pAnim->pEA_animdesc_ikrule )
|
|
{
|
|
pLS_IKRule[ idx ] = (mstudioikrule_t_PS3 *)SPUmemcpy_UnalignedGet( ikrule[ idx ], (uint32)((mstudioikrule_t_PS3 *)pAnim->pEA_animdesc_ikrule + iRule), sizeof(mstudioikrule_t_PS3) );
|
|
mstudioikrule_t_PS3 *pIKRule = pLS_IKRule[ idx ];
|
|
|
|
float dt = 0.0f;
|
|
if( prevStart != 0.0f )
|
|
{
|
|
if( pIKRule->start - prevStart > 0.5f )
|
|
{
|
|
dt = -1.0f;
|
|
}
|
|
else if( pIKRule->start - prevStart < -0.5f )
|
|
{
|
|
dt = 1.0f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
prevStart = pIKRule->start;
|
|
}
|
|
|
|
ikRule.start += (pIKRule->start + dt) * pAnim->seqdesc_weight;
|
|
ikRule.peak += (pIKRule->peak + dt) * pAnim->seqdesc_weight;
|
|
ikRule.tail += (pIKRule->tail + dt) * pAnim->seqdesc_weight;
|
|
ikRule.end += (pIKRule->end + dt) * pAnim->seqdesc_weight;
|
|
}
|
|
else
|
|
{
|
|
//mstudioikrulezeroframe_t *pZeroFrameRule = panim[i]->pIKRuleZeroFrame( iRule );
|
|
if( pAnim->pEA_animdesc_ikrulezeroframe )
|
|
{
|
|
pLS_IKRuleZeroFrame[ idx ] = (mstudioikrulezeroframe_t_PS3 *)SPUmemcpy_UnalignedGet( ikrulezeroframe[ idx ], (uint32)((mstudioikrulezeroframe_t_PS3 *)pAnim->pEA_animdesc_ikrulezeroframe + iRule), sizeof(mstudioikrulezeroframe_t_PS3) );
|
|
mstudioikrulezeroframe_t_PS3 *pIKRule = pLS_IKRuleZeroFrame[ idx ];
|
|
|
|
float dt = 0.0f;
|
|
if (prevStart != 0.0f)
|
|
{
|
|
if( pIKRule->start.GetFloat() - prevStart > 0.5f )
|
|
{
|
|
dt = -1.0f;
|
|
}
|
|
else if( pIKRule->start.GetFloat() - prevStart < -0.5f )
|
|
{
|
|
dt = 1.0f;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
prevStart = pIKRule->start.GetFloat();
|
|
}
|
|
|
|
ikRule.start += (pIKRule->start.GetFloat() + dt) * pAnim->seqdesc_weight;
|
|
ikRule.peak += (pIKRule->peak.GetFloat() + dt) * pAnim->seqdesc_weight;
|
|
ikRule.tail += (pIKRule->tail.GetFloat() + dt) * pAnim->seqdesc_weight;
|
|
ikRule.end += (pIKRule->end.GetFloat() + dt) * pAnim->seqdesc_weight;
|
|
}
|
|
else
|
|
{
|
|
// Msg("%s %s - IK Stall\n", pStudioHdr->name(), seqdesc.pszLabel() );
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if( ikRule.start > 1.0f )
|
|
{
|
|
ikRule.start -= 1.0f;
|
|
ikRule.peak -= 1.0f;
|
|
ikRule.tail -= 1.0f;
|
|
ikRule.end -= 1.0f;
|
|
}
|
|
else if( ikRule.start < 0.0f )
|
|
{
|
|
ikRule.start += 1.0f;
|
|
ikRule.peak += 1.0f;
|
|
ikRule.tail += 1.0f;
|
|
ikRule.end += 1.0f;
|
|
}
|
|
|
|
ikRule.flWeight = Studio_IKRuleWeight_PS3( ikRule, flCycle );
|
|
if( ikRule.flWeight <= 0.001f )
|
|
{
|
|
// go ahead and allow IK_GROUND rules a virtual looping section
|
|
if( pAnim0->pEA_animdesc_ikrule == NULL )
|
|
return false;
|
|
|
|
int ikrule0_type;
|
|
|
|
int idx0 = pPoseEntry->animIndices[ 0 ];
|
|
|
|
if( idx0 != -1 )
|
|
{
|
|
if( pLS_IKRule[ idx0 ] == NULL )
|
|
{
|
|
pLS_IKRule[ idx0 ] = (mstudioikrule_t_PS3 *)SPUmemcpy_UnalignedGet( ikrule[ idx0 ], (uint32)((mstudioikrule_t_PS3 *)pAnim0->pEA_animdesc_ikrule + iRule), sizeof(mstudioikrule_t_PS3) );
|
|
}
|
|
|
|
ikrule0_type = pLS_IKRule[ idx0 ]->type;
|
|
|
|
if( (pLS_animdesc[ idx0 ]->flags & STUDIO_LOOPING) && ikrule0_type == IK_GROUND && ikRule.end - ikRule.start > 0.75f )
|
|
{
|
|
ikRule.flWeight = 0.001f;
|
|
//??
|
|
flCycle = ikRule.end - 0.001f;
|
|
}
|
|
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++ )
|
|
{
|
|
int idx = pPoseEntry->animIndices[i];
|
|
|
|
if( idx != -1 )
|
|
{
|
|
mstudioanimdesc_t_PS3 *pAnimdesc = pLS_animdesc[ idx ];
|
|
animData_SPU *pAnim = &pPoseEntry->anims[ idx ];
|
|
|
|
BoneVector pos1;
|
|
BoneQuaternion q1;
|
|
float w;
|
|
|
|
// mstudioikrule_t *pRule = panim[i]->pIKRule( iRule );
|
|
mstudioikrule_t_PS3 *pRule = pLS_IKRule[ idx ];
|
|
|
|
if( pRule != NULL )
|
|
{
|
|
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 * pAnim->seqdesc_weight;
|
|
ikRule.floor += pRule->floor * pAnim->seqdesc_weight;
|
|
ikRule.radius += pRule->radius * pAnim->seqdesc_weight;
|
|
ikRule.drop += pRule->drop * pAnim->seqdesc_weight;
|
|
ikRule.top += pRule->top * pAnim->seqdesc_weight;
|
|
}
|
|
else
|
|
{
|
|
// look to see if there's a zeroframe version of the rule
|
|
mstudioikrulezeroframe_t_PS3 *pZeroFrameRule = pLS_IKRuleZeroFrame[ idx ];//panim[i]->pIKRuleZeroFrame( iRule );
|
|
|
|
if( pZeroFrameRule )
|
|
{
|
|
// zeroframe doesn't contain details, so force a IK_RELEASE
|
|
ikRule.type = IK_RELEASE;
|
|
ikRule.chain = pZeroFrameRule->chain;
|
|
ikRule.slot = pZeroFrameRule->slot;
|
|
ikRule.bone = -1;
|
|
|
|
// Msg("IK_RELEASE %d %d : %.2f\n", ikRule.chain, ikRule.slot, ikRule.flWeight );
|
|
}
|
|
else
|
|
{
|
|
// Msg("%s %s - IK Stall\n", pStudioHdr->name(), seqdesc.pszLabel() );
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// keep track of tail condition
|
|
ikRule.release += Studio_IKTail_PS3( ikRule, flCycle ) * pAnim->seqdesc_weight;
|
|
|
|
// only check rules with error values
|
|
switch( ikRule.type )
|
|
{
|
|
case IK_SELF:
|
|
case IK_WORLD:
|
|
case IK_GROUND:
|
|
case IK_ATTACHMENT:
|
|
{
|
|
mstudioikrule_t_PS3 *pEA_Rule = NULL;
|
|
|
|
if( pAnim->pEA_animdesc_ikrule )
|
|
{
|
|
pEA_Rule = (mstudioikrule_t_PS3 *)pAnim->pEA_animdesc_ikrule + iRule;
|
|
}
|
|
|
|
int bResult = Studio_IKAnimationError_PS3( pAnim, pAnimdesc, (void *)pEA_Rule, pRule, flCycle, pos1, q1, w );
|
|
|
|
if (bResult)
|
|
{
|
|
ikRule.pos = ikRule.pos + pos1 * pAnim->seqdesc_weight;
|
|
QuaternionAccumulate_PS3( ikRule.q, pAnim->seqdesc_weight, q1, ikRule.q );
|
|
total += pAnim->seqdesc_weight;
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
total += pAnim->seqdesc_weight;
|
|
break;
|
|
}
|
|
|
|
ikRule.latched = Studio_IKShouldLatch_PS3( ikRule, flCycle ) * ikRule.flWeight;
|
|
|
|
if( ikRule.type == IK_ATTACHMENT )
|
|
{
|
|
// ikRule.szLabel = pRule->pszAttachment();
|
|
}
|
|
}
|
|
}
|
|
|
|
if( total <= 0.0001f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if( total < 0.999f )
|
|
{
|
|
VectorScale_PS3( ikRule.pos, 1.0f / total, ikRule.pos );
|
|
QuaternionScale_PS3( ikRule.q, 1.0f / total, ikRule.q );
|
|
}
|
|
|
|
if( ikRule.type == IK_SELF && ikRule.bone != -1 )
|
|
{
|
|
//ikRule.bone = pStudioHdr->RemapSeqBone( iSequence, ikRule.bone );
|
|
if( pPoseEntry->pEA_seqgroup_masterBone )
|
|
{
|
|
int masterbone[6] ALIGN16;
|
|
int *pLS_masterbone;
|
|
|
|
pLS_masterbone = (int *)SPUmemcpy_UnalignedGet( masterbone, (uint32)((int *)pPoseEntry->pEA_seqgroup_masterBone + ikRule.bone), sizeof(int) );
|
|
|
|
ikRule.bone = *pLS_masterbone;
|
|
}
|
|
|
|
if (ikRule.bone == -1)
|
|
return false;
|
|
}
|
|
|
|
QuaternionNormalize_PS3( ikRule.q );
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
CIKContext_PS3::CIKContext_PS3()
|
|
{
|
|
m_targetCount = 0;
|
|
|
|
m_iFramecounter = -1;
|
|
m_flTime = -1.0f;
|
|
}
|
|
|
|
|
|
void CIKContext_PS3::ClearTargets( void )
|
|
{
|
|
int i;
|
|
for (i = 0; i < m_targetCount; i++)
|
|
{
|
|
m_target[ i ].latched.iFramecounter = -9999;
|
|
}
|
|
}
|
|
|
|
void CIKContext_PS3::Init( bonejob_SPU *pBonejobSPU, const QAngle &angles, const Vector &pos, float flTime, int iFramecounter, int boneMask )
|
|
{
|
|
SNPROF_ANIM( "CIKContext_PS3::Init" );
|
|
|
|
// 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 );
|
|
// }
|
|
|
|
if( pBonejobSPU->numikchains )
|
|
{
|
|
m_targetCount = 12;
|
|
memset( m_target, 0, sizeof( m_target ) );
|
|
ClearTargets();
|
|
}
|
|
else
|
|
{
|
|
m_targetCount = 0;
|
|
}
|
|
|
|
m_numTarget = 0;
|
|
|
|
AngleMatrix_PS3( angles, pos, m_rootxform );
|
|
|
|
m_iFramecounter = iFramecounter;
|
|
m_flTime = flTime;
|
|
m_boneMask = boneMask;
|
|
|
|
}
|
|
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose:
|
|
//-----------------------------------------------------------------------------
|
|
void AddDependencies_SPU( bonejob_SPU *pBonejobSPU, accumposeentry_SPU *pPoseEntry, float flWeight )
|
|
{
|
|
SNPROF_ANIM("AddDependencies_SPU");
|
|
|
|
int i;
|
|
|
|
if( pBonejobSPU->numikchains == 0 )
|
|
return;
|
|
|
|
if( pPoseEntry->seqdesc_numikrules == 0 )
|
|
return;
|
|
|
|
mstudioikchain_t_PS3 ikchain[2] ALIGN16;
|
|
int boneIndex[4] ALIGN16;
|
|
|
|
ikcontextikrule_t_PS3 ikrule;
|
|
|
|
for( i = 0; i < pPoseEntry->seqdesc_numikrules; i++ )
|
|
{
|
|
if( !Studio_IKSequenceError_PS3( pPoseEntry, i, pBonejobSPU->poseparam, ikrule ) )
|
|
continue;
|
|
|
|
//int bone = m_pStudioHdr->pIKChain( ikrule.chain )->pLink( 2 )->bone;
|
|
mstudioikchain_t_PS3 *pEA_ikchain = (mstudioikchain_t_PS3 *)pBonejobSPU->pEA_studiohdr_ikchains + ikrule.chain;
|
|
|
|
// get chain
|
|
mstudioikchain_t_PS3 *pLS_ikchain = (mstudioikchain_t_PS3 *)SPUmemcpy_UnalignedGet( ikchain, (uint32)pEA_ikchain, sizeof(mstudioikchain_t_PS3) );
|
|
|
|
// get bone, bone is 1st in mstudioiklink
|
|
int *pEA_bone = (int *)pLS_ikchain->pLink( pEA_ikchain, 2 );
|
|
|
|
int *pbone = (int *)SPUmemcpy_UnalignedGet( boneIndex, (uint32)pEA_bone, sizeof(int) );
|
|
|
|
|
|
// don't add rule if the bone isn't going to be calculated
|
|
if( !(pBonejobSPU->boneFlags[ *pbone ] & pBonejobSPU->boneMask) )
|
|
continue;
|
|
|
|
// or if its relative bone isn't going to be calculated
|
|
if( ikrule.bone >= 0 && !(pBonejobSPU->boneFlags[ ikrule.bone ] & pBonejobSPU->boneMask) )
|
|
continue;
|
|
|
|
#if !defined( _CERT )
|
|
#if defined(__SPU__)
|
|
if( g_addDep_numIKRules >= MAX_IKRULES )
|
|
{
|
|
spu_printf( "numIkRules:%d >= MAX_IKRULES:%d\n", g_addDep_numIKRules, MAX_IKRULES );
|
|
__asm volatile ("stopd $0,$0,$0");
|
|
}
|
|
#else
|
|
AssertFatal( g_addDep_numIKRules < MAX_IKRULES );
|
|
#endif
|
|
#endif // !_CERT
|
|
|
|
ikrule.flRuleWeight = flWeight;
|
|
|
|
// rest will be done on PPU, append to list of potential ikrules to add
|
|
g_addDep_IKRules[ g_addDep_numIKRules++ ] = ikrule;
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: build boneToWorld transforms for a specific bone
|
|
//-----------------------------------------------------------------------------
|
|
void CIKContext_PS3::BuildBoneChain(
|
|
const bonejob_SPU *pBonejob,
|
|
const BoneVector pos[],
|
|
const BoneQuaternion q[],
|
|
int iBone,
|
|
matrix3x4a_t *pBoneToWorld,
|
|
CBoneBitList_PS3 &boneComputed )
|
|
{
|
|
::BuildBoneChain_PS3( pBonejob->boneParent, m_rootxform, pos, q, iBone, pBoneToWorld, boneComputed );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: turn a specific bones boneToWorld transform into a pos and q in parents bonespace
|
|
//-----------------------------------------------------------------------------
|
|
void SolveBone_PS3(
|
|
int *pBoneParent,
|
|
int iBone,
|
|
matrix3x4a_t *pBoneToWorld,
|
|
BoneVector pos[],
|
|
BoneQuaternion q[]
|
|
)
|
|
{
|
|
int iParent = pBoneParent[ iBone ];
|
|
|
|
matrix3x4a_t worldToBone;
|
|
MatrixInvert_PS3( pBoneToWorld[iParent], worldToBone );
|
|
|
|
matrix3x4a_t local;
|
|
ConcatTransforms_Aligned_PS3( worldToBone, pBoneToWorld[iBone], local );
|
|
|
|
MatrixAngles_PS3( local, q[iBone], pos[iBone] );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose:
|
|
//-----------------------------------------------------------------------------
|
|
void CIKContext_PS3::AddAutoplayLocks( bonejob_SPU *pBonejob, BoneVector pos[], BoneQuaternion q[] )
|
|
{
|
|
SNPROF_ANIM("CIKContext_PS3::AddAutoplayLocks");
|
|
|
|
// skip all array access if no autoplay locks.
|
|
if( pBonejob->numikAutoplayLocks == 0 )
|
|
{
|
|
return;
|
|
}
|
|
|
|
byte ls_iklock[ ROUNDUPTONEXT16B( sizeof(mstudioiklock_t_PS3) ) ] ALIGN16;
|
|
byte ls_ikchain[ ROUNDUPTONEXT16B( sizeof(mstudioikchain_t_PS3) ) ] ALIGN16;
|
|
byte ls_iklink0[ ROUNDUPTONEXT16B( (sizeof(int) + sizeof(Vector)) ) ] ALIGN16;
|
|
byte ls_bone1[ sizeof(int) + 0x10 ] ALIGN16;
|
|
byte ls_bone2[ sizeof(int) + 0x10 ] ALIGN16;
|
|
|
|
|
|
matrix3x4a_t *boneToWorld = (matrix3x4a_t *)PushLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
CBoneBitList_PS3 boneComputed;
|
|
|
|
#if !defined( _CERT )
|
|
#if defined(__SPU__)
|
|
if( pBonejob->numikAutoplayLocks >= MAX_IKLOCKS )
|
|
{
|
|
spu_printf( "pBonejob->numikAutoplayLocks:%d >= MAX_IKLOCKS:%d\n", pBonejob->numikAutoplayLocks, MAX_IKLOCKS );
|
|
__asm volatile ("stopd $0,$0,$0");
|
|
}
|
|
#else
|
|
AssertFatal( pBonejob->numikAutoplayLocks < MAX_IKLOCKS );
|
|
#endif
|
|
#endif // !_CERT
|
|
|
|
memset( m_ikLock, 0, sizeof( ikcontextikrule_t_PS3 ) * pBonejob->numikAutoplayLocks );
|
|
|
|
for( int i = 0; i < pBonejob->numikAutoplayLocks; i++ )
|
|
{
|
|
mstudioiklock_t_PS3 *pLock = (mstudioiklock_t_PS3 *)SPUmemcpy_UnalignedGet( ls_iklock, (uint32)pBonejob->pEA_IKAutoplayLocks[i], sizeof(mstudioiklock_t_PS3) );
|
|
mstudioikchain_t_PS3 *pEA_chain = (mstudioikchain_t_PS3 *)pBonejob->pEA_studiohdr_ikchains + pLock->chain;
|
|
mstudioikchain_t_PS3 *pLS_chain = (mstudioikchain_t_PS3 *)SPUmemcpy_UnalignedGet( ls_ikchain, (uint32)pEA_chain, sizeof(mstudioikchain_t_PS3) );
|
|
mstudioiklink_t_PS3 *pEA_iklink = pLS_chain->pLink( pEA_chain, 2 );
|
|
|
|
int *pbone2 = (int *)SPUmemcpy_UnalignedGet( ls_bone2, (uint32)pEA_iklink, sizeof(int) );
|
|
int bone = *pbone2;
|
|
|
|
if( !( pBonejob->boneFlags[ bone ] & m_boneMask ) )
|
|
continue;
|
|
|
|
// eval current ik'd bone
|
|
BuildBoneChain( pBonejob, pos, q, bone, boneToWorld, boneComputed );
|
|
|
|
ikcontextikrule_t_PS3 &ikrule = m_ikLock[ i ];
|
|
|
|
ikrule.chain = pLock->chain;
|
|
ikrule.slot = i;
|
|
ikrule.type = IK_WORLD;
|
|
|
|
MatrixAngles_PS3( boneToWorld[ bone ], ikrule.q, ikrule.pos );
|
|
|
|
pEA_iklink = pLS_chain->pLink( pEA_chain, 0 );
|
|
mstudioiklink_t_PS3 *plink0 = (mstudioiklink_t_PS3 *)SPUmemcpy_UnalignedGet( ls_iklink0, (uint32)pEA_iklink, sizeof(int) + sizeof(Vector) );
|
|
|
|
// save off current knee direction
|
|
if( plink0->kneeDir.LengthSqr() > 0.0f )
|
|
{
|
|
pEA_iklink = pLS_chain->pLink( pEA_chain, 1 );
|
|
int *pbone1 = (int *)SPUmemcpy_UnalignedGet_MustSync( ls_bone1, (uint32)pEA_iklink, sizeof(int), DMATAG_ANIM );
|
|
|
|
VectorRotate_PS3( plink0->kneeDir, boneToWorld[ plink0->bone ], ikrule.kneeDir );
|
|
|
|
SPUmemcpy_Sync( 1<<DMATAG_ANIM );
|
|
MatrixPosition_PS3( boneToWorld[ *pbone1 ], ikrule.kneePos );
|
|
}
|
|
else
|
|
{
|
|
ikrule.kneeDir.Init( );
|
|
}
|
|
}
|
|
|
|
|
|
PopLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//
|
|
//-----------------------------------------------------------------------------
|
|
void CIKContext_PS3::AddSequenceLocks( bonejob_SPU *pBonejob, accumposeentry_SPU* pPoseEntry, BoneVector pos[], BoneQuaternion q[] )
|
|
{
|
|
SNPROF_ANIM("CIKContext_PS3::AddSequenceLocks");
|
|
|
|
matrix3x4a_t *boneToWorld = (matrix3x4a_t *)PushLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
|
|
CBoneBitList_PS3 boneComputed;
|
|
|
|
memset( m_ikLock, 0, sizeof( ikcontextikrule_t_PS3 ) * pPoseEntry->seqdesc_numiklocks );
|
|
|
|
byte ls_ikchain[ ROUNDUPTONEXT16B( sizeof(mstudioikchain_t_PS3) ) ] ALIGN16;
|
|
byte ls_iklink0[ ROUNDUPTONEXT16B( (sizeof(int) + sizeof(Vector)) ) ] ALIGN16;
|
|
byte ls_bone2[ sizeof(int) + 0x10 ] ALIGN16;
|
|
|
|
// fetch all iklocks
|
|
byte ls_iklocks[ (ROUNDUPTONEXT16B( sizeof(mstudioiklock_t_PS3) ) * MAX_IKLOCKS) ] ALIGN16;
|
|
mstudioiklock_t_PS3 *piklocks = (mstudioiklock_t_PS3 *)SPUmemcpy_UnalignedGet( ls_iklocks, (uint32)pPoseEntry->pEA_seqdesc_iklocks, sizeof(mstudioiklock_t_PS3) * pPoseEntry->seqdesc_numiklocks );
|
|
|
|
for (int i = 0; i < pPoseEntry->seqdesc_numiklocks; i++)
|
|
{
|
|
mstudioiklock_t_PS3 *pLock = &piklocks[i]; //seqdesc.pIKLock( i );
|
|
mstudioikchain_t_PS3 *pEA_chain = (mstudioikchain_t_PS3 *)pBonejob->pEA_studiohdr_ikchains + pLock->chain;
|
|
mstudioikchain_t_PS3 *pLS_chain = (mstudioikchain_t_PS3 *)SPUmemcpy_UnalignedGet( ls_ikchain, (uint32)pEA_chain, sizeof(mstudioikchain_t_PS3) );
|
|
mstudioiklink_t_PS3 *pEA_iklink = pLS_chain->pLink( pEA_chain, 2 );
|
|
|
|
int *pbone = (int *)SPUmemcpy_UnalignedGet( ls_bone2, (uint32)pEA_iklink, sizeof(int) );
|
|
int bone = *pbone;//pLS_chain->pLink( pEA_chain, 2 )->bone; //pPoseEntry->ikChains[ i ].bone2;
|
|
|
|
if( !( pBonejob->boneFlags[ bone ] & m_boneMask ) )
|
|
continue;
|
|
|
|
// eval current ik'd bone
|
|
BuildBoneChain( pBonejob, pos, q, bone, boneToWorld, boneComputed );
|
|
|
|
ikcontextikrule_t_PS3 &ikrule = m_ikLock[ i ];
|
|
ikrule.chain = i;
|
|
ikrule.slot = i;
|
|
ikrule.type = IK_WORLD;
|
|
|
|
MatrixAngles_PS3( boneToWorld[ bone ], ikrule.q, ikrule.pos );
|
|
|
|
pEA_iklink = pLS_chain->pLink( pEA_chain, 0 );
|
|
mstudioiklink_t_PS3 *plink0 = (mstudioiklink_t_PS3 *)SPUmemcpy_UnalignedGet( ls_iklink0, (uint32)pEA_iklink, sizeof(int) + sizeof(Vector) );
|
|
|
|
// save off current knee direction
|
|
if( plink0->kneeDir.LengthSqr() > 0.0f )
|
|
{
|
|
VectorRotate_PS3( plink0->kneeDir, boneToWorld[ plink0->bone ], ikrule.kneeDir );
|
|
}
|
|
else
|
|
{
|
|
ikrule.kneeDir.Init( );
|
|
}
|
|
}
|
|
|
|
PopLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
}
|
|
|
|
//----------------------------------------------------------------------------------------------------------------------------------
|
|
// SolveDependencies path for SPU not currently supported, code here for reference
|
|
//----------------------------------------------------------------------------------------------------------------------------------
|
|
|
|
#if 0
|
|
matrix3x4a_t g_boneToWorld[ MAXSTUDIOBONES_PS3 ] ALIGN128;
|
|
BoneVector g_pos[ MAXSTUDIOBONES_PS3 ] ALIGN128;
|
|
BoneQuaternion g_q[ MAXSTUDIOBONES_PS3 ] ALIGN128;
|
|
|
|
struct posq
|
|
{
|
|
Vector pos;
|
|
Quaternion q;
|
|
};
|
|
|
|
|
|
void RunSolveDependenciesJob_SPU( bonejob_SPU_2 *pBonejob2 )
|
|
{
|
|
}
|
|
|
|
void RunSolveDependenciesJob_PPU( bonejob_SPU_2 *pBonejob2 )
|
|
{
|
|
SNPROF_ANIM("RunSolveDependenciesJob_PPU");
|
|
|
|
BoneVector *pos = (BoneVector *)pBonejob2->pEA_pos;
|
|
BoneQuaternion *q = (BoneQuaternion *)pBonejob2->pEA_q;
|
|
matrix3x4a_t *boneToWorld = (matrix3x4a_t *)pBonejob2->pEA_bones;
|
|
|
|
CBoneBitList_PS3 boneComputed;
|
|
|
|
|
|
// src
|
|
memcpy( g_pos, pos, sizeof(BoneVector) * pBonejob2->studiohdr_numbones );
|
|
memcpy( g_q, q, sizeof(BoneQuaternion) * pBonejob2->studiohdr_numbones );
|
|
|
|
memcpy( &boneComputed.m_markedBones, pBonejob2->pEA_boneComputed, sizeof(byte) * pBonejob2->studiohdr_numbones );
|
|
|
|
// just copy all bones for now, dma list later
|
|
//memcpy( g_boneToWorld, boneToWorld, sizeof(matrix3x4a_t) * pBonejob2->studiohdr_numbones );
|
|
int lp;
|
|
|
|
// copy matrices that are up to date
|
|
for( lp = 0; lp < pBonejob2->studiohdr_numbones; lp++ )
|
|
{
|
|
if( boneComputed.IsBoneMarked(lp) )
|
|
{
|
|
memcpy( &g_boneToWorld[lp], &boneToWorld[lp], sizeof(matrix3x4a_t) );
|
|
}
|
|
}
|
|
|
|
SolveDependencies_SPU( pBonejob2, boneComputed );
|
|
|
|
// copy results back (boneToWorld and boneComputed)
|
|
memcpy( pBonejob2->pEA_boneComputed, &boneComputed.m_markedBones, sizeof(byte) * pBonejob2->studiohdr_numbones );
|
|
|
|
// build dma list to only copy computed matrices
|
|
for( lp = 0; lp < pBonejob2->studiohdr_numbones; lp++ )
|
|
{
|
|
if( boneComputed.IsBoneMarked(lp) )
|
|
{
|
|
memcpy( &boneToWorld[lp], &g_boneToWorld[lp], sizeof(matrix3x4a_t) );
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void SolveDependencies_SPU( bonejob_SPU_2 *pBonejob2, CBoneBitList_PS3 &boneComputed )
|
|
{
|
|
SNPROF_ANIM( "SolveDependencies_SPU" );
|
|
|
|
matrix3x4a_t worldTarget;
|
|
int i, j;
|
|
|
|
ikchainresult_t_PS3 chainResult[32];
|
|
|
|
// assume all these structures contain valid data
|
|
matrix3x4a_t *boneToWorld = g_boneToWorld;
|
|
BoneVector *pos = g_pos;
|
|
BoneQuaternion *q = g_q;
|
|
|
|
|
|
// init chain rules
|
|
for( i = 0; i < pBonejob2->studiohdr_numikchains; i++ )
|
|
{
|
|
int bone = pBonejob2->ikChains[ i ].bone2;
|
|
ikchainresult_t_PS3 *pChainResult = &chainResult[ i ];
|
|
|
|
pChainResult->target = -1;
|
|
pChainResult->flWeight = 0.0f;
|
|
|
|
// don't bother with chain if the bone isn't going to be calculated
|
|
if( bone == -1 )
|
|
continue;
|
|
|
|
// eval current ik'd bone
|
|
BuildBoneChain_PS3( pBonejob2->boneParent, pBonejob2->rootxform, pos, q, bone, boneToWorld, boneComputed );
|
|
|
|
MatrixAngles_PS3( boneToWorld[bone], pChainResult->q, pChainResult->pos );
|
|
}
|
|
|
|
ikcontextikrule_t_PS3 ikrule[2] ALIGN16;
|
|
|
|
for( i = 0; i < pBonejob2->numikchainElements; i++ )
|
|
{
|
|
void *pEA_Rule = pBonejob2->ikchainElement_rules[ i ];
|
|
|
|
ikcontextikrule_t_PS3 *pRule = (ikcontextikrule_t_PS3 *)SPUmemcpy_UnalignedGet( ikrule, (uint32)pEA_Rule, sizeof(ikcontextikrule_t_PS3) );
|
|
ikchainresult_t_PS3 *pChainResult = &chainResult[ pRule->chain ];
|
|
|
|
pChainResult->target = -1;
|
|
|
|
switch( pRule->type )
|
|
{
|
|
case IK_SELF:
|
|
{
|
|
// xform IK target error into world space
|
|
matrix3x4a_t local;
|
|
|
|
QuaternionMatrix_PS3( pRule->q, pRule->pos, local );
|
|
|
|
// eval target bone space
|
|
if (pRule->bone != -1)
|
|
{
|
|
BuildBoneChain_PS3( pBonejob2->boneParent, pBonejob2->rootxform, pos, q, pRule->bone, boneToWorld, boneComputed );
|
|
ConcatTransforms_Aligned_PS3( boneToWorld[pRule->bone], local, worldTarget );
|
|
}
|
|
else
|
|
{
|
|
ConcatTransforms_Aligned_PS3( pBonejob2->rootxform, local, worldTarget );
|
|
}
|
|
|
|
float flWeight = pRule->flWeight * pRule->flRuleWeight;
|
|
pChainResult->flWeight = pChainResult->flWeight * (1.0f - flWeight) + flWeight;
|
|
|
|
BoneVector p2;
|
|
BoneQuaternion q2;
|
|
|
|
// target p and q
|
|
MatrixAngles_PS3( worldTarget, q2, p2 );
|
|
|
|
// debugLine( pChainResult->pos, p2, 0, 0, 255, true, 0.1 );
|
|
|
|
// blend in position and angles
|
|
pChainResult->pos = pChainResult->pos * (1.0f - flWeight) + p2 * flWeight;
|
|
QuaternionSlerp_PS3( pChainResult->q, q2, flWeight, pChainResult->q );
|
|
}
|
|
break;
|
|
case IK_RELEASE:
|
|
{
|
|
// move target back towards original location
|
|
float flWeight = pRule->flWeight * pRule->flRuleWeight;
|
|
int bone = pBonejob2->ikchainElement_bones[ i ];
|
|
|
|
Vector p2;
|
|
Quaternion q2;
|
|
|
|
BuildBoneChain_PS3( pBonejob2->boneParent, pBonejob2->rootxform, pos, q, bone, boneToWorld, boneComputed );
|
|
MatrixAngles_PS3( boneToWorld[bone], q2, p2 );
|
|
|
|
// blend in position and angles
|
|
pChainResult->pos = pChainResult->pos * (1.0f - flWeight) + p2 * flWeight;
|
|
QuaternionSlerp_PS3( pChainResult->q, q2, flWeight, pChainResult->q );
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
CIKTarget_PS3 target[2] ALIGN16;
|
|
|
|
for (i = 0; i < pBonejob2->iktargetcount; i++)
|
|
{
|
|
void *pEA_Target = pBonejob2->iktargets[ i ];
|
|
CIKTarget_PS3 *pTarget = (CIKTarget_PS3 *)SPUmemcpy_UnalignedGet( target, (uint32)pEA_Target, sizeof(CIKTarget_PS3) );
|
|
|
|
if( pTarget->est.flWeight > 0.0f )
|
|
{
|
|
matrix3x4a_t worldFootpad;
|
|
matrix3x4a_t local;
|
|
|
|
//mstudioikchain_t *pchain = m_pStudioHdr->pIKChain( m_target[i].chain );
|
|
ikchainresult_t_PS3 *pChainResult = &chainResult[ pTarget->chain ];
|
|
|
|
AngleMatrix_PS3(pTarget->offset.q, pTarget->offset.pos, local );
|
|
|
|
AngleMatrix_PS3( pTarget->est.q, pTarget->est.pos, worldFootpad );
|
|
|
|
ConcatTransforms_Aligned_PS3( worldFootpad, local, worldTarget );
|
|
|
|
BoneVector p2;
|
|
BoneQuaternion q2;
|
|
|
|
// target p and q
|
|
MatrixAngles_PS3( worldTarget, q2, p2 );
|
|
|
|
// MatrixAngles( worldTarget, pChainResult->q, pChainResult->pos );
|
|
|
|
// blend in position and angles
|
|
pChainResult->flWeight = pTarget->est.flWeight;
|
|
pChainResult->pos = pChainResult->pos * (1.0f - pChainResult->flWeight ) + p2 * pChainResult->flWeight;
|
|
QuaternionSlerp_PS3( 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 < pBonejob2->studiohdr_numikchains; i++)
|
|
{
|
|
ikchainresult_t_PS3 *pChainResult = &chainResult[ i ];
|
|
ikChain_SPU *pchain = &pBonejob2->ikChains[ i ];
|
|
|
|
if( pChainResult->flWeight > 0.0f )
|
|
{
|
|
Vector tmp;
|
|
MatrixPosition_PS3( boneToWorld[ pchain->bone2 ], tmp );
|
|
|
|
// do exact IK solution
|
|
// FIXME: once per link!
|
|
if( Studio_SolveIK_PS3( pchain, pChainResult->pos, boneToWorld ) )
|
|
{
|
|
Vector p3;
|
|
MatrixGetColumn_PS3( boneToWorld[ pchain->bone2 ], 3, p3 );
|
|
QuaternionMatrix_PS3( pChainResult->q, p3, boneToWorld[ pchain->bone2 ] );
|
|
|
|
// rebuild chain
|
|
// FIXME: is this needed if everyone past this uses the boneToWorld array?
|
|
SolveBone_PS3( pBonejob2->boneParent, pchain->bone2, boneToWorld, pos, q );
|
|
SolveBone_PS3( pBonejob2->boneParent, pchain->bone1, boneToWorld, pos, q );
|
|
SolveBone_PS3( pBonejob2->boneParent, pchain->bone0, boneToWorld, pos, q );
|
|
}
|
|
else
|
|
{
|
|
// FIXME: need to invalidate the targets that forced this...
|
|
if( pChainResult->target != -1 )
|
|
{
|
|
byte *pEA_Target = (byte *)pBonejob2->iktargets[ pChainResult->target ];
|
|
// only really need to get deltaPos, deltaQ
|
|
posq deltaPosQ[ 2 ] ALIGN16;
|
|
int offset_deltaPosQ = uint32(&target[0].latched.deltaPos) - uint32(&target[0]);
|
|
|
|
posq *pTarget_deltaPosQ;
|
|
|
|
pTarget_deltaPosQ = (posq *)SPUmemcpy_UnalignedGet( deltaPosQ, (uint32)(pEA_Target + offset_deltaPosQ), sizeof( posq ) );
|
|
|
|
VectorScale_PS3( pTarget_deltaPosQ->pos, 0.8f, pTarget_deltaPosQ->pos );
|
|
QuaternionScale_PS3( pTarget_deltaPosQ->q, 0.8f, pTarget_deltaPosQ->q );
|
|
|
|
// only really need to push back deltaPos, deltaQ
|
|
SPUmemcpy_UnalignedPut( pTarget_deltaPosQ, (uint32)(pEA_Target + offset_deltaPosQ), sizeof( posq ) );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose:
|
|
//-----------------------------------------------------------------------------
|
|
void CIKContext_PS3::SolveAutoplayLocks(
|
|
bonejob_SPU *pBonejob,
|
|
BoneVector pos[],
|
|
BoneQuaternion q[]
|
|
)
|
|
{
|
|
SNPROF_ANIM("CIKContext_PS3::SolveAutoplayLocks");
|
|
|
|
matrix3x4a_t *boneToWorld = (matrix3x4a_t *)PushLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
|
|
CBoneBitList_PS3 boneComputed;
|
|
|
|
int i;
|
|
|
|
byte ls_iklock[ ROUNDUPTONEXT16B( sizeof(mstudioiklock_t_PS3) ) ] ALIGN16;
|
|
|
|
for( i = 0; i < pBonejob->numikAutoplayLocks; i++ )
|
|
{
|
|
mstudioiklock_t_PS3 *pLock = (mstudioiklock_t_PS3 *)SPUmemcpy_UnalignedGet( ls_iklock, (uint32)pBonejob->pEA_IKAutoplayLocks[i], sizeof(mstudioiklock_t_PS3) );
|
|
|
|
SolveLock( pBonejob, pLock, i, pos, q, boneToWorld, boneComputed );
|
|
}
|
|
|
|
PopLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
//
|
|
//-----------------------------------------------------------------------------
|
|
void CIKContext_PS3::SolveSequenceLocks(
|
|
bonejob_SPU *pBonejob,
|
|
accumposeentry_SPU* pPoseEntry,
|
|
BoneVector pos[],
|
|
BoneQuaternion q[]
|
|
)
|
|
{
|
|
SNPROF_ANIM("CIKContext_PS3::SolveSequenceLocks");
|
|
|
|
matrix3x4a_t *boneToWorld = (matrix3x4a_t *)PushLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
|
|
CBoneBitList_PS3 boneComputed;
|
|
|
|
int i;
|
|
|
|
// fetch all iklocks
|
|
byte ls_iklocks[ (ROUNDUPTONEXT16B(sizeof(mstudioiklock_t_PS3)) * MAX_IKLOCKS) ] ALIGN16;
|
|
mstudioiklock_t_PS3 *piklocks = (mstudioiklock_t_PS3 *)SPUmemcpy_UnalignedGet( ls_iklocks, (uint32)pPoseEntry->pEA_seqdesc_iklocks, sizeof(mstudioiklock_t_PS3) * pPoseEntry->seqdesc_numiklocks );
|
|
|
|
|
|
for( i = 0; i < pPoseEntry->seqdesc_numiklocks; i++ )
|
|
{
|
|
//mstudioiklock_t_PS3 *pLock = &pPoseEntry->ikLocks[ i ];
|
|
mstudioiklock_t_PS3 *pLock = &piklocks[i];
|
|
|
|
SolveLock( pBonejob, pLock, i, pos, q, boneToWorld, boneComputed );
|
|
}
|
|
|
|
PopLSStack( sizeof(matrix3x4a_t) * pBonejob->maxBones );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose:
|
|
//-----------------------------------------------------------------------------
|
|
void CIKContext_PS3::SolveLock(
|
|
bonejob_SPU *pBonejob,
|
|
const mstudioiklock_t_PS3 *pLock,
|
|
int i,
|
|
BoneVector pos[],
|
|
BoneQuaternion q[],
|
|
matrix3x4a_t boneToWorld[],
|
|
CBoneBitList_PS3 &boneComputed
|
|
)
|
|
{
|
|
byte ls_ikchain[ ROUNDUPTONEXT16B(sizeof(mstudioikchain_t_PS3)) ] ALIGN16;
|
|
byte ls_iklink0[ ROUNDUPTONEXT16B( (sizeof(int) + sizeof(Vector)) ) ] ALIGN16;
|
|
byte ls_bone1[ sizeof(int) + 0x10 ] ALIGN16;
|
|
byte ls_bone2[ sizeof(int) + 0x10 ] ALIGN16;
|
|
|
|
|
|
mstudioikchain_t_PS3 *pEA_chain = (mstudioikchain_t_PS3 *)pBonejob->pEA_studiohdr_ikchains + pLock->chain;
|
|
mstudioikchain_t_PS3 *pLS_chain = (mstudioikchain_t_PS3 *)SPUmemcpy_UnalignedGet( ls_ikchain, (uint32)pEA_chain, sizeof(mstudioikchain_t_PS3) );
|
|
mstudioiklink_t_PS3 *pEA_iklink0 = pLS_chain->pLink( pEA_chain, 0 );
|
|
mstudioiklink_t_PS3 *pEA_iklink1 = pLS_chain->pLink( pEA_chain, 1 );
|
|
mstudioiklink_t_PS3 *pEA_iklink2 = pLS_chain->pLink( pEA_chain, 2 );
|
|
|
|
mstudioiklink_t_PS3 *plink0 = (mstudioiklink_t_PS3 *)SPUmemcpy_UnalignedGet_MustSync( ls_iklink0, (uint32)pEA_iklink0, sizeof(int) + sizeof(Vector), DMATAG_ANIM );
|
|
int *pbone1 = (int *)SPUmemcpy_UnalignedGet_MustSync( ls_bone1, (uint32)pEA_iklink1, sizeof(int), DMATAG_ANIM );
|
|
int *pbone2 = (int *)SPUmemcpy_UnalignedGet( ls_bone2, (uint32)pEA_iklink2, sizeof(int) );
|
|
|
|
int bone = *pbone2;//pChain->bone2;
|
|
|
|
// don't bother with iklock if the bone isn't going to be calculated
|
|
if( !( pBonejob->boneFlags[ bone ] & m_boneMask ) )
|
|
{
|
|
SPUmemcpy_Sync( 1<<DMATAG_ANIM );
|
|
return;
|
|
}
|
|
|
|
// eval current ik'd bone
|
|
BuildBoneChain( pBonejob, pos, q, bone, boneToWorld, boneComputed );
|
|
|
|
BoneVector p1, p2, p3;
|
|
BoneQuaternion q2, q3;
|
|
|
|
// current p and q
|
|
MatrixPosition_PS3( boneToWorld[ bone ], p1 );
|
|
|
|
// blend in position
|
|
p3 = p1 * (1.0f - pLock->flPosWeight ) + m_ikLock[ i ].pos * pLock->flPosWeight;
|
|
|
|
SPUmemcpy_Sync( 1<<DMATAG_ANIM );
|
|
|
|
// do exact IK solution
|
|
if( m_ikLock[ i ].kneeDir.LengthSqr() > 0.0f )
|
|
{
|
|
Studio_SolveIK_PS3( plink0->bone, *pbone1, *pbone2, p3, m_ikLock[ i ].kneePos, m_ikLock[ i ].kneeDir, boneToWorld );
|
|
}
|
|
else
|
|
{
|
|
Studio_SolveIK_PS3( plink0->kneeDir, plink0->bone, *pbone1, *pbone2, p3, boneToWorld );
|
|
}
|
|
|
|
// slam orientation
|
|
MatrixPosition_PS3( boneToWorld[ bone ], p3 );
|
|
QuaternionMatrix_PS3( m_ikLock[ i ].q, p3, boneToWorld[ bone ] );
|
|
|
|
// rebuild chain
|
|
q2 = q[ bone ];
|
|
SolveBone_PS3( pBonejob->boneParent, *pbone2, boneToWorld, pos, q );
|
|
QuaternionSlerp_PS3( q[ bone ], q2, pLock->flLocalQWeight, q[ bone ] );
|
|
|
|
SolveBone_PS3( pBonejob->boneParent, *pbone1, boneToWorld, pos, q );
|
|
SolveBone_PS3( pBonejob->boneParent, plink0->bone, boneToWorld, pos, q );
|
|
}
|
|
|
|
|
|
#define KNEEMAX_EPSILON (0.9998f) // (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_PS3( int8 iThigh, int8 iKnee, int8 iFoot, Vector &targetFoot, matrix3x4a_t *pBoneToWorld )
|
|
{
|
|
Vector worldFoot, worldKnee, worldThigh;
|
|
|
|
MatrixPosition_PS3( pBoneToWorld[ iThigh ], worldThigh );
|
|
MatrixPosition_PS3( pBoneToWorld[ iKnee ], worldKnee );
|
|
MatrixPosition_PS3( pBoneToWorld[ iFoot ], worldFoot );
|
|
|
|
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;
|
|
ikHalf = (worldFoot - worldThigh) * (l1 / l3);
|
|
|
|
// FIXME: what to do when the knee completely straight?
|
|
Vector ikKneeDir;
|
|
ikKneeDir = ikKnee - ikHalf;
|
|
VectorNormalize_PS3( ikKneeDir );
|
|
|
|
return Studio_SolveIK_PS3( iThigh, iKnee, iFoot, targetFoot, worldKnee, ikKneeDir, pBoneToWorld );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: Realign the matrix so that its X axis points along the desired axis.
|
|
//-----------------------------------------------------------------------------
|
|
void Studio_AlignIKMatrix_PS3( matrix3x4a_t &mMat, const Vector &vAlignTo )
|
|
{
|
|
Vector tmp1, tmp2, tmp3;
|
|
|
|
// Column 0 (X) becomes the vector.
|
|
tmp1 = vAlignTo;
|
|
VectorNormalize_PS3( tmp1 );
|
|
MatrixSetColumn_PS3( tmp1, 0, mMat );
|
|
|
|
// Column 1 (Y) is the cross of the vector and column 2 (Z).
|
|
MatrixGetColumn_PS3( mMat, 2, tmp3 );
|
|
tmp2 = tmp3.Cross( tmp1 );
|
|
VectorNormalize_PS3( tmp2 );
|
|
// FIXME: check for X being too near to Z
|
|
MatrixSetColumn_PS3( tmp2, 1, mMat );
|
|
|
|
// Column 2 (Z) is the cross of columns 0 (X) and 1 (Y).
|
|
tmp3 = tmp1.Cross( tmp2 );
|
|
MatrixSetColumn_PS3( tmp3, 2, mMat );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Purpose: Solve Knee position for a known hip and foot location, and a known knee direction
|
|
//-----------------------------------------------------------------------------
|
|
bool Studio_SolveIK_PS3( int8 iThigh, int8 iKnee, int8 iFoot, Vector &targetFoot, Vector &targetKneePos, Vector &targetKneeDir, matrix3x4a_t *pBoneToWorld )
|
|
{
|
|
Vector worldFoot, worldKnee, worldThigh;
|
|
|
|
MatrixPosition_PS3( pBoneToWorld[ iThigh ], worldThigh );
|
|
MatrixPosition_PS3( pBoneToWorld[ iKnee ], worldKnee );
|
|
MatrixPosition_PS3( pBoneToWorld[ iFoot ], worldFoot );
|
|
|
|
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.0f;
|
|
|
|
ikTargetKnee = ikKnee + targetKneeDir * d;
|
|
|
|
// too far away? (0.9998 is about 1 degree)
|
|
if( ikFoot.Length() > (l1 + l2) * KNEEMAX_EPSILON )
|
|
{
|
|
VectorNormalize_PS3( ikFoot );
|
|
VectorScale_PS3( ikFoot, (l1 + l2) * KNEEMAX_EPSILON, ikFoot );
|
|
}
|
|
|
|
// too close?
|
|
// limit distance to about an 80 degree knee bend
|
|
float minDist = MAX( fabs(l1 - l2) * 1.15, MIN( l1, l2 ) * 0.15f );
|
|
if( ikFoot.Length() < minDist )
|
|
{
|
|
// too close to get an accurate vector, just use original vector
|
|
ikFoot = (worldFoot - worldThigh);
|
|
VectorNormalize_PS3( ikFoot );
|
|
VectorScale_PS3( ikFoot, minDist, ikFoot );
|
|
}
|
|
|
|
CIKSolver_PS3 ik;
|
|
|
|
if( ik.solve( l1, l2, ikFoot.Base(), ikTargetKnee.Base(), ikKnee.Base() ) )
|
|
{
|
|
matrix3x4a_t& mWorldThigh = pBoneToWorld[ iThigh ];
|
|
matrix3x4a_t& mWorldKnee = pBoneToWorld[ iKnee ];
|
|
matrix3x4a_t& mWorldFoot = pBoneToWorld[ iFoot ];
|
|
|
|
// build transformation matrix for thigh
|
|
Studio_AlignIKMatrix_PS3( mWorldThigh, ikKnee );
|
|
VectorAligned kneeToFoot;
|
|
kneeToFoot = ikFoot - ikKnee;
|
|
Studio_AlignIKMatrix_PS3( mWorldKnee, kneeToFoot );
|
|
|
|
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
|
|
{
|
|
return false;
|
|
}
|
|
}
|