ivp_friction.cxx

hl2 source code. Do not use it illegal.

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


#include <ivp_physics.hxx>

#if defined(LINUX) || defined(SUN) || (__MWERKS__ && __POWERPC__)
#	include <alloca.h>
#endif

// IVP_EXPORT_PRIVATE


#include <ivu_memory.hxx>
#include <ivp_great_matrix.hxx>
#include <ivp_sim_unit.hxx>
#include <ivp_physic_private.hxx>
#include <ivp_core_macros.hxx>
#include <ivp_material.hxx>
#include <ivp_impact.hxx>

#include <ivp_mindist_intern.hxx> // because of Mindist

#include <ivp_friction.hxx>
#include <ivp_friction_solver.hxx>
#include <ivp_debug_manager.hxx> // bvecause of debug psi_synchrone

#include <ivp_compact_ledge.hxx>
#include <ivp_compact_ledge_solver.hxx>
#include <ivp_listener_collision.hxx>

#include <ivp_hull_manager.hxx> //because of Mindist.
#include <ivp_mindist_macros.hxx> //because of Mindist
#include <ivp_solver_core_reaction.hxx>
#include <ivp_constraint_car.hxx>

void IVP_Synapse_Friction::remove_friction_synapse_from_object(){
    if (next) next->prev = prev;
    if (prev){
	prev->next = next;
    }else{
	l_obj->friction_synapses = get_next();
    }
}

int IVP_Friction_System::friction_global_counter=0;



void IVP_Synapse_Friction::init_synapse_friction( IVP_Contact_Point *friction,
						  IVP_Real_Object *object,
						  const IVP_Compact_Edge 	*edge_in,
						  IVP_SYNAPSE_POLYGON_STATUS status_in) {
    l_obj = object;
    set_contact_point(friction);
	next = object->friction_synapses;
	prev = NULL;
	if (next) next->prev = this;
	object->friction_synapses = this;

	status = status_in;
	edge = edge_in;
}




IVP_Contact_Point::IVP_Contact_Point( IVP_Mindist *md)
{
    IVP_Synapse_Real *syn0 = md->get_sorted_synapse(0);
    synapse[0].init_synapse_friction(this, syn0->get_object(), syn0->edge, syn0->get_status());
#ifdef IVP_USE_S_VALS_FOR_PRETENSION    
    s_coords[0] = s_coords[1] = 0.0f;
#endif
    IVP_Synapse_Real *syn1 = md->get_sorted_synapse(1);
    synapse[1].init_synapse_friction(this, syn1->get_object(), syn1->edge, syn1->get_status());

    IVP_Environment *env = md->get_environment();
    this->last_time_of_recalc_friction_s_vals = env->get_current_time();

    //this->l_environment = mindist->l_environment;
    IVP_IF(1) {
	IVP_IF(env->get_debug_manager()->check_fs) {
	    fprintf(env->get_debug_manager()->out_deb_file,"create_mindist %f %lx cores %lx %lx\n",
		    env->get_current_time().get_time(),
		    (long)this,
		    (long)syn0->l_obj->physical_core,
		    (long)syn0->l_obj->physical_core);
	}
    }
    if ( syn1->get_status() == IVP_ST_TRIANGLE){
	const IVP_Compact_Edge *e1 = syn1->edge;
	IVP_U_Point wHesse_vecF_Fos;
	IVP_CLS.calc_hesse_vec_object_not_normized(e1, e1->get_compact_ledge(), &wHesse_vecF_Fos);
	inv_triangle_det = 1.0f / wHesse_vecF_Fos.real_length();
    }

    this->old_energy_dynamic_fr=0.0f;
    this->now_friction_pressure=0.0f;
    this->integrated_destroyed_energy = 0.0f;
    this->cp_status = IVP_CPBS_NEEDS_RECHECK;
    //printf("initing_mindist_fr %lx\n",(long)this);

    this->span_friction_s[0]=0.0f;
    this->span_friction_s[1]=0.0f;

    this->last_gap_len=ivp_mindist_settings.friction_dist; //at first PSI-iteration there is no energy from complex //maybe drop it
    has_negative_pull_since=0;
    //force_push_complex_last_psi=0.0f; //evtl. nicht noetig    
    slowly_turn_on_keeper = IVP_SLOWLY_TURN_ON_KEEPER;
    l_friction_system=NULL;
    two_friction_values=IVP_FALSE;
    
}


// everything in world coords
IVP_DOUBLE IVP_Contact_Point::two_values_friction(IVP_U_Float_Point *real_world_friction_vec) {
    IVP_Material *mtl[2];
    get_material_info(mtl);
    IVP_Impact_Solver_Long_Term *info = tmp_contact_info;

    int i;
    for(i=0;i<2;i++) {
        if(mtl[i]->second_friction_x_enabled) {
	    IVP_U_Float_Point x_direction;
	    IVP_Core *core=get_synapse(i)->l_obj->friction_core;
	    IVP_U_Matrix *mat=&core->m_world_f_core_last_psi;
	    mat->get_col(IVP_INDEX_X,&x_direction);
	    IVP_U_Float_Point *surf_normal=&info->surf_normal;

	    IVP_IF(1) {
		IVP_U_Point base_p;
		base_p.set(&mat->vv);
		IVP_U_Float_Point vec_p;
		vec_p.set(&x_direction);
		char *out_text=p_make_string("x_vec");
		core->environment->add_draw_vector(&base_p,&vec_p,out_text,1);
		P_FREE(out_text);
		vec_p.set(0.0f,-1.0f,0.0f);
		core->environment->add_draw_vector(&base_p,&vec_p,"grav",3);
	    }
	    
	    IVP_U_Float_Point x_in_surface;
	    x_in_surface.set_orthogonal_part(&x_direction,surf_normal);
	    
	    IVP_DOUBLE relevance_factor=x_in_surface.real_length();
	    IVP_DOUBLE second_friction_val=mtl[i]->get_second_friction_factor();
	    IVP_DOUBLE other_factor=mtl[1-i]->get_friction_factor();
	    IVP_DOUBLE using_friction=other_factor*second_friction_val;
	    IVP_DOUBLE difference_in_friction=real_friction_factor - using_friction;
	    IVP_DOUBLE effective_friction=real_friction_factor - difference_in_friction * relevance_factor;

	    if(real_friction_factor<P_DOUBLE_EPS) {
	        return 0.0f;
	    }
	    IVP_DOUBLE max_len_in_x_direction = IVP_MINIMAL_REAL_FRICTION_LEN * effective_friction/real_friction_factor;
	    IVP_U_Float_Point friction_vec;

	    IVP_Impact_Solver_Long_Term *info_this = this->tmp_contact_info;
	    friction_vec.set_multiple(&info_this->span_friction_v[0],this->span_friction_s[0]);
	    friction_vec.add_multiple(&info_this->span_friction_v[1],this->span_friction_s[1]);

	    IVP_IF(1) {
		IVP_U_Point base_p;
		base_p.set(&tmp_contact_info->contact_point_ws);
		IVP_U_Float_Point vec_p;
		vec_p.set(&friction_vec);
	        vec_p.mult(10.0f);
		char *out_text=p_make_string("f_vec");
		core->environment->add_draw_vector(&base_p,&vec_p,out_text,1);
		P_FREE(out_text);
	    }
	    
	    x_in_surface.normize();
	    IVP_U_Float_Point x_in_surface_float;
	    x_in_surface_float.set(&x_in_surface);
	    IVP_DOUBLE len_direction_x=friction_vec.dot_product(&x_in_surface_float);
	    IVP_DOUBLE abs_len=IVP_Inline_Math::fabsd(len_direction_x);

	    if((abs_len>max_len_in_x_direction)&&1) {
	        //we have to shorten the friction
		
	        IVP_U_Float_Point vec_full_friction;
		vec_full_friction.set(&friction_vec);
		vec_full_friction.add_multiple(&x_in_surface,-len_direction_x);
		IVP_U_Float_Point vec_second_dir;
		vec_second_dir.set_multiple(&x_in_surface,max_len_in_x_direction);
		if(len_direction_x<0.0f) {
		    vec_second_dir.mult(-1.0f);
		}
		IVP_U_Float_Point resulting_vec;
		resulting_vec.set(&vec_full_friction);
		resulting_vec.add(&vec_second_dir);
		*real_world_friction_vec=resulting_vec;
		this->span_friction_s[0]=resulting_vec.dot_product(&info->span_friction_v[0]);
		this->span_friction_s[1]=resulting_vec.dot_product(&info->span_friction_v[1]);
	    }
	}
    }
    
    IVP_U_Float_Point friction_vec;

    friction_vec.set_multiple(&info->span_friction_v[0],this->span_friction_s[0]);
    friction_vec.add_multiple(&info->span_friction_v[1],this->span_friction_s[1]);
    real_world_friction_vec->set(&friction_vec);
    return real_world_friction_vec->real_length_plus_normize();
}

// calculates friction pointer, pointing away from first obj, also returns length of this pointer and afterwards does normization
// #+# kill . TL: ??
IVP_DOUBLE IVP_Contact_Point::get_and_set_real_friction_len(IVP_U_Float_Point *real_world_friction_vec) {
    IVP_Impact_Solver_Long_Term *info = this->tmp_contact_info;
    real_world_friction_vec->set_multiple(&info->span_friction_v[0],this->span_friction_s[0]);
    real_world_friction_vec->add_multiple(&info->span_friction_v[1],this->span_friction_s[1]);
    
    IVP_DOUBLE len_of_spring = real_world_friction_vec->real_length_plus_normize();

    if(this->two_friction_values==IVP_TRUE) {
	return two_values_friction(real_world_friction_vec);
    }
    
    return len_of_spring;
}


void IVP_Contact_Point::static_friction_single(const IVP_Event_Sim *es,IVP_FLOAT desired_gap,IVP_FLOAT speedup_factor) {
    IVP_Impact_Solver_Long_Term *info = this->tmp_contact_info;
    IVP_DOUBLE closing_speed = info->get_closing_speed();
    IVP_DOUBLE gap_diff = desired_gap - this->get_gap_length();
    IVP_DOUBLE a = IVP_Friction_Solver::calc_desired_gap_speed(closing_speed, gap_diff,speedup_factor);
    IVP_DOUBLE b = info->virtual_mass;
    IVP_DOUBLE impulse = a * b;

    if(impulse > 0.0f) {
	this->now_friction_pressure = impulse * es->i_delta_time;    
	IVP_Friction_Solver::apply_impulse( info, impulse );
    } else {
	this->now_friction_pressure=0.0f;
    }
}

bool IVP_Contact_Point::friction_force_local_constraint_2d_wheel( IVP_Core *core_a, IVP_Impact_Solver_Long_Term *info,
																  const IVP_Event_Sim *es, IVP_FLOAT &flEnergy )
{
	IVP_Constraint_Car_Object *wheel = core_a->car_wheel;
	IVP_Constraint_Solver_Car *solver = wheel->solver_car;

    IVP_DOUBLE maximum_impulse_force = this->now_friction_pressure * this->real_friction_factor * es->delta_time;

	IVP_U_Float_Point axis_bs; 
	wheel->target_position_bs.get_col( IVP_COORDINATE_INDEX( solver->x_idx ), &axis_bs );
	IVP_U_Float_Point axis_ws;
	const IVP_U_Matrix *m_world_f_core = solver->body_object->get_core()->get_m_world_f_core_PSI();
	m_world_f_core->vmult3( &axis_bs, &axis_ws );

	// search new span system for which one axis.dot(axis_ws) == 0
	IVP_U_Float_Point span_v_0; // span which is orthogonal to any car body influences
	IVP_U_Float_Point span_v_1; // span which is influences by car body weight

	span_v_0.calc_cross_product( &axis_ws, &info->surf_normal);
	if ( span_v_0.quad_length() < 0.001f )
		return false;

	span_v_0.normize();
	span_v_1.calc_cross_product( &span_v_0, &info->surf_normal );

	IVP_DOUBLE dot_old0_new0 = info->span_friction_v[0].dot_product( &span_v_0 );
	IVP_DOUBLE dot_old0_new1 = info->span_friction_v[0].dot_product( &span_v_1 );
	IVP_DOUBLE dot_old1_new0 = info->span_friction_v[1].dot_product( &span_v_0 );
	IVP_DOUBLE dot_old1_new1 = info->span_friction_v[1].dot_product( &span_v_1 );

	IVP_DOUBLE span_s_0 = span_friction_s[0] * dot_old0_new0 + span_friction_s[1] * dot_old1_new0;
	IVP_DOUBLE span_s_1 = span_friction_s[0] * dot_old0_new1 + span_friction_s[1] * dot_old1_new1;
	
	// calculate pushing behaviour between wheel and car in wheel axis direction
	IVP_DOUBLE p_wheel;
	IVP_DOUBLE p_body;
	
	IVP_DOUBLE wheel_vel;
	IVP_DOUBLE body_vel;

    IVP_Solver_Core_Reaction tcb;
	tcb.init_reaction_solver_translation_ws( core_a, NULL, info->contact_point_ws, &span_v_0, &span_v_1, NULL );
	{
		p_wheel = tcb.m_velocity_ds_f_impulse_ds.get_elem( 1, 1 );
		wheel_vel = tcb.delta_velocity_ds.k[1];
	}

	{
		IVP_Solver_Core_Reaction sc_body;
		sc_body.init_reaction_solver_translation_ws( solver->body_object->get_core(), NULL, info->contact_point_ws, &span_v_1, NULL, NULL );
		p_body = sc_body.m_velocity_ds_f_impulse_ds.get_elem( 0, 0 );
		body_vel = sc_body.delta_velocity_ds.k[0];
	}
	
	IVP_DOUBLE a = span_s_0 * es->i_delta_time - tcb.delta_velocity_ds.k[0];
	//IVP_DOUBLE b = span_s_1 * es->i_delta_time - tcb.delta_velocity_ds.k[1];
	IVP_DOUBLE b = span_s_1 * es->i_delta_time - 1.0f * ( wheel_vel * 1.0f + body_vel * 0.0f);
		
	IVP_U_Matrix3 &tpm = tcb.m_velocity_ds_f_impulse_ds;
	
	IVP_DOUBLE inv_mat2x2[4];
	
	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 );
	
	IVP_RETURN_TYPE ret = IVP_Inline_Math::invert_2x2_matrix( tpm00, tpm01, tpm01, tpm11, &inv_mat2x2[0], &inv_mat2x2[1],&inv_mat2x2[2],&inv_mat2x2[3] );
	if ( ret != IVP_OK ) 
	{
		flEnergy = 0.0f;
		return true;
	}

    IVP_U_Float_Point impulses;
	impulses.k[0] = inv_mat2x2[0] * a + inv_mat2x2[1] * b;
	impulses.k[1] = inv_mat2x2[2] * a + inv_mat2x2[3] * b;
	
	IVP_DOUBLE project_span_v1 = IVP_Inline_Math::fabsd( span_v_1.dot_product( &axis_ws ) );

	// 
	IVP_DOUBLE relaxation_coefficient = 0.3f;  // ~1.0f tends to oscillate
	IVP_DOUBLE body_impulse_factor =  ( project_span_v1 * p_wheel/ p_body + 1.0f ) * relaxation_coefficient; 
	
	IVP_DOUBLE imp0_sqrd = impulses.k[0] * impulses.k[0];
	IVP_DOUBLE imp1_sqrd = impulses.k[1] * impulses.k[1];

	// check for sliding with body push
	core_a->car_wheel->last_contact_position_ws.set( &info->contact_point_ws );
	core_a->car_wheel->last_skid_time = es->environment->get_current_time();

	// handbrake !!!
    IVP_DOUBLE square_impulse;
	if ( core_a->car_wheel->fix_wheel_constraint )
	{
		square_impulse = ( imp0_sqrd + imp1_sqrd ) * body_impulse_factor * body_impulse_factor;
		
		if ( square_impulse > maximum_impulse_force * maximum_impulse_force ) // test for sliding
		{
			// sliding handbrake
			IVP_DOUBLE isum_impulse = IVP_Inline_Math::isqrt_float( square_impulse ); // body push
			IVP_DOUBLE f = maximum_impulse_force * isum_impulse * body_impulse_factor;
			impulses.k[0] *= f;
			impulses.k[1] *= f;
			
			f /= body_impulse_factor;
			
			span_friction_s[0] *= f; // update reference point
			span_friction_s[1] *= f;
			
			core_a->car_wheel->last_skid_value = ( 1.0f - f ) * this->now_friction_pressure * solver->body_object->get_core()->get_inv_mass();