ivp_controller_raycast_car.cxx

hl2 source code. Do not use it illegal.

// Copyright (C) Ipion Software GmbH 1999-2000. All rights reserved.

// IVP_EXPORT_PROTECTED

/********************************************************************************
 *	Name:	     	IVP_Car_System
 *	Description:	
 ********************************************************************************/
#include <ivp_physics.hxx>
#ifndef WIN32
#	pragma implementation "ivp_controller_raycast_car.hxx"
#endif

// includes for API

#include <ivp_material.hxx>
#include <ivp_cache_object.hxx>
#include <ivp_actuator.hxx>
#include <ivp_constraint_car.hxx>
#include <ivp_car_system.hxx>
#include <ivp_ray_solver.hxx>
#include <ivp_controller_raycast_car.hxx>
#include <ivp_solver_core_reaction.hxx>

//-----------------------------------------------------------------------------
// Purpose: Main raycast car simulation.
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::do_simulation_controller( IVP_Event_Sim *es, IVP_U_Vector<IVP_Core> * )
{
    IVP_Raycast_Car_Wheel_Temp	wt[IVP_CONSTRAINT_CAR_MAX_WHEELS];
    IVP_Ray_Solver_Template rst[IVP_CONSTRAINT_CAR_MAX_WHEELS];
    IVP_Ray_Hit ray_hits[IVP_CONSTRAINT_CAR_MAX_WHEELS];
    IVP_FLOAT frictions[IVP_CONSTRAINT_CAR_MAX_WHEELS];
    
    const IVP_U_Matrix *m_world_f_core = car_body->get_core()->get_m_world_f_core_PSI();
    
    IVP_Core *car_core = car_body->get_core();
    
    // Raycasts.
	SetupWheelRaycasts( rst, m_world_f_core, wt );
    do_raycasts( es, n_wheels, rst, ray_hits, frictions );

	// Wheels.
	if ( !DoSimulationWheels( rst, m_world_f_core, wt, ray_hits, frictions, car_core ) )
		return;

    // Stabilizers.
	DoSimulationStabilizers( wt );

    // Extra gravity.
    {
		car_core->speed.add_multiple( &normized_gravity_ws, extra_gravity * car_core->get_inv_mass() * es->delta_time );
    }
    
	// Springs.
	DoSimulationShocks( wt, ray_hits, es, car_core );

	// Booster.
	DoSimulationBooster( es, car_core );

	// Steering.
	DoSimulationSteering( wt, car_core, es );	
}

//-----------------------------------------------------------------------------
// Purpose: Initialize the rays to be cast from the vehicle wheel positions to
//          the "ground."
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::SetupWheelRaycasts( IVP_Ray_Solver_Template *pRaySolverTemplates, 
													 const IVP_U_Matrix *m_world_f_core, 
													 IVP_Raycast_Car_Wheel_Temp *pTempWheels )
{
	for ( int iWheel = 0; iWheel < n_wheels; ++iWheel )
	{
		IVP_Raycast_Car_Wheel *pWheel = get_wheel( IVP_POS_WHEEL( iWheel ) );
		if ( pWheel )
		{
			// Fill the in the ray solver template for the current wheel.
			IVP_Ray_Solver_Template &raySolverTemplate = pRaySolverTemplates[iWheel];

			// Transform the wheel "start" position from vehicle core-space to world-space.  This is
			// the raycast starting position.
			m_world_f_core->vmult4( &pWheel->hp_cs, &raySolverTemplate.ray_start_point );

			// Transform the shock (spring) direction from vehicle core-space to world-space.  This is
			// the raycast direction.
			m_world_f_core->vmult3( &pWheel->spring_direction_cs, &pTempWheels[iWheel].spring_direction_ws );
			raySolverTemplate.ray_normized_direction.set( &pTempWheels[iWheel].spring_direction_ws );

			// Set the length of the ray cast.
			raySolverTemplate.ray_length = pWheel->spring_len + pWheel->wheel_radius;	

			// Set the ray solver template flags.  This defines wish objects you wish to
			// collide against in the physics environment.
			raySolverTemplate.ray_flags = IVP_RAY_SOLVER_ALL;
		}
	}
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
bool IVP_Controller_Raycast_Car::DoSimulationWheels( IVP_Ray_Solver_Template *pRaySolverTemplates, 
													 const IVP_U_Matrix *m_world_f_core, 
													 IVP_Raycast_Car_Wheel_Temp *pTempWheels,
													 IVP_Ray_Hit *pRayHits, IVP_FLOAT *pFrictions, 
													 IVP_Core *pCarCore )
{
	for( int iWheel = 0; iWheel < n_wheels; ++iWheel )
	{
		// Get "hit" data.
		IVP_Raycast_Car_Wheel *pWheel = get_wheel( IVP_POS_WHEEL( iWheel ) );
		IVP_Raycast_Car_Wheel_Temp *pTempWheel = &pTempWheels[iWheel];
		IVP_Ray_Hit *pRayHit = &pRayHits[iWheel];

		if ( !pWheel || !pTempWheel || !pRayHit )
			continue;

		if ( pRayHit->hit_real_object )
		{
			// Get the hit object.
			IVP_Cache_Object *pCacheObject = pRayHit->hit_real_object->get_cache_object_no_lock();
			if ( pCacheObject )
			{
				// Transform the impact normal from object-space to world-space.
				pCacheObject->transform_vector_to_world_coords( &pRayHit->hit_surface_direction_os, &pTempWheel->ground_normal_ws );

				// Set the wheel's distance to move along the ray to reach the impact surface.
				pWheel->raycast_dist = pRayHit->hit_distance;
				// IVP_ASSERT( wheel->raycast_dist <= wheel->spring_len + wheel->wheel_radius);

				// Get the inverse portion of the surface normal in the direction of the ray cast (shock - used in the shock simulation code for the sign
				// and percentage of force applied to the shock).
				pTempWheel->inv_normal_dot_dir = 1.1f / ( IVP_Inline_Math::fabsd( pTempWheel->spring_direction_ws.dot_product( &pTempWheel->ground_normal_ws ) ) + 0.1f );
			}
		}
		else
		{
			// The wheel is in air (i.e. no pressure on the wheel and it is at full extension).
			pWheel->pressure = 0.0f;
			pWheel->raycast_dist = pWheel->spring_len + pWheel->wheel_radius;

			// Set default non-impact data.
			pTempWheel->inv_normal_dot_dir = 1.0f;	    
			pTempWheel->moveable_object_hit_by_ray = NULL;
			pTempWheel->ground_normal_ws.set_multiple( &pTempWheel->spring_direction_ws, -1 );
		}

		// Set the wheel friciton - ground friction (if any) + wheel friction.
		pTempWheel->friction_value = pFrictions[iWheel] * pWheel->friction_of_wheel;

		// Set the new wheel position (the impact point or the full ray distance).
		pTempWheel->ground_hit_ws.add_multiple( &pRaySolverTemplates[iWheel].ray_start_point, &pTempWheel->spring_direction_ws, pWheel->raycast_dist );

		// Get the speed (velocity) at the impact point.
		pCarCore->get_surface_speed_ws( &pTempWheel->ground_hit_ws, &pTempWheel->surface_speed_wheel_ws );
		pTempWheel->projected_surface_speed_wheel_ws.set_orthogonal_part( &pTempWheel->surface_speed_wheel_ws, &pTempWheel->ground_normal_ws );

		m_world_f_core->vmult3( &pWheel->axis_direction_cs, &pTempWheel->axis_direction_ws );
		pTempWheel->projected_axis_direction_ws.set_orthogonal_part( &pTempWheel->axis_direction_ws, &pTempWheel->ground_normal_ws );
		if ( pTempWheel->projected_axis_direction_ws.normize() == IVP_FAULT )
		{
			IVP_IF( 1 )
			{
				printf( "IVP_Controller_Raycast_Car::do_simulation_controller projected_axis_direction_ws.normize failed\n" );
			}

			return false;
		}
	}

	return true;
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::DoSimulationStabilizers( IVP_Raycast_Car_Wheel_Temp *pTempWheels )
{
    if ( wheels_per_axis == 2 )
	{
		for ( unsigned int iAxis = 0; iAxis < ( unsigned int )n_axis; ++iAxis )
		{
			// Get the two wheel on the axle to stabilize.
			IVP_Raycast_Car_Wheel *pWheel0 = get_wheel( IVP_POS_WHEEL( iAxis * wheels_per_axis ) );
			IVP_Raycast_Car_Wheel *pWheel1 = get_wheel( IVP_POS_WHEEL( iAxis * wheels_per_axis + 1 ) );

			// Get the distances traveled for the two wheels.
			IVP_DOUBLE flDiff0 = pWheel0->raycast_dist - pWheel0->spring_len - pWheel0->wheel_radius;
			IVP_DOUBLE flDiff1 = pWheel1->raycast_dist - pWheel1->spring_len - pWheel1->wheel_radius;

			// Raycast vehicles tend to be a bit more shaky than the real wheeled vehicles so I threw in this 0.5 factor for now.
			pTempWheels[2*iAxis].stabilizer_force = ( flDiff1 - flDiff0 ) * ( get_axis( IVP_POS_AXIS( iAxis ) )->stabilizer_constant * 0.5f );
			pTempWheels[2*iAxis+1].stabilizer_force = -pTempWheels[2*iAxis].stabilizer_force;
		}
    }
	else
	{
		for ( unsigned int iWheel = 0; iWheel < ( unsigned int )n_wheels; ++iWheel )
		{
			pTempWheels[iWheel].stabilizer_force = 0.0f;
		}
    }
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::DoSimulationShocks( IVP_Raycast_Car_Wheel_Temp *pTempWheels,
													 IVP_Ray_Hit *pRayHits,
													 IVP_Event_Sim *pEventSim,
													 IVP_Core *pCarCore )
{
    // simulate springs
	for ( int iWheel = 0; iWheel < n_wheels; ++iWheel )
	{
		IVP_Raycast_Car_Wheel *pWheel = get_wheel( IVP_POS_WHEEL( iWheel ) );
		if ( !pWheel )
			continue;

		// Check to see if we hit anything, otherwise the shocks just go to rest.
		if ( !pRayHits[iWheel].hit_real_object ) 
			continue;

		// Check to see if we hit anything, otherwise the shocks just go to rest.
		IVP_DOUBLE flDiff = pWheel->raycast_dist - pWheel->spring_len - pWheel->wheel_radius;
		if ( flDiff >= 0 ) 
			continue;

#if 0
		// Since the front and back wheels are not the same distance from the center of mass we need to adjust the spring
		// constant accordingly.
	    const IVP_U_Matrix *m_world_f_core = car_body->get_core()->get_m_world_f_core_PSI();

		IVP_Raycast_Car_Wheel *pFrontWheel = get_wheel( IVP_POS_WHEEL( 0 ) );
		IVP_Raycast_Car_Wheel *pBackWheel = get_wheel( IVP_POS_WHEEL( 3 ) );

		IVP_U_Point frontPoint, backPoint;
		m_world_f_core->vmult4( &pFrontWheel->hp_cs, &frontPoint );
		m_world_f_core->vmult4( &pBackWheel->hp_cs, &backPoint );

		const IVP_U_Point pCenterOfMass = pCarCore->get_position_PSI();

		IVP_DOUBLE flZDeltaFront = frontPoint.k[2] - pCenterOfMass.k[2];
		IVP_DOUBLE flZDeltaBack = backPoint.k[2] - pCenterOfMass.k[2];

		IVP_DOUBLE flAlpha = fabs( flZDeltaFront / flZDeltaBack );

		IVP_FLOAT flSpringConstant, flSpringRelax, flSpringCompress;
		if ( iWheel == 0 || iWheel == 1 )
		{
			flSpringConstant = pWheel->spring_constant;
			flSpringRelax = pWheel->spring_damp_relax;
			flSpringCompress = pWheel->spring_damp_compress;
		}
		else
		{
			flSpringConstant = pWheel->spring_constant / flAlpha;
			flSpringRelax = pWheel->spring_damp_relax / flAlpha;
			flSpringCompress = pWheel->spring_damp_compress / flAlpha;
		}
#endif

		IVP_FLOAT flSpringConstant, flSpringRelax, flSpringCompress;
		flSpringConstant = pWheel->spring_constant;
		flSpringRelax = pWheel->spring_damp_relax;
		flSpringCompress = pWheel->spring_damp_compress;

		// Static force.
		IVP_DOUBLE flForce = -flDiff * flSpringConstant + pTempWheels[iWheel].stabilizer_force;
		IVP_FLOAT flInvNormalDotDir = pTempWheels[iWheel].inv_normal_dot_dir;
		if ( flInvNormalDotDir < 0.0f ) { flInvNormalDotDir = 0.0f; }
		if ( flInvNormalDotDir > 3.0f ) { flInvNormalDotDir = 3.0f; }
		flForce *= flInvNormalDotDir;