// 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);
	}
}
Exemple #2
0
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);
}