C++ Fmatrix类代码示例

void CGroupObject::Move(Fvector& amount)
{
	Fvector old_r=FRotation;
	inherited::Move(amount);
    Fmatrix prev; prev.invert(FTransform);
    UpdateTransform(true);

    Fvector dr; dr.sub(FRotation,old_r);
	for (ObjectIt it=m_Objects.begin(); it!=m_Objects.end(); it++){
    	Fvector r=(*it)->PRotation; r.add(dr); (*it)->PRotation=r;
    	Fvector v=(*it)->PPosition;
        prev.transform_tiny(v);
        FTransform.transform_tiny(v);
    	(*it)->PPosition=v;
    }
}

IC void q_scalem(Fmatrix &m, float v)
{
	Fquaternion q;
	q.set(m);
	q_scale(q,v);
	m.rotation(q);
}

void CGroupObject::NumSetScale(const Fvector& scale)
{
	Fvector old_s = PScale;
	inherited::NumSetScale(scale);
    Fmatrix prev; prev.invert(FTransform);
    UpdateTransform(true);

    Fvector ds; ds.sub(FScale,old_s);
	for (ObjectIt it=m_Objects.begin(); it!=m_Objects.end(); it++){
    	Fvector s=(*it)->PScale; s.add(ds); (*it)->PScale=s;
    	Fvector v=(*it)->PPosition;
        prev.transform_tiny(v);
        FTransform.transform_tiny(v);
    	(*it)->PPosition=v;
    }
}

void CPHShell::Activate(const Fmatrix &m0,float dt01,const Fmatrix &m2,bool disable){

	if(isActive())return;
	activate(disable);
//	ELEMENT_I i;
	mXFORM.set(m0);
	//for(i=elements.begin();elements.end() != i;++i){

	//	(*i)->Activate(m0,dt01, m2, disable);
	//}
	
	{		
		ELEMENT_I i=elements.begin(),e=elements.end();
		for(;i!=e;++i)(*i)->Activate(mXFORM,disable);
	}

	{
		JOINT_I i=joints.begin(),e=joints.end();
		for(;i!=e;++i) (*i)->Activate();
	}	
	
	Fmatrix m;
	{
		Fmatrix old_m = mXFORM;//+GetGlobalTransformDynamic update mXFORM;
		GetGlobalTransformDynamic	(&m);
		mXFORM = old_m;
	}
	m.invert();m.mulA_43		(mXFORM);
	TransformPosition(m);
	if(PKinematics())
	{
		SetCallbacks( );
	}

	//bActive=true;
	//bActivating=true;
	m_flags.set(flActive,TRUE);
	m_flags.set(flActivating,TRUE);
	spatial_register();
///////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
	//mXFORM.set(m0);
	//Activate(disable);
	Fvector lin_vel;
	lin_vel.sub(m2.c,m0.c);
	set_LinearVel(lin_vel);
}

void CActor::RenderText				(LPCSTR Text, Fvector dpos, float* pdup, u32 color)
{
	if (!g_Alive()) return;
	
	CBoneInstance& BI = smart_cast<CKinematics*>(Visual())->LL_GetBoneInstance(u16(m_head));
	Fmatrix M;
	smart_cast<CKinematics*>(Visual())->CalculateBones	();
	M.mul						(XFORM(),BI.mTransform);
	//------------------------------------------------
	Fvector v0, v1;
	v0.set(M.c); v1.set(M.c);
	Fvector T        = Device.vCameraTop;
	v1.add(T);

	Fvector v0r, v1r;
	Device.mFullTransform.transform(v0r,v0);
	Device.mFullTransform.transform(v1r,v1);
	float size = v1r.distance_to(v0r);
	CGameFont* pFont = HUD().Font().pFontArial14;
	if (!pFont) return;
//	float OldFontSize = pFont->GetHeight	();	
	float delta_up = 0.0f;
	if (size < mid_size) delta_up = upsize;
	else delta_up = upsize*(mid_size/size);
	dpos.y += delta_up;
	if (size > mid_size) size = mid_size;
//	float NewFontSize = size/mid_size * fontsize;
	//------------------------------------------------
	M.c.y += dpos.y;

	Fvector4 v_res;	
	Device.mFullTransform.transform(v_res,M.c);

	if (v_res.z < 0 || v_res.w < 0)	return;
	if (v_res.x < -1.f || v_res.x > 1.f || v_res.y<-1.f || v_res.y>1.f) return;

	float x = (1.f + v_res.x)/2.f * (Device.dwWidth);
	float y = (1.f - v_res.y)/2.f * (Device.dwHeight);

	pFont->SetAligment	(CGameFont::alCenter);
	pFont->SetColor		(color);
//	pFont->SetHeight	(NewFontSize);
	pFont->Out			(x,y,Text);
	//-------------------------------------------------
//	pFont->SetHeight(OldFontSize);
	*pdup = delta_up;
};

void CHelicopter::UpdateMGunDir()
{
	IKinematics* K		= smart_cast<IKinematics*>(Visual());
	m_fire_bone_xform	= K->LL_GetTransform(m_fire_bone);

	m_fire_bone_xform.mulA_43	(XFORM());
	m_fire_pos.set				(0,0,0); 
	m_fire_bone_xform.transform_tiny(m_fire_pos);
	m_fire_dir.set				(0,0,1);
	m_fire_bone_xform.transform_dir(m_fire_dir);
	
	m_fire_dir.sub				(m_enemy.destEnemyPos,m_fire_pos).normalize_safe();

	m_left_rocket_bone_xform	= K->LL_GetTransform(m_left_rocket_bone);
	m_left_rocket_bone_xform.mulA_43	(XFORM());
	m_left_rocket_bone_xform.c.y += 1.0f;
	//.fake
	m_right_rocket_bone_xform	= K->LL_GetTransform(m_right_rocket_bone);
	m_right_rocket_bone_xform.mulA_43	(XFORM());
	m_right_rocket_bone_xform.c.y += 1.0f;
	//.fake

	m_allow_fire		= TRUE;
	Fmatrix XFi;
	XFi.invert			(XFORM());
	Fvector dep;
	XFi.transform_tiny	(dep,m_enemy.destEnemyPos);
	{// x angle
		Fvector A_;		A_.sub(dep,m_bind_x);	m_i_bind_x_xform.transform_dir(A_); A_.normalize();
		m_tgt_rot.x		= angle_normalize_signed(m_bind_rot.x-A_.getP());
		float sv_x		= m_tgt_rot.x;
		clamp			(m_tgt_rot.x,-m_lim_x_rot.y,-m_lim_x_rot.x);
		if (!fsimilar(sv_x,m_tgt_rot.x,EPS_L)) m_allow_fire=FALSE;
	}
	{// y angle
		Fvector A_;		A_.sub(dep,m_bind_y);	m_i_bind_y_xform.transform_dir(A_); A_.normalize();
		m_tgt_rot.y		= angle_normalize_signed(m_bind_rot.y-A_.getH());
		float sv_y		= m_tgt_rot.y;
		clamp			(m_tgt_rot.y,-m_lim_y_rot.y,-m_lim_y_rot.x);
		if (!fsimilar(sv_y,m_tgt_rot.y,EPS_L)) m_allow_fire=FALSE;
	}
	
	if ((angle_difference(m_cur_rot.x,m_tgt_rot.x)>deg2rad(m_barrel_dir_tolerance))||
		(angle_difference(m_cur_rot.y,m_tgt_rot.y)>deg2rad(m_barrel_dir_tolerance)))
		m_allow_fire=FALSE;

}

void CGroupObject::NumSetRotation(const Fvector& rot)
{
	Fvector old_r;
    FTransformR.getXYZ(old_r);
	inherited::NumSetRotation(rot);
    Fmatrix prev; prev.invert(FTransform);
    UpdateTransform(true);

    Fvector dr; dr.sub(FRotation,old_r);
	for (ObjectIt it=m_Objects.begin(); it!=m_Objects.end(); it++){
    	Fvector r=(*it)->PRotation; r.add(dr); (*it)->PRotation=r;
    	Fvector v=(*it)->PPosition;
        prev.transform_tiny(v);
        FTransform.transform_tiny(v);
    	(*it)->PPosition=v;
    }
}

void			CActor::MoveActor		(Fvector NewPos, Fvector NewDir)
{
	Fmatrix	M = XFORM();
	M.translate(NewPos);
	r_model_yaw				= NewDir.y;
	r_torso.yaw				= NewDir.y;
	r_torso.pitch			= -NewDir.x;
	unaffected_r_torso.yaw	= r_torso.yaw;
	unaffected_r_torso.pitch= r_torso.pitch;
	unaffected_r_torso.roll	= 0;//r_torso.roll;

	r_torso_tgt_roll		= 0;
	cam_Active()->Set		(-unaffected_r_torso.yaw,unaffected_r_torso.pitch,unaffected_r_torso.roll);
	ForceTransform(M);

	m_bInInterpolation = false;	
}

void CArtefact::UpdateXForm()
{
	if (Device.dwFrame!=dwXF_Frame)
	{
		dwXF_Frame			= Device.dwFrame;

		if (0==H_Parent())	return;

		// Get access to entity and its visual
		CEntityAlive*		E		= smart_cast<CEntityAlive*>(H_Parent());
        
		if(!E)				return	;

		const CInventoryOwner	*parent = smart_cast<const CInventoryOwner*>(E);
		if (parent && parent->use_simplified_visual())
			return;

		VERIFY				(E);
		IKinematics*		V		= smart_cast<IKinematics*>	(E->Visual());
		VERIFY				(V);
		if(CAttachableItem::enabled())
			return;

		// Get matrices
		int					boneL = -1, boneR = -1, boneR2 = -1;
		E->g_WeaponBones	(boneL,boneR,boneR2);
		if (boneR == -1)	return;

		boneL = boneR2;

		V->CalculateBones	();
		Fmatrix& mL			= V->LL_GetTransform(u16(boneL));
		Fmatrix& mR			= V->LL_GetTransform(u16(boneR));

		// Calculate
		Fmatrix				mRes;
		Fvector				R,D,N;
		D.sub				(mL.c,mR.c);	D.normalize_safe();
		R.crossproduct		(mR.j,D);		R.normalize_safe();
		N.crossproduct		(D,R);			N.normalize_safe();
		mRes.set			(R,N,D,mR.c);
		mRes.mulA_43		(E->XFORM());
//		UpdatePosition		(mRes);
		XFORM().mul			(mRes,offset());
	}
}

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
}

void game_cl_Deathmatch::PlayParticleEffect(LPCSTR EffName, Fvector& pos)
{
	if (!EffName) return;
	// вычислить позицию и направленность партикла
	Fmatrix M; 
	M.translate(pos);

//	CParticlesPlayer::MakeXFORM(pObj,0,Fvector().set(0.f,1.f,0.f),Fvector().set(0.f,0.f,0.f),pos);

	// установить particles
	CParticlesObject* ps = NULL;

	ps = CParticlesObject::Create(EffName,TRUE);

	ps->UpdateParent(M,Fvector().set(0.f,0.f,0.f));
	GamePersistent().ps_needtoplay.push_back(ps);
}

CParticlesObject* CBaseMonster::PlayParticles(const shared_str& name, const Fvector &position, const Fvector &dir, BOOL auto_remove, BOOL xformed)
{
	CParticlesObject* ps = CParticlesObject::Create(name.c_str(),auto_remove);
	
	// вычислить позицию и направленность партикла
	Fmatrix	matrix; 

	matrix.identity			();
	matrix.k.set			(dir);
	Fvector::generate_orthonormal_basis_normalized(matrix.k,matrix.j,matrix.i);
	matrix.translate_over	(position);
	
	(xformed) ?				ps->SetXFORM (matrix) : ps->UpdateParent(matrix,zero_vel); 
	ps->Play				(false);

	return ps;
}

void CLevelDebug::draw_object_info()
{

	// handle all of the objects
	for (OBJECT_INFO_MAP_IT it = m_objects_info.begin(); it != m_objects_info.end(); ++it) {

		// если объект невалидный - удалить информацию
		if (!it->first || it->first->getDestroy()) {
			for (CLASS_INFO_MAP_IT it_class = it->second.begin(); it_class != it->second.end(); ++it_class){
				xr_delete(it_class->second);
			}
			m_objects_info.erase(it);
			break;
		}

		Fmatrix		res;
		res.mul		(Device.mFullTransform,it->first->XFORM());

		Fvector4	v_res;

		float		delta_height = 0.f;

		// handle all of the classes
		for (CLASS_INFO_MAP_IT class_it = it->second.begin(); class_it != it->second.end(); ++class_it) {

			// get up on 2 meters
			res.transform(v_res, class_it->second->get_shift_pos());

			// check if the object in sight
			if (v_res.z < 0 || v_res.w < 0)										continue;
			if (v_res.x < -1.f || v_res.x > 1.f || v_res.y<-1.f || v_res.y>1.f) continue;

			// get real (x,y)
			float x = (1.f + v_res.x)/2.f * (Device.dwWidth);
			float y = (1.f - v_res.y)/2.f * (Device.dwHeight) - delta_height;
			float start_y = y;

			// handle all of the text inside class
			class_it->second->draw_info(x,y);

			delta_height = start_y - y;
		}


	}
}

void CGroupObject::RotateParent(Fvector& axis, float angle )
{
	inherited::RotateParent(axis,angle);
    Fmatrix  Ginv;
    Ginv.set		(FITransformRP);
	UpdateTransform	(true);
	for (ObjectIt it=m_Objects.begin(); it!=m_Objects.end(); it++){
    	Fmatrix 	O,On;
        O.mul		(Ginv,(*it)->FTransformRP);
        On.mul		(FTransform,O);
        Fvector 	xyz;
        On.getXYZ	(xyz);
        (*it)->NumSetRotation(xyz);
        (*it)->NumSetPosition(On.c);
//		(*it)->PivotRotateParent(m_old,FTransform,axis,angle);
    }
}

void CDrawUtilities::DrawPlane  (const Fvector& center, const Fvector2& scale, const Fvector& rotate, u32 clr_s, u32 clr_w, BOOL bCull, BOOL bSolid, BOOL bWire)
{
    Fmatrix M;
    M.setHPB		(rotate.y,rotate.x,rotate.z);
    M.translate_over(center);
	// fill VB
	_VertexStream*	Stream	= &RCache.Vertex;
	u32			vBase;
    
    if (bSolid){
	    DU_DRAW_SH(dxRenderDeviceRender::Instance().m_SelectionShader);
        FVF::L*	pv	 = (FVF::L*)Stream->Lock(5,vs_L->vb_stride,vBase);
        pv->set		(-scale.x, 0, -scale.y, clr_s); M.transform_tiny(pv->p); pv++;
        pv->set		(-scale.x, 0, +scale.y, clr_s); M.transform_tiny(pv->p); pv++;
        pv->set		(+scale.x, 0, +scale.y, clr_s); M.transform_tiny(pv->p); pv++;
        pv->set		(+scale.x, 0, -scale.y, clr_s); M.transform_tiny(pv->p); pv++;
        pv->set		(*(pv-4));
        Stream->Unlock(5,vs_L->vb_stride);
        if (!bCull) DU_DRAW_RS(D3DRS_CULLMODE,D3DCULL_NONE);
        DU_DRAW_DP		(D3DPT_TRIANGLEFAN,vs_L,vBase,2);
        if (!bCull) DU_DRAW_RS(D3DRS_CULLMODE,D3DCULL_CCW);
    }

    if (bWire){
	    DU_DRAW_SH(dxRenderDeviceRender::Instance().m_WireShader);
        FVF::L*	pv	 = (FVF::L*)Stream->Lock(5,vs_L->vb_stride,vBase);
        pv->set		(-scale.x, 0, -scale.y, clr_w); M.transform_tiny(pv->p); pv++;
        pv->set		(+scale.x, 0, -scale.y, clr_w); M.transform_tiny(pv->p); pv++;
        pv->set		(+scale.x, 0, +scale.y, clr_w); M.transform_tiny(pv->p); pv++;
        pv->set		(-scale.x, 0, +scale.y, clr_w); M.transform_tiny(pv->p); pv++;
        pv->set		(*(pv-4));
        Stream->Unlock(5,vs_L->vb_stride);
	    DU_DRAW_DP	(D3DPT_LINESTRIP,vs_L,vBase,4);
    }
}