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

void CLevelGraph::draw_covers	()
{
	float					half_size = ai().level_graph().header().cell_size()*.5f;
	xr_vector<CCoverPoint*>	nearest;
	nearest.reserve			(1000);
	ai().cover_manager().covers().nearest(Device.vCameraPosition,5.f,nearest);
	xr_vector<CCoverPoint*>::const_iterator	I = nearest.begin();
	xr_vector<CCoverPoint*>::const_iterator	E = nearest.end();
	for ( ; I != E; ++I) {
		Fvector				position = (*I)->position();
		position.y			+= 1.f;
		Level().debug_renderer().draw_aabb	(position,half_size - .01f,1.f,ai().level_graph().header().cell_size()*.5f-.01f,D3DCOLOR_XRGB(0*255,255,0*255));

		CVertex				*v = vertex((*I)->level_vertex_id());
		Fvector				direction;
		float				best_value = -1.f;

		u32 i = 0, j = 0;
		for (; i<36; ++i) {
			float				value = cover_in_direction(float(10*i)/180.f*PI,v);
			direction.setHP		(float(10*i)/180.f*PI,0);
			direction.normalize	();
			direction.mul		(value*half_size);
			direction.add		(position);
			direction.y			= position.y;
			Level().debug_renderer().draw_line(Fidentity,position,direction,D3DCOLOR_XRGB(0,0,255));
			value				= compute_square(float(10*i)/180.f*PI,PI/2.f,v);
			if (value > best_value) {
				best_value		= value;
				j				= i;
			}
		}

		direction.set		(position.x - half_size*float(v->cover(0))/15.f,position.y,position.z);
		Level().debug_renderer().draw_line(Fidentity,position,direction,D3DCOLOR_XRGB(255,0,0));

		direction.set		(position.x,position.y,position.z + half_size*float(v->cover(1))/15.f);
		Level().debug_renderer().draw_line(Fidentity,position,direction,D3DCOLOR_XRGB(255,0,0));

		direction.set		(position.x + half_size*float(v->cover(2))/15.f,position.y,position.z);
		Level().debug_renderer().draw_line(Fidentity,position,direction,D3DCOLOR_XRGB(255,0,0));

		direction.set		(position.x,position.y,position.z - half_size*float(v->cover(3))/15.f);
		Level().debug_renderer().draw_line(Fidentity,position,direction,D3DCOLOR_XRGB(255,0,0));

		float				value = cover_in_direction(float(10*j)/180.f*PI,v);
		direction.setHP		(float(10*j)/180.f*PI,0);
		direction.normalize	();
		direction.mul		(value*half_size);
		direction.add		(position);
		direction.y			= position.y;
		Level().debug_renderer().draw_line	(Fidentity,position,direction,D3DCOLOR_XRGB(0,0,0));
	}
}

//-----------------------------------------------------------------------------
void msimulator_ResolveStuck(SCollisionData& cl, Fvector& position)
{
    // intersection data
    Fvector			polyIPoint;		// polygon intersection point
    Fvector			stuckDir;
    int				stuckCount;

    float			dist;
    float			safe_R = 1.f + EPS_L*2;//psSqueezeVelocity*Device.fTimeDelta;

    for (int passes=0; passes<psCollideActStuckDepth; passes++)
    {
        // initialize
        stuckDir.set	(0,0,0);
        stuckCount		= 0;

        // for all faces
        for (u32 i_t=0; i_t!=clContactedT.size(); i_t++)
        {
            cl_tri& T=clContactedT[i_t];
            Fvector N_inv;
            N_inv.invert(T.N);

            // find plane intersection point by shooting a ray from the
            // sphere intersection point along the planes normal.
            if (CDB::TestRayTri2(position,N_inv,T.p,dist)) {
                // calculate plane intersection point
                polyIPoint.mad(position,N_inv,dist);
            } else {
                // calculate plane intersection point
                Fvector tmp;
                tmp.mad(position,N_inv,dist);
                closestPointOnTriangle(polyIPoint, T, tmp);
            }
            if (CheckPointInSphere(polyIPoint, position, safe_R))
            {
                Fvector dir;
                dir.sub(position,polyIPoint);
                float len = dir.magnitude();
                dir.mul( (safe_R-len)/len );
                stuckDir.add(dir);
                stuckCount++;
            }
        }

        if (stuckCount) {
            stuckDir.div(float(stuckCount));
            position.add(stuckDir);
            if (stuckDir.magnitude()<EPS) break;
        } else break;
    }
}

void CPHMovementControl::PathDIrPoint(const xr_vector<DetailPathManager::STravelPathPoint> &path,  int index,  float distance,  float precesition, Fvector &dir  )
{
	Fvector to_path_point;
	Fvector corrected_path_dir;CorrectPathDir(GetPathDir(),path,index,corrected_path_dir);
	to_path_point.sub(vPathPoint,vPosition);	//_new position
	float mag=to_path_point.magnitude();

	if(mag<EPS) //near the point
	{  
		if(0==index||m_path_size-1==index) //on path eidge
		{
			dir.set(corrected_path_dir);//??
			return;
		}
		dir.sub(path[index].position,path[index-1].position);
		dir.normalize_safe();
		dir.add(corrected_path_dir);
		dir.normalize_safe();
	}
	to_path_point.mul(1.f/mag);
	if(m_path_size-1==index)//on_path_edge
	{
		dir.set(to_path_point);
		return;
	}


	if(mag<EPS||fis_zero(dXZMag(to_path_point),EPS))
	{
		dir.set(corrected_path_dir);
		return;//mean dir
	}
	
	Fvector tangent;
	tangent.crossproduct(Fvector().set(0,1,0),to_path_point);

	VERIFY(!fis_zero(tangent.magnitude()));
	tangent.normalize();
	if(dir.square_magnitude()>EPS)
	{
		if(tangent.dotproduct(dir)<0.f)tangent.invert();
	}
	else
	{
		if(tangent.dotproduct(corrected_path_dir)<0.f)tangent.invert();
	}

	if(mag>FootRadius())to_path_point.mul(precesition);
	else to_path_point.mul(mag*precesition);
	dir.add(tangent,to_path_point);
	dir.normalize_safe();
}

void CPortal::OnRender	()
{
	if (psDeviceFlags.is(rsOcclusionDraw)){
		VERIFY				(poly.size());
		// draw rect
		using LVec = xr_vector<FVF::L>;
		using LVecIt = LVec::iterator;
		static LVec	V;		V.resize(poly.size()+2);
		Fvector C			= {0,0,0};
		for (u32 k=0; k<poly.size(); k++){ C.add(poly[k]); V[k+1].set(poly[k],0x800000FF);}
		V.back().set		(poly[0],0x800000FF);
		C.div				((float)poly.size());
		V[0].set			(C,0x800000FF);

		RCache.set_xform_world(Fidentity);
		// draw solid
		RCache.set_Shader	(dxRenderDeviceRender::Instance().m_SelectionShader);
		RCache.dbg_Draw		(D3DPT_TRIANGLEFAN,&*V.begin(),V.size()-2);

		// draw wire
		if (bDebug){
			RImplementation.rmNear();
		}else{
			Device.SetNearer(TRUE);
		}
		RCache.set_Shader	(dxRenderDeviceRender::Instance().m_WireShader);
		RCache.dbg_Draw		(D3DPT_LINESTRIP,&*(V.begin()+1),V.size()-2);
		if (bDebug){
			RImplementation.rmNormal();
		}else{
			Device.SetNearer(FALSE);
		}
	}
}

void CEditShape::ComputeBounds()
{
	m_Box.invalidate	();

	for (ShapeIt it=shapes.begin(); it!=shapes.end(); it++){
		switch (it->type){
		case cfSphere:{
            Fsphere&	T		= it->data.sphere;
            Fvector		P;
            P.set		(T.P);	P.sub(T.R);	m_Box.modify(P);
            P.set		(T.P);	P.add(T.R);	m_Box.modify(P);
		}break;
		case cfBox:{
            Fvector		P;
            Fmatrix&	T		= it->data.box;

            // Build points
            Fvector p;
            for (int i=0; i<DU_BOX_NUMVERTEX; i++){
                T.transform_tiny	(P,du_box_vertices[i]);
                m_Box.modify		(P);
            }
		}break;
		}
	}
	m_Box.getsphere(m_Sphere.P,m_Sphere.R);
}

//проверка на попадание "осколком" по объекту
ICF static BOOL grenade_hit_callback(collide::rq_result& result, LPVOID params)
{
	SExpQParams& ep	= *(SExpQParams*)params;
	u16 mtl_idx			= GAMEMTL_NONE_IDX;
	if(result.O){
		CKinematics* V  = 0;
		if (0!=(V=smart_cast<CKinematics*>(result.O->Visual()))){
			CBoneData& B= V->LL_GetData((u16)result.element);
			mtl_idx		= B.game_mtl_idx;
		}
	}else{
		//получить треугольник и узнать его материал
		CDB::TRI* T		= Level().ObjectSpace.GetStaticTris()+result.element;
		mtl_idx			= T->material;
	}	
	SGameMtl* mtl		= GMLib.GetMaterialByIdx(mtl_idx);
	ep.shoot_factor		*=mtl->fShootFactor;
#ifdef DEBUG
	if(ph_dbg_draw_mask.test(phDbgDrawExplosions))
	{
		Fvector p;p.set(ep.l_dir);p.mul(result.range);p.add(ep.source_p);
		u8 c	=u8(mtl->fShootFactor*255.f);
		DBG_DrawPoint(p,0.1f,D3DCOLOR_XRGB(255-c,0,c));
	}
#endif
	return				(ep.shoot_factor>0.01f);
}

bool CIKFoot::make_shift( Fmatrix &xm,const Fvector &cl_point, bool collide, const Fplane &p, const Fvector &pick_dir )const
{
	Fvector shift = pick_dir;

	//Fvector toe; ToePosition( toe ); xm.transform_tiny( toe );
	Fvector point;
	xm.transform_tiny( point, cl_point );
	float dot = p.n.dotproduct( shift );
	if( _abs( dot ) < min_dot )
	{
		shift.add( Fvector( ).mul( p.n, min_dot - _abs( dot ) ) );
		dot = p.n.dotproduct( shift );
	}
	VERIFY( !fis_zero( dot ) );
	float shift_m = ( -p.d - p.n.dotproduct( point ) )/dot;
	if(collide && shift_m > 0.f )
		return false;
	clamp( shift_m, -collide_dist, collide_dist );
	shift.mul( shift_m );
	xm.c.add( shift );
#if	0
	if(shift_m > 0.f)
	{
		DBG_OpenCashedDraw();
		DBG_DrawLine( toe, Fvector().add( toe, shift ), D3DCOLOR_XRGB( 255, 255, 255 )  );
		DBG_ClosedCashedDraw( 1000 );
	}
#endif
	return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////
// Функция ConeSphereIntersection
// Пересечение конуса (не ограниченного) со сферой
// Необходима для определения пересечения копыта плоти с баунд-сферой крысы
// Параметры: ConeVertex - вершина конуса, ConeAngle - угол конуса (между поверхностью и высотой)
// ConeDir - направление конуса, SphereCenter - центр сферы, SphereRadius - радиус сферы
bool CAI_Flesh::ConeSphereIntersection(Fvector ConeVertex, float ConeAngle, Fvector ConeDir, Fvector SphereCenter, float SphereRadius)
{
	float fInvSin = 1.0f/_sin(ConeAngle);
	float fCosSqr = _cos(ConeAngle)*_cos(ConeAngle);

	
	Fvector kCmV;	kCmV.sub(SphereCenter,ConeVertex);
	Fvector kD		= kCmV;
	Fvector tempV	= ConeDir;
	tempV.mul		(SphereRadius* fInvSin);
	kD.add			(tempV);

	float fDSqrLen = kD.square_magnitude();
	float fE = kD.dotproduct(ConeDir);
	if ( fE > 0.0f && fE*fE >= fDSqrLen*fCosSqr ) {
		
		float fSinSqr = _sin(ConeAngle)*_sin(ConeAngle);

		fDSqrLen = kCmV.square_magnitude();
		fE = -kCmV.dotproduct(ConeDir);
		if ( fE > 0.0f && fE*fE >= fDSqrLen*fSinSqr ) {
			float fRSqr = SphereRadius*SphereRadius;
			return fDSqrLen <= fRSqr;
		} else return true;
	} 
	
	return false;
}

bool CElevatorState::GetControlDir(Fvector& dir)
{
	bool ret=true;
	VERIFY(m_ladder&&m_character);
	Fvector d;
	float dist;
	switch(m_state)
	{
	case	clbDepart		: 
	case	clbNoLadder		:
	case	clbNone			: 		break;			
	case 	clbNearUp		:		dist= m_ladder->DDUpperP(m_character,d);
									if(	dXZDotNormalized(d,m_character->CamDir())>look_angle_cosine&&
										!fis_zero(dist,EPS_L)&&m_character->ControlAccel().dotproduct(d)>0.f) dir.set(d);
									break;						
	case 	clbNearDown		:		
									dist=m_ladder->DDLowerP(m_character,d);
									if(dXZDotNormalized(d,m_character->CamDir())>look_angle_cosine&&
									   !fis_zero(dist,EPS_L)&&m_character->ControlAccel().dotproduct(d)>0.f) dir.set(d);
									break;					
	case 	clbClimbingUp	:		m_ladder->DDAxis(dir);
									m_ladder->DDToAxis(m_character,d);
									dir.add(d);dir.normalize();
									break;					
	case 	clbClimbingDown	:		m_ladder->DDToAxis(m_character,d);
									if(m_ladder->BeforeLadder(m_character)||d.dotproduct(dir)>0.f)
									{
										m_ladder->DDAxis(dir);
										dir.invert();
										dir.add(d);dir.normalize();
									}
									else 
									{
#ifdef DEBUG
										if(ph_dbg_draw_mask.test(phDbgLadder))
										{
											Msg("no c dir");
										}
#endif
										ret=false;
									}
									break;				
	}
	return ret;
}

u32						u8_vec4			(Fvector N, u8 A=0)
{
	N.add				(1.f);
	N.mul				(.5f*255.f);
	s32 nx				= iFloor(N.x);	clamp(nx,0,255);
	s32 ny				= iFloor(N.y);	clamp(ny,0,255);
	s32 nz				= iFloor(N.z);	clamp(nz,0,255);
	return				color_rgba(nx,ny,nz,A);
}

void 	get_pos_bones(const vertBoned4W &vert, Fvector& p, CKinematics* Parent )
{
		Fmatrix& M0		= Parent->LL_GetBoneInstance( vert.m[0] ).mRenderTransform;
        Fmatrix& M1		= Parent->LL_GetBoneInstance( vert.m[1] ).mRenderTransform;
        Fmatrix& M2		= Parent->LL_GetBoneInstance( vert.m[2] ).mRenderTransform;
		Fmatrix& M3		= Parent->LL_GetBoneInstance( vert.m[3] ).mRenderTransform;

		Fvector	P0,P1,P2,P3;
		M0.transform_tiny(P0, vert.P); P0.mul(vert.w[0]);
        M1.transform_tiny(P1, vert.P); P1.mul(vert.w[1]);
		M2.transform_tiny(P2, vert.P); P2.mul(vert.w[2]);
        M3.transform_tiny(P3,vert.P); P3.mul(1.0f-vert.w[0]-vert.w[1]-vert.w[2]);
		
		p			= P0;
		p.add		(P1);
		p.add		(P2);
		p.add		(P3);
}

	IC void			_init		(COLLIDER* CL, Fvector* V, TRI* T, const Fvector& C, const Fvector& E)
	{
		dest		= CL;
		verts		= V;
		tris		= T;
		center		= Point(C.x,C.y,C.z);
		extents		= Point(E.x,E.y,E.z);
		b_min.sub	(C,E);
		b_max.add	(C,E);
	}

void	get_box(CPhysicsShell*	shell,const	Fmatrix& form,	Fvector&	sz,Fvector&	c)
{
	c.set(0,0,0);
	for(int i=0;3>i;++i)
	{	
		float lo,hi;
		const	Fvector &ax=cast_fv(((const	float*)&form+i*4));
		shell->get_Extensions(ax,0,lo,hi);
		sz[i]=hi-lo;c.add(Fvector().mul(ax,(lo+hi)/2));
	}
}

void random_dir(Fvector& tgt_dir, const Fvector& src_dir, float dispersion)
{
	float sigma			= dispersion/3.f;
	float alpha			= clampr		(_nrand(sigma),-dispersion,dispersion);
	float theta			= Random.randF	(0,PI);
	float r 			= tan			(alpha);
	Fvector 			U,V,T;
	Fvector::generate_orthonormal_basis	(src_dir,U,V);
	U.mul				(r*_sin(theta));
	V.mul				(r*_cos(theta));
	T.add				(U,V);
	tgt_dir.add			(src_dir,T).normalize();
}

IC BOOL	ValidateMerge	(Fbox& bb_base, Fbox& bb, float& volume, float SLimit)
{
	// Size
	Fbox	merge;	merge.merge		(bb_base,bb);
	Fvector sz;		merge.getsize	(sz);	sz.add	(EPS_L);
	if (sz.x>SLimit)		return FALSE;	// Don't exceed limits (4/3 GEOM)
	if (sz.y>SLimit)		return FALSE;
	if (sz.z>SLimit)		return FALSE;

	// Volume
	volume		= merge.getvolume	();

	// OK
	return TRUE;
}