ivp_core.cxx

hl2 source code. Do not use it illegal.

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




#ifndef WIN32
#	pragma implementation "ivp_core.hxx"
#	pragma implementation "ivp_core_macros.hxx"
#endif

#include <ivp_physics.hxx>
#include <ivu_matrix_macros.hxx>
#include <ivu_float.hxx>
#include <ivu_memory.hxx>

#include <ivp_core_macros.hxx>
#include <ivp_sim_unit.hxx>
#include <ivp_hull_manager.hxx>

#include <ivp_merge_core.hxx>
#include <ivp_cache_object.hxx>
#include <ivp_debug_manager.hxx>

#include <ivp_mindist_intern.hxx>
#include <ivp_friction.hxx>
#include <ivp_i_friction_hash.hxx>

#include <ivp_calc_next_psi_solver.hxx>

/* Note: When z is the coordinate of the Z axis,
 * and x and y lay in plane orthogonal to z
 * then following equations are valid in a modulo 3 ring:
 * x+1 = y  and y+1 = z
 * this means Axis 2 -> 0 1
 *            Axis 0 -> 1 2
 *            Axis 1 -> 2 0
 */

// The same in german:

/* Sei z die Koordinate der Achse Z
 * und sind x und y der dazu senkrechten Ebene dann gilt
 * x+1 = y   und y+1 =z im Ring Z3
 * also	Achse 2	-> 0 1
 *		Achse 0	-> 1 2
 *		Achse 1	-> 2 0
 */

#define IVP_SAFETY_FACTOR_HULL 1.1f

#define IVP_MINIMAL_TRANS_SPEED_PER_PSI_SHORT 0.01f
#define IVP_MINIMAL_ROT_SPEED_PER_PSI_SHORT 0.005f
#define IVP_MINIMAL_TRANS_SPEED_PER_PSI_LONG 0.1f
#define IVP_MINIMAL_ROT_SPEED_PER_PSI_LONG 0.2f

#define IVP_HAS_MOVED_LONG_TIME 4.0f //sec


#define MAX_PLAUSIBLE_LEN 100000.0f //slower than meter/sec  //XXX 0 added by chris!



// get surface speed; take rotation speed and center speed as input
void IVP_Core::get_surface_speed_on_test(const IVP_U_Float_Point *point_cs,
					 const IVP_U_Float_Point *center_speed_ws,
					 const IVP_U_Float_Point *rot_speed_cs,
					 IVP_U_Float_Point *speed_out_ws) const{
    IVP_U_Float_Point speed_cs;
    speed_cs.inline_calc_cross_product(rot_speed_cs,point_cs);
    this->m_world_f_core_last_psi.inline_vmult3(&speed_cs,speed_out_ws); // transform to world coords
    speed_out_ws->add(center_speed_ws);
}

void IVP_Core::get_surface_speed_ws( const IVP_U_Point *position_ws_in, IVP_U_Float_Point *speed_ws_out) {
    IVP_U_Float_Point av_ws;
    get_m_world_f_core_PSI()->vmult3( &this->rot_speed, &av_ws);
    IVP_U_Float_Point pos_rel; 	pos_rel.subtract( position_ws_in, get_position_PSI());
    IVP_U_Float_Point cross;    cross.inline_calc_cross_product(&av_ws,&pos_rel);
    speed_ws_out->add(&speed, &cross);
}


void IVP_Core::get_diff_surface_speed_of_two_cores(const IVP_Core *this_core, const IVP_Core *other_core,
						   const IVP_U_Float_Point *obj_point_this, const IVP_U_Float_Point *obj_point_other,
						   IVP_U_Float_Point *delta_velocity_ws){
    if(this_core && !this_core->physical_unmoveable)    {
	this_core->get_surface_speed_on_test(obj_point_this,&this_core->speed,&this_core->rot_speed,delta_velocity_ws);
    } else {
	delta_velocity_ws->set_to_zero();
    }

    if(other_core && !other_core->physical_unmoveable)    {
	IVP_U_Float_Point velocity_ws_2;
	other_core->get_surface_speed_on_test(obj_point_other,&other_core->speed,&other_core->rot_speed,&velocity_ws_2);
        delta_velocity_ws->subtract(&velocity_ws_2);
    }
}

void IVP_Core::get_diff_surface_speed_of_two_cores_on_test(const IVP_Core *this_core, const IVP_Core *other_core,
							   const IVP_U_Float_Point *obj_point_this, const IVP_U_Float_Point *obj_point_other,
							   const IVP_U_Float_Point *trans_speed0, const IVP_U_Float_Point *rot_speed0,
							   const IVP_U_Float_Point *trans_speed1, const IVP_U_Float_Point *rot_speed1,
							   IVP_U_Float_Point *diff_speed_this_minus_other_out)
{
    IVP_U_Float_Point world_this,world_other;
    if(!this_core->physical_unmoveable)    {
	this_core->get_surface_speed_on_test(obj_point_this,trans_speed0,rot_speed0,&world_this);
    } else {
	world_this.set_to_zero();
    }

    if(!other_core->physical_unmoveable)    {
	other_core->get_surface_speed_on_test(obj_point_other,trans_speed1,rot_speed1,&world_other);
    } else {
	world_other.set_to_zero();
    }
    diff_speed_this_minus_other_out->subtract(&world_this,&world_other);
}



IVP_DOUBLE IVP_Core::calc_virt_mass_worst_case(const IVP_U_Float_Point *core_point) const {
    IVP_U_Float_Point worst_direction;
    worst_direction.k[0]=core_point->k[1]*core_point->k[1] + core_point->k[2]*core_point->k[2];
    worst_direction.k[1]=core_point->k[0]*core_point->k[0] + core_point->k[2]*core_point->k[2];
    worst_direction.k[2]=core_point->k[0]*core_point->k[0] + core_point->k[1]*core_point->k[1];
    worst_direction.set_pairwise_mult(&worst_direction,this->get_inv_rot_inertia());
    IVP_DOUBLE max_of_coords=worst_direction.k[0];
    if(worst_direction.k[1]>max_of_coords) {
	max_of_coords=worst_direction.k[1];
    }
    if(worst_direction.k[2]>max_of_coords) {
	max_of_coords=worst_direction.k[2];
    }
    IVP_DOUBLE dv;

	if(!pinned) //@@CBPIN
	{
		dv = this->get_inv_mass() + max_of_coords;
	}
	else
	{
		dv = 1.0f + max_of_coords;
	}

    IVP_DOUBLE ret = 1.0f/dv;
    IVP_ASSERT(ret < 1e31f); // OG test for NaN
    return ret;
}

// also calculates some material dependent redundants
void IVP_Core::revive_adjacent_to_unmoveable() {
        IVP_ASSERT( this->physical_unmoveable );

	this->environment->get_sim_unit_mem()->start_memory_transaction();

	int d;
	for(d = this->objects.len()-1;d>=0;d--) {
	    IVP_Real_Object *r_obj=this->objects.element_at(d);
	    IVP_Synapse_Friction *fr_synapse,*next_syn;
	    IVP_Contact_Point *fr_mindist;
	    IVP_Friction_System *fs;
	    fr_synapse=r_obj->get_first_friction_synapse();
	    while( fr_synapse ) {
		next_syn=fr_synapse->get_next();

		fr_mindist=fr_synapse->get_contact_point();
		fs=fr_mindist->l_friction_system;
		fs->union_find_necessary=IVP_TRUE;
		IVP_Core *other_core;
		other_core=fr_mindist->get_synapse(0)->l_obj->get_core();
		if( other_core == this ) {
		    other_core=fr_mindist->get_synapse(1)->l_obj->get_core();
		}
		if( other_core->physical_unmoveable ) {
		    //printf("found_unnecassary_fmd\n");
		    P_DELETE( fr_mindist );
		} else {
		    other_core->ensure_all_core_objs_in_simulation();
			fr_mindist->recalc_friction_s_vals();
		    IVP_Impact_Solver_Long_Term info;
		    fr_mindist->read_materials_for_contact_situation(&info); //this is done only to get friction_factor ; maybe make extra function for friction_factor
		    fr_mindist->calc_virtual_mass_of_mindist();
		}
		fr_synapse=next_syn;
	    }
	}

	this->environment->get_sim_unit_mem()->end_memory_transaction();
}

void IVP_Core::values_changed_recalc_redundants() {
    int d;

    environment->sim_unit_mem->start_memory_transaction();
    
    calc_calc();

    //EMAURER
    environment->sim_unit_mem->end_memory_transaction();        
    
    IVP_Movement_Type mt_for_objs;
    if( this->physical_unmoveable ) {
	mt_for_objs=IVP_MT_STATIC;
	this->movement_state=IVP_MT_NOT_SIM; //flag changed from moveable to unmoveable
    } else {
	if( this->movement_state > IVP_MT_NOT_SIM ) {
	    this->movement_state=IVP_MT_NOT_SIM; //flag changed from unmoveable to moveable
	}
	mt_for_objs=(IVP_Movement_Type)movement_state;
    }

    for(d = this->objects.len()-1;d>=0;d--) {
	IVP_Real_Object *r_obj=this->objects.element_at(d);
	r_obj->set_movement_state(mt_for_objs);
	//r_obj->revive_nearest_objects_grow_fs();
    }

    if( !this->physical_unmoveable ) {
        //the core is movable

        //revive core for simulation
	this->ensure_core_to_be_in_simulation();

        //EMAURER
        environment->sim_unit_mem->start_memory_transaction();
	
	//change all friction mindist values (material may have changed)
	for(d = this->objects.len()-1;d>=0;d--) {
	    IVP_Real_Object *r_obj=this->objects.element_at(d);

	    IVP_Contact_Point *fr_mindist;
	    IVP_Synapse_Friction *fr_synapse;
	    IVP_Friction_System *fs;
	    fr_synapse=r_obj->get_first_friction_synapse();
	    if( fr_synapse ) {
		fr_mindist=fr_synapse->get_contact_point();
		fs=fr_mindist->l_friction_system;
	    }
	    
	    for(;fr_synapse;fr_synapse=fr_synapse->get_next()) {
		fr_mindist=fr_synapse->get_contact_point();
		IVP_Friction_System *next_fs=fr_mindist->l_friction_system;
		if(next_fs != fs) {
		    //this->environment->get_friction_manager()->fusion_friction_systems(fs,next_fs);
		    fs->fusion_friction_systems(next_fs);
		}
		IVP_Impact_Solver_Long_Term info;
		fr_mindist->recalc_friction_s_vals();
		fr_mindist->read_materials_for_contact_situation(&info); //this is only done to calculate friction_factor ; maybe make extra function for friction_factor
		fr_mindist->calc_virtual_mass_of_mindist();
		
	    }
	}
	//EMAURER
        environment->sim_unit_mem->end_memory_transaction();    
    } else {
        //the core is unmovable
      
	this->stop_movement_without_collision_recheck();

	//change friction values and revive other adjacent cores
	revive_adjacent_to_unmoveable();
	
	//this loop is slow as it is quadratic in number of fr_mindists
	for(d = this->objects.len()-1;d>=0;d--) {
	    IVP_Real_Object *r_obj=this->objects.element_at(d);
	    IVP_Synapse_Friction *fr_synapse,*next_syn;
	    IVP_Contact_Point *fr_mindist;
	    fr_synapse=r_obj->get_first_friction_synapse();
	    while( fr_synapse ) {
		next_syn=fr_synapse->get_next();
		fr_mindist=fr_synapse->get_contact_point();
		IVP_Core *other_core;
		other_core=fr_mindist->get_synapse(0)->l_obj->get_core();
		if( other_core == this ) {
		    other_core=fr_mindist->get_synapse(1)->l_obj->get_core();
		}
		
		fr_synapse=next_syn;
	    }
	}
    }
    //EMAURER
    //environment->sim_unit_mem->end_memory_transaction();    
}

IVP_DOUBLE IVP_Core::calc_correct_virt_mass(const IVP_U_Float_Point *core_point,const IVP_U_Float_Point *direction_core,const IVP_U_Float_Point *direction_world) const {
    if(!pinned) //@@CBPIN
    {
	IVP_U_Float_Point ret_center_speed,ret_rot_speed;
	this->test_push_core(core_point,direction_core,direction_world,&ret_center_speed,&ret_rot_speed);

	IVP_U_Float_Point ret_world_speed;
	this->get_surface_speed_on_test(core_point,&ret_center_speed,&ret_rot_speed,&ret_world_speed);
	IVP_DOUBLE speed_after_test_push=ret_world_speed.real_length();
	return 1.0f/speed_after_test_push;
    }
    else
    {
	return 1.0f;
    }
}

void p_calc_2d_distances_to_axis(const IVP_U_Float_Point *point,const IVP_U_Float_Point *direction, IVP_U_Float_Point *distances){
/* estimate (2d) distance of straight(point,direction) to 0,0,0
 * after setting all k[0],k[1], k[2] to zero */
    IVP_DOUBLE qlen = direction->real_length();
    if (qlen > P_DOUBLE_EPS){
	IVP_DOUBLE iqlen = 1.0f/qlen;
	distances->calc_cross_product( direction, point);
	distances->mult(iqlen);
    }else{
	distances->set_to_zero();
    }
} 


IVP_DOUBLE IVP_Core::calc_virt_mass(const IVP_U_Float_Point *core_point,const  IVP_U_Float_Point *direction)const{
    /* calculates the virtual mass of on object at point point
     * and direction direction_cs, if direction == 0 than get mimimal mass */
    IVP_U_Float_Point distances_2d;
    const IVP_Core *pc = this;
    IVP_DOUBLE dv;
    if (direction){