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

//void CEntity::Hit			(float perc, Fvector &dir, CObject* who, s16 element,Fvector position_in_object_space, float impulse, ALife::EHitType hit_type) 
void	CEntity::Hit		(SHit* pHDS)
{

//	if (bDebug)				Log("Process HIT: ", *cName());

	// *** process hit calculations
	// Calc impulse
	Fvector					vLocalDir;
	float					m = pHDS->dir.magnitude();
	VERIFY					(m>EPS);
	
	// convert impulse into local coordinate system
	Fmatrix					mInvXForm;
	mInvXForm.invert		(XFORM());
	mInvXForm.transform_dir	(vLocalDir,pHDS->dir);
	vLocalDir.invert		();

	// hit impulse
	if(pHDS->impulse) HitImpulse				(pHDS->impulse,pHDS->dir,vLocalDir); // @@@: WT
	
	// Calc amount (correct only on local player)
	float lost_health = CalcCondition(pHDS->damage());

	// Signal hit
	if(BI_NONE!=pHDS->bone())	HitSignal(lost_health,vLocalDir,pHDS->who,pHDS->boneID);

	// If Local() - perform some logic
	if (Local() && !g_Alive() && !AlreadyDie() && (m_killer_id == ALife::_OBJECT_ID(-1))) {
		KillEntity	(pHDS->whoID);
	}
	//must be last!!! @slipch
	inherited::Hit(pHDS);
}

//----------------------------------------------------
// Skeletal motion
//----------------------------------------------------
static void CalculateAnim(CBone* bone, CSMotion* motion, Fmatrix& parent)
{
    Flags8 flags; flags.zero();
    if (motion) flags 	= motion->GetMotionFlags(bone->SelfID);

    Fmatrix& M 		= bone->_MTransform();
    Fmatrix& L 		= bone->_LTransform();
    
    const Fvector& r = bone->_Rotate();
    if (flags.is(st_BoneMotion::flWorldOrient)){
        M.setXYZi	(r.x,r.y,r.z);
        M.c.set		(bone->_Offset());
        L.mul		(parent,M);
        L.i.set		(M.i);
        L.j.set		(M.j);
        L.k.set		(M.k);

        Fmatrix		 LI; LI.invert(parent);
        M.mulA_43	(LI);
    }else{
        M.setXYZi	(r.x,r.y,r.z);
        M.c.set		(bone->_Offset());
        L.mul		(parent,M);
    }
	bone->_RenderTransform().mul_43(bone->_LTransform(),bone->_RITransform());
    
    // Calculate children
    for (BoneIt b_it=bone->children.begin(); b_it!=bone->children.end(); b_it++)
    	CalculateAnim	(*b_it,motion,bone->_LTransform());
}

void CWeaponStatMgun::UpdateBarrelDir()
{
	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_allow_fire		= true;
	Fmatrix				XFi;
	XFi.invert			(XFORM());
	Fvector				dep;
	XFi.transform_dir	(dep,m_destEnemyDir);
	{// x angle
		m_i_bind_x_xform.transform_dir(dep); dep.normalize();
		m_tgt_x_rot		= angle_normalize_signed(m_bind_x_rot-dep.getP());
		float sv_x		= m_tgt_x_rot;
		
		clamp			(m_tgt_x_rot,-m_lim_x_rot.y,-m_lim_x_rot.x);
		if (!fsimilar(sv_x,m_tgt_x_rot,EPS_L)) m_allow_fire=FALSE;
	}
	{// y angle
		m_i_bind_y_xform.transform_dir(dep); dep.normalize();
		m_tgt_y_rot		= angle_normalize_signed(m_bind_y_rot-dep.getH());
		float sv_y		= m_tgt_y_rot;
		clamp			(m_tgt_y_rot,-m_lim_y_rot.y,-m_lim_y_rot.x);
		if (!fsimilar(sv_y,m_tgt_y_rot,EPS_L)) m_allow_fire=FALSE;
	}

	m_cur_x_rot		= angle_inertion_var(m_cur_x_rot,m_tgt_x_rot,0.5f,3.5f,PI_DIV_6,Device.fTimeDelta);
	m_cur_y_rot		= angle_inertion_var(m_cur_y_rot,m_tgt_y_rot,0.5f,3.5f,PI_DIV_6,Device.fTimeDelta);
}

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			CGameObject::dbg_DrawSkeleton	()
{
	CCF_Skeleton* Skeleton = smart_cast<CCF_Skeleton*>(collidable.model);
	if (!Skeleton) return;
	Skeleton->_dbg_refresh();

	const CCF_Skeleton::ElementVec& Elements = Skeleton->_GetElements();
	for (CCF_Skeleton::ElementVec::const_iterator I=Elements.begin(); I!=Elements.end(); I++){
		if (!I->valid())		continue;
		switch (I->type){
			case SBoneShape::stBox:{
				Fmatrix M;
				M.invert			(I->b_IM);
				Fvector h_size		= I->b_hsize;
				Level().debug_renderer().draw_obb	(M, h_size, color_rgba(0, 255, 0, 255));
								   }break;
			case SBoneShape::stCylinder:{
				Fmatrix M;
				M.c.set				(I->c_cylinder.m_center);
				M.k.set				(I->c_cylinder.m_direction);
				Fvector				h_size;
				h_size.set			(I->c_cylinder.m_radius,I->c_cylinder.m_radius,I->c_cylinder.m_height*0.5f);
				Fvector::generate_orthonormal_basis(M.k,M.j,M.i);
				Level().debug_renderer().draw_obb	(M, h_size, color_rgba(0, 127, 255, 255));
										}break;
			case SBoneShape::stSphere:{
				Fmatrix				l_ball;
				l_ball.scale		(I->s_sphere.R, I->s_sphere.R, I->s_sphere.R);
				l_ball.translate_add(I->s_sphere.P);
				Level().debug_renderer().draw_ellipse(l_ball, color_rgba(0, 255, 0, 255));
									  }break;
		};
	};	
}

door::door						( CPhysicObject* object ) :
	m_object					( *object ),
	m_state						( door_state_open ),
	m_previous_state			( door_state_open ),
	m_target_state				( door_state_open ),
	m_registered_position		( object->Position() ),
	m_locked					( false )
{
	VERIFY						( valid(m_state) );
	VERIFY						( valid(m_target_state) );
	VERIFY						( valid(m_previous_state) );

	R_ASSERT					( m_object.get_door_vectors( m_closed_vector, m_open_vector ) );

	Fmatrix invert;
	invert.invert				( m_object.XFORM() );
	invert.transform_dir		( m_open_vector );
	invert.transform_dir		( m_closed_vector );

	float const length			= 1.1f;
	m_open_vector.mul			( length );
	m_closed_vector.mul			( length );

	m_object.spatial.type		|=	STYPE_VISIBLEFORAI;
}

bool CEditShape::RayPick(float& distance, const Fvector& start, const Fvector& direction, SRayPickInfo* pinf)
{
    float dist					= distance;

	for (ShapeIt it=shapes.begin(); it!=shapes.end(); it++){
		switch (it->type){
		case cfSphere:{
            Fvector S,D;
            Fmatrix M;
            M.invert			(FTransformR);
            M.transform_dir		(D,direction);
            FITransform.transform_tiny(S,start);
            Fsphere&	T		= it->data.sphere;
            float bk_r = T.R;
//            T.R					= FScale.x;
            T.intersect			(S,D,dist);
            if (dist<=0.f)		dist = distance;

            T.R					= bk_r;
		}break;
		case cfBox:{
        	Fbox box;
            box.identity		();
            Fmatrix BI;
            BI.invert			(it->data.box);
		    Fvector S,D,S1,D1,P;
		    FITransform.transform_tiny	(S,start);
		    FITransform.transform_dir	(D,direction);
		    BI.transform_tiny			(S1,S);
		    BI.transform_dir			(D1,D);
            Fbox::ERP_Result	rp_res 	= box.Pick2(S1,D1,P);
            if (rp_res==Fbox::rpOriginOutside){
            	it->data.box.transform_tiny	(P);
                FTransform.transform_tiny	(P);
                P.sub			(start);
                dist			= P.magnitude();
            }
		}break;
		}
    }
    if (dist<distance){
        distance	= dist;
        return 		true;
    }
	return false;
}

void occRasterizer::on_dbg_render()
{
#if DEBUG
	if( !ps_r2_ls_flags_ext.is(R_FLAGEXT_HOM_DEPTH_DRAW) )
	{
		dbg_HOM_draw_initialized = false;
		return;
	}

	for ( int i = 0; i< occ_dim_0; ++i)
	{
		for ( int j = 0; j< occ_dim_0; ++j)
		{
			if( bDebug )
			{
				Fvector quad,left_top,right_bottom,box_center,box_r;
				quad.set( (float)j-occ_dim_0/2.f, -((float)i-occ_dim_0/2.f), (float)bufDepth_0[i][j]/occQ_s32);
				Device.mProject;

				float z = -Device.mProject._43/(float)(Device.mProject._33-quad.z);
				left_top.set		( quad.x*z/Device.mProject._11/(occ_dim_0/2.f),		quad.y*z/Device.mProject._22/(occ_dim_0/2.f), z);
				right_bottom.set	( (quad.x+1)*z/Device.mProject._11/(occ_dim_0/2.f), (quad.y+1)*z/Device.mProject._22/(occ_dim_0/2.f), z);

				box_center.set		((right_bottom.x + left_top.x)/2, (right_bottom.y + left_top.y)/2, z);
				box_r = right_bottom;
				box_r.sub(box_center);

				Fmatrix inv;
				inv.invert(Device.mView);
				inv.transform( box_center );
				inv.transform_dir( box_r );

				pixel_box& tmp = dbg_pixel_boxes[ i*occ_dim_0+j];
				tmp.center	= box_center;
				tmp.radius	= box_r;
				tmp.z 		= quad.z;
				dbg_HOM_draw_initialized = true;
			}

			if( !dbg_HOM_draw_initialized )
				return;

			pixel_box& tmp = dbg_pixel_boxes[ i*occ_dim_0+j];
			Fmatrix Transform;
			Transform.identity();
			Transform.translate(tmp.center);

			// draw wire
			Device.SetNearer(TRUE);

			RCache.set_Shader	(dxRenderDeviceRender::Instance().m_SelectionShader);
			RCache.dbg_DrawOBB( Transform, tmp.radius, D3DCOLOR_XRGB(u32(255*pow(tmp.z,20.f)),u32(255*(1-pow(tmp.z,20.f))),0) );
			Device.SetNearer(FALSE);
		}
	}
#endif
}

void PHDynamicData::InterpolateTransform(Fmatrix &transform){
	//DMXPStoFMX(dBodyGetRotation(body),
	//			dBodyGetPosition(body),BoneTransform);
	body_interpolation.InterpolateRotation(transform);
	body_interpolation.InterpolatePosition(transform.c);
	Fmatrix				zero;
	zero.set			(ZeroTransform);
	zero.invert			();
	//BoneTransform.mulB(zero);
	transform.mulB_43	(zero);
}

void CGroupObject::NumSetPosition(const Fvector& pos)
{
	inherited::NumSetPosition(pos);
    Fmatrix prev; prev.invert(FTransform);
    UpdateTransform(true);

	for (ObjectIt it=m_Objects.begin(); it!=m_Objects.end(); it++){
    	Fvector v=(*it)->PPosition;
        prev.transform_tiny(v);
        FTransform.transform_tiny(v);
    	(*it)->PPosition=v;
    }
}

void PHDynamicData::CalculateData()
{

	DMXPStoFMX(dBodyGetRotation(body),
		dBodyGetPosition(body),BoneTransform);
	Fmatrix zero;
	zero.set(ZeroTransform);
	zero.invert();
	BoneTransform.mulB_43(zero);
	for(unsigned int i=0;i<numOfChilds;++i){

		Childs[i].CalculateR_N_PosOfChilds(body);
	}
}

void CCameraManager::Dump()
{
	Fmatrix mInvCamera;
	Fvector _R,_U,_T,_P;
	
	mInvCamera.invert(Device.mView);
	_R.set( mInvCamera._11, mInvCamera._12, mInvCamera._13 );
	_U.set( mInvCamera._21, mInvCamera._22, mInvCamera._23 );
	_T.set( mInvCamera._31, mInvCamera._32, mInvCamera._33 );
	_P.set( mInvCamera._41, mInvCamera._42, mInvCamera._43 );
	Log("CCameraManager::Dump::vPosition  = ",_P);
	Log("CCameraManager::Dump::vDirection = ",_T);
	Log("CCameraManager::Dump::vNormal    = ",_U);
	Log("CCameraManager::Dump::vRight     = ",_R);
}

bool	CKinematics::	PickBone			(const Fmatrix &parent_xform,  Fvector& normal, float& dist, const Fvector& start, const Fvector& dir, u16 bone_id)
{
	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);
	for (u32 i=0; i<children.size(); i++)
			if (LL_GetChild(i)->PickBone(normal,dist,S,D,bone_id))
			{
				parent_xform.transform_dir			(normal);
				return true;
			}
	return false;
}

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 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;
    }
}