ivp_controller_raycast_car.cxx

hl2 source code. Do not use it illegal.

		
		// Dynamic force.
		IVP_U_Float_Point diff_speed; 
		diff_speed.subtract( &pTempWheels[iWheel].projected_surface_speed_wheel_ws, &pTempWheels[iWheel].surface_speed_wheel_ws );

		IVP_DOUBLE flSpeed = diff_speed.dot_product( &pTempWheels[iWheel].spring_direction_ws );
		if ( flSpeed > 0 )
		{
			flForce -= flSpringRelax * flSpeed;
		}
		else
		{
			flForce -= flSpringCompress * flSpeed;
		}
		
		if ( flForce < 0 )
		{
			flForce = 0.0f;
		}

		pWheel->pressure = flForce;

		IVP_DOUBLE flImpulse = flForce * pEventSim->delta_time;		

		IVP_U_Float_Point impulse_ws; 
		impulse_ws.set_multiple( &pTempWheels[iWheel].ground_normal_ws, flImpulse );
		pCarCore->push_core_ws( &pTempWheels[iWheel].ground_hit_ws, &impulse_ws  );
	}
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::DoSimulationBooster( IVP_Event_Sim *pEventSim, IVP_Core *pCarCore )
{
	// Update the timer for next boost.
	if ( booster_seconds_until_ready > 0.0f )
	{
		booster_seconds_until_ready -= pEventSim->delta_time;
	}
	
	// Update the timer for in boost.
	if( booster_seconds_to_go > 0.0f )
	{
		booster_seconds_to_go -= pEventSim->delta_time;

		if( booster_seconds_to_go <=0.0f )
		{
			booster_force = 0.0f;
		}
	}
	
	// If we are boosting then add the boosting force to the speed.
	if ( booster_force )
	{
		IVP_U_Float_Point booster_dir_ws;
		pCarCore->get_m_world_f_core_PSI()->get_col( IVP_COORDINATE_INDEX( index_z ), &booster_dir_ws );
		pCarCore->speed.add_multiple( &booster_dir_ws, booster_force * pEventSim->delta_time );
	}
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::DoSimulationSteering( IVP_Raycast_Car_Wheel_Temp *pTempWheels,
													  IVP_Core *pCarCore, IVP_Event_Sim *pEventSim )
{
    IVP_FLOAT forcesNeededToDriveStraight[IVP_CONSTRAINT_CAR_MAX_WHEELS];

	CalcSteeringForces( pTempWheels, pCarCore, pEventSim, forcesNeededToDriveStraight );
	ApplySteeringForces( pTempWheels, pCarCore, pEventSim, forcesNeededToDriveStraight );
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::CalcSteeringForces( IVP_Raycast_Car_Wheel_Temp *pTempWheels, 
													 IVP_Core *pCarCore, IVP_Event_Sim *pEventSim,
													 IVP_FLOAT *pForcesNeededToDriveStraight )
{
	IVP_Solver_Core_Reaction coreReactionSolver[2];
	IVP_U_Point frontPosWS;
	IVP_U_Point backPosWS;

	int iRearWheel;

	if ( wheels_per_axis == 2 )
	{
		// Get the mid point of the tire locations front and back.
		frontPosWS.set_interpolate( &pTempWheels[IVP_FRONT_LEFT].ground_hit_ws, &pTempWheels[IVP_FRONT_RIGHT].ground_hit_ws, 0.5f );
		backPosWS.set_interpolate( &pTempWheels[IVP_REAR_LEFT].ground_hit_ws,  &pTempWheels[IVP_REAR_RIGHT].ground_hit_ws, 0.5f );
		iRearWheel = IVP_REAR_LEFT;
	}
	else
	{
		// Get the tire locations front and back. (seems Hacky!!!)
		frontPosWS.set( &pTempWheels[0].ground_hit_ws );
		backPosWS.set( &pTempWheels[1].ground_hit_ws );
		iRearWheel = 1;
	}
	    
	// Initialize the reaction solvers (for the core) front and back wheels.
	coreReactionSolver[0].init_reaction_solver_translation_ws( pCarCore, NULL, frontPosWS, &pTempWheels[IVP_FRONT_LEFT].axis_direction_ws, NULL, NULL );
	coreReactionSolver[1].init_reaction_solver_translation_ws( pCarCore, NULL, backPosWS, &pTempWheels[iRearWheel].axis_direction_ws, NULL, NULL );

	// How does a push at the front/back wheel influence the back/front wheel?
	IVP_FLOAT front_back; 
	front_back = coreReactionSolver[0].cr_mult_inv0[0].dot_product( &coreReactionSolver[1].cross_direction_position_cs0[0]); // rotation part
	front_back += pCarCore->get_inv_mass() * pTempWheels[IVP_FRONT_LEFT].axis_direction_ws.dot_product( &pTempWheels[iRearWheel].axis_direction_ws );

	IVP_DOUBLE relaxation_rate = 1.2f;
	IVP_DOUBLE a = -coreReactionSolver[0].delta_velocity_ds.k[0] * pEventSim->i_delta_time * relaxation_rate;
	IVP_DOUBLE b = -coreReactionSolver[1].delta_velocity_ds.k[0] * pEventSim->i_delta_time * relaxation_rate;
	
	IVP_DOUBLE inv_mat2x2[4];
	const IVP_DOUBLE mtx2x2_00 = coreReactionSolver[0].m_velocity_ds_f_impulse_ds.get_elem(0,0);
	const IVP_DOUBLE mtx2x2_01 = front_back;
	const IVP_DOUBLE mtx2x2_10 = front_back;
	const IVP_DOUBLE mtx2x2_11 = coreReactionSolver[1].m_velocity_ds_f_impulse_ds.get_elem(0,0);

	IVP_RETURN_TYPE retValue = IVP_Inline_Math::invert_2x2_matrix( mtx2x2_00, mtx2x2_01, mtx2x2_01, mtx2x2_11, &inv_mat2x2[0], &inv_mat2x2[1],&inv_mat2x2[2],&inv_mat2x2[3] );
	if( retValue == IVP_OK ) 
	{
		pForcesNeededToDriveStraight[0] = inv_mat2x2[0] * a + inv_mat2x2[1] * b;
		pForcesNeededToDriveStraight[1] = inv_mat2x2[2] * a + inv_mat2x2[3] * b;
	} 
	else 
	{
		pForcesNeededToDriveStraight[0] = 0.0f;
		pForcesNeededToDriveStraight[1] = 0.0f;
	}
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Controller_Raycast_Car::ApplySteeringForces( IVP_Raycast_Car_Wheel_Temp *pTempWheels, 
													  IVP_Core *pCarCore, IVP_Event_Sim *pEventSim,
													  IVP_FLOAT *pForcesNeededToDriveStraight )
{
	bool bForceFixedWheel = false;
	bool bHasTorque = false;
	int iWheel;
	for ( iWheel = 0; iWheel < n_wheels; ++iWheel )
	{
		IVP_Raycast_Car_Wheel *pWheel = get_wheel( IVP_POS_WHEEL( iWheel ) );
		if ( pWheel->torque != 0.0f )
		{
			bHasTorque = true;
		}
	}

	IVP_FLOAT flSpeed = pCarCore->speed.fast_real_length();
	if ( fabs( flSpeed ) < 0.5f && !bHasTorque )
	{
		bForceFixedWheel = true;
	}

	for ( iWheel = 0; iWheel < n_wheels; ++iWheel )
	{ 
		// left right   
		IVP_Raycast_Car_Wheel *pWheel = get_wheel( IVP_POS_WHEEL( iWheel ) );
		IVP_Raycast_Car_Wheel_Temp *pTempWheel = &pTempWheels[iWheel];
		
		IVP_FLOAT flMaxForce = pWheel->pressure * pTempWheel->friction_value;
		
		IVP_U_Float_Point projected_front_dir_ws;
		projected_front_dir_ws.inline_calc_cross_product_and_normize( &pTempWheel->projected_axis_direction_ws, &pTempWheel->spring_direction_ws );
		
		IVP_FLOAT flFrForce = 0.0f;
		IVP_FLOAT flBodySpeed = pCarCore->speed.dot_product( &projected_front_dir_ws );
		
		if ( pWheel->wheel_is_fixed || bForceFixedWheel )
		{
			flFrForce = -.5f * flBodySpeed * pCarCore->get_mass() * pEventSim->i_delta_time;
			pWheel->wheel_angular_velocity = 0.0f;
		}
		else
		{
			pWheel->wheel_angular_velocity = flBodySpeed * pWheel->inv_wheel_radius;

			IVP_FLOAT flSign = flBodySpeed >= 0.0f ? 1.0f : -1.0f;
			IVP_FLOAT flFrictionForce = pTempWheel->friction_value * ( 0.25f * pCarCore->get_mass() ) * flSign;
			flFrForce = -2.5f * flFrictionForce;
			flFrForce += pWheel->torque * pWheel->inv_wheel_radius;
		}
		
		int nAxis = iWheel / wheels_per_axis;

		IVP_DOUBLE flForceStraight = pForcesNeededToDriveStraight[nAxis];
		IVP_FLOAT flQuadSumForce = flFrForce * flFrForce + flForceStraight * flForceStraight;
		if ( flQuadSumForce > flMaxForce * flMaxForce)
		{
			//printf("clipping of fr_force %f %f %f\n", force_straight, fr_force, max_force);
			
			IVP_FLOAT flFactor = IVP_Inline_Math::ivp_sqrtf( flMaxForce * flMaxForce / flQuadSumForce );
			flFrForce *= flFactor;
			flForceStraight *= flFactor;
			if ( !pWheel->wheel_is_fixed )
			{
				//wheel_angular_velocity += fr_force / (max_force + P_FLOAT_EPS);
			}
		}

		pForcesNeededToDriveStraight[nAxis] -= flForceStraight;
		
		pWheel->angle_wheel -= pWheel->wheel_angular_velocity * pEventSim->delta_time;
		
		// left right force
		IVP_U_Float_Point impulse; 
		impulse.set_multiple( &pTempWheel->projected_axis_direction_ws, flForceStraight * pEventSim->delta_time );
		pCarCore->push_core_ws( &pTempWheel->ground_hit_ws, &impulse );
		// front back force
		impulse.set_multiple( &projected_front_dir_ws, flFrForce * pEventSim->delta_time );
		pCarCore->push_core_ws( &pTempWheel->ground_hit_ws, &impulse );
	}    
}

void IVP_Controller_Raycast_Car::do_steering_wheel(IVP_POS_WHEEL wheel_nr, IVP_FLOAT s_angle)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(wheel_nr);
	
    wheel->axis_direction_cs.set_to_zero();
    wheel->axis_direction_cs.k[ index_x ] = 1.0f;
    wheel->axis_direction_cs.rotate( IVP_COORDINATE_INDEX(index_y), s_angle);
}

void IVP_Controller_Raycast_Car::change_spring_constant(IVP_POS_WHEEL pos, IVP_FLOAT spring_constant)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(pos);
    wheel->spring_constant = spring_constant;
}

void IVP_Controller_Raycast_Car::change_spring_dampening(IVP_POS_WHEEL pos, IVP_FLOAT spring_dampening)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(pos);
    wheel->spring_damp_relax = spring_dampening;
}

void IVP_Controller_Raycast_Car::change_spring_dampening_compression(IVP_POS_WHEEL pos, IVP_FLOAT spring_dampening)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(pos);
    wheel->spring_damp_compress = spring_dampening;
}

void IVP_Controller_Raycast_Car::change_spring_pre_tension(IVP_POS_WHEEL pos, IVP_FLOAT pre_tension_length)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(pos);
    wheel->spring_len = gravity_y_direction * (wheel->distance_orig_hp_to_hp - pre_tension_length);
}

void IVP_Controller_Raycast_Car::change_spring_length(IVP_POS_WHEEL pos, IVP_FLOAT spring_length)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(pos);
    wheel->spring_len = spring_length;
}


void IVP_Controller_Raycast_Car::change_wheel_torque(IVP_POS_WHEEL pos, IVP_FLOAT torque)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(pos);
    wheel->torque = torque;

	// Wake the physics object if need be!
	car_body->get_environment()->get_controller_manager()->ensure_controller_in_simulation( this );
}


void IVP_Controller_Raycast_Car::fix_wheel(IVP_POS_WHEEL pos, IVP_BOOL stop_wheel)
{
    IVP_Raycast_Car_Wheel *wheel = get_wheel(pos);
    wheel->wheel_is_fixed = stop_wheel;
}