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

void CShadowController::MaxSpeed( const Vector &velocity, const AngularImpulse &angularVelocity )
{
	IVP_Core *pCore = m_pObject->GetObject()->get_core();

	// limit additional velocity to that which is not amplifying the current velocity
	IVP_U_Float_Point ivpVel;
	ConvertPositionToIVP( velocity, ivpVel );
	IVP_U_Float_Point available = ivpVel;

	m_currentSpeed = 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_shadow.maxSpeed, available );

	// same for angular, project on to current and remove redundant (amplifying) input
	IVP_U_Float_Point ivpAng;
	ConvertAngularImpulseToIVP( angularVelocity, ivpAng );
	IVP_U_Float_Point availableAng = ivpAng;
	float lengthAng = ivpAng.real_length_plus_normize();

	float dotAng = ivpAng.dot_product( &pCore->rot_speed );
	if ( dotAng > 0 )
	{
		ivpAng.mult( dotAng * lengthAng );
		availableAng.subtract( &ivpAng );
	}

	IVP_Float_PointAbs( m_shadow.maxAngular, availableAng );
}

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

float ComputeShadowControllerIVP( IVP_Real_Object *pivp, shadowcontrol_params_t &params, float secondsToArrival, float dt )
{
	// resample fraction
	// This allows us to arrive at the target at the requested time
	float fraction = 1.0;
	if ( secondsToArrival > 0 )
	{
		fraction *= dt / secondsToArrival;
		if ( fraction > 1 )
		{
			fraction = 1;
		}
	}

	secondsToArrival -= dt;
	if ( secondsToArrival < 0 )
	{
		secondsToArrival = 0;
	}

	if ( fraction <= 0 )
		return secondsToArrival;

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

	IVP_U_Point positionIVP;
	GetObjectPosition_IVP( positionIVP, pivp );

	IVP_U_Float_Point delta_position;
	delta_position.subtract( &params.targetPosition, &positionIVP);

	// BUGBUG: Save off velocities and estimate final positions
	// measure error against these final sets
	// also, damp out 100% saved velocity, use max additional impulses
	// to correct error and damp out error velocity
	// extrapolate position
	if ( params.teleportDistance > 0 )
	{
		IVP_DOUBLE qdist;
		if ( !IsZeroVector(params.lastPosition) )
		{
			IVP_U_Float_Point tmpDelta;
			tmpDelta.subtract( &positionIVP, &params.lastPosition );
			qdist = tmpDelta.quad_length();
		}
		else
		{
			// UNDONE: This is totally bogus!  Measure error using last known estimate
			// not current position!
			qdist = delta_position.quad_length();
		}

		if ( qdist > params.teleportDistance * params.teleportDistance )
		{
			if ( pivp->is_collision_detection_enabled() )
			{
				pivp->enable_collision_detection( IVP_FALSE );
				pivp->beam_object_to_new_position( &params.targetRotation, &params.targetPosition, IVP_TRUE );
				pivp->enable_collision_detection( IVP_TRUE );
			}
			else
			{
				pivp->beam_object_to_new_position( &params.targetRotation, &params.targetPosition, IVP_TRUE );
			}
		}
	}

	float invDt = 1.0f / dt;
	IVP_Core *pCore = pivp->get_core();
	ComputeController( pCore->speed, delta_position, params.maxSpeed, fraction * invDt, params.dampFactor );

	params.lastPosition.add_multiple( &positionIVP, &pCore->speed, dt );

	IVP_U_Float_Point deltaAngles;
	// compute rotation offset
	IVP_U_Quat deltaRotation;
	QuaternionDiff( params.targetRotation, pCore->q_world_f_core_next_psi, deltaRotation );

	// convert offset to angular impulse
	Vector axis;
	float angle;
	QuaternionAxisAngle( deltaRotation, axis, angle );
	VectorNormalize(axis);

	deltaAngles.k[0] = axis.x * angle;
	deltaAngles.k[1] = axis.y * angle;
	deltaAngles.k[2] = axis.z * angle;

	ComputeController( pCore->rot_speed, deltaAngles, params.maxAngular, fraction * invDt, params.dampFactor );

	return secondsToArrival;
}