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

sPoly*	CFrustum::ClipPoly(sPoly& S, sPoly& D) const
{
    sPoly*	src		= &D;
    sPoly*	dest	= &S;
    for (int i=0; i<p_count; i++)
    {
        // cache plane and swap lists
        const fplane &P = planes[i];
        std::swap		(src,dest);
        dest->clear		();

        // classify all points relative to plane #i
        float	cls	[FRUSTUM_SAFE];
        for (u32 j=0; j<src->size(); j++) cls[j]=P.classify((*src)[j]);

        // clip everything to this plane
        cls[src->size()] = cls[0];
        src->push_back((*src)[0]);
        Fvector D;
        float denum,t;
        for (j=0; j<src->size()-1; j++)
        {
            if ((*src)[j].similar((*src)[j+1],EPS_S)) continue;

            if (negative(cls[j]))
            {
                dest->push_back((*src)[j]);
                if (positive(cls[j+1]))
                {
                    // segment intersects plane
                    D.sub((*src)[j+1],(*src)[j]);
                    denum = P.n.dotproduct(D);
                    if (denum!=0) {
                        t = -cls[j]/denum; //VERIFY(t<=1.f && t>=0);
                        dest->last().mad((*src)[j],D,t);
                        dest->inc();
                    }
                }
            } else {
                // J - outside
                if (negative(cls[j+1]))
                {
                    // J+1  - inside
                    // segment intersects plane
                    D.sub((*src)[j+1],(*src)[j]);
                    denum = P.n.dotproduct(D);
                    if (denum!=0) {
                        t = -cls[j]/denum; //VERIFY(t<=1.f && t>=0);
                        dest->last().mad((*src)[j],D,t);
                        dest->inc();
                    }
                }
            }
        }

        // here we end up with complete polygon in 'dest' which is inside plane #i
        if (dest->size()<3) return 0;
    }
    return dest;
}

ik_goal_matrix::e_collide_state	 CIKFoot::CollideFoot( float angle, float &out_angle, const Fvector	&global_toe, const Fvector	&foot_normal,  const Fvector	&global_bone_pos, const Fplane &p, const Fvector &ax )const
{

	float	dfoot_tri		=-p.d - p.n.dotproduct( global_bone_pos );												// dist from foot bone pos to tri plain
	Fvector	axp;			axp.sub( global_toe, global_bone_pos );	
	float	dfoot_toe		=p.n.dotproduct( axp );
	out_angle = angle;
	if( dfoot_tri < m_foot_width * _abs( foot_normal.dotproduct( p.n ) ) )
		return ik_goal_matrix::cl_aligned;
	axp.sub( Fvector( ).mul( ax, axp.dotproduct( ax ) ) );															//vector from nc_toe to ax
	float	dtoe_ax = axp.magnitude();	

	out_angle = 0.f;

	if( dtoe_ax<EPS_S )
		return ik_goal_matrix::cl_free;
	if( dfoot_toe > dtoe_ax - EPS_S )
		return ik_goal_matrix::cl_free;
	if( dfoot_toe < dfoot_tri )
		return ik_goal_matrix::cl_free;

	float ang_nc	= acosf( dfoot_toe/dtoe_ax );
	float ang_c		= acosf( dfoot_tri/dtoe_ax );
	out_angle = -( ang_c - ang_nc );
	return ik_goal_matrix::cl_rotational;
}

IC void closestPointOnLine(Fvector& res, const Fvector& a, const Fvector& b, const Fvector& p)
{

    // Determine t (the length of the xr_vector from ‘a’ to ‘p’)
    Fvector c;
    c.sub(p,a);
    Fvector V;
    V.sub(b,a);

    float d = V.magnitude();

    V.div(d);
    float t = V.dotproduct(c);

    // Check to see if ‘t’ is beyond the extents of the line segment
    if (t <= 0.0f)	{
        res.set(a);
        return;
    }
    if (t >= d)		{
        res.set(b);
        return;
    }

    // Return the point between ‘a’ and ‘b’
    // set length of V to t. V is normalized so this is easy
    res.mad		(a,V,t);
}

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

void  CCustomZone::OnMove()
{
	if(m_dwLastTimeMoved == 0)
	{
		m_dwLastTimeMoved = Device.dwTimeGlobal;
		m_vPrevPos.set(Position());
	}
	else
	{
		float time_delta	= float(Device.dwTimeGlobal - m_dwLastTimeMoved)/1000.f;
		m_dwLastTimeMoved	= Device.dwTimeGlobal;

		Fvector				vel;
			
		if(fis_zero(time_delta))
			vel = zero_vel;
		else
		{
			vel.sub(Position(), m_vPrevPos);
			vel.div(time_delta);
		}

		if (m_pIdleParticles)
			m_pIdleParticles->UpdateParent(XFORM(), vel);

		if(m_pLight && m_pLight->get_active())
			m_pLight->set_position(Position());

		if(m_pIdleLight && m_pIdleLight->get_active())
			m_pIdleLight->set_position(Position());
     }
}

bool CLevelChanger::get_reject_pos(Fvector& p, Fvector& r)
{
		p.set(0,0,0);
		r.set(0,0,0);
//--		db.actor:set_actor_position(patrol("t_way"):point(0))
//--		local dir = patrol("t_look"):point(0):sub(patrol("t_way"):point(0))
//--		db.actor:set_actor_direction(-dir:getH())

		if(m_ini_file && m_ini_file->section_exist("pt_move_if_reject"))
		{
			LPCSTR p_name = m_ini_file->r_string("pt_move_if_reject", "path");
			const CPatrolPath*		patrol_path = ai().patrol_paths().path(p_name);
			VERIFY					(patrol_path);
			
			const CPatrolPoint*		pt;
			pt						= &patrol_path->vertex(0)->data();
			p						= pt->position();

			Fvector tmp;
			pt						= &patrol_path->vertex(1)->data();
			tmp.sub					(pt->position(),p);
			tmp.getHP				(r.y,r.x);
			return true;
		}
		return false;
}

bool CMonsterEnemyManager::is_faced(const CEntityAlive *object0, const CEntityAlive *object1)
{
	if (object0->Position().distance_to(object1->Position()) > object0->ffGetRange())
	{
		return false;
	}

	float			yaw1, pitch1, yaw2, pitch2, fYawFov, fPitchFov, fRange;
	Fvector			tPosition = object0->Position();

	yaw1			= object0->Orientation().yaw;
	pitch1			= object0->Orientation().pitch;
	fYawFov			= angle_normalize_signed(object0->ffGetFov()*PI/180.f);
	fRange			= object0->ffGetRange();

	fYawFov			= angle_normalize_signed((_abs(fYawFov) + _abs(atanf(1.f/tPosition.distance_to(object1->Position()))))/2.f);
	fPitchFov		= angle_normalize_signed(fYawFov*1.f);
	tPosition.sub	(object1->Position());
	tPosition.mul	(-1);
	tPosition.getHP	(yaw2,pitch2);
	yaw1			= angle_normalize_signed(yaw1);
	pitch1			= angle_normalize_signed(pitch1);
	yaw2			= angle_normalize_signed(yaw2);
	pitch2			= angle_normalize_signed(pitch2);
	if ((angle_difference(yaw1,yaw2) <= fYawFov) && (angle_difference(pitch1,pitch2) <= fPitchFov))
		return		(true);
	return			(false);
}

void CLight::Render(int priority, bool strictB2F)
{
	inherited::Render(priority,strictB2F);
    if ((1==priority)&&(false==strictB2F)){
        Device.SetShader		(Device.m_WireShader);
        RCache.set_xform_world	(Fidentity);
    	u32 clr = Locked()?LOCK_COLOR:(Selected()?SEL_COLOR:(m_Flags.is(ELight::flAffectDynamic)?NORM_DYN_COLOR:NORM_COLOR));
    	switch (m_Type){
        case ELight::ltPoint:
            if (Selected()) 	DU.DrawLineSphere( PPosition, m_Range, clr, true );
            DU.DrawPointLight(PPosition,VIS_RADIUS, clr);
            if (m_Flags.is(ELight::flPointFuzzy)){
            	VERIFY(m_FuzzyData);
			    for (FvectorIt it=m_FuzzyData->m_Positions.begin(); it!=m_FuzzyData->m_Positions.end(); it++){
                	Fvector tmp; _Transform().transform_tiny(tmp,*it);
		            DU.DrawPointLight(tmp,VIS_RADIUS/6, clr);
	            }
			}
        break;
        case ELight::ltSpot:{
//			Fvector dir;
//			dir.setHP		(PRotation.y,PRotation.x);
//			DU.DrawCone		(Fidentity, PPosition, dir, Selected()?m_Range:VIS_RADIUS, radius2, clr, true, false);
        	if (Selected())	DU.DrawSpotLight( PPosition, FTransformR.k, m_Range, m_Cone, clr );
            else			DU.DrawSpotLight( PPosition, FTransformR.k, VIS_RADIUS, m_Cone, clr );
        }break;
        default: THROW;
        }
    	ESceneLightTools* lt = dynamic_cast<ESceneLightTools*>(ParentTools); VERIFY(lt);
        if (lt->m_Flags.is(ESceneLightTools::flShowControlName)){ 
            Fvector D;	D.sub(Device.vCameraPosition,PPosition);
            float dist 	= D.normalize_magn();
        	if (!Scene->RayPickObject(dist,PPosition,D,OBJCLASS_SCENEOBJECT,0,0))
	        	DU.OutText (PPosition,AnsiString().sprintf(" %s",GetLControlName()).c_str(),0xffffffff,0xff000000);
        }
    }else if ((1==priority)&&(true==strictB2F)){
        Device.SetShader		(Device.m_SelectionShader);
        RCache.set_xform_world	(Fidentity);
    	switch (m_Type){
        case ELight::ltPoint:
            if (m_Flags.is(ELight::flPointFuzzy)){
		    	u32 clr = Locked()?LOCK_COLOR:(Selected()?SEL_COLOR:(m_Flags.is(ELight::flAffectDynamic)?NORM_DYN_COLOR:NORM_COLOR));
                clr 	= subst_alpha(clr,0x40);
            	const Fvector zero={0.f,0.f,0.f};
                VERIFY(m_FuzzyData);
                switch (m_FuzzyData->m_ShapeType){
                case CLight::SFuzzyData::fstSphere: 
                	DU.DrawSphere	(_Transform(),zero,m_FuzzyData->m_SphereRadius,clr,clr,true,true);
                break;
                case CLight::SFuzzyData::fstBox:
                	DU.DrawAABB		(_Transform(),zero,m_FuzzyData->m_BoxDimension,clr,clr,true,true);
                break;
                }
			}
        break;
        case ELight::ltSpot:		break;
        default: THROW;
        }
	}
}

void CPHCollisionDamageReceiver::CollisionCallback(bool& do_colide,bool bo1,dContact& c,SGameMtl* material_1,SGameMtl* material_2)
{
	if(material_1->Flags.test(SGameMtl::flPassable)||material_2->Flags.test(SGameMtl::flPassable))return;
	dBodyID						b1					=	dGeomGetBody(c.geom.g1)	;
	dBodyID						b2					=	dGeomGetBody(c.geom.g2) ;
	dxGeomUserData				*ud_self			=	bo1 ? retrieveGeomUserData(c.geom.g1):retrieveGeomUserData(c.geom.g2);
	dxGeomUserData				*ud_damager			=	bo1 ? retrieveGeomUserData(c.geom.g2):retrieveGeomUserData(c.geom.g1);
	
	SGameMtl					*material_self		=	bo1 ? material_1:material_2;
	SGameMtl					*material_damager	=	bo1 ? material_2:material_1;
	VERIFY						(ud_self);
	CPhysicsShellHolder			*o_self			=	ud_self->ph_ref_object;
	CPhysicsShellHolder			*o_damager		=	NULL;if(ud_damager)o_damager=ud_damager->ph_ref_object;
	u16							source_id		=	o_damager ? o_damager->ID():u16(-1);
	CPHCollisionDamageReceiver	*dr	=o_self->PHCollisionDamageReceiver();
	VERIFY2(dr,"wrong callback");
	
	float damager_material_factor=material_damager->fBounceDamageFactor;

	if(ud_damager&&ud_damager->ph_object&&ud_damager->ph_object->CastType()==CPHObject::tpCharacter)
	{
		CCharacterPhysicsSupport* phs=o_damager->character_physics_support();
		if(phs->IsSpecificDamager())damager_material_factor=phs->BonceDamageFactor();
	}

	float dfs=(material_self->fBounceDamageFactor+damager_material_factor);
	if(fis_zero(dfs)) return;
	Fvector dir;dir.set(*(Fvector*)c.geom.normal);
	Fvector pos;
	pos.sub(*(Fvector*)c.geom.pos,*(Fvector*)dGeomGetPosition(bo1 ? c.geom.g1:c.geom.g2));//it is not true pos in bone space
	dr->Hit(source_id,ud_self->bone_id,E_NL(b1,b2,c.geom.normal)*damager_material_factor/dfs,dir,pos);
	
}

void CPHMovementControl::Jump(const Fvector &start_point,const Fvector &end_point, float time)
{
	Fvector velosity;
	velosity.sub(end_point,start_point);
	TransferenceToThrowVel(velosity,time,ph_world->Gravity());
	JumpV(velosity);
}

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