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

////////////////////////////////////////////////////////////////////////////////////////////////
// Функция 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;
}

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

Fvector2 PointProjection(Fvector pos)
{
	u32 scr_w = Device.dwWidth;
	u32 scr_h = Device.dwHeight;
	Fvector ppp = pos.sub(Device.vCameraPosition);
	float dp = ppp.dotproduct(Device.vCameraDirection);
	float tanf_fov2 = tanf((Device.fFOV / 2) * (PI / 180))*dp;

	return Fvector2().set(
		512 + (ppp.dotproduct(Device.vCameraRight) *  0.5f * Device.dwHeight * 1024) / (Device.dwWidth * tanf_fov2),
		384 - (ppp.dotproduct(Device.vCameraTop)   *  0.5f * 768) / (tanf_fov2));
}

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

Ivector	vpack			(Fvector src)
{
	Fvector			_v;
	int	bx			= fpack	(src.x);
	int by			= fpack	(src.y);
	int bz			= fpackZ(src.z);
	// dumb test
	float	e_best	= flt_max;
	int		r=bx,g=by,b=bz;
#ifdef DEBUG
	int		d=0;
#else
	int		d=3;
#endif
	for (int x=_max(bx-d,0); x<=_min(bx+d,255); x++)
	for (int y=_max(by-d,0); y<=_min(by+d,255); y++)
	for (int z=_max(bz-d,0); z<=_min(bz+d,255); z++)
	{
		_v				= vunpack(x,y,z);
		float	m		= _v.magnitude();
		float	me		= _abs(m-1.f);
		if	(me>0.03f)	continue;
		_v.div	(m);
		float	e		= _abs(src.dotproduct(_v)-1.f);
		if (e<e_best)	{
			e_best		= e;
			r=x,g=y,b=z;
		}
	}
	Ivector		ipck;
	ipck.set	(r,g,b);
	return		ipck;
}

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

void CCar::UpdateBack()
{
	if(b_breaks)
	{
		float k=1.f;
		float time=(Device.fTimeGlobal-m_break_start);
		if(time<m_break_time)
		{
			k*=(time/m_break_time);
		}
		xr_vector<SWheelBreak>::iterator i,e;
		i=m_breaking_wheels.begin();
		e=m_breaking_wheels.end();
		for(;i!=e;++i)
				i->Break(k);
		Fvector v;
		m_pPhysicsShell->get_LinearVel(v);
		//if(DriveWheelsMeanAngleRate()<m_breaks_to_back_rate)
		if(v.dotproduct(XFORM().k)<EPS)
		{
			StopBreaking();
			DriveBack();
		}
	}
	//else
	//{
	//	UpdatePower();
	//	if(b_engine_on&&!b_starting && m_current_rpm<m_min_rpm)Stall();
	//}
}

void CElevatorState::UpdateStClimbingDown()
{
	VERIFY(m_ladder&&m_character);
	Fvector d;
	
	if(ClimbDirection()>0.f&&m_ladder->BeforeLadder(m_character))
		SwitchState(clbClimbingUp);
	float to_ax=m_ladder->DDToAxis(m_character,d);
	Fvector ca;ca.set(m_character->ControlAccel());
	float  control_a=to_mag_and_dir(ca);
	if(!fis_zero(to_ax)&&!fis_zero(control_a)&&abs(-ca.dotproduct(Fvector(m_ladder->Norm()).normalize()))<M_SQRT1_2)SwitchState(clbDepart);
	if(m_ladder->AxDistToLowerP(m_character)-m_character->FootRadius()<stop_climbing_dist)
		SwitchState(clbNearDown);
	UpdateClimbingCommon(d,to_ax,ca,control_a);

	if(m_ladder->AxDistToUpperP(m_character)<-m_character->FootRadius())SwitchState(clbNoLadder);

	Fvector vel;
	m_character->GetVelocity(vel);
	if(vel.y>EPS_S)
	{
		m_character->ApplyForce(0.f,-m_character->Mass()*ph_world->Gravity(),0.f);
	}
	//if(to_ax-m_character->FootRadius()>out_dist)
	//														SwitchState((clbNone));
	//if(fis_zero(control_a)) 
	//	m_character->ApplyForce(d,m_character->Mass());
}

BOOL MeshExpUtility::BuildObject(CEditableObject*& exp_obj, LPCSTR m_ExportName)
{
	bool bResult = true;

	if (m_ExportName[0]==0) return false;

	ELog.Msg(mtInformation,"Building object..." );
	char fname[256]; _splitpath( m_ExportName, 0, 0, fname, 0 );
	exp_obj = xr_new<CEditableObject>(fname);	
	exp_obj->SetVersionToCurrent(TRUE,TRUE);

	ExportItemIt it = m_Items.begin();
	for(;it!=m_Items.end();it++){
		CEditableMesh *submesh = xr_new<CEditableMesh>(exp_obj);
		ELog.Msg(mtInformation,"Converting node '%s'...", it->pNode->GetName());
		if( submesh->Convert(it->pNode) ){
			// transform
			Matrix3 mMatrix;
			mMatrix = it->pNode->GetNodeTM(0)*Inverse(it->pNode->GetParentNode()->GetNodeTM(0));

			Point3	r1	= mMatrix.GetRow(0);
			Point3	r2	= mMatrix.GetRow(1);
			Point3	r3	= mMatrix.GetRow(2);
			Point3	r4	= mMatrix.GetRow(3);
			Fmatrix m;	m.identity();
			m.i.set(r1.x, r1.z, r1.y);
			m.j.set(r2.x, r2.z, r2.y);
			m.k.set(r3.x, r3.z, r3.y);
			m.c.set(r4.x, r4.z, r4.y);
			
			submesh->Transform( m );
			// flip faces
			Fvector v; v.crossproduct(m.j, m.k);
			if(v.dotproduct(m.i)<0.f)	submesh->FlipFaces();
			if(m_ObjectFlipFaces)		submesh->FlipFaces();
			submesh->RecomputeBBox();
			// append mesh
			submesh->SetName			(it->pNode->GetName());
			exp_obj->m_Meshes.push_back	(submesh);
		}else{
			ELog.Msg(mtError,"! can't convert", it->pNode->GetName());
			xr_delete(submesh);
			bResult = false;
			break;
		}
	}

	if (bResult){
		exp_obj->UpdateBox		();
		exp_obj->VerifyMeshNames();
		ELog.Msg				(mtInformation,"Object '%s' contains: %d points, %d faces",
								exp_obj->GetName(), exp_obj->GetVertexCount(), exp_obj->GetFaceCount());
	}else{
		xr_delete(exp_obj);
	}
//-------------------------------------------------------------------
	return bResult;
}

void CClimableObject::DSideToAxis		(CPHCharacter	*actor,Fvector	&dir)const
{
	VERIFY(actor);
	DToAxis(actor,dir);
	Fvector side;side.set(m_side);
	to_mag_and_dir(side);
	side.mul(side.dotproduct(dir));
	dir.set(side);
}

bool CClimableObject::BeforeLadder(CPHCharacter *actor,float tolerance/*=0.f*/)const
{
	VERIFY(actor);
	Fvector d;
	DToAxis(actor,d);
	Fvector n;n.set(Norm());
	float width=to_mag_and_dir(n);
	return d.dotproduct(n)<-(width+actor->FootRadius()/2.f+tolerance);
}

void CClimableObject::DToPlain(CPHCharacter *actor,Fvector &dist)const
{
 VERIFY(actor);
 DToAxis(actor,dist);
 Fvector norm;norm.set(m_norm);
 to_mag_and_dir(norm);
 float dot=norm.dotproduct(dist);
 norm.mul(dot);
 dist.set(norm);
}

// ----------------------------------------------------------------------
// Name  : intersectRayPlane()
// Input : rOrigin - origin of ray in world space
//         rVector - xr_vector describing direction of ray in world space
//         pOrigin - Origin of plane
//         pNormal - Normal to plane
// Notes : Normalized directional vectors expected
// Return: distance to plane in world units, -1 if no intersection.
// -----------------------------------------------------------------------
IC float intersectRayPlane(	const Fvector& rayOrigin,	const Fvector& rayDirection,
                            const Fvector& planeOrigin,	const Fvector& planeNormal)
{
    float numer = classifyPoint(rayOrigin,planeOrigin,planeNormal);
    float denom = planeNormal.dotproduct(rayDirection);

    if (denom == 0)  // normal is orthogonal to xr_vector, cant intersect
        return (-1.0f);

    return -(numer / denom);
}