ivp_mindist_friction.cxx

hl2 source code. Do not use it illegal.

        last_gap_len = length; 
	info->surf_normal.set_multiple(&world_diff_vec, 1.0f/length);
    }else{
        last_gap_len = 0.0f; 
	info->surf_normal.set_multiple(&norm, 1.0f/ IVP_Inline_Math::ivp_sqrtf(quad_dist));
    }

    info->span_friction_v[0].set(&Kvec);
    info->contact_point_ws.set(&world_edge_space0);

    if (cp_status == IVP_CPBS_NEEDS_RECHECK){
	this->cp_status = IVP_CPBS_NORMAL;
	if (sl < 0.0f || sl > 1.0f || sk < 0.0f || sk > 1.0f) {
	    info->friction_is_broken = IVP_TRUE;
	}
    }

#ifdef IVP_USE_S_VALS_FOR_PRETENSION        
    IVP_DOUBLE old_sk = s_coords[0];
    IVP_DOUBLE old_sl = s_coords[1];
    diff_contact_vec->set_multiple(&vec0,sk - old_sk);
    diff_contact_vec->add_multiple(&vec1,old_sl - sl);    
    s_coords[0] = sk;
    s_coords[1] = sl;
#endif  

  } else {
      info->friction_is_broken = IVP_TRUE;
      diff_contact_vec->set_to_zero();
      last_gap_len = ivp_mindist_settings.keeper_dist;
      info->contact_point_ws.set(&Kp);
      info->surf_normal.set(1.0f,0.0f,0.0f);
      info->span_friction_v[0].set(0.0f,1.0f,0.0f);
      sk=0.0f;
      sl=0.0f;
  }
}

void IVP_Contact_Point::reset_time(IVP_Time offset){
    last_time_of_recalc_friction_s_vals -= offset;
}

void IVP_Contact_Point::recalc_friction_s_vals(){
    IVP_Environment *env = get_synapse(0)->get_object()->get_environment();
    
    IVP_Impact_Solver_Long_Term *info = (IVP_Impact_Solver_Long_Term*)env->get_sim_unit_mem()->get_mem_transaction(sizeof(IVP_Impact_Solver_Long_Term));
    info->init_tmp_contact_info();
    this->tmp_contact_info = info;
    	    
    IVP_Synapse_Friction *syn0 = this->get_synapse(0);
    IVP_Synapse_Friction *syn1 = this->get_synapse(1);
    
    env->get_statistic_manager()->processed_fmindists++;

    IVP_SYNAPSE_POLYGON_STATUS stat0 = syn0->get_status();
    IVP_SYNAPSE_POLYGON_STATUS stat1 = syn1->get_status();
    
    // set: 	this->info.gap_distance, info->surf_normal, info->contact_points_ws[0],
    //		this->span_friction_v[0]
    //          [and add to diff_vec_contact]
    { 
      IVP_U_Float_Point diff_vec_contact; diff_vec_contact.set_to_zero();
	IVP_Cache_Ledge_Point m_cache_0;
	IVP_Cache_Ledge_Point m_cache_1;	

	const IVP_Compact_Edge *e1 = syn1->edge;
	m_cache_1.init_cache_ledge_point(syn1->l_obj,e1->get_compact_ledge());
	m_cache_1.tmp.synapse_friction = syn1;

	const IVP_Compact_Edge *e0 = syn0->edge;
	m_cache_0.init_cache_ledge_point(syn0->l_obj,e0->get_compact_ledge());
	m_cache_0.tmp.synapse_friction = syn0;
    
	IVP_U_Point point0_position_ws; // for ball and point cases

	switch(stat0){
	    
	case IVP_ST_BALL:{
	    point0_position_ws.set(m_cache_0.get_object_cache()->m_world_f_object.get_position());
	    this->cp_status = IVP_CPBS_NEEDS_RECHECK;
	    goto ball_same_as_point;
	}
   
	case IVP_ST_POINT:{
	    IVP_CLS.give_world_coords_AT(e0, &m_cache_0, &point0_position_ws);

	ball_same_as_point:	    
	    switch(stat1){
	    case IVP_ST_BALL:{
		IVP_U_Point *tp2 = m_cache_1.get_object_cache()->m_world_f_object.get_position();
		p_calc_friction_s_PP(&point0_position_ws,tp2,info,&diff_vec_contact);
		break;
	    }
	    
	    case IVP_ST_POINT:{
		IVP_U_Point tp2; IVP_CLS.give_world_coords_AT(e1, &m_cache_1, &tp2);
		p_calc_friction_s_PP(&point0_position_ws,&tp2,info,&diff_vec_contact);
		break;
	    }
	    case IVP_ST_EDGE:{
		p_calc_friction_s_PK(&point0_position_ws, e1, &m_cache_1,info,&diff_vec_contact);
		break;
	    }
	    case IVP_ST_TRIANGLE:{
		p_calc_friction_qr_PF(&point0_position_ws, e1, &m_cache_1,info,&diff_vec_contact);
		break;
	    }
	    default:
		CORE;
	    }
	    break;
	};
	case IVP_ST_EDGE:{
	    IVP_ASSERT( syn1->get_status() == IVP_ST_EDGE );
	    p_calc_friction_ss_KK(e0, e1, &m_cache_0, &m_cache_1,info,&diff_vec_contact);
	    break;
	};
	default:
	    CORE;
	}
	{  // reduce len of friction contact by extra radius
	    IVP_FLOAT first_radius = syn0->l_obj->get_extra_radius();
	    info->contact_point_ws.add_multiple(&info->surf_normal, first_radius);
	    last_gap_len -= first_radius + syn1->l_obj->get_extra_radius();
	}

	//TL: workaround for penetrating balls (shifted mass center)
	if ( last_gap_len < 0.0f ) {
		last_gap_len = 0.0f;
	}    
	m_cache_0.remove_reference();
	m_cache_1.remove_reference();
    }
    info->span_friction_v[0].normize();
    info->span_friction_v[1].inline_calc_cross_product(&info->surf_normal,&info->span_friction_v[0]);
        
// TL:
// IVP_Contact_Point needs for later calculations:
//   both points in world coords
//   both points in object coords
//   normized vector in world coords pointing to first synapse
//   two normized vectors in obj coords pointing to synapse
//   ...
// #+# maybe optimize! e.g.: obj-coords of point synapse never change !

    IVP_U_Float_Point delta_velocity_ws;
    {// calculate some values plus pretension for core 0

	IVP_Core *core0  = syn0->get_object()->get_core();
	
	if (!core0->physical_unmoveable) { // && !core0->pinned){ //@@CBPIN
	    IVP_U_Float_Point surface_normal_cs; // pointing to synapse

	    const IVP_U_Matrix *m_world_f_core = core0->get_m_world_f_core_PSI();
	    m_world_f_core->inline_vimult4(&info->contact_point_ws, &info->contact_point_cs[0]);
	    m_world_f_core->inline_vimult3(&info->surf_normal,&surface_normal_cs);

	    info->contact_cross_nomal_cs[0].inline_calc_cross_product(&info->contact_point_cs[0],& surface_normal_cs );
	    core0->get_surface_speed_on_test(&info->contact_point_cs[0],&core0->speed,&core0->rot_speed,&delta_velocity_ws);

	    IVP_U_Float_Point hp; hp.set_pairwise_mult( &info->contact_cross_nomal_cs[0], core0->get_inv_rot_inertia());
	    info->inv_virtual_mass = info->contact_cross_nomal_cs[0].dot_product( &hp) + core0->get_inv_mass();
	}else{
	    core0 = NULL;
	    delta_velocity_ws.set_to_zero();
	    info->inv_virtual_mass = 0.0f;
	    info->contact_point_cs[0].set_to_zero();
	    info->contact_cross_nomal_cs[0].set_to_zero();
	}
	info->contact_core[0] = core0;
    }
    { // core 1
	IVP_Core *core1  = syn1->get_object()->get_core();
	if (!core1->physical_unmoveable) {// && !core1->pinned){ //@@CBPIN
	    IVP_U_Float_Point surface_normal_cs; // pointing to synapse

	    const IVP_U_Matrix *m_world_f_core = core1->get_m_world_f_core_PSI();
	    m_world_f_core->inline_vimult4(&info->contact_point_ws, &info->contact_point_cs[1]);
	    m_world_f_core->inline_vimult3(&info->surf_normal, &surface_normal_cs);

	    info->contact_cross_nomal_cs[1].inline_calc_cross_product(&info->contact_point_cs[1],&surface_normal_cs );

	    IVP_U_Float_Point dh;
	    core1->get_surface_speed_on_test(&info->contact_point_cs[1],&core1->speed,&core1->rot_speed,&dh);
	    delta_velocity_ws.subtract(&dh);

	    IVP_U_Float_Point hp; hp.set_pairwise_mult( &info->contact_cross_nomal_cs[1], core1->get_inv_rot_inertia());
	    info->inv_virtual_mass += info->contact_cross_nomal_cs[1].dot_product( & hp) + core1->get_inv_mass();
	}else{
	    info->contact_point_cs[1].set_to_zero();
	    info->contact_cross_nomal_cs[1].set_to_zero();
	    core1 = NULL;
	}
	info->contact_core[1] = core1;
    }
    info->virtual_mass = 1.0f/ info->inv_virtual_mass;
    {
	IVP_Time ctime = syn0->get_object()->environment->get_current_time();
	IVP_DOUBLE dt = ctime - this->last_time_of_recalc_friction_s_vals;
	this->last_time_of_recalc_friction_s_vals = ctime;

	//vector of v[0]*s[0] + v[1]*s[1] is real world force vector of force that has to be done on first obj (is pointing to second obj)
	this->span_friction_s[0] -= delta_velocity_ws.dot_product(&info->span_friction_v[0]) * dt;
	this->span_friction_s[1] -= delta_velocity_ws.dot_product(&info->span_friction_v[1]) * dt;
    this->last_contact_point_ws.set( &info->contact_point_ws );
	}
}


IVP_Contact_Point *IVP_Friction_Manager::get_associated_contact_point(IVP_Mindist *mindist) {
    // does a contact_point with same objs and position_status already exist? 
    {
	IVP_Synapse_Friction *syn0;
	IVP_Real_Object *obj0 = mindist->get_synapse(0)->get_object();
	IVP_Real_Object *obj1 = mindist->get_synapse(1)->get_object();
	// search all friction mindist, which are connected to obj0
	// start deep search only if obj1 is connected to mindist also
	for (syn0 = obj0->get_first_friction_synapse(); syn0; syn0 = syn0->get_next()){
	    IVP_Contact_Point *md = syn0->get_contact_point();
	    if ( md->get_synapse(0)->get_object() == obj1 ||  md->get_synapse(1)->get_object() == obj1 ){
		if ( md->is_same_as(mindist)){
		    return md;		    
		}
	    }
	}
    }
    return NULL;
}

void IVP_Friction_Manager::delete_all_contact_points_of_object(IVP_Real_Object *obj0) {
    while (IVP_Synapse_Friction *syn0 = obj0->get_first_friction_synapse()){
        IVP_Contact_Point *md = syn0->get_contact_point();
	P_DELETE(md);
    }
}



// generates a friction mindist when no one exists. Is there already a friction dist, the old one is returned
IVP_Contact_Point *IVP_Friction_Manager::generate_contact_point(IVP_Mindist *mindist,IVP_BOOL *successful){
  
	//
	// Valid call with non-exact and Ipion forces a write to null to crash in its IVP_ASSERT!
	//

//    IVP_ASSERT(mindist->mindist_status == IVP_MD_EXACT); // already enabled?
    
	if ( mindist->mindist_status != IVP_MD_EXACT )
	{
		*successful = IVP_FALSE;
		return NULL;
	}

    // at this time, only polygon PF is allowed for friction mindists
    // KK: not yet, PP & PK never

    IVP_Contact_Point *already_exists_fmd = get_associated_contact_point(mindist);
    if(already_exists_fmd) {
        *successful=IVP_FALSE;
        return already_exists_fmd;
    }

// make new friction mindist
    IVP_Contact_Point *friction =  new IVP_Contact_Point(mindist);
    //insert_contact_point(friction);
    //printf("insert_nmd %lx\n",(long)friction&0x0000ffff);

    *successful=IVP_TRUE;
    return friction;
}