C++ IVP_U_Float_Point::set_multiple方法代码示例

    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 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: Water drag.
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::DoSimulationDrag( IVP_Raycast_Airboat_Pontoon_Temp *pTempPontoons, 
													   IVP_Event_Sim *pEventSim, IVP_Core *pAirboatCore )
{
	// Get the forward vector in world-space.
	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 );

	// Get the airboat speed and direction.
	IVP_U_Float_Point vecAirboatDir = pAirboatCore->speed;
	vecAirboatDir.normize();

	IVP_FLOAT flSpeed = ( IVP_FLOAT )pAirboatCore->speed.real_length();
	IVP_FLOAT flSpeed2 = flSpeed * flSpeed;

	// Increase drag the more perpendicular the airboat gets to the direction of motion.
//	IVP_FLOAT flDragTurnScale = vecAirboatDir.dot_product( &vecForwardWS );
//	flDragTurnScale = ( 1.0f - flDragTurnScale ) + 1.0f;

	// Calculate the drag.
	IVP_FLOAT flDragScale = 0.5f * AIRBOAT_DRAG_COEFFICIENT * AIRBOAT_WATER_DENSITY * AIRBOAT_DRAG_SURFACE_AREA;
//	flDragScale *= flDragTurnScale;
	IVP_FLOAT flDrag = flSpeed2 * flDragScale;

	// Apply the drag force opposite to the direction of motion.
	IVP_U_Float_Point vecAirboatNegDir;
	vecAirboatNegDir.set_negative( &vecAirboatDir );

	// Apply the drag force.
	IVP_U_Float_Point vecImpulse; 
	vecImpulse.set_multiple( &vecAirboatNegDir, flDrag * pEventSim->delta_time );
	pAirboatCore->center_push_core_multiple_ws( &vecImpulse );
}

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

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

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::DoSimulationTurbine( IVP_Raycast_Airboat_Pontoon_Temp *pTempPontoons,
											              IVP_Event_Sim *pEventSim, IVP_Core *pAirboatCore )
{
	// Get the forward vector in world-space.
	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 );

	// Forward (Front/Back) force
	IVP_U_Float_Point vecImpulse; 
	vecImpulse.set_multiple( &vecForwardWS, m_flThrust * pEventSim->delta_time );
	pAirboatCore->center_push_core_multiple_ws( &vecImpulse );
}

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

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Airboat::DoSimulationSteering( IVP_Raycast_Airboat_Pontoon_Temp *pTempPontoons,
													       IVP_Core *pAirboatCore, IVP_Event_Sim *pEventSim )
{
	IVP_U_Float_Point vecRightWS; 
    const IVP_U_Matrix *matWorldFromCore = pAirboatCore->get_m_world_f_core_PSI();
	matWorldFromCore->get_col( IVP_COORDINATE_INDEX( index_x ), &vecRightWS );

	IVP_FLOAT flForceRotational = 0.0f;

	// Check for rotation.
	if ( fabsf( m_SteeringAngle ) > 0.01 )
	{
		// Get the steering sign.
		IVP_FLOAT flSteeringSign = m_SteeringAngle < 0.0f ? -1.0f : 1.0f;
		flForceRotational = IVP_RAYCAST_AIRBOAT_STEERING_RATE * pAirboatCore->get_mass() * pEventSim->i_delta_time;
		flForceRotational *= flSteeringSign;
	}

	// General force that will help get us back to zero rotation.
	IVP_FLOAT flRotSpeedSign = pAirboatCore->rot_speed.k[1] < 0.0f ? -1.0f : 1.0f;
	IVP_FLOAT flRotationalDrag = -0.5f * IVP_RAYCAST_AIRBOAT_STEERING_RATE * pAirboatCore->get_mass() * pEventSim->i_delta_time;
	flRotationalDrag *= flRotSpeedSign;
	flForceRotational += flRotationalDrag;
	
	IVP_U_Float_Point vecImpulse; 
	for ( int iPoint = 0; iPoint < 4; ++iPoint )
	{
		IVP_Raycast_Airboat_Wheel *pPontoonPoint = get_wheel( IVP_POS_WHEEL( iPoint ) );
		IVP_Raycast_Airboat_Pontoon_Temp *pTempPontoonPoint = &pTempPontoons[iPoint];

		IVP_FLOAT flPontoonSign = iPoint >= 2 ? -1.0f : 1.0f;
		IVP_FLOAT flForceRot = flForceRotational * flPontoonSign;

		vecImpulse.set_multiple( &vecRightWS, flForceRot * pEventSim->delta_time );

		IVP_U_Float_Point vecPointPosCS, vecPointPosWS;
		vecPointPosCS = pPontoonPoint->raycast_start_cs;
		matWorldFromCore->vmult3( &vecPointPosCS, &vecPointPosWS );
		IVP_U_Point vecPointPositionWS( vecPointPosWS );
		pAirboatCore->push_core_ws( &vecPointPositionWS, &vecImpulse );		
	}
}

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