ivp_constraint_car.cxx

hl2 source code. Do not use it illegal.

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

#include <ivp_physics.hxx>
#include <ivp_solver_core_reaction.hxx>
#include <ivp_great_matrix.hxx>
#include <math.h>
#include <ivu_vector.hxx>
#include <ivp_constraint_local.hxx>
#include <ivp_constraint_car.hxx>
#include <ivp_template_constraint.hxx>

//=============================================================================
//
// Car constraint object functions.
//

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
IVP_Constraint_Car_Object::IVP_Constraint_Car_Object( IVP_Constraint_Solver_Car *solver_car_, 
													  IVP_Real_Object *i_real_obj_app,
													  IVP_Real_Object *i_real_obj_body, 
													  IVP_U_Float_Point *target_Bos_override )
{
    this->real_object = i_real_obj_app;
    this->solver_car = solver_car_;
    this->fix_wheel_constraint = NULL;

    i_real_obj_app->get_core()->car_wheel = this;

	last_skid_value = 0.0f;
	last_contact_position_ws.set_to_zero();
	last_skid_time = 0.0f;

    // missing: backward ref from obj to this, to manage obj deletion... @@@OG

    /*** remember as default: target_position and rotation in body system ***/
    if (i_real_obj_body )
	{
		//// treat appendix
		IVP_Core *core_A = i_real_obj_app->get_core();
		IVP_Core *core_B = i_real_obj_body->get_core();
		
		// translation and rotation
		const IVP_U_Matrix *m_world_f_B = core_B->get_m_world_f_core_PSI();
		const IVP_U_Matrix *m_world_f_A = core_A->get_m_world_f_core_PSI();

		if ( target_Bos_override )
		{
			this->target_position_bs.init();
			IVP_U_Matrix m_core_f_object;
			i_real_obj_body->calc_m_core_f_object(&m_core_f_object);
			m_core_f_object.vmult4(target_Bos_override, &this->target_position_bs.vv);
		}
		else
		{
			m_world_f_B->mimult4(m_world_f_A, &this->target_position_bs);
		}
    }
	else
	{
		//// body itself: gets unit pos and rot
		this->target_position_bs.init();
    }
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
IVP_Constraint_Car_Object::~IVP_Constraint_Car_Object()
{
    real_object->get_core()->car_wheel = NULL;
}

//=============================================================================
//
// Car constraint solver functions.
//

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
IVP_Constraint_Solver_Car::IVP_Constraint_Solver_Car( IVP_COORDINATE_INDEX right, 
													  IVP_COORDINATE_INDEX up, 
													  IVP_COORDINATE_INDEX forward, 
													  IVP_BOOL is_left_hand )
{
    // P_MEM_CLEAR(this); NO !!!
	
    this->x_idx = right;
    this->y_idx = up;
    this->z_idx = forward;
    this->angle_sign = ( is_left_hand ) ? -1.0f : 1.0f;
	
    this->max_delta_speed = 80.0f * 3.0f;				// greater ??
    this->local_translation_in_use = IVP_FALSE;

    // constraint_is_disabled is inited in Builder.
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
IVP_Constraint_Solver_Car::~IVP_Constraint_Solver_Car()
{
	//delete silently
    IVP_Controller_Manager::remove_controller_from_environment( this, IVP_TRUE ); 

    P_DELETE( body_object );
    for ( int app_nr = 0; app_nr < wheel_objects.len(); app_nr++ )
	{
		delete wheel_objects.element_at( app_nr );
    }
    
    for ( int k = 0; k < wheel_objects.len(); k++ )
	{
		P_DELETE( this->c_local_ballsocket[k] );
    }

    wheel_objects.clear();

    P_FREE( this->co_matrix.matrix_values );
    P_FREE( this->co_matrix.result_vector );
    P_FREE( this->co_matrix.desired_vector );
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
IVP_RETURN_TYPE IVP_Constraint_Solver_Car::init_constraint_system( IVP_Environment *env, IVP_Real_Object *body,
																   IVP_U_Vector<IVP_Real_Object> &wheels,	
																   IVP_U_Vector<IVP_U_Float_Point> &p_Bos )
{ 
    environment = env;

    body_object = new IVP_Constraint_Car_Object( this, body, 0, NULL );
    cores_of_constraint_system.add( body->get_core() );

    for ( int i = 0; i < wheels.len(); i++ )
	{
		wheel_objects.add( new IVP_Constraint_Car_Object( this, wheels.element_at(i), body, p_Bos.element_at( i ) ) );
		cores_of_constraint_system.add( wheels.element_at(i)->get_core() );
		c_local_ballsocket[i] = NULL;
    }

    env->get_controller_manager()->announce_controller_to_environment( this );
    
    IVP_Constraint_Solver_Car_Builder *builder;
    builder = new IVP_Constraint_Solver_Car_Builder( this );

    builder->disable_constraint( y_idx );	// free Y trans
    builder->disable_constraint( x_idx+3 ); // free X rot
    builder->disable_constraint( y_idx+3 ); // free y rot
    builder->disable_constraint( z_idx+3 ); // free z rot

    IVP_RETURN_TYPE res =  builder->calc_constraint_matrix();

    P_DELETE( builder );

    return res;
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Constraint_Solver_Car::do_simulation_controller_rotation( IVP_Event_Sim *es, 
																   IVP_Core *core_B, 
																   const IVP_U_Matrix *m_world_f_B )
{
    // For all appendices (wheels).
	for (int app_cnt = 0; app_cnt < wheel_objects.len(); app_cnt++ )
	{
		IVP_Constraint_Car_Object *app = wheel_objects.element_at( app_cnt );
		IVP_Core *core_A = app->get_core();
		const IVP_U_Matrix *m_world_f_A = core_A->get_m_world_f_core_PSI();
	
		// calc y and z angles from vector
		if (1) 
		{ // new version try
			// calc target vector in A
			IVP_U_Point target_vec_as;
			IVP_U_Point tv_bs, tv_ws; 
			app->target_position_bs.get_col( IVP_COORDINATE_INDEX( x_idx ), &tv_bs );
			m_world_f_B->vmult3( &tv_bs, &tv_ws );
			m_world_f_A->vimult3( &tv_ws, & target_vec_as );
	
			IVP_DOUBLE cur_angles_B[2];
			{
				// turn back target vec depending on rot speed
				// thus improving behavior with high rotation speed
#ifdef IVP_FAST_WHEELS_ENABLED
				IVP_DOUBLE ny = target_vec_as.k[z_idx];
				IVP_DOUBLE nx = target_vec_as.k[y_idx];
#else
				IVP_DOUBLE alpha = core_A->rot_speed.k[x_idx] * es->delta_time * 0.45f;				// no angle sign needed
				IVP_DOUBLE sina = angle_sign * IVP_Inline_Math::approx5_sin( alpha );
				IVP_DOUBLE cosa = IVP_Inline_Math::approx5_cos( alpha );
				IVP_DOUBLE ny = target_vec_as.k[z_idx] * cosa - target_vec_as.k[y_idx] * sina;
				IVP_DOUBLE nx = target_vec_as.k[y_idx] * cosa + target_vec_as.k[z_idx] * sina;
#endif
	  
				cur_angles_B[0] = angle_sign * -IVP_Inline_Math::atan2d( ny, target_vec_as.k[x_idx] );
				cur_angles_B[1] = angle_sign * IVP_Inline_Math::atan2d( nx, target_vec_as.k[x_idx] );
			}

			IVP_U_Float_Point y_rot_axis_world2; 
			m_world_f_A->get_col( IVP_COORDINATE_INDEX( y_idx ), &y_rot_axis_world2 );
			IVP_U_Float_Point z_rot_axis_world2; 
			m_world_f_A->get_col( IVP_COORDINATE_INDEX( z_idx ), &z_rot_axis_world2 );
			
			IVP_Solver_Core_Reaction tcb;
			tcb.init_reaction_solver_rotation_ws( core_B, core_A, &y_rot_axis_world2, &z_rot_axis_world2, NULL );
			
			//// invert matrix
			IVP_DOUBLE ia1, ia2, ib1, ib2;
			IVP_RETURN_TYPE r;
			IVP_U_Matrix3 &tpm = tcb.m_velocity_ds_f_impulse_ds;
			
			const IVP_DOUBLE tpm00 = tpm.get_elem( 0, 0 );
			const IVP_DOUBLE tpm01 = tpm.get_elem( 0, 1 );
			const IVP_DOUBLE tpm10 = tpm.get_elem( 1 ,0 );
			const IVP_DOUBLE tpm11 = tpm.get_elem( 1, 1 );
	  
			r = IVP_Inline_Math::invert_2x2_matrix( tpm00, tpm01, tpm01 /*@@CB FIXME?*/,tpm11, &ia1, &ib1, &ia2, &ib2 );
			if ( r == IVP_FAULT)
			{
				ivp_message("do_constraint_system: Couldn't invert rot matrix!\n");
				return;
			}

			IVP_DOUBLE a = -es->i_delta_time * cur_angles_B[0] - tcb.delta_velocity_ds.k[0];
			IVP_DOUBLE b = -es->i_delta_time * cur_angles_B[1] - tcb.delta_velocity_ds.k[1];
			
			//// mult with inv matrix
			IVP_U_Float_Point rot_impulse_ds;
			rot_impulse_ds.k[0] = a * ia1 + b * ib1;
			rot_impulse_ds.k[1] = a * ia2 + b * ib2;
	  
			tcb.exert_angular_impulse_dim2(core_B, core_A, rot_impulse_ds);

#if 0
			// 4-wheel - debugging!
			if ( app_cnt >= 0 && app_cnt < 4 )
			{
				m_wheelRotationTorque[app_cnt][0] = rot_impulse_ds.k[0];
				m_wheelRotationTorque[app_cnt][1] = rot_impulse_ds.k[1];
				m_wheelRotationTorque[app_cnt][2] = rot_impulse_ds.k[2];
			}
#endif
		}	
	}
}

//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void IVP_Constraint_Solver_Car::do_simulation_controller( IVP_Event_Sim *es, 
														  IVP_U_Vector<IVP_Core> * /*core_list*/ ) 
{
    IVP_DOUBLE inv_dtime = es->i_delta_time;    

    // Pushes body and appendices so that total impulse is the same
    // and all parts will fulfil the specified constraints in next PSI
    IVP_Constraint_Car_Object *body = body_object;
    IVP_Core *core_B = body->get_core();
    const IVP_U_Matrix *m_world_f_B = core_B->get_m_world_f_core_PSI();

    /*********** ROTATION ************/
	do_simulation_controller_rotation( es, core_B, m_world_f_B );
      
    /*********** TRANSLATION ************/
//	move the below stuff into - do_simulation_controller_translation( );

    IVP_DOUBLE *input_vec_ptr = this->co_matrix.desired_vector;
    IVP_ASSERT( this->co_matrix.columns == wheel_objects.len() * 2 );

    int init_local_translation = 0;					// flag
    int invalid_count= 0;							// for plan B removal
    IVP_FLOAT delta_time = es->delta_time;
    
    // for all appendices
    for(int app_cnt=0; app_cnt < wheel_objects.len(); app_cnt++){
	IVP_Constraint_Car_Object *app = wheel_objects.element_at(app_cnt);
	IVP_Core *core_A = app->get_core();
	// core_A->ensure_core_to_be_in_simulation(); // already done for rotation
	const IVP_U_Matrix *m_world_f_A = core_A->get_m_world_f_core_PSI();
	
	/**** translation divergence (after delta time) ***/
	IVP_U_Point delta_speed_vec;
	{
	    // calc current positions in B
	    IVP_U_Float_Point cur_pos_A_in_B;
	    IVP_U_Float_Point cur_pos_B_in_B(app->target_position_bs.get_position());
	    m_world_f_B->vimult4(&m_world_f_A->vv, &cur_pos_A_in_B); // takes center of app

	    // calc speed and rotspeed in B
	    IVP_U_Float_Point surspeed_A_in_B;
	    IVP_U_Float_Point surspeed_B_in_world;
	    IVP_U_Float_Point surspeed_B_in_B;

	    m_world_f_B->vimult3(&core_A->speed, &surspeed_A_in_B); // takes app center speed
	
	    core_B->get_surface_speed(&cur_pos_B_in_B, &surspeed_B_in_world);
	    m_world_f_B->vimult3(&surspeed_B_in_world, &surspeed_B_in_B);

	    // calc positions at next PSI in B
	    IVP_U_Float_Point next_pos_A_in_B;
	    IVP_U_Float_Point next_pos_B_in_B;

	    next_pos_A_in_B.add_multiple(&cur_pos_A_in_B, &surspeed_A_in_B, delta_time);
	    next_pos_B_in_B.add_multiple(&cur_pos_B_in_B, &surspeed_B_in_B, delta_time);
	
	    // calc delta speed vec that has to be realized
	    delta_speed_vec.subtract(&next_pos_A_in_B, &next_pos_B_in_B); 
	    delta_speed_vec.mult(inv_dtime * 1.0f);

	    // difference too high?
	    {
		IVP_DOUBLE quad_speed = delta_speed_vec.quad_length();
		if( quad_speed > (this->max_delta_speed*this->max_delta_speed)){
		  invalid_count ++;
		  // IVP_DOUBLE factor = this->max_delta_speed / delta_speed_vec.fast_real_length();
		  // delta_speed_vec.mult(factor);
		    psis_left_for_plan_B = 10;
		  if(this->local_translation_in_use == IVP_FALSE){
		    // switch to ball and socket local solution
		    init_local_translation = 1;
		    break;
		  }
		}
	    }
	}

	/**** add to input vector for matrix mult ***/
	// supercar special solution without if's!
	IVP_ASSERT(this->constraint_is_disabled[y_idx] == IVP_TRUE);
	IVP_ASSERT(this->constraint_is_disabled[x_idx+3] == IVP_TRUE);
	IVP_ASSERT(this->constraint_is_disabled[y_idx+3] == IVP_TRUE);
	IVP_ASSERT(this->constraint_is_disabled[z_idx+3] == IVP_TRUE);
	(*input_vec_ptr++) = delta_speed_vec.k[x_idx];
	(*input_vec_ptr++) = delta_speed_vec.k[z_idx];	
    }

    if(invalid_count == 0 && (this->local_translation_in_use==IVP_TRUE)){
      // switch back to regular translation
	psis_left_for_plan_B--;
	if (psis_left_for_plan_B <0){
	      int app_nr;
	      for(app_nr=0; app_nr< wheel_objects.len(); app_nr++){
		P_DELETE(this->c_local_ballsocket[app_nr]);
		this->c_local_ballsocket[app_nr] = NULL;
	      }
	      this->local_translation_in_use = IVP_FALSE;
	      IVP_IF(1){
		printf("plan B deactivated.\n");
	      }