ivp_friction.cxx

hl2 source code. Do not use it illegal.

	fr_data1.revert_friction_data();
    }
    
    IVP_U_Float_Point force0_vec,force1_vec;
    force0_vec.subtract(&fr_data0.world0,&fr_data1.world0);
    force1_vec.subtract(&fr_data0.world1,&fr_data1.world1);
    
    IVP_DOUBLE lendiff=IVP_Inline_Math::fabsd(force0_vec.real_length()-force1_vec.real_length());
    IVP_IF(1) {

	IVP_Synapse_Polygon *syn0,*syn1;
	syn0=dist0->synapse[0]->to_poly();
	syn1=dist1->synapse[0]->to_poly();
	IVP_SYNAPSE_POLYGON_STATUS ty0,ty1;
	ty0=syn0->status;
	ty1=syn1->status;
	IVP_Core *core00,*core01,*core10,*core11;
	core00=dist0->synapse[0]->l_obj->to_real()->physical_core;
	core01=dist0->synapse[1]->l_obj->to_real()->physical_core;
	core10=dist0->synapse[0]->l_obj->to_real()->physical_core;
	core11=dist1->synapse[1]->l_obj->to_real()->physical_core;
	//printf("tcores %lx %lx %lx %lx ",(long)core00,(long)core01,(long)core10,(long)core11);
	printf("test_ease %lx %lx  ",(long)dist0,(long)dist1);
	if(ty0==IVP_ST_EDGE) {
	  printf("edge-edge ");
	} else {
	  printf("point-sur ");
	}
	if(ty1==IVP_ST_EDGE) {
	  printf("edge-edge  ");
	} else {
	  printf("point-sur  ");
	}
	printf("diff %f l0 %f l1 %f\n",lendiff,force0_vec.real_length(),force1_vec.real_length());
	printf("   parallel_ease %f %f %f   %f %f %f\n",force0_vec.k[0],force0_vec.k[1],force0_vec.k[2],force1_vec.k[0],force1_vec.k[1],force1_vec.k[2]);
    }
#endif    
}

void IVP_Friction_Solver::ease_two_mindists(IVP_Contact_Point *dist0,IVP_Contact_Point *dist1,
					    IVP_U_Float_Point *ease_diff_vec0,IVP_U_Float_Point *ease_diff_vec1,
					    IVP_DOUBLE ease_factor) 
{    
    IVP_Core *rev_core=dist0->get_synapse(0)->l_obj->physical_core; //forces seen relative to this core

    int second_core_reversed;
    IVP_FLOAT reverse_factor;
    if(rev_core!=dist1->get_synapse(0)->l_obj->physical_core) {
	second_core_reversed=1;
	reverse_factor = -1.0f;
    } else {
	second_core_reversed=0;
	reverse_factor = 1.0f;
    }
    
    IVP_Impact_Solver_Long_Term *info0 = dist0->tmp_contact_info;
    IVP_Impact_Solver_Long_Term *info1 = dist1->tmp_contact_info;

    IVP_U_Float_Point world_vec0,world_vec1;
    world_vec0.set_multiple(&info0->span_friction_v[0],dist0->span_friction_s[0]);
    world_vec0.add_multiple(&info0->span_friction_v[1],dist0->span_friction_s[1]);

    world_vec1.set_multiple(&info1->span_friction_v[0],dist1->span_friction_s[0]);
    world_vec1.add_multiple(&info1->span_friction_v[1],dist1->span_friction_s[1]);

    IVP_U_Point *world_point[2];
    world_point[0] = &info0->contact_point_ws;
    world_point[1] = &info1->contact_point_ws;
    
    IVP_U_Float_Point world_connection;
    world_connection.subtract(world_point[0],world_point[1]);

    IVP_IF(0) {
	const char *out_text=p_make_string("ease");
	rev_core->environment->add_draw_vector(world_point[1],&world_connection,out_text,3);
	P_FREE(out_text);
    }

    world_connection.fast_normize();
    
    IVP_U_Float_Point ease_part0,ease_part1;
    IVP_DOUBLE part_val0 = world_connection.dot_product(&world_vec0);    
    IVP_DOUBLE part_val1 = world_connection.dot_product(&world_vec1);
    
    ease_part0.set_multiple(&world_connection,part_val0);
    ease_part1.set_multiple(&world_connection,part_val1 * reverse_factor);

    IVP_U_Float_Point optimal_vec;
    optimal_vec.add(&ease_part0,&ease_part1);
    optimal_vec.mult(0.5f);

    IVP_U_Float_Point diff_vec0,diff_vec1;
    diff_vec0.inline_subtract_and_mult(&optimal_vec,&ease_part0,ease_factor); //no full easing
    diff_vec1.inline_subtract_and_mult(&optimal_vec,&ease_part1,ease_factor * reverse_factor);

    ease_diff_vec0->add(&diff_vec0);
    ease_diff_vec1->add(&diff_vec1);
}

void IVP_Friction_Solver::ease_friction_pair(IVP_Friction_Core_Pair *my_pair,IVP_U_Memory *my_mem)
{
    //IVP_U_Float_Point average_v;
    //my_pair->get_average_friction_vector(&average_v);
    //my_pair->set_friction_vectors(&average_v);
    //return;
    
    int total_n = my_pair->number_of_pair_dists();
    IVP_Contact_Point **all_my_dists=(IVP_Contact_Point**)my_mem->get_mem(total_n*sizeof(IVP_Contact_Point*));

    IVP_DOUBLE easing_factor = 1.0f/((IVP_DOUBLE)total_n + P_DOUBLE_EPS);
    
#if defined(IVP_NO_ALLOCA)
    IVP_U_Float_Point *ease_diff_force_vec_stack = (IVP_U_Float_Point*)my_mem->get_mem(total_n*sizeof(IVP_U_Float_Point));
#else
    IVP_U_Float_Point *ease_diff_force_vec_stack=(IVP_U_Float_Point*)alloca(total_n*sizeof(IVP_U_Float_Point));
#endif    
    
    int i=0;
    for (i = my_pair->fr_dists.len()-1; i>=0;i--){
	IVP_Contact_Point *fr_dist = my_pair->fr_dists.element_at(i);
	all_my_dists[i]=fr_dist;
	ease_diff_force_vec_stack[i].set_to_zero();
    }
    
    IVP_IF(0)    {
	printf("ease_testt\n");
	for(i=0;i<total_n-1;i++)
	{
	    IVP_Contact_Point *fr_dist=all_my_dists[i];
	    if((fr_dist->now_friction_pressure>P_DOUBLE_EPS)||1)
	    {
		for(int j=i+1;j<total_n;j++)
		{
		    IVP_Contact_Point *fr_dist2;
		    fr_dist2=all_my_dists[j];
		    if((fr_dist2->now_friction_pressure>P_DOUBLE_EPS)||1)
		    {
			IVP_DOUBLE dir_conform=fr_dist->get_lt()->surf_normal.dot_product(&fr_dist2->get_lt()->surf_normal); //near -1.0f or near 1.0f
			dir_conform=IVP_Inline_Math::fabsd(dir_conform)-1.0f;
			dir_conform=IVP_Inline_Math::fabsd(dir_conform);
			//printf("dirconform %f\n",dir_conform);
			if((dir_conform<1E-3f)||0) //due to random stray in distances the vectors between point-surface and edge-edge are not really parallel
			{
			    //printf("teest ");
			    ease_test_two_mindists(fr_dist2,fr_dist,&fr_dist->get_lt()->surf_normal);
			    //ease_two_mindists(fr_dist2,fr_dist,&world_normal1);
			    //ease_two_mindists(fr_dist2,fr_dist,&world_normal1);
			    //printf("three_eases done \n");
			} else {
			    //printf("noeasingstarted\n");
			}
		    }
		}
	    }
	}
    }
    //printf("real_easing\n");

    IVP_IF(1) {
	my_pair->debug_store_vector_before_ease();
    }
    
    for(i=0;i<total_n-1;i++){
	IVP_Contact_Point *fr_dist=all_my_dists[i];
	if((fr_dist->now_friction_pressure>P_DOUBLE_EPS)||1){
	    for(int j=i+1;j<total_n;j++){
		//printf("epair %d %d\n",i,j);
		IVP_Contact_Point *fr_dist2 = all_my_dists[j];
		if((fr_dist2->now_friction_pressure>P_DOUBLE_EPS)||1){
		    IVP_DOUBLE dir_conform=fr_dist->get_lt()->surf_normal.dot_product(&fr_dist2->get_lt()->surf_normal); //near -1.0f or near 1.0f
		    
		    dir_conform=IVP_Inline_Math::fabsd(dir_conform)-1.0f;
		    dir_conform=IVP_Inline_Math::fabsd(dir_conform);
		    //printf("dirconform %f\n",dir_conform);
		    if((dir_conform<1E-3f)||0) //due to random stray in distances the vectors between point-surface and edge-edge are not really parallel
		    {
		        //printf("eaase ");
			ease_two_mindists(fr_dist2,fr_dist,&ease_diff_force_vec_stack[j],&ease_diff_force_vec_stack[i],easing_factor);
			//ease_two_mindists(fr_dist2,fr_dist,&world_normal1);
			//ease_two_mindists(fr_dist2,fr_dist,&world_normal1);
			//printf("three_eases done \n");
		    } else {
			//printf("noeasingstarted\n");
		    }
		}
	    }
	}
    }

    if(1)    {
	//printf("set_the_friction_force\n");
	for(i=0;i<total_n;i++)	{
	    IVP_Contact_Point *fr_dist=all_my_dists[i];
	    if((fr_dist->now_friction_pressure>P_DOUBLE_EPS)||1)	    {
		fr_dist->ease_the_friction_force(&ease_diff_force_vec_stack[i]);
	    }
	}
    }

    IVP_IF(1) {
	my_pair->debug_read_vector_after_ease();
    }
    
    IVP_IF(0)
    {    
	printf("ease_testt\n");
	for(i=0;i<total_n-1;i++){
	    IVP_Contact_Point *fr_dist=all_my_dists[i];
	    if((fr_dist->now_friction_pressure>P_DOUBLE_EPS)||1)
	    {
		for(int j=i+1;j<total_n;j++)
		{
		    IVP_Contact_Point *fr_dist2;
		    fr_dist2=all_my_dists[j];
		    if((fr_dist2->now_friction_pressure>P_DOUBLE_EPS)||1)
		    {
			IVP_DOUBLE dir_conform=fr_dist->get_lt()->surf_normal.dot_product(&fr_dist2->get_lt()->surf_normal); //near -1.0f or near 1.0f
			dir_conform=IVP_Inline_Math::fabsd(dir_conform)-1.0f;
			dir_conform=IVP_Inline_Math::fabsd(dir_conform);
			//printf("dirconform %f\n",dir_conform);
			if((dir_conform<1E-3f)||0) //due to random stray in distances the vectors between point-surface and edge-edge are not really parallel
			{
			    //printf("teest ");
			    IVP_Friction_Solver::ease_test_two_mindists(fr_dist2,fr_dist,&fr_dist->get_lt()->surf_normal);
			    //ease_two_mindists(fr_dist2,fr_dist,&world_normal1);
			    //ease_two_mindists(fr_dist2,fr_dist,&world_normal1);
			    //printf("three_eases done \n");
			} else {
			    //printf("noeasingstarted\n");
			}
		    }
		}
	    }
	}
    }
}



#if 0 /* not needed any more (snief), but function is correct */
	 //given rot and trans of a core and a surface normal. wanted virtual center p in surface where no translation exists (relative to surface  2D!) 
IVP_RETURN_TYPE IVP_Friction_Solver::calc_virtual_rotation_center(IVP_Core *core,IVP_U_Float_Point *rotation,IVP_U_Float_Point *translation,IVP_U_Float_Point *surf_normal,IVP_U_Float_Point *obj_p_out)
{
    IVP_U_Matrix mat_world_f_z;
    mat_world_f_z.init_normized(surf_normal);

    IVP_U_Float_Point rot_z,trans_z,out_p_z;
    mat_world_f_z.vmult3(rotation,&rot_z);

    IVP_U_Float_Point test_n;
    mat_world_f_z.vmult3(surf_normal,&test_n);
    printf("z_normal %f %f %f\n",test_n.k[0],test_n.k[1],test_n.k[2]);
    
    if(IVP_Inline_Math::fabsd(rot_z.k[2]) < DOUBLE_EPS) {
	return IVP_FAULT;
    }

    IVP_DOUBLE inv_z=1.0f/rot_z.k[2];
    
    mat_world_f_z.vmult3(translation,&trans_z);
    
    out_p_z.k[2]=0.0f;
    out_p_z.k[0]=-trans_z.k[1]*inv_z;
    
    out_p_z.k[1]=trans_z.k[0]*inv_z;

    mat_world_f_z.vimult3(&out_p_z,obj_p_out);

    return IVP_OK;
}
#endif

// move points of synapses in a way unnecessary forces are reduced
void IVP_Friction_System::ease_friction_forces()
{
    IVP_Friction_System *fs=this;
    for (int i = fr_pairs_of_objs.len()-1; i>=0; i--){
    IVP_Friction_Core_Pair *my_pairs = fr_pairs_of_objs.element_at(i);
	my_pairs->next_ease_nr_psi--;
	if(my_pairs->next_ease_nr_psi==0) {
	    //printf("easefr_pair %lx %lx\n",(long)my_pairs->objs[0]&0x0000ffff,(long)my_pairs->objs[1]&0x0000ffff);
	    IVP_Friction_Solver::ease_friction_pair(my_pairs,fs->l_environment->get_memory_manager());
	    my_pairs->next_ease_nr_psi=IVP_EASE_EVERY_NTH_PSI; //do only 5 times a second
	}
    }
}

IVP_Contact_Point::~IVP_Contact_Point(){
    IVP_Environment *env = get_synapse(0)->get_object()->get_environment();

    IVP_Synapse_Friction *syn0 = get_synapse(0);
    syn0->get_object()->get_surface_manager()->remove_reference_to_ledge(syn0->edge->get_compact_ledge());
    IVP_Synapse_Friction *syn1 = get_synapse(1);
    syn1->get_object()->get_surface_manager()->remove_reference_to_ledge(syn1->edge->get_compact_ledge());
    {
	IVP_Event_Friction event_friction;
        event_friction.environment= env;

	IVP_Real_Object *obj0 = get_synapse(0)->l_obj;
	IVP_Real_Object *obj1 = get_synapse(1)->l_obj;
	IVP_Contact_Situation contact_situation;
	contact_situation.objects[0] = obj0;
	contact_situation.objects[1] = obj1;

	contact_situation.compact_edges[0] = get_synapse(0)->edge;
	contact_situation.compact_edges[1] = get_synapse(1)->edge;

	event_friction.contact_situation=&contact_situation;
	
	event_friction.friction_handle = this;
	event_friction.environment->fire_event_friction_deleted(&event_friction);
	{
	    if (obj0->flags.collision_listener_exists){
		IVP_Cluster_Manager *clus_man = env->get_cluster_manager();
		clus_man->fire_event_friction_deleted(obj0, &event_friction);
	    }
	    if (obj1->flags.collision_listener_exists){
		IVP_Cluster_Manager *clus_man = env->get_cluster_manager();
		clus_man->fire_event_friction_deleted(obj1, &event_friction);
	    }
	}
    }
   {
     IVP_Core *core0,*core1;
        IVP_IF( env->get_debug_manager()->check_fs ) {
	    core0=get_synapse(0)->l_obj->friction_core;
	    core1=get_synapse(1)->l_obj->friction_core;
	}
	
	get_synapse(0)->remove_friction_synapse_from_object();
	get_synapse(1)->remove_friction_synapse_from_object();

	IVP_IF( env->get_debug_manager()->check_fs ) {
	    if(core0->physical_unmoveable) {
	        core0->unmovable_core_debug_friction_hash();
	    }
	    if(core1->physical_unmoveable) {
	        core1->unmovable_core_debug_friction_hash();
	    }
	}
    }
}


IVP_FLOAT IVP_Contact_Point_API::get_eliminated_energy(IVP_Contact_Point *friction_handle){
    return friction_handle->integrated_destroyed_energy;
}

void  IVP_Contact_Point_API::reset_eliminated_energy(IVP_Contact_Point *friction_handle){
    friction_handle->integrated_destroyed_energy = 0.0f;
}