ivp_impact.cxx

hl2 source code. Do not use it illegal.

	}
    }

    if(two_friction_values==IVP_FALSE) {

#if 0	
	percent_deformation=1.0f;
#endif	
	if((angle>-cos_friction)) //negativ value
	    {
		//angle less cos_friction degree -> no more friction
		//devide world_push_direction in parts in direction of surface and surface normal
		//direction in surface normal is kept. direction in surface is shortened in a way the angle is border-angle
#if 0
//not used at the moment -> needed by single_impact
		percent_deformation=angle/-cos_friction;
#endif
		IVP_DOUBLE mangle = - angle;
		part_in_direction_surf.add_multiple(&world_push_direction,surf_normal,mangle);
		part_in_direction_surf.normize();
		part_in_direction_surf.mult(sin_friction); //part in direction of surface is shortend to be length of sin
	
		//part_in_direction_norm.set_multiple(surf_normal,-cos_friction);
		world_push_direction.add_multiple(&part_in_direction_surf,surf_normal, -cos_friction); //recombine; vector has exactly length 1.0

		//printf("recomb2 %f %f %f\n",world_push_direction.k[0],world_push_direction.k[1],world_push_direction.k[2]);
	    }
    } else {
	get_world_push_direction_two_friction(angle);
    }
}

// when friction values are low, the part in direction surface of world push direction is not allowed to be big
// we calculate the length of the part in direction surface of world push direction
// then we calculate the allowed length
// therefore we calculate the angle in 2D (spanned by the two friction vectors)
// and the allowed length is a position on a 2D ellipse
// when allowed length is shorter, we have to take the shorter value
void IVP_Impact_Solver::get_world_push_direction_two_friction(IVP_DOUBLE part_direction_surf_normal) {
        IVP_U_Float_Point part_in_direction_surf; //decomposition of velo vec in direction normal and rest (this is the rest)
#if 0	
	percent_deformation=1.0f;
#endif	
	IVP_DOUBLE mangle = - part_direction_surf_normal;
	part_in_direction_surf.add_multiple(&world_push_direction,surf_normal,mangle);
	//existing length
	IVP_FLOAT length_direction_surf=part_in_direction_surf.real_length_plus_normize(); 

	//now calculate allowed length
	//we have an ellipse with first radius sin_friction and second radius sin_second_friction
	//second axis is world_direction_second_friction
	IVP_DOUBLE part_second_friction=part_in_direction_surf.dot_product(&world_direction_second_friction);
	
	IVP_FLOAT abs_second_part=IVP_Inline_Math::fabsd(part_second_friction);
	IVP_FLOAT angle_2d = IVP_Inline_Math::asind(abs_second_part);
	IVP_FLOAT axis_first_friction = IVP_Inline_Math::approx5_cos(angle_2d);
	IVP_FLOAT axis_second_friction=abs_second_part; //equals to sin(angle_2d)
	IVP_FLOAT quad_len_allowed=axis_first_friction*axis_first_friction*sin_friction*sin_friction + axis_second_friction*axis_second_friction*sin_second_friction*sin_second_friction;
	
	if( length_direction_surf*length_direction_surf > quad_len_allowed ) {
	    //the combined friction values are too low to allow existing push direction
	    IVP_FLOAT allowed_len = IVP_Inline_Math::ivp_sqrtf(quad_len_allowed);

#if 0	    
	    IVP_FLOAT factor_length;
	    if(length_direction_surf>P_DOUBLE_EPS) {
		factor_length=allowed_len/length_direction_surf;
	    } else {
		factor_length=0.0f;
	    }
	    percent_deformation=factor_length;
#endif
	    
	    IVP_FLOAT length_angle = IVP_Inline_Math::asind(allowed_len);
	    IVP_FLOAT cos_part= IVP_Inline_Math::approx5_cos(length_angle);
	    IVP_U_Float_Point world_push_dir_fl;
	    world_push_dir_fl.set_multiple(surf_normal,-cos_part);
	    world_push_direction.set(&world_push_dir_fl);
	    world_push_direction.add_multiple(&part_in_direction_surf,allowed_len); //length is now exactly 1.0
	}
	    
	    //IVP_DOUBLE part_second_friction=world_push_direction.dot_product(&world_direction_second_friction);
}

void IVP_Impact_Solver::confirm_impact(int core_nr)
{
    // transfer values to object core
    // if objects are unmovale, values are zero
    IVP_Core *c = core[core_nr];
    c->speed.set(&trans_speed[core_nr]);
    c->rot_speed.set(&rot_speed[core_nr]);
    IVP_Anomaly_Limits *al = c->environment->get_anomaly_limits();

    if( c->impacts_since_last_PSI > al->get_max_collisions_per_psi() ) {
	IVP_Anomaly_Manager *am = c->environment->get_anomaly_manager();
	c->temporarily_unmovable =  am->max_collisions_exceeded_check_freezing(al,  c );
    }
}

void IVP_Impact_Solver::undo_push()
{
    // make last push undone
    IVP_IF(core[0]->environment->debug_information->debug_impact)
    {
	printf("impact_undo_push\n");
    }
    if(!core[0]->physical_unmoveable)
    {
	rot_speed[0].subtract(&rot_speed_change[0]);
	trans_speed[0].subtract(&trans_speed_change[0]);
    }
    if(!core[1]->physical_unmoveable)
    {
	rot_speed[1].subtract(&rot_speed_change[1]);    
	trans_speed[1].subtract(&trans_speed_change[1]);
	IVP_IF(core[0]->environment->debug_information->debug_impact)
	{
	    printf("undoing trans %.3f %.3f %.3f\n",trans_speed_change[1].k[0],trans_speed_change[1].k[1],trans_speed_change[1].k[2]);
	}
    }
}

//push_dir_norm: normized push pointer, points to first core
//rescue push gives rotation free pushes when needed, to assure that objects are moving away from from each other
void IVP_Impact_Solver::do_rescue_push(IVP_U_Float_Point *push_dir_norm,IVP_BOOL panic_mode)
{
    //printf("rescuepush\n\n");
        IVP_U_Point world_point_obj0,world_point_obj1;
	m_world_f_core[0]->vmult4(obj_point[0],&world_point_obj0);
	m_world_f_core[1]->vmult4(obj_point[1],&world_point_obj1);
	
	IVP_U_Float_Point diff_vec_world; //vector of position difference. points from first synapse to second
	//diff_vec_world.subtract(&world_point_obj1,&world_point_obj0);
//	IVP_DOUBLE dist_len_now=diff_vec_world.fast_real_length();
	
	//diff_vec_world.normize();
	diff_vec_world.set_multiple(push_dir_norm,-1.0f); //diff_vec_world must point from first to second

	IVP_U_Float_Point translation,rotation,world_speed0,world_speed1;
	
	//rotation.set(&core0->rot_speed);
	rotation.set(&rot_speed[0]);
	//translation.set(&core0->speed);
	translation.set(&trans_speed[0]);
	core[0]->get_surface_speed_on_test(obj_point[0],&translation,&rotation,&world_speed0);

	//rotation.set(&core1->rot_speed);
	rotation.set(&rot_speed[1]);
	//translation.set(&core1->speed);
	translation.set(&trans_speed[1]);
	core[1]->get_surface_speed_on_test(obj_point[1],&translation,&rotation,&world_speed1);

	IVP_U_Float_Point rel_speed_world; //speed of second synapse against first, in world coords
	rel_speed_world.subtract(&world_speed1,&world_speed0);

	IVP_DOUBLE dist_velocity=rel_speed_world.dot_product(&diff_vec_world); //velocity of distance change. negative means getting closer.
	if((panic_mode==IVP_TRUE)) {
	    if(dist_velocity>0.0f) {
		dist_velocity=0.0f; //assure that a positive push is really done
	    }
	}
	
//	IVP_Environment *env = core0->environment;
	//if (dist_velocity > 0) dist_velocity = 0;	// 
	IVP_DOUBLE desired_velo_change= //(ivp_mindist_settings.coll_dist + 2 * ivp_mindist_settings.mindist_change_force_dist - dist_len_now) / env->get_delta_PSI_time() +
	    this->rescue_speed_impact_solver - dist_velocity; 

	if(desired_velo_change<0.0f) {
	    return;
	}
#if 0
	//new version with translation only -> seems to be no good
	IVP_IF(1) {
	    if(panic_mode) {
		core[0]->environment->impact_hard_rescue_counter++;
	    } else {
		core[0]->environment->impact_rescue_after_counter++;
	    }
	}
	
	IVP_DOUBLE speed_change_scalar=0.0f;
	if(!core[0]->physical_unmoveable) {
	    speed_change_scalar+=core[0]->inv_mass;
	}
	if(!core[1]->physical_unmoveable) {
	    speed_change_scalar+=core[1]->inv_mass;
	}
	
	IVP_DOUBLE resulting_push=desired_velo_change/speed_change_scalar;
	
	if(!core[0]->physical_unmoveable) {
	    trans_speed_change[0].set_multiple(push_dir_norm,resulting_push*core[0]->inv_mass);
	    trans_speed[0].add(&trans_speed_change[0]); // if push has to be undone, values are subtracted later
	}	    

	if(!core[1]->physical_unmoveable) {
	    trans_speed_change[1].set_multiple(push_dir_norm,-resulting_push*core[1]->inv_mass);
	    trans_speed[1].add(&trans_speed_change[1]); // if push has to be undone, values are subtracted later
	}	    
	
	
#else //old version: translation and rotation	
	IVP_U_Float_Point push_vec_obj;
	IVP_U_Float_Point push_vec_world;
	IVP_U_Float_Point rotation_vec;
	IVP_U_Float_Point translation_vec;
	IVP_U_Float_Point speed_change_vec;
	IVP_DOUBLE speed_change_scalar=0.0f;
	
	
	push_vec_world.set(&diff_vec_world);
	IVP_U_Matrix *mat;
	
	if(!core[1]->physical_unmoveable)
	{
	    //give second obj a test push and look how distance reacts
	    mat=m_world_f_core[1];
	    mat->vimult3(&push_vec_world,&push_vec_obj);

      	    core[1]->test_push_core(obj_point[1],&push_vec_obj,&push_vec_world,&translation_vec,&rotation_vec);	    
	    core[1]->get_surface_speed_on_test(obj_point[1],&translation_vec,&rotation_vec,&speed_change_vec);
	    speed_change_scalar+=push_vec_world.dot_product(&speed_change_vec);
	}

	if(!core[0]->physical_unmoveable)
	{
	    //give first obj a test push and look how distance reacts
	    mat=m_world_f_core[0];
	    push_vec_world.mult(-1.0f);
	    mat->vimult3(&push_vec_world,&push_vec_obj);

	    core[0]->test_push_core(obj_point[0],&push_vec_obj,&push_vec_world,&translation_vec,&rotation_vec);
	    core[0]->get_surface_speed_on_test(obj_point[0],&translation_vec,&rotation_vec,&speed_change_vec);
	    speed_change_scalar+=push_vec_world.dot_product(&speed_change_vec);
	}

	IVP_DOUBLE resulting_push=desired_velo_change/(speed_change_scalar + 1e-15f);
	if(resulting_push<0.0f) {
	    return;
	}

	IVP_IF(1) {
	    if(panic_mode) {
		core[0]->environment->get_statistic_manager()->impact_hard_rescue_counter++;
	    } else {
		core[0]->environment->get_statistic_manager()->impact_rescue_after_counter++;
	    }
	}
	
	IVP_IF(core[0]->environment->debug_information->debug_impact||0)
	    {
		printf("doing_res_push %f dvc %f\n",resulting_push,desired_velo_change);
	    }
		
	IVP_U_Float_Point obj_push, world_push;
	
	if(!core[0]->physical_unmoveable)
	{
	    mat=m_world_f_core[0];
	    world_push.set_multiple(&diff_vec_world, -resulting_push); //negativ: dist_vector_world points to second core
	    mat->vimult3(&world_push,&obj_push);
	    //core0->push_core(obj_point0,&obj_push,&world_push);
	    //optimization, but debugging impossible, instead:
	    core[0]->test_push_core(obj_point[0],&obj_push,&world_push,&trans_speed_change[0],&rot_speed_change[0]);
	    rot_speed[0].add(&rot_speed_change[0]); // ...speed0 has sum of pushes and beginning speed before impact
	    trans_speed[0].add(&trans_speed_change[0]); // if push has to be undone, values are subtracted later
	    
	}
	
	if(!core[1]->physical_unmoveable)
	{
	    mat=m_world_f_core[1];
	    world_push.set_multiple(&diff_vec_world, resulting_push); //negativ: dist_vector_world points to second core
	    mat->vimult3(&world_push,&obj_push);
	    //core1->push_core(obj_point1,&obj_push,&world_push); 
	    //optimization, but debugging impossible, instead:
	    core[1]->test_push_core(obj_point[1],&obj_push,&world_push,&trans_speed_change[1],&rot_speed_change[1]);
	    rot_speed[1].add(&rot_speed_change[1]); // ...speed0 has sum of pushes and beginning speed before impact
	    trans_speed[1].add(&trans_speed_change[1]); // if push has to be undone, values are subtracted later
	}
#endif	
}

void IVP_Impact_Solver::do_push_on_core(IVP_U_Float_Point *push_vec_world,int num_core) {
    IVP_U_Float_Point push_vec_obj;
    IVP_U_Matrix *mat=m_world_f_core[num_core];
    mat->vimult3(push_vec_world,&push_vec_obj);

    core[num_core]->test_push_core(obj_point[num_core],&push_vec_obj,push_vec_world,&trans_speed_change[num_core],&rot_speed_change[num_core]);
    rot_speed[num_core].add(&rot_speed_change[num_core]); // ...speed0 has sum of pushes and beginning speed before impact
    trans_speed[num_core].add(&trans_speed_change[num_core]); // if push has to be undone, values are subtracted later
}


// first_push and second_push are pointers to cores that are pushed instantly (if not set to NULL)
// when allow_delaying is TRUE, one core may rest untouched and is changed in next PSI
void IVP_Impact_Solver::do_impact(IVP_Core *pushed_cores[2],IVP_BOOL allow_delaying,int pushes_while_system,IVP_FLOAT rescue_speed_addon)
{
    // impact of surface (obj0) and point (obj1)
    // impulse is estimated, but problem cant be solved in one step -> iteration of pushes
    // there are done several pushes with push direction changing, when desired energy level interval is reached : Stop
    // there may be situations where energy level cant be reached ( very flat impact angle). Then do a single short push to avoid (hopefully) collision: do_rescue_push

    //printf("\n\n\nimpacting\n\n\n");
    IVP_DOUBLE virtual_speed,give_back_speed = 0.0f,speed_before;
    IVP_DOUBLE relative_trans_speed_before;
    IVP_DOUBLE used_conservation;

    IVP_IF(1) {
	core[0]->environment->get_debug_manager()->all_time_impacts += 1.0f;
    }

    used_conservation = 1.0f - (1.0f/(pushes_while_system*IVP_INV_HALF_CONSERVATION_STEPS+1.0f))
	* (1.0f-percent_energy_conservation);
    
    this->rescue_speed_impact_solver= (IVP_FLOAT)(ivp_mindist_settings.min_vertical_speed_at_collision + rescue_speed_addon) * IVP_SAFETY_FACTOR_FOR_DELAY;
    
    delaying_is_allowed=allow_delaying;
    //delaying_is_allowed=IVP_FALSE;
    pushed_cores[0]=core[0];
    pushed_cores[1]=core[1];
     
    core[0]->environment->get_statistic_manager()->impact_counter++;
        
    m_world_f_core[0]	=	&core[0]->m_world_f_core_last_psi;
    m_world_f_core[1] =	&core[1]->m_world_f_core_last_psi;

    IVP_DOUBLE mass0,mass1;
    mass0 = core[0]->get_mass();
    mass1 = core[1]->get_mass();
    if(core[0]->physical_unmoveable || core[0]->pinned)    { //@@CBPIN
	mass0=mass1 * 10000000.0f;
    }

    if(core[1]->physical_unmoveable || core[1]->pinned)    { //@@CBPIN
	mass1=mass0 * 10000000.0f;
    }