ivp_impact.cxx

hl2 source code. Do not use it illegal.

    IVP_IF(0){
	rot_speed[0]  .set( &core[0]->rot_speed );
	trans_speed[0].set( &core[0]->speed);
	rot_speed[1]  .set( &core[1]->rot_speed);
	trans_speed[1].set( &core[1]->speed);

	this->get_relative_speed_vector();
	IVP_DOUBLE speed_parallel_mindist = relative_world_speed.dot_product(surf_normal); //normally negative value
	printf("impact %.20G\n",speed_parallel_mindist); //@@CB
    }
    
    rot_speed[0]  .add( &core[0]->rot_speed, &core[0]->rot_speed_change);
    trans_speed[0].add( &core[0]->speed,     &core[0]->speed_change);
    rot_speed[1]  .add( &core[1]->rot_speed, &core[1]->rot_speed_change);
    trans_speed[1].add( &core[1]->speed,     &core[1]->speed_change);
    
    IVP_IF(1) {
	IVP_U_Float_Point trans_vec;
	trans_vec.subtract( &trans_speed[0],&trans_speed[1] );
	relative_trans_speed_before=trans_vec.real_length();
    }
    
    //IVP_DOUBLE full_energy = this->get_total_energy();
    
    energy_deformation= 0.0f;
    
    //target_energy_high = full_energy;
    //target_energy_low  = target_energy_high;

    this->get_relative_speed_vector();
    speed->set(&relative_world_speed); //return value
    
    int push_counter=0;

    IVP_DOUBLE impulse=this->estimate_push_impulse() * 0.1f; //do only 10 percent pushes

    IVP_DOUBLE speed_parallel_mindist = relative_world_speed.dot_product(surf_normal); //normally negative value

    if(speed_parallel_mindist > -IMPACT_EPS)
    {
	// something went wrong, do a rescue push
        //printf("bad_rescue_push\n");
        IVP_U_Float_Point push_dir;	push_dir.set_negative(surf_normal);
	do_rescue_push(&push_dir,IVP_TRUE); //panic
	//confirm_impact(0);
	//confirm_impact(1);
	//clear_change_values_cores();
	
	//return IVP_OK;
	if(pushes_while_system>10) {
	    // I think I am jammed 
	    delaying_is_allowed=IVP_FALSE;
	}
	goto delaying_test;
    }

    //maybe do an extra rescue_push when speed is very low, this time with friction

    IVP_IF(core[0]->environment->debug_information->debug_impact)    {
	printf("speed %.4f %.4f %.4f rot %.4f %.4f %.4f\n",trans_speed[1].k[0],trans_speed[1].k[1],trans_speed[1].k[2],rot_speed[1].k[0],rot_speed[1].k[1],rot_speed[1].k[2]);
    }
    
    this->get_world_push_direction();

    //this->calc_virt_masses_impact_solver(&world_push_direction);
    
    //last_energy=full_energy;

    integral_pushes_world.set_to_zero();
    virtual_speed=-surf_normal->dot_product(&relative_world_speed); // alternative: world_push_direction->dot_pro..
    speed_before=virtual_speed;
    IVP_ASSERT(virtual_speed>0.0f);

    give_back_speed = virtual_speed*IVP_Inline_Math::ivp_sqrtf(used_conservation);
    give_back_speed+=IVP_SPEED_ADDON_SYSTEM_IMPACT * IVP_Inline_Math::ivp_sqrtf((IVP_DOUBLE)pushes_while_system);
    
    while((virtual_speed > 0.0f)&&(push_counter<100)) {
	push_counter++;
	do_push(impulse);
	//integral_pushes_world.add(&world_push_direction);
	get_relative_speed_vector();
	virtual_speed=-surf_normal->dot_product(&relative_world_speed);
	get_world_push_direction();
    }

    give_back_speed += virtual_speed; //virtual_speed is negative, and that speed is already given back
    world_push_direction.set_multiple(surf_normal,-1.0f);
    //world_push_direction.set(&integral_pushes_world);
    //world_push_direction.normize();
    if((give_back_speed > 0.0f)&&(push_counter!=100)) {
	do_push(1); //test push @@CB
	get_relative_speed_vector();
	IVP_DOUBLE speed_test=surf_normal->dot_product(&relative_world_speed);
	IVP_ASSERT(( speed_test > -virtual_speed));
	IVP_DOUBLE speed_diff=speed_test+virtual_speed;
                IVP_DOUBLE target_push;
				// was fabs, which was a sml call
                if( IVP_Inline_Math::fabsd(speed_diff) > IMPACT_EPS) { 
		target_push=give_back_speed/(speed_diff);
                } else {
		target_push=0.0f; //@@CB double to float
                }
	undo_push();
	do_push(target_push);
    }

    //energy_at_end=this->get_total_energy();
    //factor=energy_at_end / full_energy;
    //printf("energy_change_factor_impact %f\n",factor);
      
    do_rescue_push(&world_push_direction,IVP_FALSE); //to be sure that minimal velocity is reached, not panic

    IVP_IF(1==1)
    {
	IVP_U_Float_Point rel_speed_now;
	IVP_Core::get_diff_surface_speed_of_two_cores_on_test(core[0], core[1],obj_point[0],obj_point[1],&trans_speed[0],&rot_speed[0],&trans_speed[1],&rot_speed[1],&rel_speed_now);
	//printf("world_speed_after_imp %.3f %.3f %.3f  ",rel_speed_now.k[0],rel_speed_now.k[1],rel_speed_now.k[2]);
	//printf("rel_sppeed %.4f\n",rel_speed_now.dot_product(surf_normal));
	if(rel_speed_now.dot_product(surf_normal)>0.0f)
	{
	    IVP_IF(1) { printf("impact_objects_have_illegal_speed_afterwards\n"); }
	    // CORE;
	}
    }
    
delaying_test:
    
    IVP_IF(0) {
    	get_relative_speed_vector();
	IVP_DOUBLE testtest=surf_normal->dot_product(&relative_world_speed);
	IVP_USE(testtest);
	IVP_DOUBLE speed_gain=testtest-speed_before; //speed before not initialized when hard rescue
	if(speed_gain>0.0f) {
	    if(speed_gain > core[0]->environment->get_statistic_manager()->max_speed_gain) {
		core[0]->environment->get_statistic_manager()->max_speed_gain=speed_gain;
	    }
	}
    }

    core[0]->clip_velocity(&trans_speed[0], &rot_speed[0]);
    core[1]->clip_velocity(&trans_speed[1], &rot_speed[1]);
    
    //printf("speed_after_imp %f %f  %f %f\n",trans_speed[0].real_length(),rot_speed[0].real_length(),trans_speed[1].real_length(),rot_speed[1].real_length());

    delay_decision(pushed_cores);

    if( core[0]->temporarily_unmovable | core[1]->temporarily_unmovable ) {
	IVP_IF(0) {
	    printf("switched_to_temporarily_unmov %lx %lx %f\n",(long)core[0],(long)core[1],core[0]->environment->get_current_time().get_time());
	}
	core[0]->environment->get_statistic_manager()->impact_unmov++;

	{
	    IVP_Core *c = core[0];
	    c->temporarily_unmovable=(IVP_BOOL)(1-(int)c->physical_unmoveable);
	    c->speed_change.add(&c->speed);
	    c->rot_speed_change.add(&c->rot_speed);
	    c->speed.set_to_zero();
	    c->rot_speed.set_to_zero();
	    pushed_cores[0]=c;
	}
	{
	    IVP_Core *c = core[1];
	    c->temporarily_unmovable=(IVP_BOOL)(1-(int)c->physical_unmoveable);
	    c->speed_change.add(&c->speed);
	    c->rot_speed_change.add(&c->rot_speed);
	    c->speed.set_to_zero();
	    c->rot_speed.set_to_zero();
	    pushed_cores[1]=c;
	}
    }
    
    //this->confirm_impact();
    return;
}

void IVP_Impact_Solver::delay_decision(IVP_Core *pushed_cores[2]) {
    //for movables increase impact counter 
    core[0]->impacts_since_last_PSI+=(1-(int)core[0]->physical_unmoveable);
    core[1]->impacts_since_last_PSI+=(1-(int)core[1]->physical_unmoveable);
    
    if(delaying_is_allowed) {
		
	int try_to_delay_core;
	int other_core;

	if(virt_mass[0]>virt_mass[1]) {
	    try_to_delay_core=0;
	    other_core = 1;
	} else {
	    try_to_delay_core=1;
	    other_core=0;
	}

	if(pushed_cores[try_to_delay_core]->physical_unmoveable) {
	    // no delaying on unmoveable cores
	    goto no_delaying;
	}
	
	//if(pushed_cores[try_to_delay_core]->get_current_sim_man_slot() > 0) {
	    // this is a core that is not simulated every psi -> not any longer
	//goto no_delaying;
	    //}

	IVP_U_Float_Point delayer_trans_speed, delayer_rot_speed;
	IVP_U_Float_Point   other_trans_speed, other_rot_speed;
	
	other_rot_speed.set   (&rot_speed  [other_core]);	// use speeds after impact
	other_trans_speed.set (&trans_speed[other_core]);
	delayer_rot_speed.set (&core [try_to_delay_core]->rot_speed);	// use original speeds
	delayer_trans_speed.set(&core[try_to_delay_core]->speed);
	
	IVP_DOUBLE closing_speed;
	{
	    IVP_U_Float_Point rel_world_speed;
	    IVP_Core::get_diff_surface_speed_of_two_cores_on_test(core[try_to_delay_core], core[other_core],
							      obj_point[try_to_delay_core],obj_point[other_core],
							      &delayer_trans_speed, &delayer_rot_speed,
							      &other_trans_speed,   &other_rot_speed,
							      &rel_world_speed);
	    closing_speed = rel_world_speed.dot_product(surf_normal);
	    if(try_to_delay_core) {
		closing_speed *= -1.0f;
	    }
	}

	if(closing_speed< -this->rescue_speed_impact_solver * IVP_INV_SAFETY_FACTOR_FOR_DELAY) {
	    core[0]->environment->get_statistic_manager()->impact_delayed_counter++;
	    pushed_cores[try_to_delay_core]=NULL;
	    clear_change_values_cores();
	    confirm_impact(1-try_to_delay_core);
	    //printf("delaying_a_push\n");
	    delay_of_impact(try_to_delay_core);
	    core[try_to_delay_core]->impacts_since_last_PSI--;
	} else {
	    goto no_delaying;
	}
    } else {
no_delaying:
	clear_change_values_cores();
	confirm_impact(0);
	confirm_impact(1);
    }
}

void IVP_Impact_Solver::delay_of_impact(int delayed_core_nr) {
    //calc needed change values
    IVP_Core *c = core[delayed_core_nr];
    c->speed_change.subtract (   &trans_speed[delayed_core_nr], &c->speed);
    c->rot_speed_change.subtract (&rot_speed[delayed_core_nr],  &c->rot_speed);
    //c->environment->add_delayed_push_core(c);
}

void IVP_Impact_Solver::clear_change_values_cores()
{
    core[0]->rot_speed_change.set_to_zero();
    core[0]->speed_change.set_to_zero();
    core[1]->rot_speed_change.set_to_zero();
    core[1]->speed_change.set_to_zero();    
}

// gives impulse push on both objects
// direction (pointing to second obj) is world_push_direction (is normized)
// push quantity is impulse_val
void IVP_Impact_Solver::do_push(IVP_DOUBLE impulse_val)
{
    IVP_U_Float_Point obj_push;
    IVP_U_Float_Point world_push;

    world_push.set(&world_push_direction);

    IVP_IF(core[0]->environment->debug_information->debug_impact)    {
	IVP_U_Float_Point surf_dir;
	surf_dir.set(0.0f,1.0f,0.0f);
	IVP_DOUBLE surf_cos=surf_dir.dot_product(&world_push_direction);
	printf("push_dir_cos %f\n",surf_cos);
    }
    
    world_push.mult(impulse_val); // vector gets longer with impulse_val

    if(!core[0]->physical_unmoveable)    {
        IVP_IF(core[0]->environment->debug_information->debug_impact)
	{
	    //printf("p %.2f %.2f %.2f ",world_push_direction.k[0],world_push_direction.k[1],world_push_direction.k[2]);
	}
	m_world_f_core[0]->inline_vimult3(&world_push,&obj_push); // obj_push now in object coords
	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)    {
	world_push.mult(-1.0f);
	IVP_IF(core[0]->environment->debug_information->debug_impact)
	{
	    //printf("p %.2f %.2f %.2f ",world_push_direction.k[0],world_push_direction.k[1],world_push_direction.k[2]);
	}
	m_world_f_core[1]->inline_vimult3(&world_push,&obj_push);
	core[1]->test_push_core(obj_point[1],&obj_push,&world_push,&trans_speed_change[1],&rot_speed_change[1]);
	IVP_IF(core[0]->environment->debug_information->debug_impact)
	{
	    //printf("did_push trans %.3f %.3f %.3f add %.3f %.3f %.3f\n",trans_speed1.k[0],trans_speed1.k[1],trans_speed1.k[2],trans_speed1_change.k[0],trans_speed1_change.k[1],trans_speed1_change.k[2]);
	}
	rot_speed[1].add(&rot_speed_change[1]);
	trans_speed[1].add(&trans_speed_change[1]);
    } 
}

void IVP_Impact_Solver::calc_virt_masses_impact_solver(const IVP_U_Float_Point *world_direction_normal) {
    int i;
    IVP_U_Float_Point direction_world;
    direction_world.set(world_direction_normal);
    for(i=1;i>=0;i--) {
	IVP_U_Float_Point direction_obj;
	m_world_f_core[i]->vimult3(&direction_world,&direction_obj);
	this->virt_mass[i]=core[i]->calc_correct_virt_mass(obj_point[i],(IVP_Vec_PCore *)&direction_obj,&direction_world);
	direction_world.mult(-1.0f);
    }
    
    if(core[0]->physical_unmoveable)    {
	virt_mass[0]=virt_mass[1]*100000.0f; //@@CBPIN
    }
    if(core[1]->physical_unmoveable)    {
	virt_mass[1]=virt_mass[0]*100000.0f; //@@CBPIN
    }
}

IVP_DOUBLE IVP_Impact_Solver::estimate_push_impulse()
{
    // estimates impulse, impulse is always > 0
    // virtual masses are along relative_world_speed , alternative would be to change relative_world_speed according (not existing) friction and then calc virtual mass
    // no longer!

    // get virtual masses along surface normal
    // get speed in direction of surface normal
    // impulse is mass * velocity

    calc_virt_masses_impact_solver(surf_normal);