ivp_friction.cxx

hl2 source code. Do not use it illegal.

	    if(obj->physical_unmoveable)    {
	        //printf("isumv");
		// fixed objs first belong to both systems and get two IVP_Friction_Info_For_Core, later deleted if necessary
		IVP_Friction_Info_For_Core *fr_i=new IVP_Friction_Info_For_Core();
		fr_i->l_friction_system=new_fr_sys;
		new_fr_sys->add_core_to_system(obj);
		obj->add_friction_info(fr_i);
	    } else {
		if(obj->union_find_get_father()==split_father)
		{
		    fr_sys->remove_core_from_system(obj);
		    new_fr_sys->add_core_to_system(obj);
		    IVP_Friction_Info_For_Core *fr_i=obj->get_friction_info(fr_sys);
		    fr_i->l_friction_system=new_fr_sys;
		}
	    }
	}
	//printf("\n");
    }

    {
	// transfer mindists (take pairs as orientation)
	for (int n = fr_sys->fr_pairs_of_objs.len()-1; n>=0; n--){
	IVP_Friction_Core_Pair *fr_pair = fr_sys->fr_pairs_of_objs.element_at(n);
	    IVP_Core *robj=fr_pair->objs[0]; //is used to identify fr sys my pair should belong to
	    IVP_Core *robj2=fr_pair->objs[1]; // used when fixed obj is involved 
	    IVP_Friction_Info_For_Core *fr_i_old = NULL,*fr_i_new = NULL; // when fixed obj exists, dists have to be transfered from fr_i_old to fr_i_new
	    if(robj->physical_unmoveable)   {
	        IVP_Core *ct;
		ct=robj2;
		robj2=robj;
		robj=ct;
		goto found_fixed;
	    } else {
		if(robj2->physical_unmoveable){
		found_fixed:    
		    fr_i_old=robj2->get_friction_info(fr_sys);
		    fr_i_new=robj2->get_friction_info(new_fr_sys);
		}
	    }

      	    //printf("reassignpair %lx infoc %lx",(long)fr_pair&0x0000ffff,(long)robj&0x0000ffff);
	    if(robj->union_find_get_father()==split_father)	    {
	      //printf(" to_new");
		fr_sys->del_fr_pair(fr_pair);
		new_fr_sys->add_fr_pair(fr_pair);
		for (int k = fr_pair->fr_dists.len()-1; k>=0; k--){
		    IVP_Contact_Point *fr_dist=fr_pair->fr_dists.element_at(k);
		    int found_mine=0;
		    IVP_Contact_Point *mindist;
		    for(mindist=fr_sys->get_first_friction_dist();mindist;mindist=fr_sys->get_next_friction_dist(mindist))   {
			if(mindist==fr_dist) {
			    found_mine=1;
			}
		    }
		    if(!found_mine)
		    {
			IVP_IF(1) { printf("removing_dist that doesnt belong to sys\n"); }
			CORE;
		    }
		    fr_sys->remove_dist_from_system(fr_dist);
		    new_fr_sys->add_dist_to_system(fr_dist);
		    //printf("add_to_new %lx fs_num_now %ld\n",(long)fr_dist,new_fr_sys->friction_dist_number); UFTEST
		    if(fr_i_old)
		    {
			// a fixed obj is involved. fixed objs have friction_info in BOTH systems
			fr_i_old->friction_info_delete_friction_dist(fr_dist);
			fr_i_new->friction_info_insert_friction_dist(fr_dist);
		    }
		}
	    }
	    //printf("jjj\n");
	}
    }

    //printf("\nbefore_removal\n");
    //fr_sys->debug_fs_out_ascii();
    //new_fr_sys->debug_fs_out_ascii();
    {
	// test if fixed objs can be removed from old system
	for (int l = fr_sys->cores_of_friction_system.len()-1; l>=0; l--){
	    IVP_Core *obj=fr_sys->cores_of_friction_system.element_at(l);
	    if(obj->physical_unmoveable)    {
		// fixed objs belong to both systems and have two IVP_Friction_Info_For_Obj
		IVP_Friction_Info_For_Core *fr_i_new=obj->get_friction_info(new_fr_sys);
		IVP_Friction_Info_For_Core *fr_i_old=obj->get_friction_info(fr_sys);
		if(fr_i_new->dist_number()==0) {
		  //printf("uf_rem %lx from new %lx\n",(long)obj&0x0000ffff,(long)new_fr_sys&0x0000ffff);
		    obj->delete_friction_info(fr_i_new);
		    new_fr_sys->remove_core_from_system(obj);
		}
		if(fr_i_old->dist_number()==0) {
		  //printf("uf_rem %lx from old %lx\n",(long)obj&0x0000ffff,(long)fr_sys&0x0000ffff);
		    obj->delete_friction_info(fr_i_old);
		    fr_sys->remove_core_from_system(obj);
		}
	    } 
	}
    } 
    
    if(new_fr_sys->friction_obj_number<2) {
      //printf("split_only_one_new\n"); //UFTEST
	IVP_Core *obj=new_fr_sys->cores_of_friction_system.element_at(0);
	IVP_Friction_Info_For_Core *fr_i = obj->get_friction_info(new_fr_sys);
	obj->delete_friction_info(fr_i);
	P_DELETE(fr_i);
	P_DELETE(new_fr_sys);
	return ;
    }
    if(fr_sys->friction_obj_number<2) {
      //printf("split_only_one_old\n"); //UFTEST
	IVP_Core *obj=fr_sys->cores_of_friction_system.element_at(0);
	IVP_Friction_Info_For_Core *fr_i=obj->get_friction_info(fr_sys);
	obj->delete_friction_info(fr_i);
	P_DELETE(fr_i);
	P_DELETE(fr_sys);
	return ;
    }
    //printf("splitted_frs %lx\n",(long)fr_sys); //UFTEST
    //new_fr_sys->fr_solver.calc_calc_solver(new_fr_sys);
    //fr_sys->fr_solver.calc_calc_solver(fr_sys);
    //this->add_friction_system(new_fr_sys);
    IVP_IF(1==1) //expensive but important
    {
      //printf("split_first_result\n");
      //new_fr_sys->debug_fs_out_ascii();
      //printf("split_second_result\n");
      //fr_sys->debug_fs_out_ascii();
      new_fr_sys->test_hole_fr_system_data();
      fr_sys->test_hole_fr_system_data();
    }
    {
	// it may happen, that a system can be split in more than two parts (very seldom)
	IVP_Core *obj=fr_sys->union_find_fr_sys();
	if(obj) {
	    fr_sys->split_friction_system(obj); //split recorsively if necessary
	}
    }
}

void IVP_Friction_System::print_all_dists()
{
    IVP_IF(1) {
    printf("fs %lx  ",(long)this&0x0000ffff);
		for(IVP_Contact_Point *mindist=this->get_first_friction_dist();mindist;mindist=this->get_next_friction_dist(mindist))
		{
		    printf("%lx ",(long)mindist&0x0000ffff);
		}    
    printf("\n");
    printf("      ");
    for (int i = fr_pairs_of_objs.len()-1; i>=0; i--){
	IVP_Friction_Core_Pair *fr_pair = fr_pairs_of_objs.element_at(i);
	printf("p %lx %lx  ",(long)fr_pair->objs[0]&0x0000ffff,(long)fr_pair->objs[1]&0x0000ffff);
	for (int c = fr_pair->fr_dists.len()-1;c>=0; c--){
	    IVP_Contact_Point *fr_dist=fr_pair->fr_dists.element_at(c);
	    printf("%lx ",(long)fr_dist&0x0000ffff);
	}
    }
    printf("\n");
    }
}

void IVP_Friction_System::reset_time(IVP_Time offset){
    for (int i = fr_pairs_of_objs.len()-1; i>=0; i--){
	IVP_Friction_Core_Pair *fr_pair = fr_pairs_of_objs.element_at(i);
	for (int c = fr_pair->fr_dists.len()-1;c>=0; c--){
	    IVP_Contact_Point *my_dist = fr_pair->fr_dists.element_at(c);
	    my_dist->reset_time(offset);
	}
    }
}

// return zero when pair no longer exists
int IVP_Friction_Core_Pair::check_all_fr_mindists_to_be_valid(IVP_Friction_System *my_fs) {
    int total_number_remaining = this->fr_dists.len();
    for (int k = fr_dists.len()-1; k>=0; k--){
	IVP_Contact_Point *my_dist=this->fr_dists.element_at(k);
	my_dist->recalc_friction_s_vals();
	IVP_Impact_Solver_Long_Term *info=my_dist->tmp_contact_info;
	my_dist->read_materials_for_contact_situation(info);
	
	//printf("impact_sys_update_contact_vals %lx\n",(long)my_dist);
	if( info->friction_is_broken == IVP_TRUE) {
	    total_number_remaining--;
	    my_fs->delete_friction_distance(my_dist);
	}
    }
    //warning: at this point 'this' might have been deleted

    return total_number_remaining;
}


void IVP_Friction_System::remove_energy_gained_by_real_friction()
{
    for (int i = fr_pairs_of_objs.len()-1; i>=0; i--){
	IVP_Friction_Core_Pair *my_pair = fr_pairs_of_objs.element_at(i);
	my_pair->remove_energy_gained_by_real_friction();
    }
}

void IVP_Friction_System::clear_integrated_anti_energy()
{
    for (int k = fr_pairs_of_objs.len()-1; k>=0; k--){
	IVP_Friction_Core_Pair *my_pair = fr_pairs_of_objs.element_at(k);
	my_pair->integrated_anti_energy=0.0f;
    }
}


void IVP_Friction_Core_Pair::remove_energy_gained_by_real_friction()
{
    integrated_anti_energy *= objs[0]->environment->get_integrated_energy_damp(); //to avoid accumulation to neverending
    if(integrated_anti_energy<0.0f){ 
	return;
    }
        //printf("destroyyenergy %.8f\n",integrated_anti_energy);
#ifdef IVP_FAST_WHEELS_ENABLED
    if (objs[0]->car_wheel || objs[1]->car_wheel){
	return;
    }
#endif

#ifndef NO_MUTUAL_ENERGYDESTROY
    IVP_DOUBLE amount_energy_destr;
    amount_energy_destr=destroy_mutual_energy(integrated_anti_energy);
    //printf("destr_eee %f\n",amount_energy_destr);
    //printf("destroyed %f\n",amount_energy_destr);
//	IVP_IF(1) {
	objs[0]->environment->get_statistic_manager()->sum_energy_destr+=amount_energy_destr;
//	}
    integrated_anti_energy-=amount_energy_destr;
#endif
}

IVP_DOUBLE IVP_Mutual_Energizer::calc_impulse_to_reduce_energy_level(IVP_DOUBLE speed_pot,IVP_DOUBLE inv_mass0,IVP_DOUBLE inv_mass1,IVP_DOUBLE delta_e)
{
    delta_e*=2.0f;
    IVP_DOUBLE divisor=1.0f/(inv_mass0+inv_mass1);
    IVP_DOUBLE root=(speed_pot*speed_pot-(inv_mass0+inv_mass1)*delta_e); ///(mass1*speed_pot*speed_pot-target_e));
    root = IVP_Inline_Math::fabsd(root);
    root = IVP_Inline_Math::ivp_sqrtf(root);
    IVP_DOUBLE x=(speed_pot-root)*divisor;
    return x;
}

IVP_DOUBLE IVP_Mutual_Energizer::calc_energy_potential(IVP_DOUBLE speed_pot,IVP_DOUBLE mass0,IVP_DOUBLE mass1,IVP_DOUBLE inv_mass0,IVP_DOUBLE inv_mass1)
{
    IVP_DOUBLE x; //impulse done on objects to destroy potential, first obj is still
    //Energy E1 of obj1: mass1 * (speed_pot - x*inv_mass1)^2
    //Energy E0 of obj0: mass0 * (x*inv_mass0)^2
    //point of minimal Energy: E1'(x) = -E0'(x)

    IVP_DOUBLE energy_now=mass1*speed_pot*speed_pot;
    
    x=speed_pot/(inv_mass0+inv_mass1); //solution of E1'(x) = -E0'(x) -> speed of both objects get identical, speed_pot gets zero

    IVP_DOUBLE E0,E1;
    IVP_DOUBLE speed;
    speed=speed_pot - x*inv_mass1;
    E1=mass1*speed*speed;

    speed=x*inv_mass0;
    E0=mass0*speed*speed;

    IVP_DOUBLE tmp = energy_now+P_DOUBLE_EPS - (E0+E1);
	// VALVE: For very large energies (e.g. 3e15) this can be slightly off due to precision errors :(
	// Clamp instead
	// IVP_ASSERT(tmp>=0);
	if ( tmp < 0 )
	{
		tmp = 0;
	}

    return 0.5f*(energy_now-E0-E1);
}

// rot moment in a direction
// rot_vec_obj is normized
void IVP_Mutual_Energizer::get_rot_inertia(IVP_Core *core,IVP_U_Float_Point *rot_vec_obj,IVP_DOUBLE *rot_inertia,IVP_DOUBLE *inv_rot_inertia)
{
    IVP_U_Float_Point rot_vec_abs;
    rot_vec_abs.set_pairwise_mult(rot_vec_obj,core->get_rot_inertia());
    *rot_inertia=rot_vec_abs.real_length();
    if(*rot_inertia<P_DOUBLE_EPS) {
	*rot_inertia=1.0f;
	*inv_rot_inertia=1.0f;
    } else {
	*inv_rot_inertia=1.0f/(*rot_inertia);
    }
}

void IVP_Mutual_Energizer::init_mutual_energizer(IVP_Core *core0,IVP_Core *core1) {
    if(core1->physical_unmoveable || core1->pinned) { //@@CBPIN
	this->core[0]=core1;
	this->core[1]=core0;
    } else {
	this->core[1]=core1;
	this->core[0]=core0;
    }
    //now core1 is moveable

    {
	trans_vec_world.subtract(&core[1]->speed,&core[0]->speed);
	trans_speed_potential=trans_vec_world.real_length_plus_normize();
    }
    {
	IVP_U_Float_Point rot0_world;   	core[0]->m_world_f_core_last_psi.vmult3(&core[0]->rot_speed,&rot0_world);
	IVP_U_Float_Point rot1_world;   	core[1]->m_world_f_core_last_psi.vmult3(&core[1]->rot_speed,&rot1_world);
	IVP_U_Float_Point rot_vec_world; 	rot_vec_world.subtract(&rot1_world,&rot0_world);
	
	rot_speed_potential = rot_vec_world.real_length_plus_normize();
	core[0]->m_world_f_core_last_psi.vimult3(&rot_vec_world,&rot_vec_obj[0]);
	rot_vec_world.mult(-1.0f);
	core[1]->m_world_f_core_last_psi.vimult3(&rot_vec_world,&rot_vec_obj[1]);
	
	get_rot_inertia(core[0],&rot_vec_obj[0],&rot_inertia[0],&inv_rot_inertia[0]);
	get_rot_inertia(core[1],&rot_vec_obj[1],&rot_inertia[1],&inv_rot_inertia[1]);
    }
    {
	trans_inertia[1] = core[1]->get_mass();
	inv_trans_inertia[1]=core[1]->get_inv_mass();
	if(core[0]->physical_unmoveable || core[0]->pinned) { //@@CBPIN
	    trans_inertia[0]=10000*trans_inertia[1];
	    inv_trans_inertia[0]=inv_trans_inertia[1]*(1.0f/10000.0f);
	    rot_inertia[0]=10000*rot_inertia[1];
	    inv_rot_inertia[0]=inv_rot_inertia[1]*(1.0f/10000.0f);
	} else {
	    trans_inertia[0]=core[0]->get_mass();
	    inv_trans_inertia[0]=core[0]->get_inv_mass();
	}
    }
}

void IVP_Mutual_Energizer::calc_energy_potential() {   
	rot_energy_potential = calc_energy_potential(rot_speed_potential,rot_inertia[0],rot_inertia[1],inv_rot_inertia[0],inv_rot_inertia[1]);
	trans_energy_potential = calc_energy_potential(trans_speed_potential,trans_inertia[0],trans_inertia[1],inv_trans_inertia[0],inv_trans_inertia[1]);
	whole_mutual_energy = trans_energy_potential+rot_energy_potential;
}

void IVP_Mutual_Energizer::destroy_percent_energy(IVP_DOUBLE percent_energy_to_destroy)
{
    IVP_DOUBLE rot_impulse=calc_impulse_to_reduce_energy_level(rot_speed_potential,inv_rot_inertia[0],inv_rot_inertia[1],percent_energy_to_destroy*rot_energy_potential);
    IVP_DOUBLE trans_impulse=calc_impulse_to_reduce_energy_level(trans_speed_potential,inv_trans_inertia[0],inv_trans_inertia[1],percent_energy_to_destroy*trans_energy_potential);
    if(!core[0]->physical_unmoveable) {
	core[0]->speed_change.add_multiple(&trans_vec_world,inv_trans_inertia[0]*trans_impulse);
	core[0]->rot_speed_change.add_multiple(&rot_vec_obj[0],inv_rot_inertia[0]*rot_impulse);
    }
    trans_vec_wor