C++ Fmatrix::mul_43方法代码示例

void CIKLimb::Calculate(CKinematics* K,const Fmatrix &obj)
{
	SCalculateData cd(this,K,obj);
	m_prev_state_anim=true;
	Collide(&cd);
	if(cd.m_tri)
	{
		Matrix		m	;
		GoalMatrix	(m,&cd)	;
		
#ifdef DEBUG 
		if(m_limb.SetGoal(m,ph_dbg_draw_mask.test(phDbgIKLimits)))
#else
		if(m_limb.SetGoal(m,TRUE))
#endif
		{
			Fmatrix hip;
			CBoneData& BD=K->LL_GetData(m_bones[0]);
			hip.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
			hip.invert();
			float x[7];
			Fvector pos;
			pos.set(K->LL_GetTransform(m_bones[1]).c);
			hip.transform_tiny(pos);
			xm2im.transform_tiny(pos);
			if(m_limb.SolveByPos(cast_fp(pos),x))
			{
						cd.m_angles=x;
						CalculateBones(&cd);
						m_prev_state_anim=false;
			}
		}
	}
}

void CKinematics::DebugRender(Fmatrix& XFORM)
{
	CalculateBones	();

	CBoneData::BoneDebug	dbgLines;
	(*bones)[iRoot]->DebugQuery	(dbgLines);

	Fvector Z;  Z.set(0,0,0);
	Fvector H1; H1.set(0.01f,0.01f,0.01f);
	Fvector H2; H2.mul(H1,2);
	for (u32 i=0; i<dbgLines.size(); i+=2)	{
		Fmatrix& M1 = bone_instances[dbgLines[i]].mTransform;
		Fmatrix& M2 = bone_instances[dbgLines[i+1]].mTransform;

		Fvector P1,P2;
		M1.transform_tiny(P1,Z);
		M2.transform_tiny(P2,Z);
		RCache.dbg_DrawLINE(XFORM,P1,P2,D3DCOLOR_XRGB(0,255,0));

		Fmatrix M;
		M.mul_43(XFORM,M2);
		RCache.dbg_DrawOBB(M,H1,D3DCOLOR_XRGB(255,255,255));
		RCache.dbg_DrawOBB(M,H2,D3DCOLOR_XRGB(255,255,255));
	}

	for (u32 b=0; b<bones->size(); b++)
	{
		Fobb&		obb		= (*bones)[b]->obb;
		Fmatrix&	Mbone	= bone_instances[b].mTransform;
		Fmatrix		Mbox;	obb.xform_get(Mbox);
		Fmatrix		X;		X.mul(Mbone,Mbox);
		Fmatrix		W;		W.mul(XFORM,X);
		RCache.dbg_DrawOBB(W,obb.m_halfsize,D3DCOLOR_XRGB(0,0,255));
	}
}

void 	CIKLimb::BonesCallback2				(CBoneInstance* B)
{
	SCalculateData* D=(SCalculateData*)B->Callback_Param;
	CIKLimb*		L=D->m_limb							;
	float	const	*x=D->m_angles						;
	{
		Fmatrix 	bm;//=B->mTransform					;
		//tmp_pos		.set(bm.c)						;
	//	Fvector		ra;B->mTransform.getXYZ(ra)			;
	//	bm			.setXYZ(-x[4],-x[5],-x[6])				;
		Matrix tbm;
		float t[3];
		
		 t[0] = x[6]; t[1] = x[5]; t[2] = x[4]			;
		EulerEval(ZXY,t,tbm)							;
		IM2XM(tbm,bm)									;
		CKinematics	*K=D->m_K;
		CBoneData& BD=K->LL_GetData(L->m_bones[2])		;
		Fmatrix start;start.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
		Fmatrix inv_start;inv_start.invert(start);
		Fmatrix dif;dif.mul_43(inv_start,B->mTransform);
		Fmatrix ikdif;
		Fvector a;
		dif.getXYZ(a);
		XM2IM(dif,ikdif);

		//B->mTransform.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
		//B->mTransform.mulB_43(bm)						;
		
		B->mTransform.mul_43(start,bm);
#ifdef DEBUG

		if(ph_dbg_draw_mask.test(phDbgDrawIKGoal))
		{
			Fmatrix DBGG;
			DBGG.mul_43(*D->m_obj,B->mTransform);
			DBG_DrawMatrix(DBGG,0.3f);
			
			
			DBGG.mul_43(*D->m_obj,start);
			DBG_DrawMatrix(DBGG,0.3f);

		}
#endif
		//bm.c		.set(tmp_pos)						;
	}
}

IC bool cmp( const Fmatrix &f0, const Fmatrix &f1 )
{
	Fmatrix if0; if0.invert( f0 );
	Fmatrix cm;cm.mul_43( if0, f1 );
	
	Fvector ax; float ang;
	Fquaternion q;
	q.set( cm );
	q.get_axis_angle( ax, ang );

	return ang < cmp_angle && cm.c.square_magnitude() < cmp_ldisp* cmp_ldisp ;
}

Fvector	CScriptGameObject::bone_position	(LPCSTR bone_name) const
{
	u16					bone_id;
	if (xr_strlen(bone_name))
		bone_id			= smart_cast<IKinematics*>(object().Visual())->LL_BoneID(bone_name);
	else
		bone_id			= smart_cast<IKinematics*>(object().Visual())->LL_GetBoneRoot();

	Fmatrix				matrix;
	matrix.mul_43		(object().XFORM(),smart_cast<IKinematics*>(object().Visual())->LL_GetBoneInstance(bone_id).mTransform);
	return				(matrix.c);
}

void CSkeletonX::_Render	(ref_geom& hGeom, u32 vCount, u32 iOffset, u32 pCount)
{
	RCache.stat.r.s_dynamic.add		(vCount);
	switch (RenderMode)
	{
	case RM_SKINNING_SOFT:
		_Render_soft		(hGeom,vCount,iOffset,pCount);
		RCache.stat.r.s_dynamic_sw.add	(vCount);
		break;
	case RM_SINGLE:	
		{
			Fmatrix	W;	W.mul_43	(RCache.xforms.m_w,Parent->LL_GetTransform_R	(u16(RMS_boneid)));
			RCache.set_xform_world	(W);
			RCache.set_Geometry		(hGeom);
			RCache.Render			(D3DPT_TRIANGLELIST,0,0,vCount,iOffset,pCount);
			RCache.stat.r.s_dynamic_inst.add	(vCount);
		}
		break;
	case RM_SKINNING_1B:
	case RM_SKINNING_2B:
	case RM_SKINNING_3B:
	case RM_SKINNING_4B:
		{
			// transfer matrices
			ref_constant			array	= RCache.get_c(s_bones_array_const);
			u32						count	= RMS_bonecount;
			for (u32 mid = 0; mid<count; mid++)	
			{
				Fmatrix&	M				= Parent->LL_GetTransform_R(u16(mid));
				u32			id				= mid*3;
				RCache.set_ca				(&*array,id+0,M._11,M._21,M._31,M._41);
				RCache.set_ca				(&*array,id+1,M._12,M._22,M._32,M._42);
				RCache.set_ca				(&*array,id+2,M._13,M._23,M._33,M._43);
			}

			// render
			RCache.set_Geometry				(hGeom);
			RCache.Render					(D3DPT_TRIANGLELIST,0,0,vCount,iOffset,pCount);
			if (RM_SKINNING_1B==RenderMode)	
				RCache.stat.r.s_dynamic_1B.add	(vCount);
			else
			if (RM_SKINNING_2B==RenderMode)	
				RCache.stat.r.s_dynamic_2B.add	(vCount);
			else
			if (RM_SKINNING_3B==RenderMode)	
				RCache.stat.r.s_dynamic_3B.add	(vCount);
			else
			if (RM_SKINNING_4B==RenderMode)	
				RCache.stat.r.s_dynamic_4B.add	(vCount);
		}
		break;
	}
}

bool CSpaceRestrictorWrapper::inside				(const Fvector &position, float radius) const
{
	Fsphere							sphere;
	sphere.P						= position;
	sphere.R						= radius;

	typedef CShapeData::ShapeVec	ShapeVec;
	ShapeVec::const_iterator		I = object().shapes.begin();
	ShapeVec::const_iterator		E = object().shapes.end();
	for ( ; I != E; ++I) {
		switch ((*I).type) {
			case 0 : {
				Fsphere				temp;
				m_xform.transform_tiny(temp.P,(*I).data.sphere.P);
				temp.R				= (*I).data.sphere.R;
				if (sphere.intersect(temp))
					return			(true);

				continue;
			}
			case 1 : {
				Fmatrix				temp;
				temp.mul_43			(m_xform,(*I).data.box);

				// Build points
				Fvector				vertices;
				Fvector				points[8];
				Fplane				plane;

				vertices.set(-.5f, -.5f, -.5f);	temp.transform_tiny(points[0],vertices);
				vertices.set(-.5f, -.5f, +.5f);	temp.transform_tiny(points[1],vertices);
				vertices.set(-.5f, +.5f, +.5f);	temp.transform_tiny(points[2],vertices);
				vertices.set(-.5f, +.5f, -.5f);	temp.transform_tiny(points[3],vertices);
				vertices.set(+.5f, +.5f, +.5f);	temp.transform_tiny(points[4],vertices);
				vertices.set(+.5f, +.5f, -.5f);	temp.transform_tiny(points[5],vertices);
				vertices.set(+.5f, -.5f, +.5f);	temp.transform_tiny(points[6],vertices);
				vertices.set(+.5f, -.5f, -.5f);	temp.transform_tiny(points[7],vertices);

				plane.build(points[0],points[3],points[5]);	if (plane.classify(sphere.P)>sphere.R) break;
				plane.build(points[1],points[2],points[3]);	if (plane.classify(sphere.P)>sphere.R) break;
				plane.build(points[6],points[5],points[4]);	if (plane.classify(sphere.P)>sphere.R) break;
				plane.build(points[4],points[2],points[1]);	if (plane.classify(sphere.P)>sphere.R) break;
				plane.build(points[3],points[2],points[4]);	if (plane.classify(sphere.P)>sphere.R) break;
				plane.build(points[1],points[0],points[6]);	if (plane.classify(sphere.P)>sphere.R) break;
				return				(true);
			}
			default :				NODEFAULT;
		}
	}

	return							(false);
}

void CSpaceRestrictionShape::fill_shape		(const CCF_Shape::shape_def &shape)
{
	Fvector							start,dest;
	switch (shape.type) {
		case 0 : {
			start.sub				(Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
			dest.add				(Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
			start.add				(m_restrictor->Position());
			dest.add				(m_restrictor->Position());
			break;
		}
		case 1 : {
			Fvector					points[8] = {
				Fvector().set(-.5f,-.5f,-.5f),
				Fvector().set(-.5f,-.5f,+.5f),
				Fvector().set(-.5f,+.5f,-.5f),
				Fvector().set(-.5f,+.5f,+.5f),
				Fvector().set(+.5f,-.5f,-.5f),
				Fvector().set(+.5f,-.5f,+.5f),
				Fvector().set(+.5f,+.5f,-.5f),
				Fvector().set(+.5f,+.5f,+.5f)
			};
			start					= Fvector().set(flt_max,flt_max,flt_max);
			dest					= Fvector().set(flt_min,flt_min,flt_min);
			Fmatrix					Q;
			Q.mul_43				(m_restrictor->XFORM(),shape.data.box);
			Fvector					temp;
			for (int i=0; i<8; ++i) {
                Q.transform_tiny	(temp,points[i]);
				start.x				= _min(start.x,temp.x);
				start.y				= _min(start.y,temp.y);
				start.z				= _min(start.z,temp.z);
				dest.x				= _max(dest.x,temp.x);
				dest.y				= _max(dest.y,temp.y);
				dest.z				= _max(dest.z,temp.z);
			}
			break;
		}
		default : NODEFAULT;
	}
	ai().level_graph().iterate_vertices(start,dest,CBorderMergePredicate(this));

#ifdef DEBUG
	ai().level_graph().iterate_vertices(start,dest,CShapeTestPredicate(this));
#endif
}

bool object::inside			(Fvector const &position) const
{
	CCF_Shape						*shape = static_cast<CCF_Shape*>(collidable.model);
	VERIFY							(shape);

	typedef xr_vector<CCF_Shape::shape_def>	Shapes;
	Shapes::const_iterator			i = shape->shapes.begin();
	Shapes::const_iterator			e = shape->shapes.end();
	for ( ; i != e; ++i) {
		switch ((*i).type) {
			case 0 : {
				if ((*i).data.sphere.P.distance_to(position) <= (*i).data.sphere.R)
					return			(true);

				continue;
			}
			case 1 : {
				Fmatrix				matrix;
				const Fmatrix		&box = (*i).data.box;
				matrix.mul_43		(XFORM(),box);
				Fvector				A,B[8];
				Fplane				plane;
				A.set				(-.5f, -.5f, -.5f);	matrix.transform_tiny(B[0],A);
				A.set				(-.5f, -.5f, +.5f);	matrix.transform_tiny(B[1],A);
				A.set				(-.5f, +.5f, +.5f);	matrix.transform_tiny(B[2],A);
				A.set				(-.5f, +.5f, -.5f);	matrix.transform_tiny(B[3],A);
				A.set				(+.5f, +.5f, +.5f);	matrix.transform_tiny(B[4],A);
				A.set				(+.5f, +.5f, -.5f);	matrix.transform_tiny(B[5],A);
				A.set				(+.5f, -.5f, +.5f);	matrix.transform_tiny(B[6],A);
				A.set				(+.5f, -.5f, -.5f);	matrix.transform_tiny(B[7],A);

				plane.build			(B[0],B[3],B[5]); if (plane.classify(position) <= 0.f) return (true);
				plane.build			(B[1],B[2],B[3]); if (plane.classify(position) <= 0.f) return (true);
				plane.build			(B[6],B[5],B[4]); if (plane.classify(position) <= 0.f) return (true);
				plane.build			(B[4],B[2],B[1]); if (plane.classify(position) <= 0.f) return (true);
				plane.build			(B[3],B[2],B[4]); if (plane.classify(position) <= 0.f) return (true);
				plane.build			(B[1],B[0],B[6]); if (plane.classify(position) <= 0.f) return (true);

				continue;
			}
			default : NODEFAULT;
		}
	}

	return							(false);
}

bool CBone::Pick(float& dist, const Fvector& S, const Fvector& D, const Fmatrix& parent)
{
	Fvector start, dir;
    Fmatrix M; M.mul_43(parent,_LTransform());
    M.invert();
    M.transform_tiny(start,S);
    M.transform_dir(dir,D);
	switch (shape.type){
    case SBoneShape::stBox:		return shape.box.intersect		(start,dir,dist);		
    case SBoneShape::stSphere:  return shape.sphere.intersect	(start,dir,dist);
    case SBoneShape::stCylinder:return shape.cylinder.intersect	(start,dir,dist);
    default:
    	Fsphere S;
        S.P.set(0,0,0);
        S.R=0.025f;
        return S.intersect(start,dir,dist);
    }
}

void CSpaceRestrictorWrapper::fill_shape			(const CShapeData::shape_def &shape)
{
	Fvector							start,dest;
	switch (shape.type) {
		case 0 : {
			start.sub				(Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
			dest.add				(Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
			start.add				(object().o_Position);
			dest.add				(object().o_Position);
			break;
		}
		case 1 : {
			Fvector					points[8] = {
				Fvector().set(-.5f,-.5f,-.5f),
				Fvector().set(-.5f,-.5f,+.5f),
				Fvector().set(-.5f,+.5f,-.5f),
				Fvector().set(-.5f,+.5f,+.5f),
				Fvector().set(+.5f,-.5f,-.5f),
				Fvector().set(+.5f,-.5f,+.5f),
				Fvector().set(+.5f,+.5f,-.5f),
				Fvector().set(+.5f,+.5f,+.5f)
			};
			start					= Fvector().set(flt_max,flt_max,flt_max);
			dest					= Fvector().set(flt_min,flt_min,flt_min);
			Fmatrix					Q;
			Q.mul_43				(m_xform,shape.data.box);
			Fvector					temp;
			for (int i=0; i<8; ++i) {
                Q.transform_tiny	(temp,points[i]);
				start.x				= _min(start.x,temp.x);
				start.y				= _min(start.y,temp.y);
				start.z				= _min(start.z,temp.z);
				dest.x				= _max(dest.x,temp.x);
				dest.y				= _max(dest.y,temp.y);
				dest.z				= _max(dest.z,temp.z);
			}
			break;
		}
		default : NODEFAULT;
	}
	
	level_graph().iterate_vertices	(start,dest,border_merge_predicate(this,m_level_graph));
}

	void Vision::o_trace	(Fvector& P, float dt, float vis_threshold)	{
		RQR.r_clear			();
		xr_vector<feel_visible_Item>::iterator I=feel_visible.begin(),E=feel_visible.end();
		for (; I!=E; I++){
			if (0==I->O->CFORM())	{ I->fuzzy = -1; continue; }

			// verify relation
			if (positive(I->fuzzy) && I->O->Position().similar(I->cp_LR_dst,lr_granularity) && P.similar(I->cp_LR_src,lr_granularity))
				continue;

			I->cp_LR_dst		= I->O->Position();
			I->cp_LR_src		= P;

			// Fetch data
			Fvector				OP;
			Fmatrix				mE;
			const Fbox&			B = I->O->CFORM()->getBBox();
			const Fmatrix&		M = I->O->XFORM();

			// Build OBB + Ellipse and X-form point
			Fvector				c,r;
			Fmatrix				T,mR,mS;
			B.getcenter			(c);
			B.getradius			(r);
			T.translate			(c);
			mR.mul_43			(M,T);
			mS.scale			(r);
			mE.mul_43			(mR,mS); 
			mE.transform_tiny	(OP,I->cp_LP);
			I->cp_LAST			= OP;

			// 
			Fvector				D;	
			D.sub				(OP,P);
			float				f = D.magnitude();
			if (f>fuzzy_guaranteed){
				D.div						(f);
				// setup ray defs & feel params
				collide::ray_defs RD		(P,D,f,CDB::OPT_CULL,collide::rq_target(collide::rqtStatic|collide::rqtObstacle));
				SFeelParam	feel_params		(this,&*I,vis_threshold);
				// check cache
				if (I->Cache.result&&I->Cache.similar(P,D,f)){
					// similar with previous query
					feel_params.vis			= I->Cache_vis;
//					Log("cache 0");
				}else{
					float _u,_v,_range;
					if (CDB::TestRayTri(P,D,I->Cache.verts,_u,_v,_range,false)&&(_range>0 && _range<f))	{
						feel_params.vis		= 0.f;
//						Log("cache 1");
					}else{
						// cache outdated. real query.
						VERIFY(!fis_zero(RD.dir.square_magnitude()));
						if (g_pGameLevel->ObjectSpace.RayQuery	(RQR, RD, feel_vision_callback, &feel_params, NULL, NULL))	{
							I->Cache_vis	= feel_params.vis	;
							I->Cache.set	(P,D,f,TRUE	)		;
						}else{
							I->Cache.set	(P,D,f,FALSE)		;
						}
//						Log("query");
					}
				}
//				Log("Vis",feel_params.vis);
				if (feel_params.vis<feel_params.vis_threshold){
					// INVISIBLE, choose next point
					I->fuzzy				-=	fuzzy_update_novis*dt;
					clamp					(I->fuzzy,-.5f,1.f);
					I->cp_LP.random_dir		();
					I->cp_LP.mul			(.7f);
				}else{
					// VISIBLE
					I->fuzzy				+=	fuzzy_update_vis*dt;
					clamp					(I->fuzzy,-.5f,1.f);
				}
			}
			else {
				// VISIBLE, 'cause near
				I->fuzzy				+=	fuzzy_update_vis*dt;
				clamp					(I->fuzzy,-.5f,1.f);
			}
		}
	}

void IM_XM(const Matrix	&IM,Fmatrix	&XM)
{
	XM.mul_43(xm2im,*((Fmatrix*)(&IM)));
}

void XM_IM(const Fmatrix	&XM,Fmatrix	&IM)
{
	IM.mul_43(xm2im,XM);
}

void render_box						(IRenderVisual *visual, const Fmatrix &xform, const Fvector &additional, bool draw_child_boxes, const u32 &color)
{
	CDebugRenderer			&renderer = Level().debug_renderer();
	IKinematics				*kinematics = smart_cast<IKinematics*>(visual);
	VERIFY					(kinematics);
	u16						bone_count = kinematics->LL_BoneCount();
	VERIFY					(bone_count);
	u16						visible_bone_count = kinematics->LL_VisibleBoneCount();
	if (!visible_bone_count)
		return;

	Fmatrix					matrix;
	Fvector					*points = (Fvector*)_alloca(visible_bone_count*8*sizeof(Fvector));
	Fvector					*I = points;
	for (u16 i=0; i<bone_count; ++i) {
		if (!kinematics->LL_GetBoneVisible(i))
			continue;
		
		const Fobb			&obb = kinematics->LL_GetData(i).obb;
		if (fis_zero(obb.m_halfsize.square_magnitude())) {
			VERIFY			(visible_bone_count > 1);
			--visible_bone_count;
			continue;
		}

		Fmatrix				Mbox;
		obb.xform_get		(Mbox);

		const Fmatrix		&Mbone = kinematics->LL_GetBoneInstance(i).mTransform;
		Fmatrix				X;
		matrix.mul_43		(xform,X.mul_43(Mbone,Mbox));

		Fvector				half_size = Fvector().add(obb.m_halfsize,additional);
		matrix.mulB_43		(Fmatrix().scale(half_size));

		if (draw_child_boxes)
			renderer.draw_obb	(matrix,color);

		static const Fvector	local_points[8] = {
			Fvector().set(-1.f,-1.f,-1.f),
			Fvector().set(-1.f,-1.f,+1.f),
			Fvector().set(-1.f,+1.f,+1.f),
			Fvector().set(-1.f,+1.f,-1.f),
			Fvector().set(+1.f,+1.f,+1.f),
			Fvector().set(+1.f,+1.f,-1.f),
			Fvector().set(+1.f,-1.f,+1.f),
			Fvector().set(+1.f,-1.f,-1.f)
		};
		
		for (u32 i=0; i<8; ++i, ++I)
			matrix.transform_tiny	(*I,local_points[i]);
	}

	VERIFY						(visible_bone_count);
	if (visible_bone_count == 1) {
		renderer.draw_obb		(matrix,color);
		return;
	}

	VERIFY						((I - points) == (visible_bone_count*8));
	MagicBox3					box = MagicMinBox(visible_bone_count*8,points);
	box.ComputeVertices			(points);
	
	Fmatrix						result;
	result.identity				();

	result.c					= box.Center();

	result.i.sub(points[3],points[2]).normalize();
	result.j.sub(points[2],points[1]).normalize();
	result.k.sub(points[2],points[6]).normalize();

	Fvector						scale;
	scale.x						= points[3].distance_to(points[2])*.5f;
	scale.y						= points[2].distance_to(points[1])*.5f;
	scale.z						= points[2].distance_to(points[6])*.5f;
	result.mulB_43				(Fmatrix().scale(scale));

	renderer.draw_obb			(result,color);
}