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

bool CSnork::trace_geometry(const Fvector &d, float &range)
{
	Fvector				dir;
	float				h, p;

	Fvector				Pl,Pc,Pr;
	Fvector				center;
	Center				(center);

	range				= trace (d);
	if (range > TRACE_RANGE) return false;
	
	float angle			= asin(1.f / range);

	// trace center ray
	dir					= d;

	dir.getHP			(h,p);
	p					+= angle;
	dir.setHP			(h,p);
	dir.normalize_safe	();

	range				= trace (dir);
	if (range > TRACE_RANGE) return false;

	Pc.mad				(center, dir, range);

	// trace left ray
	Fvector				temp_p;
	temp_p.mad			(Pc, XFORM().i, Radius() / 2);
	dir.sub				(temp_p, center);
	dir.normalize_safe	();

	range				= trace (dir);
	if (range > TRACE_RANGE) return false;

	Pl.mad				(center, dir, range);

	// trace right ray
	Fvector inv			= XFORM().i; 
	inv.invert			();
	temp_p.mad			(Pc, inv, Radius() / 2);
	dir.sub				(temp_p, center);
	dir.normalize_safe	();

	range				= trace (dir);
	if (range > TRACE_RANGE) return false;

	Pr.mad				(center, dir, range);

	float				h1,p1,h2,p2;

	Fvector().sub(Pl, Pc).getHP(h1,p1);
	Fvector().sub(Pc, Pr).getHP(h2,p2);

	return (fsimilar(h1,h2,0.1f) && fsimilar(p1,p2,0.1f));
}

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

IC float PLC_energy	(Fvector& P, Fvector& N, light* L, float E)
{
	Fvector Ldir;
	if (L->flags.type==IRender_Light::DIRECT)
	{
		// Cos
		Ldir.invert	(L->direction);
		float D		= Ldir.dotproduct( N );
		if( D <=0 )						return 0;
		
		// Trace Light
		float A		= D*E;
		return A;
	} else {
		// Distance
		float sqD	= P.distance_to_sqr(L->position);
		if (sqD > (L->range*L->range))	return 0;
		
		// Dir
		Ldir.sub	(L->position,P);
		Ldir.normalize_safe();
		float D		= Ldir.dotproduct( N );
		if( D <=0 )						return 0;
		
		// Trace Light
		float R		= _sqrt		(sqD);
		float att	= 1-(1/(1+R));
		float A		= D * E * att;
		return A;
	}
}

void		CPHMovementControl::CorrectPathDir			(const Fvector &real_path_dir,const xr_vector<DetailPathManager::STravelPathPoint> & path,int index,Fvector &corrected_path_dir)
{
	const float epsilon=0.1f;
	float plane_motion=dXZMag(real_path_dir);
	if(fis_zero(plane_motion,epsilon))
	{
		if(!fis_zero(plane_motion,EPS))
		{
			corrected_path_dir.set(real_path_dir);
			corrected_path_dir.y=0.f;
			corrected_path_dir.mul(1.f/plane_motion);
		}
		else if(index!=m_path_size-1)
		{
			corrected_path_dir.sub(path[index+1].position,path[index].position);
			corrected_path_dir.normalize_safe();
			CorrectPathDir(corrected_path_dir,path,index+1,corrected_path_dir);
		}
		else 
		{
			corrected_path_dir.set(real_path_dir);
		}
	}
	else
	{
		corrected_path_dir.set(real_path_dir);
	}
}

void player_hud::update_inertion(Fmatrix& trans)
{
	if ( inertion_allowed() )
	{
		Fmatrix								xform;
		Fvector& origin						= trans.c; 
		xform								= trans;

		static Fvector						st_last_dir={0,0,0};

		// calc difference
		Fvector								diff_dir;
		diff_dir.sub						(xform.k, st_last_dir);

		// clamp by PI_DIV_2
		Fvector last;						last.normalize_safe(st_last_dir);
		float dot							= last.dotproduct(xform.k);
		if (dot<EPS){
			Fvector v0;
			v0.crossproduct					(st_last_dir,xform.k);
			st_last_dir.crossproduct		(xform.k,v0);
			diff_dir.sub					(xform.k, st_last_dir);
		}

		// tend to forward
		st_last_dir.mad						(diff_dir,TENDTO_SPEED*Device.fTimeDelta);
		origin.mad							(diff_dir,ORIGIN_OFFSET);

		// pitch compensation
		float pitch							= angle_normalize_signed(xform.k.getP());
		origin.mad							(xform.k,	-pitch * PITCH_OFFSET_D);
		origin.mad							(xform.i,	-pitch * PITCH_OFFSET_R);
		origin.mad							(xform.j,	-pitch * PITCH_OFFSET_N);
	}
}

u32	CScriptGameObject::vertex_in_direction(u32 level_vertex_id, Fvector direction, float max_distance) const
{
    CCustomMonster	*monster = smart_cast<CCustomMonster*>(&object());
    if (!monster)
    {
        ai().script_engine().script_log(ScriptStorage::eLuaMessageTypeError, "CCustomMonster : cannot access class member vertex_in_direction!");
        return		(u32(-1));
    }

    if (!monster->movement().restrictions().accessible(level_vertex_id))
    {
        ai().script_engine().script_log(ScriptStorage::eLuaMessageTypeError, "CCustomMonster::vertex_in_direction - start vertex id is not accessible!");
        return		(u32(-1));
    }

    direction.normalize_safe();
    direction.mul(max_distance);
    Fvector			start_position = ai().level_graph().vertex_position(level_vertex_id);
    Fvector			finish_position = Fvector(start_position).add(direction);
    u32				result = u32(-1);
    monster->movement().restrictions().add_border(level_vertex_id, max_distance);
    ai().level_graph().farthest_vertex_in_direction(level_vertex_id, start_position, finish_position, result, 0, true);
    monster->movement().restrictions().remove_border();
    return			(ai().level_graph().valid_vertex_id(result) ? result : level_vertex_id);
}

void CAI_Rat::set_rew_cur_position()
{
	Fvector tTemp;
	tTemp.setHP(-movement().m_body.current.yaw,-movement().m_body.current.pitch);
	tTemp.normalize_safe();
	tTemp.mul(m_fUnderFireDistance);
	m_tSpawnPosition.add(Position(),tTemp);
}

void CAI_Trader::g_fireParams(const CHudItem* pHudItem, Fvector& P, Fvector& D)
{
	VERIFY			(inventory().ActiveItem());
	if (g_Alive() && inventory().ActiveItem()) {
		Center(P);
		D.setHP(0,0);
		D.normalize_safe();
	}
}

void CCustomMonster::mk_rotation					(Fvector &dir, SRotation &R)
{
	// parse yaw
	Fvector DYaw;	
	DYaw.set							(dir.x,0.f,dir.z); 
	DYaw.normalize_safe					();
	clamp								(DYaw.x,-0.9999999f,0.9999999f);
	clamp								(DYaw.y,-0.9999999f,0.9999999f);
	clamp								(DYaw.z,-0.9999999f,0.9999999f);

	if ( DYaw.x >= 0 )
		R.yaw							= acosf(DYaw.z);
	else			
		R.yaw							= 2*PI-acosf(DYaw.z);

	// parse pitch
	dir.normalize_safe					();
	R.pitch								= -asinf(dir.y);
}

u32	vertex_in_direction(u32 level_vertex_id, Fvector direction, float max_distance)
{
	direction.normalize_safe();
	direction.mul	(max_distance);
	Fvector			start_position = ai().level_graph().vertex_position(level_vertex_id);
	Fvector			finish_position = Fvector(start_position).add(direction);
	u32				result = u32(-1);
	ai().level_graph().farthest_vertex_in_direction(level_vertex_id,start_position,finish_position,result,0);
	return			(ai().level_graph().valid_vertex_id(result) ? result : level_vertex_id);
}

void CAI_Rat::set_rew_position()
{
	Fvector tTemp;
	tTemp.sub(memory().enemy().selected()->Position(),Position());
	vfNormalizeSafe(tTemp);
	tTemp.sub(Position(),memory().enemy().selected()->Position());
	tTemp.normalize_safe();
	tTemp.mul(m_fRetreatDistance);
	m_tSpawnPosition.add(Position(),tTemp);
}

void CAI_Rat::init_state_under_fire()
{
	if (!switch_if_enemy()&&get_if_dw_time()&&m_tLastSound.dwTime >= m_dwLastUpdateTime)
	{
		Fvector tTemp;
		tTemp.setHP(-movement().m_body.current.yaw,-movement().m_body.current.pitch);
		tTemp.normalize_safe();
		tTemp.mul(m_fUnderFireDistance);
		m_tSpawnPosition.add(Position(),tTemp);
	}
	m_tGoalDir = m_tSpawnPosition;
}

void CAI_Rat::init_free_recoil()
{
	m_dwLostRecoilTime	= Device.dwTimeGlobal;
	m_tRecoilPosition	= m_tLastSound.tSavedPosition;
	if (!switch_if_enemy()&&!switch_if_time())
	{
		Fvector tTemp;
		tTemp.setHP(-movement().m_body.current.yaw,-movement().m_body.current.pitch);
		tTemp.normalize_safe();
		tTemp.mul(m_fUnderFireDistance);
		m_tSpawnPosition.add(Position(),tTemp);
	}
}

void CElevatorState::GetJumpDir(const Fvector& accel,Fvector& dir)
{
	VERIFY(m_ladder&&m_character);
	Fvector norm,side;
	m_ladder->DDNorm(norm);
	m_ladder->DDSide(side);
	Fvector ac;ac.set(accel).normalize_safe();
	float side_component=ac.dotproduct(side);
	dir.set(norm);
	if(_abs(side_component)>M_SQRT1_2)
	{
		if(side_component<0.f)side.invert();
		dir.add(side);
		dir.normalize_safe();
	}
}

void		CTeleWhirlwindObject::		keep					()
{
	CPhysicsShell*	p					=	get_object()	->PPhysicsShell();
	if(!p||!p->isActive())	
		return;
	else
	{
		p->SetAirResistance(0.f,0.f);
		p->set_ApplyByGravity(FALSE);
	}

	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)
	{
		
		CPhysicsElement* E=	p->get_ElementByStoreOrder(element);
		if(maxE->getMass()<E->getMass())maxE=E;
		Fvector			dir;dir.sub(center,E->mass_Center());
		dir.normalize_safe();
		Fvector vel;
		E->get_LinearVel(vel);
		float force=dir.dotproduct(vel)*E->getMass()/2.f;
		if(force<0.f)
		{
			dir.mul(force);
		}
	}
	
	maxE->setTorque(Fvector().set(0,500.f,0));

	Fvector dist;dist.sub(center,maxE->mass_Center());
	if(dist.magnitude()>m_telekinesis->keep_radius()*1.5f)
	{
		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));
		p->set_ApplyByGravity(TRUE);
		switch_state(TS_Raise);
	}

}