ivp_mindist_event.cxx

hl2 source code. Do not use it illegal.

    /** second: when does any neighbor edge of P lead to a KK case? **/
    IVP_DOUBLE dist_PK_now = mindist->get_length();
    IVP_DOUBLE max_dev;
    {
	// deviation for s:
	// s is sin(  angle( Lot(P,K), P_next ) )
	// s' < angle( Lot(P,K), P_next ) '
	// s' < |omega1| + |omega0| + Lot(P,K)'
	// Lot' < (|speed K| + |speed P|) / distance
	// Note: Lot' does not use |omegax|, because it is independent from rotation of objects
	
	    IVP_DOUBLE mindist_PK = dist_PK_now - this->worst_case_speed * (event_time_out-t_now);
	    if (mindist_PK < P_RES_EPS) mindist_PK = P_RES_EPS;

	    const IVP_U_Float_Point *spp= p_object;
	    IVP_DOUBLE speed_P = spp->fast_real_length() * solver_P->abs_omega + m_cache_P->get_core()->current_speed;
	    IVP_DOUBLE speed_K = m_cache_K->get_core()->max_surface_rot_speed * solver_K->abs_omega + m_cache_K->get_core()->current_speed;
	    max_dev = (speed_P + speed_K) / mindist_PK + sum_abs_omega;
    }

    {
	IVP_3D_Solver_PK_KK solver_pk_kk;	// calcs: - s_val * tmp.common.real_len
//	const IVP_U_Float_Point *p_object = IVP_CLS.give_object_coords(P,m_cache_P);
	const IVP_U_Float_Point *k_object = IVP_CLS.give_object_coords(K,m_cache_K);
	const IVP_U_Float_Point *kn_object = IVP_CLS.give_object_coords(K->get_next(),m_cache_K);

	solver_pk_kk.P_object.set(p_object);
	solver_pk_kk.K_object.set(k_object);
	solver_pk_kk.K_vec_object.subtract(kn_object,k_object);
	solver_pk_kk.K_vec_object.fast_normize();

	solver_pk_kk.set_max_deviation(max_dev);
	IVP_DOUBLE dist;
	IVP_DOUBLE coll_dist = mindist->get_coll_dist();
	if (dist_PK_now < coll_dist){
	    dist = dist_PK_now;
	}else{
	    dist = coll_dist;
	}
	// exponentially go 
	IVP_DOUBLE sec_dist = -0.3f * dist * (m_cache_K->get_core()->inv_object_diameter * 2.0f);
	
	for(const IVP_Compact_Edge *e=P->get_prev()->get_opposite(); 1; e=e->get_prev()->get_opposite()){
	  const IVP_U_Float_Point *en_object = IVP_CLS.give_object_coords(e->get_next(),m_cache_P);
	  
	    solver_pk_kk.P_ne_vec_object.subtract( en_object, p_object);
	    solver_pk_kk.P_ne_vec_object.fast_normize();

	    ivp_u_bool found_flag =
		solver_pk_kk.find_first_t_for_value_max_dev( sec_dist,   t_now, event_time_out, 0, 
							     &cache_P, &cache_K, 0 , &event_time_out );
	    if (found_flag){
		event_type_out = IVP_COLL_PK_KK;
	    }	
	    if(e==P) break;
	}
    }
}

/***************************** PK ******************************************/
/***************************** PK ******************************************/
void IVP_Mindist_Event_Solver::calc_next_event_BK(IVP_Ball * ball,const  IVP_Compact_Edge *K,
						    IVP_Cache_Object *m_cache_B,
						    IVP_Cache_Ledge_Point *m_cache_K){


    IVP_U_Matrix_Cache cache_P(m_cache_B);
    IVP_U_Matrix_Cache cache_K(m_cache_K->clp_cache_object);
    
    IVP_DOUBLE sum_abs_omega;
    IVP_Core *solver_P = ball->get_core();
    IVP_Core *solver_K = m_cache_K->get_core();
    sum_abs_omega = solver_P->abs_omega + solver_K->abs_omega;

    event_time_out =  t_max;

    IVP_U_Point p_object; p_object.set_to_zero();
    
    // check collision of point and edge
    {
	IVP_3D_Solver_PK_COLL solver_pk_coll;
	const IVP_U_Float_Point *k_object =  IVP_CLS.give_object_coords(K,m_cache_K);
	const IVP_U_Float_Point *kn_object =  IVP_CLS.give_object_coords(K->get_next(),m_cache_K);

	solver_pk_coll.P_object.set( &p_object );
	solver_pk_coll.K_object.set( k_object);
	solver_pk_coll.K_vec_object.subtract( kn_object,  k_object);
	solver_pk_coll.K_vec_object.fast_normize();
	solver_pk_coll.set_max_deviation(this->max_coll_speed);
	IVP_FLOAT radius = mindist->sum_extra_radius;
	solver_pk_coll.plane_offset = 0.5f * ( mindist->get_coll_dist() + radius);

	// find initial virtual collision plane
	{
	    IVP_U_Point K_world;
	    IVP_U_Point K_vec_world;	// normized direction
	    IVP_U_Point *P_world = m_cache_B->m_world_f_object.get_position();

	    m_cache_K->get_object_cache()->transform_position_to_world_coords( &solver_pk_coll.K_object,  &K_world);
	    m_cache_K->get_object_cache()->transform_vector_to_world_coords(   &solver_pk_coll.K_vec_object, &K_vec_world);

	    IVP_U_Point PK;
	    PK.subtract(&K_world,P_world);
	    IVP_U_Point H;
	    H.calc_cross_product( &PK, &K_vec_world);

	    m_cache_K->get_object_cache()->transform_vector_to_object_coords(&H, &solver_pk_coll.K_Lot_object);
	    solver_pk_coll.K_Lot_object.normize();
	}
	
	ivp_u_bool found =
	    solver_pk_coll.find_first_t_for_value_coll( mindist->get_coll_dist() + radius,
							ivp_mindist_settings.real_coll_dist + radius * 0.9f,
 							t_now, t_max,  
							&cache_P, &cache_K, 0, &event_time_out );
	    if (found){
		event_type_out = IVP_COLL_PK_COLL;
	    }    
    }


    /***** find next sector change *****/

    /** first: when does case 'P over F0/F1 of K' occur? **/
    
    // both areas F0/F1
    {
	IVP_3D_Solver_PF_COLL solver_pk_pf;
	IVP_ASSERT( worst_case_speed > max_coll_speed );
	solver_pk_pf.set_max_deviation(this->worst_case_speed);	// when does point P move through lot area

	solver_pk_pf.point_object.set(&p_object);
	
	const IVP_Compact_Edge *FF[2];
	FF[0] = K;
	FF[1] = K->get_opposite();
    
	int i;
	for(i=0; i<=1; i++){
	    // Generation of  collision surface for PF sector  lot = (hesse) x edge 0
	    const IVP_Compact_Edge *F = FF[i]; // area to consider
	    IVP_U_Point v0;
	    const IVP_U_Float_Point *f_object = IVP_CLS.give_object_coords(F,m_cache_K);
	    const IVP_U_Float_Point *fn_object = IVP_CLS.give_object_coords(F->get_next(),m_cache_K);
	    const IVP_U_Float_Point *fp_object = IVP_CLS.give_object_coords(F->get_prev(),m_cache_K);
	    v0.subtract( fn_object, f_object );// @@@ already calculated	    
	    IVP_U_Point fhesse_vec;
	    fhesse_vec.inline_set_vert_to_area_defined_by_three_points(f_object, fp_object, fn_object);
	    solver_pk_pf.hesse_of_area_object.calc_cross_product(&v0, &fhesse_vec);
	    solver_pk_pf.hesse_of_area_object.normize();	// @@@ fast normize 
	    
	    solver_pk_pf.point_of_area_object.set(f_object);
	    
	    IVP_DOUBLE sec_dist = -ivp_mindist_settings.mindist_change_force_dist;
	    ivp_u_bool found_flag =
		solver_pk_pf.find_first_t_for_value_max_dev( sec_dist,   t_now, event_time_out, 0, 
							     &cache_P, &cache_K, 0 , &event_time_out );
	    if (found_flag){
		event_type_out = IVP_COLL_PK_PF;
	    }	
	}
    }
    
}


void IVP_Mindist_Event_Solver::next_event_B_POLY(IVP_Mindist_Event_Solver *mim)
{    
    IVP_Synapse_Real *syn0;
    IVP_Synapse_Real *syn1;
    syn0 = mim->mindist->get_synapse(0);
    syn1 = mim->mindist->get_synapse(1);    

    const IVP_Compact_Edge *e1 = syn1->edge;
    IVP_Ball *ball = syn0->get_object()->to_ball();
    IVP_Cache_Object *m_cache_0 = ball->get_cache_object();
    IVP_Cache_Ledge_Point m_cache_1(syn1->get_object()->to_poly(),e1->get_compact_ledge());
    
    mim->event_type_out = IVP_COLL_NONE;
#ifdef DEBUG    
    m_cache_1.tmp.synapse = NULL;
#endif    

    
    switch(syn1->get_status()){  
    case IVP_ST_POINT:
	mim->calc_next_event_BP( ball, e1, m_cache_0, &m_cache_1);
	break;
    case IVP_ST_EDGE:
	mim->calc_next_event_BK( ball, e1, m_cache_0, &m_cache_1);
	break;
    case IVP_ST_TRIANGLE:
	mim->calc_next_event_BF( e1, m_cache_0, &m_cache_1);
	break;
    default:
	CORE;
    };

    m_cache_0->remove_reference();
    m_cache_1.remove_reference();

}

void IVP_Mindist_Event_Solver::next_event_BB(IVP_Mindist_Event_Solver *mim)
{    
    IVP_Synapse_Real *syn0;
    IVP_Synapse_Real *syn1;
    syn0 = mim->mindist->get_synapse(0);
    syn1 = mim->mindist->get_synapse(1);    

    IVP_Ball *ball0 = syn0->get_object()->to_ball();
    IVP_Ball *ball1 = syn1->get_object()->to_ball();
    IVP_Cache_Object *m_cache_0 = ball0->get_cache_object();
    IVP_Cache_Object *m_cache_1 = ball1->get_cache_object();
    
    mim->event_type_out = IVP_COLL_NONE;

    mim->calc_next_event_BB( m_cache_0, m_cache_1);

    m_cache_0->remove_reference();
    m_cache_1->remove_reference();
}



void IVP_Mindist_Event_Solver::next_event_default_poly_poly(IVP_Mindist_Event_Solver *mim)
{    
    IVP_Synapse_Real *syn0, *syn1;
    syn0 = mim->mindist->get_sorted_synapse(0);
    syn1 = mim->mindist->get_sorted_synapse(1);    

    const IVP_Compact_Edge *e0 = syn0->edge;
    const IVP_Compact_Edge *e1 = syn1->edge;
    
    IVP_Cache_Ledge_Point m_cache_0(syn0->get_object()->to_poly(),e0->get_compact_ledge()); // @@@ faster way? -OG
    IVP_Cache_Ledge_Point m_cache_1(syn1->get_object()->to_poly(),e1->get_compact_ledge());

#ifdef DEBUG    
    m_cache_0.tmp.synapse = NULL;	// remember order of synapses
    m_cache_1.tmp.synapse = NULL;
#endif    
    
    mim->event_type_out = IVP_COLL_NONE;

    
    switch(syn0->get_status()){
      case IVP_ST_POINT:{
	  switch(syn1->get_status()){  
	  case IVP_ST_POINT:{	
	      mim->calc_next_event_PP(e0, e1, &m_cache_0, &m_cache_1);
	      break;
	    }
	    case IVP_ST_EDGE:{
	      mim->calc_next_event_PK(e0, e1, &m_cache_0, &m_cache_1);
	      break;
	    }
	    case IVP_ST_TRIANGLE:{
	      mim->calc_next_event_PF(e0, e1, &m_cache_0, &m_cache_1);
	      break;
	    }
	    default:
		CORE;
	  }
	  break;
      };
      case IVP_ST_EDGE:{
	  switch(syn1->get_status()){  
	    case IVP_ST_EDGE:{
	      mim->calc_next_event_KK(e0, e1, &m_cache_0, &m_cache_1);
	      break;
	    }
	    default:
		CORE;
	  }
	  break;
      };
    default:
	CORE; // unknown synapse status
    }
    
    m_cache_0.remove_reference();
    m_cache_1.remove_reference();
}



void IVP_Mindist_Event_Solver::next_event_illegal(IVP_Mindist_Event_Solver *){
    // error: inconsistency between IVP_Mindist_Event_Solver and IVP_Mindist_Minimize_Solver
    CORE;
}

void (*IVP_Mindist_Event_Solver::mim_function_table[IVP_ST_MAX_LEGAL][IVP_ST_MAX_LEGAL])(IVP_Mindist_Event_Solver *mim);

void IVP_Mindist_Event_Solver::init_mim_function_table(){
    for (int i=0; i< IVP_ST_MAX_LEGAL; i++){
	for (int j=0; j< IVP_ST_MAX_LEGAL; j++){
	    mim_function_table[i][j] = next_event_illegal;
	}
    }
    mim_function_table[IVP_ST_POINT] [IVP_ST_POINT]    = next_event_default_poly_poly;
    mim_function_table[IVP_ST_POINT] [IVP_ST_EDGE]     = next_event_default_poly_poly;
    mim_function_table[IVP_ST_POINT] [IVP_ST_TRIANGLE] = next_event_default_poly_poly;
    mim_function_table[IVP_ST_POINT] [IVP_ST_BALL]     = next_event_illegal;

    mim_function_table[IVP_ST_EDGE] [IVP_ST_POINT]    = next_event_illegal;
    mim_function_table[IVP_ST_EDGE] [IVP_ST_EDGE]     = next_event_default_poly_poly;
    mim_function_table[IVP_ST_EDGE] [IVP_ST_TRIANGLE] = next_event_illegal;
    mim_function_table[IVP_ST_EDGE] [IVP_ST_BALL]     = next_event_illegal;

    mim_function_table[IVP_ST_TRIANGLE] [IVP_ST_POINT]    = next_event_illegal;
    mim_function_table[IVP_ST_TRIANGLE] [IVP_ST_EDGE]     = next_event_illegal;
    mim_function_table[IVP_ST_TRIANGLE] [IVP_ST_TRIANGLE] = next_event_illegal;
    mim_function_table[IVP_ST_TRIANGLE] [IVP_ST_BALL]     = next_event_illegal;

    mim_function_table[IVP_ST_BALL] [IVP_ST_POINT]    = next_event_B_POLY;
    mim_function_table[IVP_ST_BALL] [IVP_ST_EDGE]     = next_event_B_POLY;
    mim_function_table[IVP_ST_BALL] [IVP_ST_TRIANGLE] = next_event_B_POLY;
    mim_function_table[IVP_ST_BALL] [IVP_ST_BALL]     = next_event_BB;
}