C++ Fvector::mknormal方法代码示例

void CExplosive::FindNormal(Fvector& normal)
{
	collide::rq_result RQ;

	Fvector pos, dir;
	dir.set(0,-1.f,0);
	cast_game_object()->Center(pos);

	BOOL result = Level().ObjectSpace.RayPick(pos, dir, cast_game_object()->Radius(), 
											 collide::rqtBoth, RQ, NULL);
	if(!result || RQ.O){
		normal.set(0,1,0);
	//если лежим на статике
	//найти треугольник и вычислить нормаль по нему
	}else
	{
		Fvector*	pVerts	= Level().ObjectSpace.GetStaticVerts();
		CDB::TRI*	pTri	= Level().ObjectSpace.GetStaticTris() + RQ.element;
		normal.mknormal	(pVerts[pTri->verts[0]],pVerts[pTri->verts[1]],pVerts[pTri->verts[2]]);
	}
}

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

BOOL	ValidNode(vertex& N)
{
	// *** Query and cache polygons for ray-casting
	Fvector	PointUp;		PointUp.set(N.Pos);		PointUp.y	+= RCAST_Depth/2;
	Fvector	PointDown;		PointDown.set(N.Pos);	PointDown.y	-= RCAST_Depth/2;

	Fbox	BB;				BB.set	(PointUp,PointUp);		BB.grow(g_params.fPatchSize/2);	// box 1
	Fbox	B2;				B2.set	(PointDown,PointDown);	B2.grow(g_params.fPatchSize/2);	// box 2
	BB.merge(B2			);
	BoxQuery(BB,false	);
	u32	dwCount = XRC.r_count();
	if (dwCount==0)	{
		Log("chasm1");
		return FALSE;			// chasm?
	}

	// *** Transfer triangles and compute sector
	R_ASSERT(dwCount<RCAST_MaxTris);
	static svector<tri,RCAST_MaxTris> tris;		tris.clear();
	for (u32 i=0; i<dwCount; i++)
	{
		tri&		D = tris.last();
		CDB::RESULT&rp = XRC.r_begin()[i];
		*(Level.get_tris()+XRC.r_begin()[i].id);

		D.v[0].set	(rp.verts[0]);
		D.v[1].set	(rp.verts[1]);
		D.v[2].set	(rp.verts[2]);

		Fvector		N;
		N.mknormal	(D.v[0],D.v[1],D.v[2]);
		if (N.y<=0)	continue;

		tris.inc	();
	}
	if (tris.size()==0)	{
		Log("chasm2");
		return FALSE;			// chasm?
	}

	// *** Perform ray-casts and calculate sector
	Fvector P,D,PLP; D.set(0,-1,0);

	float coeff = 0.5f*g_params.fPatchSize/float(RCAST_Count);

	int num_successed_rays = 0;
	for (int x=-RCAST_Count; x<=RCAST_Count; x++) 
	{
		P.x = N.Pos.x + coeff*float(x);
		for (int z=-RCAST_Count; z<=RCAST_Count; z++) {
			P.z = N.Pos.z + coeff*float(z);
			P.y = N.Pos.y;
			N.Plane.intersectRayPoint(P,D,PLP);	// "project" position
			P.y = PLP.y+RCAST_DepthValid/2;

			float	tri_min_range	= flt_max;
			int		tri_selected	= -1;
			float	range = 0.f,u,v;
			for (i=0; i<u32(tris.size()); i++) 
			{
				if (CDB::TestRayTri(P,D,tris[i].v,u,v,range,false)) 
				{
					if (range<tri_min_range) {
						tri_min_range	= range;
						tri_selected	= i;
					}
				}
			}
			if (tri_selected>=0) {
				if (range<RCAST_DepthValid)	num_successed_rays++;

			}
		}
	}
	if (float(num_successed_rays)/float(RCAST_Total) < 0.5f) {
		Msg		("Floating node.");
		return	FALSE;
	}
	return TRUE;
}

void CWallmarksEngine::AddWallmark_internal	(CDB::TRI* pTri, const Fvector* pVerts, const Fvector &contact_point, ref_shader hShader, float sz)
{
	// query for polygons in bounding box
	// calculate adjacency
	{
		Fbox				bb_query;
		Fvector				bbc,bbd;
		bb_query.set		(contact_point,contact_point);
		bb_query.grow		(sz*2.5f);
		bb_query.get_CD		(bbc,bbd);
		xrc.box_options		(CDB::OPT_FULL_TEST);
		xrc.box_query		(g_pGameLevel->ObjectSpace.GetStaticModel(),bbc,bbd);
		u32	triCount		= xrc.r_count	();
		if (0==triCount)	return;
		CDB::TRI* tris		= g_pGameLevel->ObjectSpace.GetStaticTris();
		sml_collector.clear	();
		sml_collector.add_face_packed_D	(pVerts[pTri->verts[0]],pVerts[pTri->verts[1]],pVerts[pTri->verts[2]],0);
		for (u32 t=0; t<triCount; t++)	{
			CDB::TRI*	T	= tris+xrc.r_begin()[t].id;
			if (T==pTri)	continue;
			sml_collector.add_face_packed_D		(pVerts[T->verts[0]],pVerts[T->verts[1]],pVerts[T->verts[2]],0);
		}
		sml_collector.calc_adjacency	(sml_adjacency);
	}

	// calc face normal
	Fvector	N;
	N.mknormal			(pVerts[pTri->verts[0]],pVerts[pTri->verts[1]],pVerts[pTri->verts[2]]);
	sml_normal.set		(N);

	// build 3D ortho-frustum
	Fmatrix				mView,mRot;
	BuildMatrix			(mView,1/sz,contact_point);
	mRot.rotateZ		(::Random.randF(deg2rad(-20.f),deg2rad(20.f)));
	mView.mulA_43		(mRot);
	sml_clipper.CreateFromMatrix	(mView,FRUSTUM_P_LRTB);

	// create wallmark
	static_wallmark* W	= static_wm_allocate();
	RecurseTri			(0,mView,*W);

	// calc sphere
	if (W->verts.size()<3) { static_wm_destroy(W); return; }
	else {
		Fbox bb;	bb.invalidate();

		FVF::LIT* I=&*W->verts.begin	();
		FVF::LIT* E=&*W->verts.end		();
		for (; I!=E; I++)	bb.modify	(I->p);
		bb.getsphere					(W->bounds.P,W->bounds.R);
	}

	if (W->bounds.R < 1.f)	
	{
		// search if similar wallmark exists
		wm_slot* slot			= FindSlot	(hShader);
		if (slot){
			StaticWMVecIt it	=	slot->static_items.begin	();
			StaticWMVecIt end	=	slot->static_items.end	();
			for (; it!=end; it++)	{
				static_wallmark* wm		=	*it;
				if (wm->bounds.P.similar(W->bounds.P,0.02f)){ // replace
					static_wm_destroy	(wm);
					*it					=	W;
					return;
				}
			}
		} else {
			slot		= AppendSlot(hShader);
		}

		// no similar - register _new_
		slot->static_items.push_back(W);
	}
}

void CHM_Static::Update	()
{
	Fvector&	view	= Device.vCameraPosition;
	int			v_x		= iFloor(view.x/dhm_size);
	int			v_z		= iFloor(view.z/dhm_size);
	
	// *****	SCROLL
	if (v_x!=c_x)	{
		if (v_x>c_x)	{
			// scroll matrix to left
			++c_x;
			for (int z=0; z<dhm_matrix; ++z)
			{
				Slot*	S	= data[z][0];
				if (S->bReady)	{	S->bReady = FALSE; task.push_back(S); }
				for (int x=1; x<dhm_matrix; ++x)	data[z][x-1] = data[z][x];
				data[z][dhm_matrix-1] = S;
				S->set	(c_x-dhm_line+dhm_matrix-1, c_z-dhm_line+z);
			}
		} else {
			// scroll matrix to right
			--c_x;
			for (int z=0; z<dhm_matrix; ++z)
			{
				Slot*	S	= data[z][dhm_matrix-1];
				if (S->bReady)	{	S->bReady = FALSE; task.push_back(S); }
				for (int x=dhm_matrix-1; x>0; --x)	data[z][x] = data[z][x-1]; 
				data[z][0]	= S;
				S->set	(c_x-dhm_line+0,c_z-dhm_line+z);
			}
		}
	}
	if (v_z!=c_z)	{
		if (v_z>c_z)	{
			// scroll matrix down a bit
			++c_z;
			for (int x=0; x<dhm_matrix; ++x)
			{
				Slot*	S	= data[dhm_matrix-1][x];
				if (S->bReady)	{	S->bReady = FALSE; task.push_back(S); }
				for (int z=dhm_matrix-1; z>0; --z)	data[z][x] = data[z-1][x];
				data[0][x]	= S;
				S->set	(c_x-dhm_line+x,c_z-dhm_line+0);
			}
		} else {
			// scroll matrix up
			--c_z;
			for (int x=0; x<dhm_matrix; ++x)
			{
				Slot*	S = data[0][x];
				if (S->bReady)	{	S->bReady = FALSE; task.push_back(S); }
				for (int z=0; z<dhm_matrix; ++z)	data[z-1][x] = data[z][x];
				data[dhm_matrix-1][x]	= S;
				S->set	(c_x-dhm_line+x,c_z-dhm_line+dhm_matrix-1);
			}
		}
	}
	
	// *****	perform TASKs
	for (int taskid=0; (taskid<tasksPerFrame) && (!task.empty()); ++taskid)
	{
		Slot*	S	= task.back	();	task.pop_back();
		S->bReady	= TRUE;

		// Build BBox
		Fbox				bb;
		bb.min.set			(S->x*dhm_size,		view.y-limit_down,	S->z*dhm_size);
		bb.max.set			(bb.min.x+dhm_size,	view.y+limit_up,	bb.min.z+dhm_size);
		bb.grow				(EPS_L);
		
		// Select polygons
		XRC.BBoxMode		(0); // BBOX_TRITEST
		XRC.BBoxCollide		(precalc_identity,g_pGameLevel->ObjectSpace.GetStaticModel(),precalc_identity,bb);
		u32	triCount	= XRC.GetBBoxContactCount();
		if (0==triCount)	{
			S->clear	();
			continue;
		}
		
		// Cull polys
		RAPID::tri* tris	= g_pGameLevel->ObjectSpace.GetStaticTris();
		Fvector		vecUP;	vecUP.set(0,1,0);
		for (u32 tid=0; tid<triCount; ++tid)
		{
			RAPID::tri&	T		= tris[XRC.BBoxContact[tid].id];
			Poly		P;
			Fvector		N;
			P.v[0].set	(*T.verts[0]);	P.v[1].set	(*T.verts[1]);	P.v[2].set	(*T.verts[2]);
			N.mknormal	(P.v[0],P.v[1],P.v[2]);
			if (N.dotproduct(vecUP)<=0)	continue;
			polys.push_back		(P);
		}
		
		// Perform testing
		for (int z=0; z<dhm_precision; ++z)
		{
			for (int x=0; x<dhm_precision; ++x)
			{
				float	rx	= (float(x)/float(dhm_precision))*dhm_size + bb.min.x;
				float	rz	= (float(z)/float(dhm_precision))*dhm_size + bb.min.z;
//.........这里部分代码省略.........