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

//.........这里部分代码省略.........
		u32 move	= mcAnyMove|mcAccel;

		if (((mstate_real&mcCrouch)))
		{
			if (!isActorAccelerated(mstate_real, IsZoomAimingMode()) && isActorAccelerated(mstate_wf, IsZoomAimingMode()))
			{
				character_physics_support()->movement()->EnableCharacter();
				if(!character_physics_support()->movement()->ActivateBoxDynamic(1))move	&=~mcAccel;
			}

			if (isActorAccelerated(mstate_real, IsZoomAimingMode()) && !isActorAccelerated(mstate_wf, IsZoomAimingMode()))
			{
				character_physics_support()->movement()->EnableCharacter();
				if(character_physics_support()->movement()->ActivateBoxDynamic(2))mstate_real	&=~mcAccel;
			}
		}

		if ((mstate_wf&mcSprint) && !CanSprint())
		{
			mstate_wf				&= ~mcSprint;
		}

		mstate_real &= (~move);
		mstate_real |= (mstate_wf & move);

		if(mstate_wf&mcSprint)
			mstate_real|=mcSprint;
		else
			mstate_real&=~mcSprint;
		if(!(mstate_real&(mcFwd|mcLStrafe|mcRStrafe))||mstate_real&(mcCrouch|mcClimb)|| !isActorAccelerated(mstate_wf, IsZoomAimingMode()))
		{
			mstate_real&=~mcSprint;
			mstate_wishful&=~mcSprint;
		}
				
		// check player move state
		if (mstate_real&mcAnyMove)
		{
			BOOL	bAccelerated		= isActorAccelerated(mstate_real, IsZoomAimingMode())&&CanAccelerate();



			// correct "mstate_real" if opposite keys pressed
			if (_abs(vControlAccel.z)<EPS)	mstate_real &= ~(mcFwd+mcBack		);
			if (_abs(vControlAccel.x)<EPS)	mstate_real &= ~(mcLStrafe+mcRStrafe);

			// normalize and analyze crouch and run
			float	scale				= vControlAccel.magnitude();
			if (scale>EPS)	{
				scale	=	m_fWalkAccel/scale;
				if (bAccelerated)
					if (mstate_real&mcBack)
						scale *= m_fRunBackFactor;
					else
						scale *= m_fRunFactor;
				else
					if (mstate_real&mcBack)
						scale *= m_fWalkBackFactor;



				if (mstate_real&mcCrouch)	scale *= m_fCrouchFactor;
				if (mstate_real&mcClimb)	scale *= m_fClimbFactor;
				if (mstate_real&mcSprint)	scale *= m_fSprintFactor;

				if (mstate_real&(mcLStrafe|mcRStrafe) && !(mstate_real&mcCrouch))
				{
					if (bAccelerated)
						scale *= m_fRun_StrafeFactor;
					else
						scale *= m_fWalk_StrafeFactor;
				}

				vControlAccel.mul			(scale);
			}else{
				//				mstate_real	&= ~mcAnyMove;
			}
		}		
	}else{
		//		mstate_real			&=~ mcAnyMove;
	}

	//-------------------------------------------------------------------------------	
	

	//transform local dir to world dir
	Fmatrix				mOrient;
	mOrient.rotateY		(-r_model_yaw);
	mOrient.transform_dir(vControlAccel);
	//XFORM().transform_dir(vControlAccel);

	/*
	if(mstate_real&mcClimb&&mstate_real&mcAnyMove&&
	inventory().ActiveItem()&&inventory().ActiveItem()->HandDependence()==hd2Hand)
	{
		//inventory().ActiveItem()->Deactivate();
		inventory().Activate(NO_ACTIVE_SLOT);
	}
*/
}

void		CTeleWhirlwindObject::		raise					(float step)
{

		CPhysicsShell*	p					=	get_object()	->PPhysicsShell();
	
		if(!p||!p->isActive())	
			return;
		else
			{
				p->SetAirResistance(0.f,0.f);
				p->set_ApplyByGravity(TRUE);
			}
		u16				element_number		=	p				->get_ElementsNumber();
		Fvector			center				=	m_telekinesis	->Center();
		CPhysicsElement* maxE=p->get_ElementByStoreOrder(0);
		for(u16 element=0;element<element_number;++element)
		{
			float k=strength;//600.f;
			float predict_v_eps=0.1f;
			float mag_eps	   =.01f;

			CPhysicsElement* E=	p->get_ElementByStoreOrder(element);
			if(maxE->getMass()<E->getMass())	maxE=E;
			if (!E->isActive()) continue;
			Fvector pos=E->mass_Center();

			Fvector diff;
			diff.sub(center,pos);
			float mag=_sqrt(diff.x*diff.x+diff.z*diff.z);
			Fvector lc;lc.set(center);
			if(mag>1.f)
			{
				lc.y/=mag;
			}
			diff.sub(lc,pos);
			mag=diff.magnitude();
			float accel=k/mag/mag/mag;//*E->getMass()
			Fvector dir;
			if(mag<mag_eps)
			{
				accel=0.f;
				//Fvector zer;zer.set(0,0,0);
				//E->set_LinearVel(zer);
				dir.random_dir();
			}
			else
			{
				dir.set(diff);dir.mul(1.f/mag);
			}
			Fvector vel;
			E->get_LinearVel(vel);
			float delta_v=accel*fixed_step;
			Fvector delta_vel; delta_vel.set(dir);delta_vel.mul(delta_v);
			Fvector predict_vel;predict_vel.add(vel,delta_vel);
			Fvector delta_pos;delta_pos.set(predict_vel);delta_pos.mul(fixed_step);
			Fvector predict_pos;predict_pos.add(pos,delta_pos);
			
			Fvector predict_diff;predict_diff.sub(lc,predict_pos);
			float predict_mag=predict_diff.magnitude();
			float predict_v=predict_vel.magnitude();

			Fvector force;force.set(dir);
			if(predict_mag>mag && predict_vel.dotproduct(dir)>0.f && predict_v>predict_v_eps)
			{
	
				Fvector motion_dir;motion_dir.set(predict_vel);motion_dir.mul(1.f/predict_v);
				float needed_d=diff.dotproduct(motion_dir);
				Fvector needed_diff;needed_diff.set(motion_dir);needed_diff.mul(needed_d);
				Fvector nearest_p;nearest_p.add(pos,needed_diff);//
				Fvector needed_vel;needed_vel.set(needed_diff);needed_vel.mul(1.f/fixed_step);
				force.sub(needed_vel,vel);
				force.mul(E->getMass()/fixed_step);
			}
			else
			{
				force.mul(accel*E->getMass());
			}
			
			
			E->applyForce(force.x,force.y+get_object()->EffectiveGravity()*E->getMass(),force.z);
		}
		Fvector dist;dist.sub(center,maxE->mass_Center());
		if(dist.magnitude()<m_telekinesis->keep_radius()&&b_destroyable)
		{
			p->setTorque(Fvector().set(0,0,0));
			p->setForce(Fvector().set(0,0,0));
			p->set_LinearVel(Fvector().set(0,0,0));
			p->set_AngularVel(Fvector().set(0,0,0));
			switch_state(TS_Keep);
		}
}

void CPEDef::ExecuteCollision(PAPI::Particle* particles, u32 p_cnt, float dt, CParticleEffect* owner, CollisionCallback cb)
{
	pVector pt,n;
	// Must traverse list in reverse order so Remove will work
	for(int i = p_cnt-1; i >= 0; i--){
		Particle &m = particles[i];

		bool pick_needed;
		int pick_cnt=0;
		do{		
			pick_needed = false;
			Fvector 	dir;
			dir.sub		(m.pos,m.posB);
			float dist 	= dir.magnitude();
			if (dist>=EPS){
				dir.div	(dist);
#ifdef _EDITOR                
				if (Tools->RayPick(m.posB,dir,dist,&pt,&n)){
#else
				collide::rq_result	RQ;
                collide::rq_target	RT = m_Flags.is(dfCollisionDyn)?collide::rqtBoth:collide::rqtStatic;
				if (g_pGameLevel->ObjectSpace.RayPick(m.posB,dir,dist,RT,RQ,NULL)){	
					pt.mad	(m.posB,dir,RQ.range);
					if (RQ.O){
						n.set(0.f,1.f,0.f);
					}else{
						CDB::TRI*	T		=  	g_pGameLevel->ObjectSpace.GetStaticTris()+RQ.element;
						Fvector*	verts	=	g_pGameLevel->ObjectSpace.GetStaticVerts();
						n.mknormal(verts[T->verts[0]],verts[T->verts[1]],verts[T->verts[2]]);
					}
#endif
					pick_cnt++;
					if (cb&&(pick_cnt==1)) if (!cb(owner,m,pt,n)) break;
					if (m_Flags.is(dfCollisionDel)){ 
	                   	ParticleManager()->RemoveParticle(owner->m_HandleEffect,i);
					}else{
						// Compute tangential and normal components of velocity
						float nmag = m.vel * n;
						pVector vn(n * nmag); 	// Normal Vn = (V.N)N
						pVector vt(m.vel - vn);	// Tangent Vt = V - Vn

						// Compute _new velocity heading out:
						// Don't apply friction if tangential velocity < cutoff
						if(vt.length2() <= m_fCollideSqrCutoff){
							m.vel = vt - vn * m_fCollideResilience;
						}else{
							m.vel = vt * m_fCollideOneMinusFriction - vn * m_fCollideResilience;
						}
						m.pos	= m.posB + m.vel * dt; 
						pick_needed = true;
					}
				}
			}else{
				m.pos	= m.posB;
			}
		}while(pick_needed&&(pick_cnt<2));
	}
}

//------------------------------------------------------------------------------
// I/O part
//------------------------------------------------------------------------------
BOOL CPEDef::Load(IReader& F)
{
	R_ASSERT		(F.find_chunk(PED_CHUNK_VERSION));
	u16 version		= F.r_u16();

	if (version!=PED_VERSION)
		return FALSE;

	R_ASSERT		(F.find_chunk(PED_CHUNK_NAME));
	F.r_stringZ		(m_Name);

	R_ASSERT		(F.find_chunk(PED_CHUNK_EFFECTDATA));
	m_MaxParticles	= F.r_u32();

	u32 sz			= F.find_chunk(PED_CHUNK_ACTIONLIST); R_ASSERT(sz);
	m_Actions.w		(F.pointer(),sz);

	F.r_chunk		(PED_CHUNK_FLAGS,&m_Flags);

	if (m_Flags.is(dfSprite)){
		R_ASSERT	(F.find_chunk(PED_CHUNK_SPRITE));
		F.r_stringZ	(m_ShaderName);
		F.r_stringZ	(m_TextureName);
	}

	if (m_Flags.is(dfFramed)){
		R_ASSERT	(F.find_chunk(PED_CHUNK_FRAME));
		F.r			(&m_Frame,sizeof(SFrame));
	}

	if (m_Flags.is(dfTimeLimit)){
		R_ASSERT(F.find_chunk(PED_CHUNK_TIMELIMIT));
		m_fTimeLimit= F.r_float();
	}

	if (m_Flags.is(dfCollision)){
		R_ASSERT(F.find_chunk(PED_CHUNK_COLLISION));
		m_fCollideOneMinusFriction 	= F.r_float();
//.........这里部分代码省略.........

//-----------------------------------------------------------------------------
// Name: collideWithWorld()
// Desc: Recursive part of the collision response. This function is the
//       one who actually calls the collision check on the meshes
//-----------------------------------------------------------------------------
Fvector msimulator_CollideWithWorld(SCollisionData& cl, Fvector position, Fvector velocity, WORD cnt)
{
    //
    msimulator_ResolveStuck(cl,position);

    // do we need to worry ?
//	if (fsimilar(position.x,target.x,EPS_L)&&fsimilar(position.z,target.z,EPS_L))
    if (velocity.magnitude()<EPS_L)
    {
        cl.vVelocity.set(0,0,0);
        return position;
    }
    if (cnt>psCollideActDepth)				return cl.vLastSafePosition;

    Fvector ret_pos;
    Fvector destinationPoint;
    destinationPoint.add(position,velocity);

    // reset the collision package we send to the mesh
    cl.vVelocity.set	(velocity);
    cl.vSourcePoint.set	(position);
    cl.bFoundCollision	= FALSE;
    cl.bStuck			= FALSE;
    cl.fNearestDistance	= -1;

    // Check collision
    msimulator_CheckCollision	(cl);

    // check return value here, and possibly call recursively
    if (cl.bFoundCollision == FALSE  && !cl.bStuck)
    {
        // if no collision move very close to the desired destination.
        float l = velocity.magnitude();
        Fvector V;
        V.mul(velocity,(l-cl_epsilon)/l);

        // return the final position
        ret_pos.add(position,V);

        // update the last safe position for future error recovery
        cl.vLastSafePosition.set(ret_pos);
        return ret_pos;
    } else {
        // There was a collision
        // OK, first task is to move close to where we hit something :
        Fvector newSourcePoint;

        // If we are stuck, we just back up to last safe position
        if (cl.bStuck) {
            return cl.vLastSafePosition;
        }

        // only update if we are not already very close
        if (cl.fNearestDistance >= cl_epsilon) {
            Fvector V;
            V.set(velocity);
            V.set_length(cl.fNearestDistance-cl_epsilon);
            newSourcePoint.add(cl.vSourcePoint,V);
        } else {
            newSourcePoint.set(cl.vSourcePoint);
        }

        // Now we must calculate the sliding plane
        Fvector slidePlaneOrigin;
        slidePlaneOrigin.set(cl.vNearestPolygonIntersectionPoint);
        Fvector slidePlaneNormal;
        slidePlaneNormal.sub(newSourcePoint,cl.vNearestPolygonIntersectionPoint);

        // We now project the destination point onto the sliding plane
        float l = intersectRayPlane(destinationPoint, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);

        // We can now calculate a _new destination point on the sliding plane
        Fvector newDestinationPoint;
        newDestinationPoint.mad(destinationPoint,slidePlaneNormal,l);

        // Generate the slide xr_vector, which will become our _new velocity xr_vector
        // for the next iteration
        Fvector newVelocityVector;
        newVelocityVector.sub(newDestinationPoint, cl.vNearestPolygonIntersectionPoint);

        // now we recursively call the function with the _new position and velocity
        cl.vLastSafePosition.set(position);
        return msimulator_CollideWithWorld(cl, newSourcePoint, newVelocityVector,cnt+1);
    }
}

void CDeflector::OA_Export()
{
	if (UVpolys.empty()) return;

	// Correct normal
	//  (semi-proportional to pixel density)
	FPU::m64r		();
	Fvector			tN;
	tN.set			(0,0,0);
	float density	= 0;
	float fcount	= 0;
	for (UVIt it = UVpolys.begin(); it!=UVpolys.end(); it++)
	{
		Face	*F = it->owner;
		Fvector	SN;
		SN.set	(F->N);
		SN.mul	(1+EPS*F->CalcArea());
		tN.add	(SN);

		density	+= F->Shader().lm_density;
		fcount	+= 1.f;
	}
	if (tN.magnitude()>EPS_S && _valid(tN))	normal.set(tN).normalize();
	else									clMsg("* ERROR: Internal precision error in CDeflector::OA_Export");
	density			/= fcount;
	
	// Orbitrary Oriented Ortho - Projection
	Fmatrix		mView;
    Fvector		at,from,up,right,y;
	at.set		(0,0,0);
	from.add	(at,normal);
	y.set		(0,1,0);
	if (_abs(normal.y)>.99f)		y.set(1,0,0);
	right.crossproduct(y,normal);	right.normalize_safe();
	up.crossproduct(normal,right);	up.normalize_safe();
	mView.build_camera(from,at,up);

	Fbox bb; bb.invalidate();
	for (UVIt it = UVpolys.begin(); it!=UVpolys.end(); it++)
	{
		UVtri	*T = &*it;
		Face	*F = T->owner;
		Fvector	P;	// projected

		for (int i=0; i<3; i++) {
			mView.transform_tiny	(P,F->v[i]->P);
			T->uv[i].set			(P.x,P.y);
			bb.modify				(F->v[i]->P);
		}
	}
	bb.getsphere(Sphere.P,Sphere.R);

	// UV rect
	Fvector2		min,max,size;
	GetRect			(min,max);
	size.sub		(max,min);

	// Surface
	u32 dwWidth		= iCeil(size.x*g_params.m_lm_pixels_per_meter*density+.5f); clamp(dwWidth, 1u,512u-2*BORDER);
	u32 dwHeight	= iCeil(size.y*g_params.m_lm_pixels_per_meter*density+.5f); clamp(dwHeight,1u,512u-2*BORDER);
	layer.create	(dwWidth,dwHeight);
}

bool CPHMovementControl:: ActivateBoxDynamic(DWORD id,int num_it/*=8*/,int num_steps/*5*/,float resolve_depth/*=0.01f*/)
{
	bool  character_exist=CharacterExist();
	if(character_exist&&trying_times[id]!=u32(-1))
	{
		Fvector dif;dif.sub(trying_poses[id],cast_fv(dBodyGetPosition(m_character->get_body())));
		if(Device.dwTimeGlobal-trying_times[id]<500&&dif.magnitude()<0.05f)
																	return false;
	}
	if(!m_character||m_character->PhysicsRefObject()->PPhysicsShell())return false;
	DWORD old_id=BoxID();

	bool  character_disabled=character_exist && !m_character->IsEnabled();
	if(character_exist&&id==old_id)return true;

	if(!character_exist)
	{
		CreateCharacter();
	}

	//m_PhysicMovementControl->ActivateBox(id);
	m_character->CPHObject::activate();
	ph_world->Freeze();
	UnFreeze();

	saved_callback=ObjectContactCallback();
	SetOjectContactCallback(TestDepthCallback);
	SetFootCallBack(TestFootDepthCallback);
	max_depth=0.f;



	//////////////////////////////////pars///////////////////////////////////////////
//	int		num_it=8;
//	int		num_steps=5;
//	float	resolve_depth=0.01f;

	
	if(!character_exist)
	{
		num_it=20;
		num_steps=1;		
		resolve_depth=0.1f;
	}
	///////////////////////////////////////////////////////////////////////
	float	fnum_it=float(num_it);
	float	fnum_steps=float(num_steps);
	float	fnum_steps_r=1.f/fnum_steps;

	Fvector vel;
	Fvector pos;
	GetCharacterVelocity(vel);
	GetCharacterPosition(pos);
	//const Fbox& box =Box();
	float pass=	character_exist ? _abs(Box().getradius()-boxes[id].getradius()) : boxes[id].getradius();
	float max_vel=pass/2.f/fnum_it/fnum_steps/fixed_step;
	float max_a_vel=M_PI/8.f/fnum_it/fnum_steps/fixed_step;
	dBodySetForce(GetBody(),0.f,0.f,0.f);
	dBodySetLinearVel(GetBody(),0.f,0.f,0.f);
	Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
	CVelocityLimiter vl(GetBody(),max_vel,max_vel);
	max_vel=1.f/fnum_it/fnum_steps/fixed_step;

	bool	ret=false;
	m_character->SwitchOFFInitContact();
	vl.Activate();
	vl.l_limit*=(fnum_it*fnum_steps/5.f);
	vl.y_limit=vl.l_limit;
////////////////////////////////////
	for(int m=0;30>m;++m)
	{
		Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
		EnableCharacter();
		m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
		max_depth=0.f;
		ph_world->Step();
		if(max_depth	<	resolve_depth) 
		{
			break;
		}	
		ph_world->CutVelocity(max_vel,max_a_vel);
	}
	vl.l_limit/=(fnum_it*fnum_steps/5.f);
	vl.y_limit=vl.l_limit;
/////////////////////////////////////

	for(int m=0;num_steps>m;++m)
	{
		float param =fnum_steps_r*(1+m);
		InterpolateBox(id,param);
		ret=false;
		for(int i=0;num_it>i;++i){
			max_depth=0.f;
			Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
			EnableCharacter();
			m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
			ph_world->Step();
			ph_world->CutVelocity(max_vel,max_a_vel);
			if(max_depth	<	resolve_depth) 
			{
//.........这里部分代码省略.........

// -----------------------------------------------------------------------
void msimulator_CheckCollision(SCollisionData& cl)
{
    // from package
    Fvector			source;
    Fvector			velocity;
    source.set		(cl.vSourcePoint);
    velocity.set	(cl.vVelocity);

    // keep a copy of this as it's needed a few times
    Fvector			normalizedVelocity;
    normalizedVelocity.normalize_safe(cl.vVelocity);

    // intersection data
    Fvector			sIPoint;    // sphere intersection point
    Fvector			pIPoint;    // plane intersection point
    Fvector			polyIPoint; // polygon intersection point

    // how long is our velocity
    float			distanceToTravel = velocity.magnitude();

    float			distToPlaneIntersection;
    float			distToEllipsoidIntersection;

    for (u32 i_t=0; i_t!=clContactedT.size(); i_t++) {
        cl_tri& T=clContactedT[i_t];

        //ignore backfaces. What we cannot see we cannot collide with ;)
        if (T.N.dotproduct(normalizedVelocity) < 0.0f)
        {
            // calculate sphere intersection point (in future :)
            // OLES: 'cause our radius has unit length, this point lies exactly on sphere
            sIPoint.sub(source, T.N);

            // find the plane intersection point
            // classify point to determine if ellipsoid span the plane
            BOOL bInsideTri;
            if ((sIPoint.dotproduct(T.N)+T.d) < -EPS_S)
            {
                // plane is embedded in ellipsoid / sphere
                // find plane intersection point by shooting a ray from the
                // sphere intersection point along the planes normal.
                bInsideTri = CDB::TestRayTri2(sIPoint,T.N,T.p,distToPlaneIntersection);

                // calculate plane intersection point
                pIPoint.mad(sIPoint,T.N,distToPlaneIntersection);
            }
            else
            {
                // shoot ray along the velocity xr_vector
                bInsideTri = CDB::TestRayTri2(sIPoint,normalizedVelocity,T.p,distToPlaneIntersection);

                // calculate plane intersection point
                pIPoint.mad(sIPoint,normalizedVelocity,distToPlaneIntersection);
            }


            // find polygon intersection point. By default we assume its equal to the
            // plane intersection point

            polyIPoint.set(pIPoint);
            distToEllipsoidIntersection = distToPlaneIntersection;

            if (!bInsideTri)
            {
                // if not in triangle
                closestPointOnTriangle(polyIPoint, T, pIPoint);

                Fvector _normalizedVelocity;
                _normalizedVelocity.invert(normalizedVelocity);
                distToEllipsoidIntersection = intersectRaySphere(polyIPoint, _normalizedVelocity, source, 1.0f);

                if (distToEllipsoidIntersection >= 0) {
                    // calculate true sphere intersection point
                    sIPoint.mad(polyIPoint, normalizedVelocity, -distToEllipsoidIntersection);
                }
            }

            // Here we do the error checking to see if we got ourself stuck last frame
            if (CheckPointInSphere(polyIPoint, source, 1.0f))
            {
                cl.bStuck = TRUE;
            }

            // Ok, now we might update the collision data if we hit something
            if ((distToEllipsoidIntersection >= 0) && (distToEllipsoidIntersection <= distanceToTravel))
            {
                if ((cl.bFoundCollision == FALSE) || (distToEllipsoidIntersection < cl.fNearestDistance))
                {
                    // if we are hit we have a closest hit so far. We save the information
                    cl.fNearestDistance = distToEllipsoidIntersection;
                    cl.vNearestIntersectionPoint.set(sIPoint);
                    cl.vNearestPolygonIntersectionPoint.set(polyIPoint);
                    cl.bFoundCollision = TRUE;
                }
            }
        } // if not backface
    } // for all faces
}

void CPHMovementControl::Calculate(const xr_vector<DetailPathManager::STravelPathPoint>& path,float speed,  u32& travel_point,  float& precision  )
{
#ifdef DEBUG
	if(ph_dbg_draw_mask1.test(ph_m1_DbgTrackObject)&&(!!pObject->cName())&&stricmp(PH_DBG_ObjectTrack(),*pObject->cName())==0)
	{
		Msg("CPHMovementControl::Calculate in %s (Object Position) %f,%f,%f",PH_DBG_ObjectTrack(),pObject->Position().x,pObject->Position().y,pObject->Position().z);
		Msg("CPHMovementControl::Calculate in %s (CPHMovementControl::vPosition) %f,%f,%f",PH_DBG_ObjectTrack(),vPosition.x,vPosition.y,vPosition.z);
	}
#endif
	if(!m_character->b_exist)	return;
	if(m_capture) 
	{
		if(m_capture->Failed()) xr_delete(m_capture);
	}
	

	Fvector new_position;
	m_character->IPosition(new_position);

	int index=0;//nearest point
	//float distance;//distance

	bool  near_line;
	m_path_size=path.size();
	Fvector dir;
	dir.set(0,0,0);
	if(m_path_size==0)
	{
		speed=0;
		vPosition.set(new_position);
	}
	else if(b_exect_position)
	{
		m_start_index=travel_point;

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		if((m_path_size-1)>(int)travel_point)
			dir.sub(path[travel_point+1].position,path[travel_point].position);
		else
			dir.sub(path[travel_point].position,new_position);
		m_start_index=travel_point;
		dir.y=0.f;
		dir.normalize_safe();
		vPosition.set(new_position);
		m_path_distance=0;
		SetPathDir	(dir);
		vPathPoint.set(vPosition);


	}
	else {
		Fvector dif;

		dif.sub(new_position,vPathPoint);
		float radius = dif.magnitude()*2.f;
		if(m_path_size==1)
		{
			speed=0.f;
			vPosition.set(new_position);	//todo - insert it in PathNearestPoint
			index=0;
			vPathPoint.set(path[0].position);
			Fvector _d;
			_d.sub(path[0].position,new_position);
			SetPathDir	(_d);
			m_path_distance=GetPathDir().magnitude();
			if(m_path_distance>EPS)
			{
				Fvector _d = GetPathDir();
				_d.mul(1.f/m_path_distance);
				SetPathDir(_d);
			}
			near_line=false;
		}
		else
		{
			m_path_distance=dInfinity;
			near_line=true;
			if(m_start_index<m_path_size)
			{
				PathNearestPointFindUp(path,new_position,index,radius,near_line);
				PathNearestPointFindDown(path,new_position,index,radius,near_line);
			}
			if(m_path_distance>radius)
			{
				m_start_index=0;
				PathNearestPoint(path,new_position,index,near_line);
			}
			vPosition.set(new_position);//for PathDirLine && PathDirPoint
			if(near_line) PathDIrLine(path,index,m_path_distance,precision,dir);
			else		  PathDIrPoint(path,index,m_path_distance,precision,dir);
			
	
			travel_point=(u32)index;
			m_start_index=index;
			if(fis_zero(speed)) dir.set(0,0,0);
		}

	}
	
//.........这里部分代码省略.........

BOOL CObjectSpace::_RayTest	( const Fvector &start, const Fvector &dir, float range, collide::rq_target tgt, collide::ray_cache* cache, CObject* ignore_object)
{
	VERIFY					(_abs(dir.magnitude()-1)<EPS);
	r_temp.r_clear			();

	xrc.ray_options			(CDB::OPT_ONLYFIRST);
	collide::ray_defs	Q	(start,dir,range,CDB::OPT_ONLYFIRST,tgt);

	// dynamic test
	if (tgt&rqtDyn){
		u32			d_flags =	STYPE_COLLIDEABLE|((tgt&rqtObstacle)?STYPE_OBSTACLE:0)|((tgt&rqtShape)?STYPE_SHAPE:0);
		// traverse object database
		g_SpatialSpace->q_ray	(r_spatial,0,d_flags,start,dir,range);
		// Determine visibility for dynamic part of scene
		for (u32 o_it=0; o_it<r_spatial.size(); o_it++)
		{
			ISpatial*	spatial			= r_spatial[o_it];
			CObject*	collidable		= spatial->dcast_CObject	();
			if (collidable && (collidable!=ignore_object))	{
				ECollisionFormType tp	= collidable->collidable.model->Type();
				if ((tgt&(rqtObject|rqtObstacle))&&(tp==cftObject)&&collidable->collidable.model->_RayQuery(Q,r_temp))	return TRUE;
				if ((tgt&rqtShape)&&(tp==cftShape)&&collidable->collidable.model->_RayQuery(Q,r_temp))		return TRUE;
			}
		}
	}
	// static test
	if (tgt&rqtStatic){
		// If we get here - test static model
		if (cache) 
		{
			// 0. similar query???
			if (cache->similar(start,dir,range))	{
				return cache->result;
			}

			// 1. Check cached polygon
			float _u,_v,_range;
			if (CDB::TestRayTri(start,dir,cache->verts,_u,_v,_range,false)) 
			{
				if (_range>0 && _range<range) return TRUE;
			}
			
			// 2. Polygon doesn't pick - real database query
			xrc.ray_query	(&Static,start,dir,range);
			if (0==xrc.r_count()) {
				cache->set		(start,dir,range,FALSE);
				return FALSE;
			} else {
				// cache polygon
				cache->set		(start,dir,range,TRUE);
				CDB::RESULT*	R	= xrc.r_begin();
				CDB::TRI&		T	= Static.get_tris() [ R->id ];
				Fvector*		V	= Static.get_verts();
				cache->verts[0].set	(V[T.verts[0]]);
				cache->verts[1].set	(V[T.verts[1]]);
				cache->verts[2].set	(V[T.verts[2]]);
				return TRUE;
			}
		} else {
			xrc.ray_query		(&Static,start,dir,range);
			return xrc.r_count	();
		}
	}
	return FALSE;
}

void CPoltergeisMovementManager::move_along_path(CPHMovementControl *movement_control, Fvector &dest_position, float time_delta)
{
	if (!m_monster->is_hidden()) {
		inherited::move_along_path(movement_control, dest_position, time_delta);
		return;
	}

	dest_position		= m_monster->m_current_position;

	// Если нет движения по пути
	if (!enabled() || 
		path_completed() || 
		detail().path().empty() ||
		detail().completed(m_monster->m_current_position,true) || 
		(detail().curr_travel_point_index() >= detail().path().size() - 1) ||
		fis_zero(old_desirable_speed())
		)
	{
		m_speed	= 0.f;
		dest_position		= CalculateRealPosition();
		return;
	}

	if (time_delta < EPS) {
		dest_position	= CalculateRealPosition();
		return;
	}

	// Вычислить пройденную дистанцию, определить целевую позицию на маршруте, 
	//			 изменить detail().curr_travel_point_index()

	float				desirable_speed		=	old_desirable_speed();				// желаемая скорость объекта
	float				dist				=	desirable_speed * time_delta;		// пройденное расстояние в соостветствие с желаемой скоростью 
	float				desirable_dist		=	dist;

	// определить целевую точку
	Fvector				target;

	u32 prev_cur_point_index = detail().curr_travel_point_index();

	// обновить detail().curr_travel_point_index() в соответствие с текущей позицией
	while (detail().curr_travel_point_index() < detail().path().size() - 2) {

		float pos_dist_to_cur_point			= dest_position.distance_to(detail().path()[detail().curr_travel_point_index()].position);
		float pos_dist_to_next_point		= dest_position.distance_to(detail().path()[detail().curr_travel_point_index()+1].position);
		float cur_point_dist_to_next_point	= detail().path()[detail().curr_travel_point_index()].position.distance_to(detail().path()[detail().curr_travel_point_index()+1].position);

		if ((pos_dist_to_cur_point > cur_point_dist_to_next_point) && (pos_dist_to_cur_point > pos_dist_to_next_point)) {
			++detail().m_current_travel_point;			
		} else break;
	}

	target.set			(detail().path()[detail().curr_travel_point_index() + 1].position);
	// определить направление к целевой точке
	Fvector				dir_to_target;
	dir_to_target.sub	(target, dest_position);

	// дистанция до целевой точки
	float				dist_to_target = dir_to_target.magnitude();

	while (dist > dist_to_target) {
		dest_position.set	(target);

		if (detail().curr_travel_point_index() + 1 >= detail().path().size())	break;
		else {
			dist			-= dist_to_target;
			++detail().m_current_travel_point;
			if ((detail().curr_travel_point_index()+1) >= detail().path().size())
				break;
			target.set			(detail().path()[detail().curr_travel_point_index() + 1].position);
			dir_to_target.sub	(target, dest_position);
			dist_to_target		= dir_to_target.magnitude();
		}
	}

	if (prev_cur_point_index != detail().curr_travel_point_index()) on_travel_point_change(prev_cur_point_index);

	if (dist_to_target < EPS_L) {
		detail().m_current_travel_point = detail().path().size() - 1;
		m_speed			= 0.f;
		dest_position	= CalculateRealPosition();
		return;
	}

	// установить позицию
	Fvector				motion;
	motion.mul			(dir_to_target, dist / dist_to_target);
	dest_position.add	(motion);

	// установить скорость
	float	real_motion	= motion.magnitude() + desirable_dist - dist;
	float	real_speed	= real_motion / time_delta;

	m_speed				= 0.5f * desirable_speed + 0.5f * real_speed;

	// Обновить позицию
	m_monster->m_current_position	= dest_position;
	m_monster->Position()			= CalculateRealPosition();
	dest_position					= m_monster->Position();
}

//.........这里部分代码省略.........
			if (isActorAccelerated(mstate_real, IsZoomAimingMode()) && !isActorAccelerated(mstate_wf, IsZoomAimingMode()))
			{
				character_physics_support()->movement()->EnableCharacter();
				if(character_physics_support()->movement()->ActivateBoxDynamic(2))mstate_real	&=~mcAccel;
			}
		}

		if ((mstate_wf&mcSprint) && !CanSprint())
			mstate_wf				&= ~mcSprint;

		mstate_real &= (~move);
		mstate_real |= (mstate_wf & move);

		if(mstate_wf&mcSprint)
			mstate_real|=mcSprint;
		else
			mstate_real&=~mcSprint;
		if(!(mstate_real&(mcFwd|mcLStrafe|mcRStrafe))||mstate_real&(mcCrouch|mcClimb)|| !isActorAccelerated(mstate_wf, IsZoomAimingMode()))
		{
			mstate_real&=~mcSprint;
			mstate_wishful&=~mcSprint;
		}
				
		// check player move state
		if(mstate_real&mcAnyMove)
		{
			BOOL	bAccelerated		= isActorAccelerated(mstate_real, IsZoomAimingMode())&&CanAccelerate();

			// correct "mstate_real" if opposite keys pressed
			if (_abs(vControlAccel.z)<EPS)	mstate_real &= ~(mcFwd+mcBack		);
			if (_abs(vControlAccel.x)<EPS)	mstate_real &= ~(mcLStrafe+mcRStrafe);

			// normalize and analyze crouch and run
			float	scale			= vControlAccel.magnitude();
			if(scale>EPS)	
			{
				scale	=	m_fWalkAccel/scale;
				if (bAccelerated)
					if (mstate_real&mcBack)
						scale *= m_fRunBackFactor;
					else
						scale *= m_fRunFactor;
				else
					if (mstate_real&mcBack)
						scale *= m_fWalkBackFactor;



				if (mstate_real&mcCrouch)	scale *= m_fCrouchFactor;
				if (mstate_real&mcClimb)	scale *= m_fClimbFactor;
				if (mstate_real&mcSprint)	scale *= m_fSprintFactor;

				if (mstate_real&(mcLStrafe|mcRStrafe) && !(mstate_real&mcCrouch))
				{
					if (bAccelerated)
						scale *= m_fRun_StrafeFactor;
					else
						scale *= m_fWalk_StrafeFactor;
				}

				vControlAccel.mul			(scale);
				cam_eff_factor				= scale;
			}//scale>EPS
		}//(mstate_real&mcAnyMove)
	}//peOnGround || peAtWall

void CInventoryItem::CalculateInterpolationParams()
{
	net_updateData* p = NetSync();
	p->IStartPos.set(object().Position());
	p->IStartRot.set(object().XFORM());

	Fvector P0, P1, P2, P3;

	CPHSynchronize* pSyncObj = NULL;
	pSyncObj = object().PHGetSyncItem(0);
	
	Fmatrix xformX0, xformX1;	

	if (m_flags.test(FInInterpolation))
	{
		u32 CurTime = Level().timeServer();
		float factor	= float(CurTime - p->m_dwIStartTime)/(p->m_dwIEndTime - p->m_dwIStartTime);
		if (factor > 1.0f) factor = 1.0f;

		float c = factor;
		for (u32 k=0; k<3; k++)
		{
			P0[k] = c*(c*(c*p->SCoeff[k][0]+p->SCoeff[k][1])+p->SCoeff[k][2])+p->SCoeff[k][3];
			P1[k] = (c*c*p->SCoeff[k][0]*3+c*p->SCoeff[k][1]*2+p->SCoeff[k][2])/3; // сокрость из формулы в 3 раза превышает скорость при расчете коэффициентов !!!!
		};
		P0.set(p->IStartPos);
		P1.add(p->IStartPos);
	}	
	else
	{
		P0 = p->IStartPos;

		if (p->LastState.linear_vel.x == 0 && 
			p->LastState.linear_vel.y == 0 && 
			p->LastState.linear_vel.z == 0)
		{
			pSyncObj->cv2obj_Xfrom(p->RecalculatedState.previous_quaternion, p->RecalculatedState.previous_position, xformX0);
			pSyncObj->cv2obj_Xfrom(p->RecalculatedState.quaternion, p->RecalculatedState.position, xformX1);
		}
		else
		{
			pSyncObj->cv2obj_Xfrom(p->LastState.previous_quaternion, p->LastState.previous_position, xformX0);
			pSyncObj->cv2obj_Xfrom(p->LastState.quaternion, p->LastState.position, xformX1);
		};

		P1.sub(xformX1.c, xformX0.c);
		P1.add(p->IStartPos);
	}

	P2.sub(p->PredictedState.position, p->PredictedState.linear_vel);
	pSyncObj->cv2obj_Xfrom(p->PredictedState.quaternion, P2, xformX0);
	P2.set(xformX0.c);

	pSyncObj->cv2obj_Xfrom(p->PredictedState.quaternion, p->PredictedState.position, xformX1);
	P3.set(xformX1.c);
	/////////////////////////////////////////////////////////////////////////////
	Fvector TotalPath;
	TotalPath.sub(P3, P0);
	float TotalLen = TotalPath.magnitude();
	
	SPHNetState	State0 = (p->NET_IItem.back()).State;
	SPHNetState	State1 = p->PredictedState;

	float lV0 = State0.linear_vel.magnitude();
	float lV1 = State1.linear_vel.magnitude();

	u32		ConstTime = u32((fixed_step - ph_world->m_frame_time)*1000)+ Level().GetInterpolationSteps()*u32(fixed_step*1000);
	
	p->m_dwIStartTime = p->m_dwILastUpdateTime;

	if (( lV0 + lV1) > 0.000001 && g_cl_lvInterp == 0)
	{
		u32		CulcTime = iCeil(TotalLen*2000/( lV0 + lV1));
		p->m_dwIEndTime = p->m_dwIStartTime + _min(CulcTime, ConstTime);
	}
	else
		p->m_dwIEndTime = p->m_dwIStartTime + ConstTime;
	/////////////////////////////////////////////////////////////////////////////
	Fvector V0, V1;
	V0.sub(P1, P0);
	V1.sub(P3, P2);
	lV0 = V0.magnitude();
	lV1 = V1.magnitude();

	if (TotalLen != 0)
	{
		if (V0.x != 0 || V0.y != 0 || V0.z != 0)
		{
			if (lV0 > TotalLen/3)
			{
				V0.normalize();
				V0.mul(TotalLen/3);
				P1.add(V0, P0);
			}
		}
		
		if (V1.x != 0 || V1.y != 0 || V1.z != 0)
		{
			if (lV1 > TotalLen/3)
			{
//.........这里部分代码省略.........