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

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

void CControllerPsyHit::death_glide_start()
{
	if (!check_conditions_final()) {
		m_man->deactivate	(this);
		return;
	}
	
	HUD().SetRenderable(false);

	// Start effector
	CEffectorCam* ce = Actor()->Cameras().GetCamEffector(eCEControllerPsyHit);
	VERIFY(!ce);
	
	Fvector src_pos		= Actor()->cam_Active()->vPosition;
	Fvector target_pos	= m_object->Position();
	target_pos.y		+= 1.2f;
	
	Fvector				dir;
	dir.sub				(target_pos,src_pos);
	
	float dist			= dir.magnitude();
	dir.normalize		();

	target_pos.mad		(src_pos,dir,dist-4.8f);
	
	Actor()->Cameras().AddCamEffector(new CControllerPsyHitCamEffector(eCEControllerPsyHit, src_pos,target_pos, m_man->animation().motion_time(m_stage[1], m_object->Visual())));
	smart_cast<CController *>(m_object)->draw_fire_particles();

	dir.sub(src_pos,target_pos);
	dir.normalize();
	float h,p;
	dir.getHP(h,p);
	dir.setHP(h,p+PI_DIV_3);
	Actor()->character_physics_support()->movement()->ApplyImpulse(dir,Actor()->GetMass() * 530.f);

	set_sound_state					(eStart);

	NET_Packet			P;
	Actor()->u_EventGen	(P, GEG_PLAYER_WEAPON_HIDE_STATE, Actor()->ID());
	P.w_u32				(INV_STATE_BLOCK_ALL);
	P.w_u8				(u8(true));
	Actor()->u_EventSend(P);
	
	m_blocked			= true;

	//////////////////////////////////////////////////////////////////////////
	// set direction
	SControlDirectionData			*ctrl_dir = (SControlDirectionData*)m_man->data(this, ControlCom::eControlDir); 
	VERIFY							(ctrl_dir);
	ctrl_dir->heading.target_speed	= 3.f;
	ctrl_dir->heading.target_angle	= m_man->direction().angle_to_target(Actor()->Position());

	//////////////////////////////////////////////////////////////////////////
}

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 MK_Frustum(CFrustum& F, float FOV, float _FAR, float A, Fvector &P, Fvector &D, Fvector &U)
{
    float YFov	= deg2rad(FOV);
    float XFov	= deg2rad(FOV/A);

    // calc window extents in camera coords
    float wR=tanf(XFov*0.5f);
    float wL=-wR;
    float wT=tanf(YFov*0.5f);
    float wB=-wT;

    // calc x-axis (viewhoriz) and store cop
    // here we are assuring that vectors are perpendicular & normalized
    Fvector			R,COP;
    D.normalize		();
    R.crossproduct	(D,U);
    R.normalize		();
    U.crossproduct	(R,D);
    U.normalize		();
    COP.set			(P);

    // calculate the corner vertices of the window
    Fvector			sPts[4];  // silhouette points (corners of window)
    Fvector			Offset,T;
    Offset.add		(D,COP);

    sPts[0].mul(R,wR);
    T.mad(Offset,U,wT);
    sPts[0].add(T);
    sPts[1].mul(R,wL);
    T.mad(Offset,U,wT);
    sPts[1].add(T);
    sPts[2].mul(R,wL);
    T.mad(Offset,U,wB);
    sPts[2].add(T);
    sPts[3].mul(R,wR);
    T.mad(Offset,U,wB);
    sPts[3].add(T);

    // find projector direction vectors (from cop through silhouette pts)
    Fvector ProjDirs[4];
    ProjDirs[0].sub(sPts[0],COP);
    ProjDirs[1].sub(sPts[1],COP);
    ProjDirs[2].sub(sPts[2],COP);
    ProjDirs[3].sub(sPts[3],COP);

    Fvector _F[4];
    _F[0].mad(COP, ProjDirs[0], _FAR);
    _F[1].mad(COP, ProjDirs[1], _FAR);
    _F[2].mad(COP, ProjDirs[2], _FAR);
    _F[3].mad(COP, ProjDirs[3], _FAR);

    F.CreateFromPoints(_F,4,COP);
}

bool CPolterFlame::get_valid_flame_position(const CObject *target_object, Fvector &res_pos)
{
	const CGameObject *Obj = smart_cast<const CGameObject *>(target_object);
	if (!Obj) return (false);

	Fvector dir;
	float h,p;

	Fvector vertex_position;
	Fvector new_pos;

	for (u32 i=0; i<FIND_POINT_ATTEMPT_COUNT; i++) {
		
		target_object->Direction().getHP(h,p);
		h = Random.randF(0, PI_MUL_2);
		dir.setHP(h,p);
		dir.normalize();

		vertex_position = ai().level_graph().vertex_position(Obj->ai_location().level_vertex_id());
		new_pos.mad(vertex_position, dir, Random.randF(m_min_flame_dist, m_max_flame_dist));

		u32 node = ai().level_graph().check_position_in_direction(Obj->ai_location().level_vertex_id(), vertex_position, new_pos);
		if (node != u32(-1)) {
			res_pos = ai().level_graph().vertex_position(node);
			res_pos.y += Random.randF(m_min_flame_height, m_max_flame_height);
			return (true);
		}
	}


	float angle = ai().level_graph().vertex_high_cover_angle(Obj->ai_location().level_vertex_id(),PI_DIV_6,std::less<float>());

	dir.set(1.f,0.f,0.f);
	dir.setHP(angle + PI, 0.f);
	dir.normalize();

	vertex_position = ai().level_graph().vertex_position(Obj->ai_location().level_vertex_id());
	new_pos.mad(vertex_position, dir, Random.randF(m_min_flame_dist, m_max_flame_dist));

	u32 node = ai().level_graph().check_position_in_direction(Obj->ai_location().level_vertex_id(), vertex_position, new_pos);
	if (node != u32(-1)) {
		res_pos = ai().level_graph().vertex_position(node);
		res_pos.y += Random.randF(m_min_flame_height, m_max_flame_height);
		return (true);
	}

	return (false);
}

void CController::draw_fire_particles()
{
	if (!EnemyMan.get_enemy()) return;
	CEntityAlive *enemy	= const_cast<CEntityAlive*>(EnemyMan.get_enemy());
	if (!EnemyMan.see_enemy_now()) return;

	// вычислить позицию и направленность партикла
	Fvector my_head_pos;
	my_head_pos.set	(get_head_position(this));
	
	Fvector position;
	position.set	(get_head_position(enemy));
	position.y		-= 0.5f;
	
	Fvector			dir;
	dir.sub			(position, my_head_pos);
	dir.normalize	();

	PlayParticles(particles_fire, my_head_pos, dir);

	// check probability
	/*if (Random.randI(100) > 30)*/
	{
//		Hit_Psy						(enemy, m_tube_damage / 2.f);
		play_control_sound_hit		();
	}

	//m_sound_hit_fx.set_volume(10.0f);
	//if(!m_sndShockEffector)
	//	m_sndShockEffector = xr_new<SndShockEffector>();

	//m_sndShockEffector->Start(m_sound_hit_fx._handle()->length_ms(), 10.f );
	//m_sound_hit_fx.play_at_pos(this, Level().CurrentEntity()->Position());
}

void CAI_Stalker::update_throw_params		()
{
	if (m_throw_actual) {
		if (m_computed_object_position.similar(Position())) {
			if (m_computed_object_direction.similar(Direction())) {
				VERIFY		(_valid(m_throw_force));
				return;
			}
		}
	}

	m_throw_actual			= true;
	m_computed_object_position	= Position();
	m_computed_object_direction	= Direction();

	m_throw_position		= eye_matrix.c;

	// computing velocity with minimum magnitude
	Fvector					velocity;
	velocity.sub			(m_throw_target,m_throw_position);
	float					time = ThrowMinVelTime(velocity,ph_world->Gravity());
	TransferenceToThrowVel	(velocity,time,ph_world->Gravity());
	m_throw_force			= velocity.magnitude();
	m_throw_direction		= velocity.normalize();
	VERIFY					(_valid(m_throw_force));
}

IC static void generate_orthonormal_basis1(const Fvector& dir,Fvector& updir, Fvector& right)
{

	right.crossproduct(dir,updir); //. <->
	right.normalize();
	updir.crossproduct(right,dir);
}

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

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 CCustomZone::PlayEntranceParticles(CGameObject* pObject)
{
	m_entrance_sound.play_at_pos		(0, pObject->Position());

	LPCSTR particle_str				= NULL;

	if(pObject->Radius()<SMALL_OBJECT_RADIUS)
	{
		if(!m_sEntranceParticlesSmall) 
			return;

		particle_str = m_sEntranceParticlesSmall.c_str();
	}
	else
	{
		if(!m_sEntranceParticlesBig) 
			return;

		particle_str				= m_sEntranceParticlesBig.c_str();
	}

	Fvector							vel;
	CPhysicsShellHolder* shell_holder=smart_cast<CPhysicsShellHolder*>(pObject);
	if(shell_holder)
		shell_holder->PHGetLinearVell(vel);
	else 
		vel.set						(0,0,0);
	
	//выбрать случайную косточку на объекте
	CParticlesPlayer* PP			= smart_cast<CParticlesPlayer*>(pObject);
	if (PP)
	{
		u16 play_bone				= PP->GetRandomBone(); 
		
		if (play_bone!=BI_NONE)
		{
			CParticlesObject* pParticles = CParticlesObject::Create(particle_str, TRUE);
			Fmatrix					xform;
			Fvector					dir;
			if(fis_zero				(vel.magnitude()))
				dir.set				(0,1,0);
			else
			{
				dir.set				(vel);
				dir.normalize		();
			}

			PP->MakeXFORM			(pObject,play_bone,dir,Fvector().set(0,0,0),xform);
			pParticles->UpdateParent(xform, vel);
			pParticles->Play		(false);
		}
	}
	if(m_zone_flags.test(eBoltEntranceParticles) && smart_cast<CBolt*>(pObject))
		PlayBoltEntranceParticles();
}

Fvector   CMonsterSquad::calc_monster_target_dir (CBaseMonster* monster, const CEntity* enemy)
{
	VERIFY(monster);
	VERIFY(enemy);

	const Fvector enemy_pos = enemy->Position();
	Fvector home2enemy = enemy_pos;
	home2enemy.sub(monster->Home->get_home_point());

	const float home2enemy_mag = home2enemy.magnitude();

	// enemy pos == home pos?
	const float near_zero = 0.00001f;
	if ( home2enemy_mag < near_zero )
	{
		Fvector enemy2monster = monster->Position();
		enemy2monster.sub(enemy_pos);
		const float enemy2monster_mag = enemy2monster.magnitude();
		// monster pos == enemy pos?
		if ( enemy2monster_mag < near_zero )
		{
			VERIFY2(false, "Enemy and Monster should not have same pos!");
			Fvector dir = { 1.f, 0.f, 0.f }; // happy with random dir then :)
			return dir;
		}

		enemy2monster.normalize();
		return enemy2monster;
	}

	const u8 squad_size  = squad_alife_count();
	VERIFY(squad_size);

	u8 squad_index = get_index(monster);
	if ( squad_index == -1 )
	{
		squad_index = 0;
	}

	float heading, pitch;
	home2enemy.getHP(heading, pitch);

	// 2pi * index/num - encircle
	heading += M_PI * 2.f * squad_index / squad_size;
	heading = angle_normalize(heading);

	Fvector dir;
	dir.setHP(heading, pitch);
	dir.normalize();

	return dir;
}

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

	r1v_vert	(Fvector3 _P, Fvector _N, base_basis _T, base_basis _B, base_color _CC, Fvector2 tc_base)
	{
		base_color_c	_C;	_CC._get	(_C);
		_N.normalize	();
		std::pair<s16,u8>	tc_u		= s24_tc_base	(tc_base.x);
		std::pair<s16,u8>	tc_v		= s24_tc_base	(tc_base.y);
		P				= _P;
		N				= u8_vec4		(_N,u8_clr(_C.hemi));
		T				= u8_vec4		(_T,tc_u.second	);
		B				= u8_vec4		(_B,tc_v.second	);
		C				= color_rgba	(u8_clr(_C.rgb.x),u8_clr(_C.rgb.y),u8_clr(_C.rgb.z),u8_clr(_C.sun));
		tc0x			= tc_u.first	;	
		tc0y			= tc_v.first	;
	}

Fvector CDetailPathManager::direction() const
{
	if ((m_path.size() < 2) || (m_path.size() <= m_current_travel_point + 1))
		return				(Fvector().set(0,0,1));
	
	Fvector					direction;
	direction.sub			(m_path[m_current_travel_point + 1].position, m_path[m_current_travel_point].position);

	if (direction.square_magnitude() < EPS_L)
		direction.set		(0.f,0.f,1.f);
	else
		direction.normalize	();

	return					(direction);
}