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

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

//----------------------------------------------------
IC bool build_mesh(const Fmatrix& parent, CEditableMesh* mesh, CGeomPartExtractor* extractor, u32 game_mtl_mask, BOOL ignore_shader)
{
	bool bResult 			= true;
    mesh->GenerateVNormals	(&parent);
    // fill faces
    for (SurfFaces::const_iterator sp_it=mesh->GetSurfFaces().begin(); sp_it!=mesh->GetSurfFaces().end(); sp_it++){
		const IntVec& face_lst 	= sp_it->second;
        CSurface* surf 		= sp_it->first;
		int gm_id			= surf->_GameMtl(); 
        if (gm_id==GAMEMTL_NONE_ID){ 
        	ELog.DlgMsg		(mtError,"Object '%s', surface '%s' contain invalid game material.",mesh->Parent()->m_LibName.c_str(),surf->_Name());
        	bResult 		= FALSE; 
            break; 
        }
        SGameMtl* M 		= GMLib.GetMaterialByID(gm_id);
        if (0==M){
        	ELog.DlgMsg		(mtError,"Object '%s', surface '%s' contain undefined game material.",mesh->Parent()->m_LibName.c_str(),surf->_Name());
        	bResult 		= FALSE; 
            break; 
        }
        if (!M->Flags.is(game_mtl_mask)) continue;

        // check engine shader compatibility
        if (!ignore_shader){
            IBlender* 		B = EDevice.Resources->_FindBlender(surf->_ShaderName()); 
            if (FALSE==B){
                ELog.Msg	(mtError,"Can't find engine shader '%s'. Object '%s', surface '%s'. Export interrupted.",surf->_ShaderName(),mesh->Parent()->m_LibName.c_str(),surf->_Name());
                bResult 	= FALSE; 
                break; 
            }
            if (TRUE==B->canBeLMAPped()){ 
                ELog.Msg	(mtError,"Object '%s', surface '%s' contain static engine shader - '%s'. Export interrupted.",mesh->Parent()->m_LibName.c_str(),surf->_Name(),surf->_ShaderName());
                bResult 	= FALSE; 
                break; 
            }
        }
                                                  
        const st_Face* faces	= mesh->GetFaces();        	VERIFY(faces);
        const Fvector*	vn	 	= mesh->GetVNormals();		VERIFY(vn);
        const Fvector*	pts 	= mesh->GetVertices();		VERIFY(pts);
	    for (IntVec::const_iterator f_it=face_lst.begin(); f_it!=face_lst.end(); f_it++){
			const st_Face& face = faces[*f_it];
            Fvector 		v[3],n[3];
            parent.transform_tiny	(v[0],pts[face.pv[0].pindex]);
            parent.transform_tiny	(v[1],pts[face.pv[1].pindex]);
            parent.transform_tiny	(v[2],pts[face.pv[2].pindex]);
            parent.transform_dir	(n[0],vn[*f_it*3+0]); n[0].normalize();
            parent.transform_dir	(n[1],vn[*f_it*3+1]); n[1].normalize();
            parent.transform_dir	(n[2],vn[*f_it*3+2]); n[2].normalize();
            const Fvector2*	uv[3];
            mesh->GetFaceTC			(*f_it,uv);
            extractor->AppendFace	(surf,v,n,uv);
        }
        if (!bResult) break;
    }
    mesh->UnloadVNormals	();
    return bResult;
}

void pDomain::transform(const pDomain& domain, const Fmatrix& m)
{
	switch (type)
	{
	case PDBox:{
		Fbox* bb_dest=(Fbox*)&p1;
		Fbox* bb_from=(Fbox*)&domain.p1;
		bb_dest->xform(*bb_from,m);
		}break;
	case PDPlane:
		m.transform_tiny(p1,domain.p1);
		m.transform_dir(p2,domain.p2);
		// radius1 stores the d of the plane eqn.
		radius1 = -(p1 * p2);
		break;
	case PDSphere:
		m.transform_tiny(p1,domain.p1);
		break;
	case PDCylinder:
	case PDCone:
		m.transform_tiny(p1,domain.p1);
		m.transform_dir(p2,domain.p2);
		m.transform_dir(u,domain.u);
		m.transform_dir(v,domain.v);
		break;
	case PDBlob:
		m.transform_tiny(p1,domain.p1);
		break;
	case PDPoint:
		m.transform_tiny(p1,domain.p1);
		break;
	case PDLine:
		m.transform_tiny(p1,domain.p1);
		m.transform_dir(p2,domain.p2);
		break;
	case PDRectangle:
		m.transform_tiny(p1,domain.p1);
		m.transform_dir(p2,domain.p2);
		m.transform_dir(u,domain.u);
		m.transform_dir(v,domain.v);
		break;
	case PDTriangle:
		m.transform_tiny(p1,domain.p1);
		m.transform_dir(p2,domain.p2);
		m.transform_dir(u,domain.u);
		m.transform_dir(v,domain.v);
		break;
	case PDDisc:
		m.transform_tiny(p1,domain.p1);
		m.transform_dir(p2,domain.p2);
		m.transform_dir(u,domain.u);
		m.transform_dir(v,domain.v);
		break;
	default:
    	NODEFAULT;
	}
}

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 CDrawUtilities::DrawCross(const Fvector& p, float szx1, float szy1, float szz1, float szx2, float szy2, float szz2, u32 clr, BOOL bRot45)
{
	_VertexStream*	Stream	= &RCache.Vertex;
	// actual rendering
	u32			vBase;
	FVF::L*	pv	 	= (FVF::L*)Stream->Lock(bRot45?12:6,vs_L->vb_stride,vBase);
    pv->set(p.x+szx2,p.y,p.z,clr); pv++;
    pv->set(p.x-szx1,p.y,p.z,clr); pv++;
    pv->set(p.x,p.y+szy2,p.z,clr); pv++;
    pv->set(p.x,p.y-szy1,p.z,clr); pv++;
    pv->set(p.x,p.y,p.z+szz2,clr); pv++;
    pv->set(p.x,p.y,p.z-szz1,clr); pv++;
    if (bRot45){
    	Fmatrix M;
        M.setHPB(PI_DIV_4,PI_DIV_4,PI_DIV_4);
	    for(int i=0;i<6;i++,pv++){
        	pv->p.sub((pv-6)->p,p);
        	M.transform_dir(pv->p);
        	pv->p.add(p);
            pv->color = clr;
        }
    }
	// unlock VB and Render it as triangle list
	Stream->Unlock(bRot45?12:6,vs_L->vb_stride);
    DU_DRAW_DP			(D3DPT_LINELIST,vs_L,vBase,bRot45?6:3);
}

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

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

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
}

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 CCarWeapon::UpdateBarrelDir()
{
	CKinematics* K		= smart_cast<CKinematics*>(m_object->Visual());
	m_fire_bone_xform	= K->LL_GetTransform(m_fire_bone);

	m_fire_bone_xform.mulA_43(m_object->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_norm.set(0,1,0);
	m_fire_bone_xform.transform_dir(m_fire_norm);


	m_allow_fire		= true;
	Fmatrix XFi;
	XFi.invert			(m_object->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());
		clamp			(m_tgt_x_rot,-m_lim_x_rot.y,-m_lim_x_rot.x);
	}
	{// 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());
		clamp			(m_tgt_y_rot,-m_lim_y_rot.y,-m_lim_y_rot.x);
	}

	m_cur_x_rot		= angle_inertion_var(m_cur_x_rot,m_tgt_x_rot,m_min_gun_speed,m_max_gun_speed,PI,Device.fTimeDelta);
	m_cur_y_rot		= angle_inertion_var(m_cur_y_rot,m_tgt_y_rot,m_min_gun_speed,m_max_gun_speed,PI,Device.fTimeDelta);
	static float dir_eps = deg2rad(5.0f);
	if( !fsimilar(m_cur_x_rot,m_tgt_x_rot,dir_eps)|| !fsimilar(m_cur_y_rot,m_tgt_y_rot,dir_eps))
		m_allow_fire=FALSE;

#if (0)
	if(Device.dwFrame%200==0){
		Msg("m_cur_x_rot=[%f]",m_cur_x_rot);
		Msg("m_cur_y_rot=[%f]",m_cur_y_rot);
	}
#endif
}

void CBone::ShapeRotate(const Fvector& _amount)
{
    Fvector amount=_amount;
	Fmatrix _IT;_IT.invert(_LTransform());
    if (Tools->GetSettings(etfCSParent)) _IT.transform_dir(amount);
	switch (shape.type){
    case SBoneShape::stBox:{
    	Fmatrix R;
        R.setXYZi(amount.x,amount.y,amount.z);
        shape.box.transform(shape.box,R);
    }break;
    case SBoneShape::stSphere:	break;
    case SBoneShape::stCylinder:{
    	Fmatrix R;
        R.setXYZi(amount.x,amount.y,amount.z);
        R.transform_dir(shape.cylinder.m_direction);
    }break;
    }
}

void CPHGeometryOwner::add_Shape(const SBoneShape& shape,const Fmatrix& offset)
{
	switch(shape.type) {
	case SBoneShape::stBox	:
		{
			Fobb box=shape.box;
			Fmatrix m;
			m.set(offset);
			//Fmatrix position;
			//position.set(box.m_rotate);
			//position.c.set(box.m_translate);
			//position.mulA(offset);
			//box.m_rotate.set(position);
			//box.m_translate.set(position.c);
			box.transform(box,m);
			add_Box(box);
			break;
		}
	case SBoneShape::stSphere	:
		{
			Fsphere sphere=shape.sphere;
			offset.transform_tiny(sphere.P);
			add_Sphere(sphere);
			break;
		}


	case SBoneShape::stCylinder :
		{
			Fcylinder C=shape.cylinder;
			offset.transform_tiny(C.m_center);
			offset.transform_dir(C.m_direction);
			add_Cylinder(C);
			break;
		}


	case SBoneShape::stNone :
		break;
	default: NODEFAULT;
	}
}

void __fastcall TUI_ControlAIMapNodeRotate::Move(TShiftState _Shift)
{
    if (_Shift.Contains(ssLeft)){
        float amount = -UI->m_DeltaCpH.x * UI->m_MouseSR;

        if( Tools->GetSettings(etfASnap) ) CHECK_SNAP(m_fRotateSnapAngle,amount,Tools->m_RotateSnapAngle);

        Fmatrix R;
        if 	(fis_zero(m_RotateVector.x)) 	R.rotateZ(amount);
        else								R.rotateX(amount);
        
       	AINodeVec& lst 		= ((ESceneAIMapTool*)parent_tool)->Nodes();
        for(AINodeIt _F = lst.begin();_F!=lst.end();_F++)
            if((*_F)->flags.is(SAINode::flSelected)){
            	Fvector 	new_n;
            	R.transform_dir(new_n,(*_F)->Plane.n);
                if (Fvector().set(0,1,0).dotproduct(new_n)>0.02f){
	            	(*_F)->Plane.build((*_F)->Pos,new_n);           	
                }
            }
    }
}

void CIKLimb::GoalMatrix(Matrix &M,SCalculateData* cd)
{
		VERIFY(cd->m_tri&&cd->m_tri_hight!=-dInfinity);
		const Fmatrix &obj=*cd->m_obj;
		CDB::TRI	*tri=cd->m_tri;
		CKinematics *K=cd->m_K;
		Fvector*	pVerts	= Level().ObjectSpace.GetStaticVerts();
		Fvector normal;
		normal.mknormal	(pVerts[tri->verts[0]],pVerts[tri->verts[1]],pVerts[tri->verts[2]]);
		VERIFY(!fis_zero(normal.magnitude()));

		Fmatrix iobj;iobj.invert(obj);iobj.transform_dir(normal);

		Fmatrix xm;xm.set(K->LL_GetTransform(m_bones[2]));

		//Fvector dbg;
		//dbg.set(Fvector().mul(normal,normal.y*tri_hight
		//	-normal.dotproduct(xm.i)*m_toe_position.x
		//	-normal.dotproduct(xm.j)*m_toe_position.y
		//	-normal.dotproduct(xm.k)*m_toe_position.z-m_toe_position.x
		//	));
		
		normal.invert();
		Fvector ax;ax.crossproduct(normal,xm.i);
		float s=ax.magnitude();
		if(!fis_zero(s))
		{
			ax.mul(1.f/s);

			xm.mulA_43(Fmatrix().rotation(ax,asinf(-s)));
		}

		Fvector otri;iobj.transform_tiny(otri,pVerts[tri->verts[0]]);
		float tp=normal.dotproduct(otri);
		Fvector add;
		add.set(Fvector().mul(normal,-m_toe_position.x+tp-xm.c.dotproduct(normal)));

		xm.c.add(add);
		
	
		Fmatrix H;
		CBoneData& bd=K->LL_GetData(m_bones[0]);
		H.set(bd.bind_transform);
	
		H.mulA_43(K->LL_GetTransform(bd.GetParentID()));
		H.c.set(K->LL_GetTransform(m_bones[0]).c);

	
#ifdef DEBUG
		if(ph_dbg_draw_mask.test(phDbgIKAnimGoalOnly))	xm.set(K->LL_GetTransform(m_bones[2]));
		if(ph_dbg_draw_mask.test(phDbgDrawIKGoal))
		{
			Fmatrix DBGG;
			DBGG.mul_43(obj,xm);
			DBG_DrawMatrix(DBGG,0.2f);

			
			DBGG.mul_43(obj,H);
			DBG_DrawMatrix(DBGG,0.2f);
		}
#endif
		H.invert();
		Fmatrix G; 
		G.mul_43(H,xm);
		XM2IM(G,M);
}