ivp_car_system.cxx

hl2 source code. Do not use it illegal.

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

// IVP_EXPORT_PUBLIC

/********************************************************************************
 *	Name:	     	IVP_Car_System
 *	Description:	
 ********************************************************************************/
// includes for API
#include <ivp_physics.hxx>

#ifndef WIN32
#	pragma implementation "ivp_car_system.hxx"
#endif

#include <ivp_material.hxx>
#include <ivp_actuator.hxx>

#include <ivp_template_constraint.hxx>
#include <ivp_constraint_car.hxx>
#include <ivp_car_system.hxx>


/////////////////////////////////


/********************************************************************************
 *	Name:	       		change_spring_dampening
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_spring_dampening(IVP_POS_WHEEL wheel_nr, IVP_FLOAT damp_factor)
{
  IVP_Actuator_Suspension *spring = this->car_spring[(int)wheel_nr];
  spring->set_damp(damp_factor);
}

/********************************************************************************
 *	Name:	       		change_spring_dampening
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_max_body_force(IVP_POS_WHEEL wheel_nr, IVP_FLOAT mforce)
{
  IVP_Actuator_Suspension *spring = this->car_spring[(int)wheel_nr];
  spring->set_max_body_force(mforce);
}

/********************************************************************************
 *	Name:	       		change_spring_dampening_compression
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_spring_dampening_compression(IVP_POS_WHEEL wheel_nr, IVP_FLOAT damp_factor)
{
  IVP_Actuator_Suspension *spring = this->car_spring[(int)wheel_nr];
  spring->set_spring_damp_compression(damp_factor);
}

/********************************************************************************
 *	Name:	       		change_spring_pre_tension
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_spring_pre_tension(IVP_POS_WHEEL wheel_nr, IVP_FLOAT pre_tension_len)
{
  IVP_Actuator_Suspension *spring = this->car_spring[(int)wheel_nr];
  // IVP_FLOAT length= spring->get_spring_length_zero_force();
  spring->set_len(500.0f - pre_tension_len);
}

/********************************************************************************
 *	Name:	       		change_spring_length
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_spring_length(IVP_POS_WHEEL wheel_nr, IVP_FLOAT spring_length)
{
  IVP_Actuator_Suspension *spring = this->car_spring[(int)wheel_nr];
  spring->set_len(spring_length);
}

/********************************************************************************
 *	Name:	       		change_stabilizer_constant
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_stabilizer_constant(IVP_POS_AXIS axis_nr, IVP_FLOAT stabi_constant)
{
  IVP_Actuator_Stabilizer *stabi = this->car_stabilizer[(int)axis_nr];
  stabi->set_stabi_constant(stabi_constant);
}

void IVP_Car_System_Real_Wheels::change_fast_turn_factor( IVP_FLOAT fast_turn_factor_ ){
	fast_turn_factor = fast_turn_factor_;
}

/********************************************************************************
 *	Name:	       		change_wheel_torque
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_wheel_torque(IVP_POS_WHEEL wheel_nr, IVP_FLOAT torque)
{
  IVP_Actuator_Torque *actuator = this->car_act_torque[(int)wheel_nr];
  actuator->set_torque(torque);
}

/********************************************************************************
 *	Name:	       		change_wheel_dampening
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_wheel_speed_dampening( IVP_POS_WHEEL wheel_nr, IVP_FLOAT dampening )
{
	IVP_Real_Object *pWheel = car_wheel[(int)wheel_nr];
	IVP_Core *pWheelCore = pWheel->get_core();
	pWheelCore->speed_damp_factor = dampening;
}

/********************************************************************************
 *	Name:	       		change_body_downforce
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_body_downforce(IVP_FLOAT force)
{
  IVP_Actuator_Force *act = this->car_act_down_force;
  act->set_force(force);
}

/********************************************************************************
 *	Name:	       		update_body_countertorque
 *	Description:		Updates countertorque resulting from the torques produced by the wheels
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::update_body_countertorque()
{
    IVP_FLOAT counter_torque = 0.0f;
    
    // sum up all (negative) wheel torques
    for( int iWheel = 0; iWheel < n_wheels; ++iWheel )
	{
		IVP_Actuator_Torque *pTorqueAct = car_act_torque[( int )iWheel];
		counter_torque -= pTorqueAct->get_torque();
    }
    
    car_act_torque_body->set_torque( counter_torque * body_counter_torque_factor );
}

void IVP_Car_System_Real_Wheels::do_steering_wheel(IVP_POS_WHEEL wheel_pos, IVP_FLOAT s_angle)
{
    int wheel_nr = (int)wheel_pos;

    IVP_Constraint_Solver_Car *cs_car=this->car_constraint_solver;

    IVP_U_Matrix *target_mat = &cs_car->wheel_objects.element_at(wheel_nr)->target_position_bs;
    IVP_U_Point old_translation;
    old_translation.set(&target_mat->vv); // remember
    
    IVP_U_Point hp;
    hp.k[cs_car->x_idx] = 0.0f;
    hp.k[cs_car->y_idx] = s_angle;
    hp.k[cs_car->z_idx] = 0.0f;
    
    if(this->wheel_reversed_sign[wheel_nr]<0.0f){
	hp.k[cs_car->y_idx] += IVP_PI;
    }
    target_mat->init_rot_multiple(&hp, 1.0f);
    target_mat->vv.set(&old_translation);
}

void IVP_Car_System_Real_Wheels::do_steering(IVP_FLOAT s_angle)
{
    // tell constraint system new steering positions of wheels
    if (  steering_angle == s_angle) return;

    IVP_Constraint_Solver_Car *cs_car=this->car_constraint_solver;

	// start to spin the object 
	IVP_DOUBLE d_alpha = s_angle - steering_angle;

    IVP_FLOAT dx_front_wheels = 1.0f;
    IVP_FLOAT dz_axles = 1.0f;

	if( n_wheels >=4 ) {
		dx_front_wheels = this->get_orig_front_wheel_distance();
		dz_axles = this->get_orig_axles_distance();
	}

	IVP_DOUBLE angular_spin = d_alpha * get_body_speed(IVP_COORDINATE_INDEX(cs_car->z_idx)) / dz_axles;
	car_body->get_core()->rot_speed_change.k[IVP_COORDINATE_INDEX(cs_car->y_idx)] -= angular_spin * this->fast_turn_factor;

    this->steering_angle = s_angle;

    environment->get_controller_manager()->ensure_controller_in_simulation( cs_car );

    int wheels_per_axis = n_wheels / n_axis;
    for (int i=0; i < wheels_per_axis; i++){ /* turn front wheels */
	if( (i&1) == (s_angle>0.0f)){
    	    this->do_steering_wheel(IVP_POS_WHEEL(i), s_angle);
	    // ivp_message("%d regular angle: %g\n", i, s_angle);
	}else{
		IVP_FLOAT s_angle_2;
		if( n_wheels >=4 ) {
	        s_angle_2 = this->calc_ackerman_angle(steering_angle, dx_front_wheels, dz_axles);
		} else {
			s_angle_2 = steering_angle;
		}
    	    this->do_steering_wheel(IVP_POS_WHEEL(i), s_angle_2);
	    // ivp_message("%d ackerman angle: %g\n\n", i, s_angle_2);
	}
    }

    // hack for middle wheels
    if ( this->n_wheels > 4){ 
	this->do_steering_wheel(IVP_POS_WHEEL(4), s_angle  * .5f);
	this->do_steering_wheel(IVP_POS_WHEEL(5), s_angle  * .5f);
    }
}


/********************************************************************************
 *	Name:	       		change_spring_constant
 *	Description:		object is ensured to be in simulation
 ********************************************************************************/
void IVP_Car_System_Real_Wheels::change_spring_constant(IVP_POS_WHEEL wheel_nr, IVP_FLOAT constant)
{
  IVP_Actuator_Suspension *spring = this->car_spring[(int)wheel_nr];
  spring->set_constant(constant);
}

IVP_DOUBLE IVP_Car_System_Real_Wheels::get_body_speed(IVP_COORDINATE_INDEX idx_z)
{
    // return (IVP_FLOAT)car_body->get_geom_center_speed();
    IVP_U_Float_Point *vec_ws;
    vec_ws = &car_body->get_core()->speed;
    // works well as we do not use merged cores
    const IVP_U_Matrix *mat_ws = car_body->get_core()->get_m_world_f_core_PSI();
    IVP_U_Point orientation;
    mat_ws->get_col(idx_z, &orientation);

    return (IVP_FLOAT)orientation.dot_product(vec_ws);
}

IVP_DOUBLE IVP_Car_System_Real_Wheels::get_orig_front_wheel_distance()
{
    IVP_U_Matrix *m_left_wheel_bs = &this->car_constraint_solver->wheel_objects.element_at(0)->target_position_bs;
    IVP_U_Matrix *m_right_wheel_bs = &this->car_constraint_solver->wheel_objects.element_at(1)->target_position_bs;

    IVP_DOUBLE dist = m_left_wheel_bs->get_position()->k[this->car_constraint_solver->x_idx] -
 	              m_right_wheel_bs->get_position()->k[this->car_constraint_solver->x_idx];

    return -dist;
}

IVP_DOUBLE IVP_Car_System_Real_Wheels::get_orig_axles_distance()
{
    IVP_U_Matrix *m_front_wheel_bs =&this->car_constraint_solver->wheel_objects.element_at(0)->target_position_bs;
    IVP_U_Matrix *m_rear_wheel_bs = &this->car_constraint_solver->wheel_objects.element_at(2)->target_position_bs;

    IVP_DOUBLE dist = m_front_wheel_bs->get_position()->k[this->car_constraint_solver->z_idx] -
 	              m_rear_wheel_bs->get_position()->k[this->car_constraint_solver->z_idx];

    return IVP_Inline_Math::fabsd(dist);  // was fabs, which was a sml call
}

IVP_DOUBLE IVP_Car_System_Real_Wheels::get_wheel_angular_velocity(IVP_POS_WHEEL i){
    return this->car_act_torque[i]->rot_speed_out;
}


IVP_Car_System_Real_Wheels::IVP_Car_System_Real_Wheels( IVP_Environment *env, IVP_Template_Car_System *templ )
{
	// Builds up car system, using values from the template.
    n_wheels = templ->n_wheels;
    n_axis = templ->n_axis;
	
    booster_actuator[0] = 0;
	booster_actuator[1] = 0;
    booster_seconds_to_go = 0;
    booster_seconds_until_ready = 0;
    
	fast_turn_factor = templ->fast_turn_factor;

    environment = env;

	/////////////////////////////////////////////////////////////
	////////////  OBJECTS  //////////////////////////////////////
	/////////////////////////////////////////////////////////////
	this->car_body = templ->car_body;
	int i;
	for ( i = 0; i < n_wheels; i++ )
	{
		this->car_wheel[i] = templ->car_wheel[i];
		this->wheel_reversed_sign[i] = templ->wheel_reversed_sign[i];
	} 

	///////////////////////////////////////////////////////////////////
	////////    CONSTRAINT SYSTEM    //////////////////////////////////
	///////////////////////////////////////////////////////////////////
	this->car_constraint_solver = new IVP_Constraint_Solver_Car( templ->index_x, templ->index_y, templ->index_z, templ->is_left_handed );	
	// (IVP_INDEX_X, IVP_INDEX_Y, IVP_INDEX_Z, IVP_FALSE);

	IVP_U_Vector<IVP_Real_Object> wheels;
	IVP_U_Vector<IVP_U_Float_Point> hard_points;

	for ( i = 0; i< n_wheels; i++ )
	{
		wheels.add( car_wheel[i]);
		hard_points.add( &templ->wheel_pos_Bos[i] );
	}
	
	car_constraint_solver->init_constraint_system( environment, car_body, wheels, hard_points );
	IVP_Constraint_Solver_Car *cs_car = this->car_constraint_solver;

	/////////////////////////////////////////////////////////////
	//////// SPRINGS / SHOCK ABSORBERS //////////////////////////
	/////////////////////////////////////////////////////////////    
	int wheel_nr;
	IVP_Template_Anchor anchor_body_template[IVP_CAR_SYSTEM_MAX_WHEELS];
	IVP_Template_Anchor anchor_wheel_template[IVP_CAR_SYSTEM_MAX_WHEELS];	

	for ( wheel_nr = 0; wheel_nr <n_wheels; wheel_nr++ ) 
	{    
		IVP_Template_Suspension spring_template;

		// defaults
		spring_template.spring_values_are_relative=IVP_FALSE;
		spring_template.spring_constant =templ->spring_constant[wheel_nr];
		spring_template.spring_damp = templ->spring_dampening[wheel_nr];
		spring_template.spring_dampening_compression = templ->spring_dampening_compression[wheel_nr];
		spring_template.max_body_force = templ->max_body_force[wheel_nr];
		spring_template.rel_pos_damp = 0.00f;
      
	    // @@@OG be careful with car sizes around 500m/feet ;-) !
		spring_template.spring_len = 500.0f;
      
		// Create spring
		IVP_U_Float_Point hp( &templ->wheel_pos_Bos[wheel_nr] );
		hp.k[cs_car->y_idx] -= spring_template.spring_len;
		anchor_body_template[wheel_nr].set_anchor_position_os( car_body, &hp );
		
		// @@@OG os origin does not need to be the center! Better use core system's origin!
		anchor_wheel_template[wheel_nr].set_anchor_position_os( car_wheel[wheel_nr], 0.0f, 0.0f, 0.0f );
		
		spring_template.anchors[0] = &anchor_body_template[wheel_nr];
		spring_template.anchors[1] = &anchor_wheel_template[wheel_nr];
		
		spring_template.spring_len -= templ->spring_pre_tension[wheel_nr];
		this->car_spring[wheel_nr] = environment->create_suspension( &spring_template );

		///////////////////////////////////////////////////////////////////
		////////   ROTATION MOTORS (some wheel drive)   //////////////////////
		///////////////////////////////////////////////////////////////////
		IVP_Template_Anchor anchor_left_template;
		IVP_Template_Anchor anchor_right_template;
		IVP_Template_Torque torque_template;
			
		// defaults
		torque_template.max_rotation_speed = templ->wheel_max_rotation_speed[(wheel_nr&2)?1:0];
		
		// create torque actuator
		
		// @@@OG Should better use core system.
		hp.set_to_zero();
		hp.k[cs_car->x_idx] = templ->wheel_reversed_sign[wheel_nr];
		anchor_left_template.set_anchor_position_os( car_wheel[wheel_nr], &hp );
		
		hp.k[cs_car->x_idx] = -templ->wheel_reversed_sign[wheel_nr];
		anchor_right_template.set_anchor_position_os( car_wheel[wheel_nr], &hp );