// calculate single bone with key blending and callbck calling
void CKinematicsAnimated::CLBone(const CBoneData* bd,CBoneInstance& BONE_INST,const Fmatrix *parent,const CBlendInstance::BlendSVec &Blend, u8 channel_mask /*= (1<<0)*/)
{
u16 SelfID = bd->GetSelfID();
if (LL_GetBoneVisible(SelfID)){
if (BONE_INST.Callback_overwrite){
if (BONE_INST.Callback) BONE_INST.Callback(&BONE_INST);
} else {
CKey R[MAX_CHANNELS][MAX_BLENDED]; //all keys
CKey BK[MAX_CHANNELS][MAX_BLENDED]; //base keys
float BA[MAX_CHANNELS][MAX_BLENDED]; //all factors
int b_counts[MAX_CHANNELS] = {0,0,0,0}; //channel counts
//float BCA[MAX_CHANNELS] = {0,0,0,0}; //channel factors
BlendSVecCIt BI;
for (BI=Blend.begin(); BI!=Blend.end(); BI++)
{
CBlend* B = *BI;
int &b_count = b_counts[B->channel];
CKey* D = &R[B->channel][b_count];
if(!(channel_mask&(1<<B->channel)))
continue;
u8 channel = B->channel;
BA[channel][b_count] = B->blendAmount;
//BCA[channel] += B->blendAmount;
CMotion &M =*LL_GetMotion(B->motionID,SelfID);
Dequantize(*D,*B,M);
QR2Quat( M._keysR[0], BK[channel][b_count].Q );
if(M.test_flag(flTKeyPresent))
QT2T(M._keysT[0] ,M ,BK[channel][b_count].T );
else
BK[channel][b_count].T.set(M._initT);
++b_count;
/// PSGP.blerp (D,&K1,&K2,delta);
}
// Blend them together
CKey channels[MAX_CHANNELS];
float BC [MAX_CHANNELS];
u16 ch_count = 0;
for(u16 j= 0;MAX_CHANNELS>j;++j)
{
if(j!=0&&b_counts[j]==0)
continue;
//data for channel mix cycle based on ch_count
CKey &C = channels[ch_count];
BC[ch_count] = channel_factors[j];//3.f;//BCA[j]*
if(j != 0)
keys_substruct(R[j],BK[j],b_counts[j]);
MixInterlerp( C, R[j], BA[j], b_counts[j] );
++ch_count;
}
CKey Result;
//Mix channels
//MixInterlerp(Result,channels,BCA,ch_count);
MixChannels( Result, channels, BC, ch_count );
Fmatrix RES;
RES.mk_xform (Result.Q,Result.T);
BONE_INST.mTransform.mul_43(*parent,RES);
#ifdef DEBUG
if(!check_scale(RES))
{
VERIFY(check_scale(BONE_INST.mTransform));
}
/*
if(!is_similar(BONE_INST.mPrevTransform,RES,0.3f))
{
Msg("bone %s",*bd->name) ;
}
BONE_INST.mPrevTransform.set(RES);
*/
#endif
/*
if(BONE_INST.mTransform.c.y>10000)
{
Log("BLEND_INST",BLEND_INST.Blend.size());
Log("Bone",LL_BoneName_dbg(SelfID));
Msg("Result.Q %f,%f,%f,%f",Result.Q.x,Result.Q.y,Result.Q.z,Result.Q.w);
Log("Result.T",Result.T);
Log("lp parent",(u32)parent);
Log("parent",*parent);
Log("RES",RES);
Log("mT",BONE_INST.mTransform);
CBlend* B = *BI;
CMotion& M = *LL_GetMotion(B->motionID,SelfID);
float time = B->timeCurrent*float(SAMPLE_FPS);
u32 frame = iFloor(time);
u32 count = M.get_count();
float delta = time-float(frame);
Log("flTKeyPresent",M.test_flag(flTKeyPresent));
Log("M._initT",M._initT);
Log("M._sizeT",M._sizeT);
// translate
if (M.test_flag(flTKeyPresent))
{
CKeyQT* K1t = &M._keysT[(frame+0)%count];
CKeyQT* K2t = &M._keysT[(frame+1)%count];
Fvector T1,T2,Dt;
T1.x = float(K1t->x)*M._sizeT.x+M._initT.x;
T1.y = float(K1t->y)*M._sizeT.y+M._initT.y;
T1.z = float(K1t->z)*M._sizeT.z+M._initT.z;
T2.x = float(K2t->x)*M._sizeT.x+M._initT.x;
T2.y = float(K2t->y)*M._sizeT.y+M._initT.y;
T2.z = float(K2t->z)*M._sizeT.z+M._initT.z;
Dt.lerp (T1,T2,delta);
Msg("K1t %d,%d,%d",K1t->x,K1t->y,K1t->z);
Msg("K2t %d,%d,%d",K2t->x,K2t->y,K2t->z);
Log("count",count);
Log("frame",frame);
Log("T1",T1);
Log("T2",T2);
Log("delta",delta);
Log("Dt",Dt);
}else
{
D->T.set (M._initT);
}
VERIFY(0);
}
*/
if (BONE_INST.Callback) BONE_INST.Callback(&BONE_INST);
}
BONE_INST.mRenderTransform.mul_43(BONE_INST.mTransform,bd->m2b_transform);
}
}
void CKinematics::AddWallmark(const Fmatrix* parent_xform, const Fvector3& start, const Fvector3& dir, ref_shader shader, float size)
{
Fvector S,D,normal = {0,0,0};
// transform ray from world to model
Fmatrix P; P.invert (*parent_xform);
P.transform_tiny (S,start);
P.transform_dir (D,dir);
// find pick point
float dist = flt_max;
BOOL picked = FALSE;
DEFINE_VECTOR (Fobb,OBBVec,OBBVecIt);
OBBVec cache_obb;
cache_obb.resize (LL_BoneCount());
for (u16 k=0; k<LL_BoneCount(); k++){
CBoneData& BD = LL_GetData(k);
if (LL_GetBoneVisible(k)&&!BD.shape.flags.is(SBoneShape::sfNoPickable)){
Fobb& obb = cache_obb[k];
obb.transform (BD.obb,LL_GetBoneInstance(k).mTransform);
if (CDB::TestRayOBB(S,D, obb))
for (u32 i=0; i<children.size(); i++)
if (LL_GetChild(i)->PickBone(normal,dist,S,D,k)) picked=TRUE;
}
}
if (!picked) return;
// calculate contact point
Fvector cp; cp.mad (S,D,dist);
// collect collide boxes
Fsphere test_sphere;
test_sphere.set (cp,size);
U16Vec test_bones;
test_bones.reserve (LL_BoneCount());
for (k=0; k<LL_BoneCount(); k++){
CBoneData& BD = LL_GetData(k);
if (LL_GetBoneVisible(k)&&!BD.shape.flags.is(SBoneShape::sfNoPickable)){
Fobb& obb = cache_obb[k];
if (CDB::TestSphereOBB(test_sphere, obb))
test_bones.push_back(k);
}
}
// find similar wm
for (u32 wm_idx=0; wm_idx<wallmarks.size(); wm_idx++){
intrusive_ptr<CSkeletonWallmark>& wm = wallmarks[wm_idx];
if (wm->Similar(shader,cp,0.02f)){
if (wm_idx<wallmarks.size()-1)
wm = wallmarks.back();
wallmarks.pop_back();
break;
}
}
// ok. allocate wallmark
intrusive_ptr<CSkeletonWallmark> wm = xr_new<CSkeletonWallmark>(this,parent_xform,shader,cp,Device.fTimeGlobal);
wm->m_LocalBounds.set (cp,size*2.f);
wm->XFORM()->transform_tiny (wm->m_Bounds.P,cp);
wm->m_Bounds.R = wm->m_Bounds.R;
Fvector tmp; tmp.invert (D);
normal.add(tmp).normalize ();
// build UV projection matrix
Fmatrix mView,mRot;
BuildMatrix (mView,1/(0.9f*size),normal,cp);
mRot.rotateZ (::Random.randF(deg2rad(-20.f),deg2rad(20.f)));
mView.mulA_43 (mRot);
// fill vertices
for (u32 i=0; i<children.size(); i++){
CSkeletonX* S = LL_GetChild(i);
for (U16It b_it=test_bones.begin(); b_it!=test_bones.end(); b_it++)
S->FillVertices (mView,*wm,normal,size,*b_it);
}
wallmarks.push_back (wm);
}
