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

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

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