C++ IVP_U_Float_Point类代码示例

void CPhysicsObject::CalculateVelocityOffset( const Vector &forceVector, const Vector &worldPosition, Vector &centerVelocity, AngularImpulse &centerAngularVelocity )
{
	IVP_U_Point pos;
	IVP_U_Float_Point force;

	ConvertForceImpulseToIVP( forceVector, force );
	ConvertPositionToIVP( worldPosition, pos );

	IVP_Core *core = m_pObject->get_core();

    const IVP_U_Matrix *m_world_f_core = core->get_m_world_f_core_PSI();

    IVP_U_Float_Point point_d_ws;
	point_d_ws.subtract(&pos, m_world_f_core->get_position());

    IVP_U_Float_Point cross_point_dir;

    cross_point_dir.calc_cross_product( &point_d_ws, &force);
    m_world_f_core->inline_vimult3( &cross_point_dir, &cross_point_dir);

    cross_point_dir.set_pairwise_mult( &cross_point_dir, core->get_inv_rot_inertia());
	ConvertAngularImpulseToHL( cross_point_dir, centerAngularVelocity );
    force.set_multiple( &force, core->get_inv_mass() );
	ConvertForceImpulseToHL( force, centerVelocity );
}

//-----------------------------------------------------------------------------
// Purpose: Handle pontoons on water.
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::DoSimulationPontoonsWater( IVP_Raycast_Airboat_Wheel *pPontoonPoint, 
									 						    IVP_Raycast_Airboat_Pontoon_Temp *pTempPontoon,
													            IVP_Raycast_Airboat_Impact *pImpact, IVP_Event_Sim *pEventSim,
													            IVP_Core *pAirboatCore, IVP_BOOL bFront )
{
	IVP_U_Float_Point vecForwardWS; 
    const IVP_U_Matrix *matWorldFromCore = pAirboatCore->get_m_world_f_core_PSI();
	matWorldFromCore->get_col( IVP_COORDINATE_INDEX( index_z ), &vecForwardWS );

	IVP_FLOAT flVolumeScalar = 1.0f;
	if ( vecForwardWS.k[index_y] > 0.0f && !bFront )
	{
		flVolumeScalar += -vecForwardWS.k[index_y];
	} 
	else if ( flVolumeScalar > 0.0f && bFront )
	{
		flVolumeScalar += vecForwardWS.k[index_y];
	}

	IVP_FLOAT flForce = AIRBOAT_BUOYANCY_SCALAR * 0.25f * pAirboatCore->get_mass() * AIRBOAT_GRAVITY * flVolumeScalar;
	
	IVP_DOUBLE flImpulse = flForce * pEventSim->delta_time;		
	
	IVP_U_Float_Point vecImpulseWS; 
	vecImpulseWS.set_multiple( &pTempPontoon->raycast_dir_ws, flImpulse );
	pAirboatCore->push_core_ws( &pTempPontoon->ground_hit_ws, &vecImpulseWS );
}

    virtual void do_simulation_controller(IVP_Event_Sim *event,IVP_U_Vector<IVP_Core> *core_list)
	{
		int i;
		for( i = core_list->len()-1; i >=0; i--) 
		{
			IVP_Core *pCore = core_list->element_at(i);

			IVP_Real_Object *pivp = pCore->objects.element_at(0);
			CPhysicsObject *pPhys = static_cast<CPhysicsObject *>(pivp->client_data);
			
			float dragForce = -0.5 * pPhys->GetDragInDirection( pCore->speed ) * m_airDensity * event->delta_time;
			if ( dragForce < -1.0f )
				dragForce = -1.0f;
			if ( dragForce < 0 )
			{
				IVP_U_Float_Point dragVelocity;
				dragVelocity.set_multiple( &pCore->speed, dragForce );
				pCore->speed.add( &dragVelocity );
			}
			float angDragForce = -pPhys->GetAngularDragInDirection( pCore->rot_speed ) * m_airDensity * event->delta_time;
			if ( angDragForce < -1.0f )
				angDragForce = -1.0f;
			if ( angDragForce < 0 )
			{
				IVP_U_Float_Point angDragVelocity;
				angDragVelocity.set_multiple( &pCore->rot_speed, angDragForce );
				pCore->rot_speed.add( &angDragVelocity );
			}
		}
	}

void CPlayerController::do_simulation_controller( IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> *)
{
	if ( !m_enable )
		return;

	IVP_Real_Object *pivp = m_pObject->GetObject();
	IVP_Environment *pIVPEnv = pivp->get_environment();
	CPhysicsEnvironment *pVEnv = (CPhysicsEnvironment *)pIVPEnv->client_data;

	float psiScale = pVEnv->GetInvPSIScale();
	if ( !psiScale )
		return;

	IVP_Core *pCore = pivp->get_core();
	// current situation
	const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI();
	const IVP_U_Point *cur_pos_ws = m_world_f_core->get_position();

	// ---------------------------------------------------------
	// Translation
	// ---------------------------------------------------------

	IVP_U_Float_Point delta_position;  delta_position.subtract( &m_targetPosition, cur_pos_ws);


	if (!pivp->flags.shift_core_f_object_is_zero)
	{
		IVP_U_Float_Point shift_cs_os_ws;
		m_world_f_core->vmult3( pivp->get_shift_core_f_object(), &shift_cs_os_ws);
		delta_position.subtract( &shift_cs_os_ws );
	}


	IVP_DOUBLE qdist = delta_position.quad_length();

	// UNDONE: This is totally bogus!  Measure error using last known estimate
	// not current position!
	if ( qdist > m_maxDeltaPosition * m_maxDeltaPosition )
	{
		if ( TryTeleportObject() )
			return;
	}

	// float to allow stepping
	if ( m_onground )
	{
		const IVP_U_Point *pgrav = es->environment->get_gravity();
		IVP_U_Float_Point gravSpeed;
		gravSpeed.set_multiple( pgrav, es->delta_time );
		pCore->speed.subtract( &gravSpeed );
	}

	ComputeController( pCore->speed, delta_position, m_maxSpeed, psiScale / es->delta_time, m_dampFactor );
}

// convert square velocity magnitude from IVP to HL
float CPhysicsObject::GetEnergy()
{
	IVP_Core *pCore = m_pObject->get_core();
	IVP_FLOAT energy = 0.0f;
	IVP_U_Float_Point tmp;

	energy = 0.5f * pCore->get_mass() * pCore->speed.dot_product(&pCore->speed); // 1/2mvv
	tmp.set_pairwise_mult(&pCore->rot_speed, pCore->get_rot_inertia()); // wI
	energy += 0.5f * tmp.dot_product(&pCore->rot_speed); // 1/2mvv + 1/2wIw
	
	return ConvertEnergyToHL( energy );
}

// Apply force impulse (momentum) to the object
void CPhysicsObject::ApplyForceCenter( const Vector &forceVector )
{
//	Assert(IsMoveable());
	if ( !IsMoveable() )
		return;

	IVP_U_Float_Point tmp;

	ConvertForceImpulseToIVP( forceVector, tmp );
	IVP_Core *core = m_pObject->get_core();
	tmp.mult( core->get_inv_mass() );
	tmp.k[0] = clamp( tmp.k[0], -MAX_SPEED, MAX_SPEED );
	tmp.k[1] = clamp( tmp.k[1], -MAX_SPEED, MAX_SPEED );
	tmp.k[2] = clamp( tmp.k[2], -MAX_SPEED, MAX_SPEED );
	m_pObject->async_add_speed_object_ws( &tmp );
}

//-----------------------------------------------------------------------------
// Purpose: Handle pontoons on ground.
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::DoSimulationPontoonsGround( IVP_Raycast_Airboat_Wheel *pPontoonPoint, 
																 IVP_Raycast_Airboat_Pontoon_Temp *pTempPontoon,
													             IVP_Raycast_Airboat_Impact *pImpact, IVP_Event_Sim *pEventSim,
													             IVP_Core *pAirboatCore )
{
	// Check to see if we hit anything, otherwise the no force on this point.
	IVP_DOUBLE flDiff = pPontoonPoint->raycast_dist - pPontoonPoint->raycast_length;
	if ( flDiff >= 0 ) 
		return;
	
	IVP_FLOAT flSpringConstant, flSpringRelax, flSpringCompress;
	flSpringConstant = pPontoonPoint->spring_constant;
	flSpringRelax = pPontoonPoint->spring_damp_relax;
	flSpringCompress = pPontoonPoint->spring_damp_compress;
	
	IVP_DOUBLE flForce = -flDiff * flSpringConstant;
	IVP_FLOAT flInvNormalDotDir = pTempPontoon->inv_normal_dot_dir;
	if ( flInvNormalDotDir < 0.0f ) { flInvNormalDotDir = 0.0f; }
	if ( flInvNormalDotDir > 3.0f ) { flInvNormalDotDir = 3.0f; }
	flForce *= flInvNormalDotDir;
	
	IVP_U_Float_Point vecSpeedDelta; 
	vecSpeedDelta.subtract( &pTempPontoon->projected_surface_speed_wheel_ws, &pTempPontoon->surface_speed_wheel_ws );
	
	IVP_DOUBLE flSpeed = vecSpeedDelta.dot_product( &pTempPontoon->raycast_dir_ws );
	if ( flSpeed > 0 )
	{
		flForce -= flSpringRelax * flSpeed;
	}
	else
	{
		flForce -= flSpringCompress * flSpeed;
	}
	
	if ( flForce < 0 )
	{
		flForce = 0.0f;
	}
	
	IVP_DOUBLE flImpulse = flForce * pEventSim->delta_time;		
	
	IVP_U_Float_Point vecImpulseWS; 
	vecImpulseWS.set_multiple( &pTempPontoon->ground_normal_ws, flImpulse );
	pAirboatCore->push_core_ws( &pTempPontoon->ground_hit_ws, &vecImpulseWS );
}

void CPlayerController::Update( const Vector& position, const Vector& velocity, bool onground, IPhysicsObject *ground )
{
	IVP_U_Point	targetPositionIVP;
	IVP_U_Float_Point targetSpeedIVP;

	ConvertPositionToIVP( position, targetPositionIVP );
	ConvertPositionToIVP( velocity, targetSpeedIVP );
	
	// if the object hasn't moved, abort
	if ( targetSpeedIVP.quad_distance_to( &m_currentSpeed ) < 1e-6 )
	{
		if ( targetPositionIVP.quad_distance_to( &m_targetPosition ) < 1e-6 )
		{
			return;
		}
	}

	m_targetPosition = targetPositionIVP;
	m_currentSpeed = targetSpeedIVP;

	IVP_Real_Object *pivp = m_pObject->GetObject();
	IVP_Core *pCore = pivp->get_core();
	IVP_Environment *pEnv = pivp->get_environment();
	pEnv->get_controller_manager()->ensure_core_in_simulation(pCore);

	m_enable = true;
	// m_onground makes this object anti-grav
	// UNDONE: Re-evaluate this
	m_onground = false;//onground;

	if ( velocity.LengthSqr() <= 0.1f )
	{
		// no input velocity, just go where physics takes you.
		m_enable = false;
		ground = NULL;
	}
	else
	{
		MaxSpeed( velocity );
	}
}

void CPhysicsObject::GetVelocity( Vector *velocity, AngularImpulse *angularVelocity )
{
	if ( !velocity && !angularVelocity )
		return;

	IVP_Core *core = m_pObject->get_core();
	if ( velocity )
	{
		IVP_U_Float_Point speed;
		speed.add( &core->speed, &core->speed_change );
		ConvertPositionToHL( speed, *velocity );
	}

	if ( angularVelocity )
	{
		IVP_U_Float_Point rotSpeed;
		rotSpeed.add( &core->rot_speed, &core->rot_speed_change );
		// xform to HL space
		ConvertAngularImpulseToHL( rotSpeed, *angularVelocity );
	}
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::DoSimulationTurbine(IVP_Event_Sim *pEventSim)
{
	IVP_Core *pAirboatCore = m_pAirboatBody->get_core();

	IVP_FLOAT thrust = m_flThrust;
	if (m_bAirborneIdle || (m_bAirborne && (thrust < 0.0f)))
		thrust *= 0.5f;

	IVP_U_Float_Point vecImpulse;
	pAirboatCore->get_m_world_f_core_PSI()->get_col(IVP_COORDINATE_INDEX(index_z), &vecImpulse);
	IVP_FLOAT flForwardY = vecImpulse.k[1];

	if ((flForwardY < -0.5f) && (thrust > 0.0f))
		thrust *= 1.0f + flForwardY;
	else if ((flForwardY > 0.5f) && (thrust < 0.0f))
		thrust *= 1.0f - flForwardY;

	
	vecImpulse.mult(pAirboatCore->get_mass() * thrust * pEventSim->delta_time);
	pAirboatCore->center_push_core_multiple_ws(&vecImpulse);
}

bool CTraceSolverSweptObject::SweepHitsSphereOS( IVP_U_Float_Point *sphereCenter, float radius )
{
	// the ray is actually a line-swept-sphere with sweep object's radius
	radius += m_sweepObject->Radius();

    IVP_DOUBLE qrad_sum = m_rayLengthOS * 0.5f + radius;
    qrad_sum *= qrad_sum;
    IVP_U_Float_Point delta_vec; // quick check for ends of ray
    delta_vec.subtract(sphereCenter, &m_rayCenterOS);
    IVP_DOUBLE quad_center_dist = delta_vec.quad_length();
    
	// Could a ray in any direction away from the ray center intersect this sphere?
	if ( quad_center_dist >= qrad_sum )
	{
		return false;
    }

	// Is the sphere close enough to the ray at the center?
    IVP_DOUBLE qsphere_rad = radius * radius;
    if ( quad_center_dist < qsphere_rad )
	{
		return true;
    }
    
	// If this is a 0 length ray, then the conservative test is 100% accurate
	if ( m_rayLengthOS > 0 )
	{
		// Calculate the perpendicular distance to the sphere 
		// The perpendicular forms a right triangle with the vector between the ray/sphere centers
		// and the ray direction vector.  Calculate the projection of the hypoteneuse along the perpendicular
		IVP_U_Float_Point h;
		h.inline_calc_cross_product(&m_rayDirOS, &delta_vec);
    
		IVP_FLOAT quad_dist = (IVP_FLOAT)h.quad_length();
    
		if( quad_dist < qsphere_rad )
			return true;
    }
    return false;
}

void CTraceSolverSweptObject::SweepLedgeTree_r( const IVP_Compact_Ledgetree_Node *node )
{
    // Recursive function
    // Check whether ray hits the ledge tree node sphere
    IVP_U_Float_Point center; center.set( node->center.k);
	
	if ( !SweepHitsSphereOS( &center, node->radius ) )
		return;

	if ( node->is_terminal() == IVP_TRUE )
	{
		const IVP_Compact_Ledge *ledge = node->get_compact_ledge();
		unsigned int ledgeContents = m_pConvexInfo->GetContents( ledge->get_client_data() );
		if ( m_contentsMask & ledgeContents )
		{
			m_obstacle->SetLedge( ledge );
			if ( SweepSingleConvex() )
			{
				float fraction = m_pTotalTrace->fraction * m_trace.fraction;
				*m_pTotalTrace = m_trace;
				m_pTotalTrace->fraction = fraction;
				m_ray->Reset( m_ray->m_length * m_trace.fraction );

				// Update OS ray to limit tests
				m_rayCenterOS.add_multiple( &m_rayStartOS, &m_rayDirOS, m_ray->m_length*0.5f );
				m_rayEndOS.add_multiple( &m_rayStartOS,  &m_rayDirOS, m_ray->m_length );
				m_rayLengthOS = m_ray->m_length;
				m_pTotalTrace->contents = ledgeContents;
			}
		}
	
		return;
    }
    
    // check nodes children
    SweepLedgeTree_r(node->left_son());
    SweepLedgeTree_r(node->right_son());
}

void CPlayerController::MaxSpeed( const Vector &velocity )
{
	IVP_Core *pCore = m_pObject->GetObject()->get_core();
	IVP_U_Float_Point ivpVel;
	ConvertPositionToIVP( velocity, ivpVel );
	IVP_U_Float_Point available = ivpVel;
	
	// normalize and save length
	float length = ivpVel.real_length_plus_normize();

	float dot = ivpVel.dot_product( &pCore->speed );
	if ( dot > 0 )
	{
		ivpVel.mult( dot * length );
		available.subtract( &ivpVel );
	}

	IVP_Float_PointAbs( m_maxSpeed, available );
}

void CPhysicsObject::GetVelocityAtPoint( const Vector &worldPosition, Vector &velocity )
{
	IVP_Core *core = m_pObject->get_core();
	IVP_U_Point pos;
	ConvertPositionToIVP( worldPosition, pos );

	IVP_U_Float_Point rotSpeed;
	rotSpeed.add( &core->rot_speed, &core->rot_speed_change );

    IVP_U_Float_Point av_ws;
    core->get_m_world_f_core_PSI()->vmult3( &rotSpeed, &av_ws);

    IVP_U_Float_Point pos_rel; 	
	pos_rel.subtract( &pos, core->get_position_PSI());
    IVP_U_Float_Point cross;    
	cross.inline_calc_cross_product(&av_ws,&pos_rel);

	IVP_U_Float_Point speed;
    speed.add(&core->speed, &cross);
    speed.add(&core->speed_change);

	ConvertPositionToHL( speed, velocity );
}

void ComputeController( IVP_U_Float_Point &currentSpeed, const IVP_U_Float_Point &delta, const IVP_U_Float_Point &maxSpeed, float scaleDelta, float damping )
{
	// scale by timestep
	IVP_U_Float_Point acceleration;
	acceleration.set_multiple( &delta, scaleDelta );
	
	if ( currentSpeed.quad_length() < 1e-6 )
	{
		currentSpeed.set_to_zero();
	}

	acceleration.add_multiple( &currentSpeed, -damping );

	for(int i=2; i>=0; i--)
	{
		if(IVP_Inline_Math::fabsd(acceleration.k[i]) < maxSpeed.k[i]) 
			continue;
		
		// clip force
		acceleration.k[i] = (acceleration.k[i] < 0) ? -maxSpeed.k[i] : maxSpeed.k[i];
	}

	currentSpeed.add( &acceleration );
}