ivp_mindist_mcases.cxx

hl2 source code. Do not use it illegal.

#include <ivp_physics.hxx>

#include <ivu_float.hxx>

#include <ivp_mindist_intern.hxx>
#include <ivp_mindist_minimize.hxx>

#include <ivp_compact_ledge.hxx>
#include <ivp_cache_object.hxx>
#include <ivp_cache_ledge_point.hxx>
#include <ivp_compact_ledge_solver.hxx>

// IVP_EXPORT_PRIVATE

/***************************** PF ******************************************/
/***************************** PF ******************************************/
/***************************** PF ******************************************/
/***************************** PF ******************************************/
/***************************** PF ******************************************/
/***************************** PF ******************************************/

// Vertex to Surface

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_PF(const IVP_Compact_Edge *P,
							const IVP_Compact_Edge *F,
							IVP_Cache_Ledge_Point *m_cache_P,
							IVP_Cache_Ledge_Point *m_cache_F)
{
    // Case: Vertex / Surface
    
    // F (the surface) is defined by Q, R (and S)
    // where (Q=F, R=F->next, S=F->prev)
    
    // Checks situation (is P above E?) and takes the appropriate branch.
    
    IVP_ASSERT(m_cache_P->tmp.synapse == mindist->get_sorted_synapse(0));    
#ifdef DEBUG_CHECK_LEN
    check_len_PF(P,F, m_cache_P, m_cache_F);
#endif
    IVP_U_Point p_Fos;
    IVP_CLS.calc_pos_other_space( P, m_cache_P, m_cache_F, &p_Fos);
    
    IVP_Unscaled_QR_Result qr;
    const IVP_Compact_Ledge *c_ledge = m_cache_F->get_compact_ledge();
    IVP_CLS.calc_unscaled_qr_vals_F_space( c_ledge, F, &p_Fos, &qr);

    if ( ! qr.is_outside() ){
	return p_minimize_Leave_PF(P, &p_Fos, F, m_cache_P, m_cache_F);
    }

    // search s_values to find best edge
    const IVP_Compact_Edge *e;
    int j;
    
 // find out the best edge to walk to
    IVP_DOUBLE found_minimal_dist = P_DOUBLE_MAX;
    const IVP_Compact_Edge *min_edge = 0;
    for (e=F,j=0; j<3; e=e->get_next(),j++){
	// if (checks[j] > 0 ) continue; /* inside triangle */
	IVP_DOUBLE dist = IVP_CLS.calc_qlen_PK_K_space( &p_Fos, m_cache_F->get_compact_ledge(), e);
	if(dist < found_minimal_dist){
	    found_minimal_dist = dist;
	    min_edge = e;
	}
    }
    IVP_ASSERT(min_edge);
    return p_minimize_PK( P, min_edge, m_cache_P, m_cache_F);   // s val in range
}


IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_BF(IVP_Cache_Ball *m_cache_B, const IVP_Compact_Edge *F,
				    IVP_Cache_Ledge_Point *m_cache_F)
{
    // Case: Ball - Surface
    
    // F (the surface) is defined by Q, R (and S)
    // where (Q=F, R=F->next, S=F->prev)
    
    // if status indicates friction mode, point is always kept the same
    


    IVP_U_Point p_Fos;
    IVP_U_Point *center_B = m_cache_B->cache_object->m_world_f_object.get_position();
    m_cache_F->clp_cache_object->transform_position_to_object_coords(center_B, &p_Fos);
    
    IVP_Unscaled_QR_Result qr;
    const IVP_Compact_Ledge *c_ledge = m_cache_F->get_compact_ledge();
    IVP_CLS.calc_unscaled_qr_vals_F_space( c_ledge, F, &p_Fos, &qr);

    if ( ! qr.is_outside() ){
	const IVP_U_Float_Point *F_O_os = IVP_CLS.give_object_coords(F,m_cache_F);
    
	IVP_U_Point wHesse_vecF_os;
	IVP_CLS.calc_hesse_vec_object_not_normized(F, m_cache_F->get_compact_ledge(), &wHesse_vecF_os); // should be normized: see below
	wHesse_vecF_os.normize();
    IVP_U_Point contact_plane;
	m_cache_F->get_object_cache()->transform_vector_to_world_coords( &wHesse_vecF_os, &contact_plane);
	mindist->contact_plane.set(&contact_plane);

	IVP_DOUBLE dist_plus_radius = p_Fos.dot_product( &wHesse_vecF_os ) -  wHesse_vecF_os.dot_product(F_O_os);
	m_cache_F->tmp.synapse->update_synapse(F, IVP_ST_TRIANGLE);
	if (dist_plus_radius < 0.0f){	// check for backside
	    m_cache_F->tmp.synapse->update_synapse(F, IVP_ST_BACKSIDE);
	    this->pos_opposite_BacksideOs.set(&p_Fos);
	    return IVP_MRC_BACKSIDE;
	}
	mindist->len_numerator = dist_plus_radius - mindist->sum_extra_radius;

#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED	
	IVP_U_Float_Point diff_center;
	diff_center.subtract( &m_cache_B->cache_object->core_pos, &m_cache_F->get_object_cache()->core_pos );
	mindist->contact_dot_diff_center = diff_center.dot_product(&mindist->contact_plane);
#endif
	return IVP_MRC_OK;
    }

    // search s_values to find best edge
    const IVP_Compact_Edge *e;
    int j;
    
    // find out the best edge to walk to
    IVP_DOUBLE found_minimal_dist = P_DOUBLE_MAX;
    const IVP_Compact_Edge *min_edge = 0;
    for (e=F,j=0; j<3; e=e->get_next(),j++){
	IVP_DOUBLE dist = IVP_CLS.calc_qlen_PK_K_space(&p_Fos, m_cache_F->get_compact_ledge(), e);
	if(dist < found_minimal_dist){
	    found_minimal_dist = dist;
	    min_edge = e;
	}
    }
    IVP_ASSERT(min_edge);
    return p_minimize_BK( m_cache_B, min_edge, m_cache_F);
}




IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_Leave_PF(const IVP_Compact_Edge *P, const IVP_U_Point *P_Fos,
							      const IVP_Compact_Edge *F,
							      IVP_Cache_Ledge_Point *m_cache_P,
							      IVP_Cache_Ledge_Point *m_cache_F)
{
    // just searching a neighbour point of P, which is nearer to F than P,
    // or, if there is no such point within triangle F, look for nearer edge

    if(--P_Finish_Counter < 0){
	if( check_loop_hash(IVP_ST_POINT, P, IVP_ST_TRIANGLE, F) ){
	    return IVP_MRC_ENDLESS_LOOP;
	}
    }

    
    IVP_U_Point hesse_vec_Pos;
 
    IVP_DOUBLE back_side_check_distance = 0.0f; //-0.5f * ivp_mindist_settings.coll_dist;	// when should we treat surface as backside
    {
	IVP_U_Point wHesse_vecF_Fos;
	IVP_CLS.calc_hesse_vec_object_not_normized(F, m_cache_F->get_compact_ledge(), &wHesse_vecF_Fos); // should be normized: see below
	wHesse_vecF_Fos.fast_normize();
	IVP_U_Point hesse_vec_ws;
	m_cache_F->get_object_cache()->transform_vector_to_world_coords( &wHesse_vecF_Fos, &hesse_vec_ws);
	m_cache_P->get_object_cache()->transform_vector_to_object_coords( &hesse_vec_ws, &hesse_vec_Pos);
    
	// if backside than reverse hesse
	const IVP_U_Float_Point *F_O_Fos = IVP_CLS.give_object_coords(F,m_cache_F);
	mindist->len_numerator = wHesse_vecF_Fos.dot_product( P_Fos ) -  wHesse_vecF_Fos.dot_product(F_O_Fos);
	mindist->contact_plane.set( &hesse_vec_ws);
	
	if (mindist->len_numerator < back_side_check_distance){
	    hesse_vec_Pos.mult(-1.0f);
	}

	mindist->len_numerator -= mindist->sum_extra_radius;

#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED	
	IVP_U_Float_Point diff_center;
	diff_center.subtract( &m_cache_P->get_object_cache()->core_pos, &m_cache_F->get_object_cache()->core_pos );
	mindist->contact_dot_diff_center = diff_center.dot_product(&mindist->contact_plane);
#endif
    }

#ifdef DEBUG_CHECK_LEN
    check_len_PF(P,F, m_cache_P, m_cache_F);	// check wether world_pos_hash is afflicted
#endif
	
    IVP_DOUBLE min_d_pos = 0.0f;
    const IVP_Compact_Edge *min_edge = NULL;
    {
	// loop through all neighbour points of point and search a nearer one
	// backsides are treated correctly (excluded)
	const IVP_Compact_Poly_Point *we = IVP_CLS.give_object_coords(P,m_cache_P);

	const IVP_Compact_Edge *Pm = P->get_prev();
	const IVP_Compact_Edge *e = Pm->get_opposite()->get_prev();

	for( ; 1; e=e->get_opposite()->get_prev()){ 
	    const IVP_Compact_Poly_Point *wenext = IVP_CLS.give_object_coords(e,m_cache_P);
	    IVP_U_Point vec_Pos;
	    
	    vec_Pos.subtract(wenext, we);
	    IVP_DOUBLE dist = hesse_vec_Pos.dot_product(&vec_Pos);
	    if (dist < 0){
		IVP_DOUBLE e_qlen = vec_Pos.quad_length();
		dist *= IVP_Inline_Math::isqrt_float(e_qlen);
		if(dist  < min_d_pos ){
		    min_edge = e;
		    min_d_pos = dist;
		}
	    }
	    if(e==Pm) break;
	}
    }
    if(min_edge == NULL){ // no nearer point found
	// END: vertex - surface
      
	m_cache_P->tmp.synapse->update_synapse(P, IVP_ST_POINT);
	m_cache_F->tmp.synapse->update_synapse(F, IVP_ST_TRIANGLE);

	if (mindist->len_numerator + mindist->sum_extra_radius <  back_side_check_distance){	// check for backside
	  this->pos_opposite_BacksideOs.set(P_Fos);
	  m_cache_F->tmp.synapse->update_synapse(F, IVP_ST_BACKSIDE);
	  return IVP_MRC_BACKSIDE;
	}
	return IVP_MRC_OK;
    }

    // Is nearest/best P within triangle ?
    // if not, find direction to go (KK case). 
    // remark, this also works if we are already behind the area !!!!
    
    const IVP_Compact_Edge *P2 = min_edge;
    
    // calc direction to go
    IVP_Unscaled_QR_Result qr;
    IVP_U_Point P2_Fos;
    IVP_CLS.calc_pos_other_space(P2, m_cache_P, m_cache_F, &P2_Fos);
    IVP_CLS.calc_unscaled_qr_vals_F_space(F->get_compact_ledge(), F, &P2_Fos, &qr);

    // when new point is located within area -> new PF case with same F
    if ( ! qr.is_outside() ){
	return p_minimize_Leave_PF(P2,&P2_Fos, F, m_cache_P, m_cache_F);
    }


    // now that we are outside, let's search for exactly one edge e of F with these restrictions:
    // 		1. new P is outside e
    //		2. shortest distance between e and P_NP is on e ( 0 < s_val < 1);
    
    int violated_q_rs = 0;
    { // count number of violated q+rs
	for (int j = 0;j<3;j++){
	    if (qr.checks[j] >= 0) continue;
	    violated_q_rs ++;
	}
    }
    // first try to fast find a KK case, as only one qr val is violated
    if (violated_q_rs == 1){
	int j;
	const IVP_Compact_Edge *e;
	for (e=F,j=0; j<3; e=e->get_next(),j++){
	    if (qr.checks[j] >= 0 ) continue;  // inside triangle
	    // check all sides of old area for crossing of new edge and area side
	    return p_minimize_KK(min_edge,e, m_cache_P, m_cache_F);
	}
    }
    IVP_ASSERT( violated_q_rs == 2);
    // now real checking of len
    const IVP_Compact_Edge *best_edge = NULL;
    IVP_DOUBLE best_len = P_DOUBLE_MAX;
    {
	int j;
	const IVP_Compact_Edge *e;
	for (e=F,j=0; j<3; e=e->get_next(),j++){
	    if (qr.checks[j] > 0 ) continue;  // on correct side of triangle, no check needed
	    IVP_DOUBLE len = IVP_CLS.calc_qlen_KK(min_edge, e, m_cache_P, m_cache_F);
	    if (len < best_len){
		best_len = len;
		best_edge = e;
	    }
	}
    }
    return p_minimize_KK(min_edge,best_edge, m_cache_P, m_cache_F);
} // p_minimize_Leave_PF


// OKOKOK


/***************************** KK ******************************************/
/***************************** KK ******************************************/
/***************************** KK ******************************************/
/***************************** KK ******************************************/
/***************************** KK ******************************************/


// Case: Edge - Edge

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_KK(const IVP_Compact_Edge *K, const IVP_Compact_Edge *L,
							IVP_Cache_Ledge_Point *m_cache_K,
							IVP_Cache_Ledge_Point *m_cache_L)
{
    // Calculate next distance and next iteration situation.
    // Use sk und sl are scaling values:
    // in the edge's space: ]0,1[ means inside,
    // pk+ is pk1, if sk>=1, pk- is the other point


    IVP_KK_Input kkin( K, L, m_cache_K, m_cache_L);
    IVP_Unscaled_KK_Result kkr;
    IVP_CLS.calc_unscaled_KK_vals(kkin, &kkr);

    IVP_ASSERT(m_cache_K->tmp.synapse == mindist->get_sorted_synapse(0));
#ifdef DEBUG_CHECK_LEN
    check_len_KK(K,L,m_cache_K, m_cache_L);
#endif
    
    if( !kkr.is_outside_L()) {     
	if ( kkr.checks_K[0] < 0.0f){
	    return p_minimize_PK(K, L, m_cache_K, m_cache_L);
	}
	if ( kkr.checks_K[1] < 0.0f){
	    return p_minimize_PK(K->get_next(), L,  m_cache_K, m_cache_L);
	}
	return p_minimize_Leave_KK(K, L, kkin, &kkr, m_cache_K, m_cache_L);
    }
    
    if( !kkr.is_outside_K()) {
	sort_synapses(m_cache_L->tmp.synapse, m_cache_K->tmp.synapse);

	if(kkr.checks_L[0] < 0.0f){		// sl is already checked for interval [0 .. 1], now check side only
	    return p_minimize_PK(L, K, m_cache_L, m_cache_K);
	}else{
	    return p_minimize_PK(L->get_next(), K, m_cache_L, m_cache_K);
	}
    }

    // Now minimal dist is outside of both edges, meaning that the lines have the nearest distance outside the edges
    // before switching to PP case we have to check whether a PK case
    // should be called

    const IVP_Compact_Edge *pkp, *plp, *plm, *pkm;
    // search point of K which is nearer to L then K->next: == sort points
    if(kkr.checks_K[0]< kkr.checks_K[1]){
	pkp = K;
	pkm = K->get_next();
    }else{
	pkp = K->get_next();
	pkm = K;
    }
    if(kkr.checks_L[0] < kkr.checks_L[1]){
	plp = L;
	plm = L->get_next();
    }else{
	plp = L->get_next();
	plm = L;
    }

    IVP_U_Point pkp_Los;
    IVP_CLS.calc_pos_other_space( pkp, m_cache_K, m_cache_L, &pkp_Los);
    IVP_Unscaled_S_Result sr_kL;
    IVP_CLS.calc_unscaled_s_val_K_space(m_cache_L->get_compact_ledge(), L, & pkp_Los, &sr_kL);

    if(!sr_kL.is_outside()){
	return p_minimize_PK(pkp, L, m_cache_K, m_cache_L);
    }

    IVP_U_Point plp_Kos;
    IVP_CLS.calc_pos_other_space( plp, m_cache_L, m_cache_K, &plp_Kos);
    IVP_Unscaled_S_Result sr_lK;
    IVP_CLS.calc_unscaled_s_val_K_space(m_cache_K->get_compact_ledge(), K, & plp_Kos, &sr_lK);


    if(!sr_lK.is_outside()){
	sort_synapses(m_cache_L->tmp.synapse,m_cache_K->tmp.synapse);
	return p_minimize_PK(plp, K, m_cache_L, m_cache_K);
    }
    
    if( sr_kL.checks[0] * kkr.checks_L[0] < 0.0f){
	return p_minimize_PP(pkp, plm, m_cache_K, m_cache_L);
    }