ivp_mindist_minimize.cxx

hl2 source code. Do not use it illegal.

		IVP_DOUBLE qdist = IVP_CLS.quad_dist_edge_to_point_K_space(cc_A[i]->get_compact_ledge(), pA, &pB_Aos );
		if ( qdist * (1+P_DOUBLE_RES) < min_qdist){
		  cc_A[i]->tmp.synapse->update_synapse(pA, IVP_ST_EDGE);
		  cc_B[i]->tmp.synapse->update_synapse(pB, IVP_ST_POINT);
		  min_qdist = qdist;
		}
	      }
	    }
	}
    }
    {
	// check all 9 edge edge combinations
	const IVP_Compact_Edge *pA, *pB;
	int a, b;
	for (pA=A,a=0; a<3; pA=pA->get_next(),a++){
	    for (pB=B,b=0; b<3; pB=pB->get_next(),b++){
	      IVP_KK_Input kkin( pA, pB, m_cache_A, m_cache_B);
	      IVP_Unscaled_KK_Result kkr;
	      IVP_CLS.calc_unscaled_KK_vals(kkin, &kkr);
	      if (kkr.is_outside_K()) continue;
	      if (kkr.is_outside_L()) continue;
	      
	      IVP_DOUBLE qdist = kkin.calc_quad_distance_edge_edge();
		if (qdist * (1+P_DOUBLE_RES) < min_qdist){
		    m_cache_A->tmp.synapse->update_synapse(pA, IVP_ST_EDGE);
		    m_cache_B->tmp.synapse->update_synapse(pB, IVP_ST_EDGE);
		    min_qdist = qdist;
		}
	    }
	}
    }

    

    IVP_Synapse_Real *syn0, *syn1, *syn_h;
    syn0 = m_cache_A->tmp.synapse;
    syn1 = m_cache_B->tmp.synapse;
    IVP_Cache_Ledge_Point *m_cache_0, *m_cache_1;

    m_cache_0 = m_cache_A;
    m_cache_1 = m_cache_B;

    
    if((syn1->get_status() == IVP_ST_POINT) && (syn0->get_status() != IVP_ST_POINT)){
	// @@@ one might drop the second && case
	syn_h=syn0; syn0=syn1; syn1 = syn_h;
	m_cache_0 = m_cache_B;
	m_cache_1 = m_cache_A;
    }
    
    // like recalc_mindist sub
    sort_synapses(m_cache_0->tmp.synapse,m_cache_1->tmp.synapse);

    const IVP_Compact_Edge *e0 = syn0->edge;
    const IVP_Compact_Edge *e1 = syn1->edge;

    IVP_ASSERT(m_cache_0->tmp.synapse == syn0);
    IVP_ASSERT(m_cache_1->tmp.synapse == syn1);

    IVP_MRC_TYPE ret_val = IVP_MRC_UNINITIALIZED; // (un)initialize

    switch(syn0->get_status()){
      case IVP_ST_POINT:{
	  switch(syn1->get_status()){  
	    case IVP_ST_POINT:{
		ret_val = p_minimize_PP(e0, e1, m_cache_0, m_cache_1);
		break;
	    }
	    case IVP_ST_EDGE:{
		ret_val =  p_minimize_PK(e0, e1, m_cache_0, m_cache_1);
		break;
	    }
	    case IVP_ST_TRIANGLE:{
	      ret_val = p_minimize_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:{
		ret_val = p_minimize_KK(e0, e1, m_cache_0, m_cache_1);
		break;
	    }
	  default:
	      CORE;
	      break;
	  }
	  break;
      }
      default:
	CORE;
    }
    
    return ret_val;
}


IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_default_poly_poly(IVP_Mindist_Minimize_Solver *mms)
{
    IVP_Synapse_Real *syn0, *syn1;
    
    syn0 = mms->mindist->get_sorted_synapse(0);
    syn1 = mms->mindist->get_sorted_synapse(1);

    // get caches
    IVP_Polygon *poly_0 = syn0->get_object()->to_poly();
    IVP_Polygon *poly_1 = syn1->get_object()->to_poly();
    
    
    const IVP_Compact_Edge *e0 = syn0->edge;
    const IVP_Compact_Edge *e1 = syn1->edge;
    
    IVP_Cache_Ledge_Point m_cache_0(poly_0,e0->get_compact_ledge());
    IVP_Cache_Ledge_Point m_cache_1(poly_1,e1->get_compact_ledge());

    
    m_cache_0.tmp.synapse = syn0;	// remember order of synapses
    m_cache_1.tmp.synapse = syn1;

    IVP_MRC_TYPE ret_val = IVP_MRC_UNINITIALIZED; // (un)initialize

    IVP_IF(ivp_check_debug_mindist(mms->mindist)){
	printf("%32s statii: %i:%i \n","minimize_default_poly_poly", syn0->get_status(), syn1->get_status());
    }

#define SYN_COMBINE(a,b) (a * IVP_ST_MAX_LEGAL + b)
    switch( SYN_COMBINE(syn0->get_status(), syn1->get_status())){
      case SYN_COMBINE(IVP_ST_POINT,IVP_ST_POINT):{
		ret_val = mms->p_minimize_PP(e0, e1, &m_cache_0, &m_cache_1);
		break;
      }
      case SYN_COMBINE(IVP_ST_POINT,IVP_ST_EDGE):{
		ret_val = mms->p_minimize_PK(e0, e1, &m_cache_0, &m_cache_1);
		break;
	    }
      case SYN_COMBINE(IVP_ST_POINT,IVP_ST_TRIANGLE):{
	      ret_val = mms->p_minimize_PF(e0, e1, &m_cache_0, &m_cache_1);
	      break;
	    }
      case SYN_COMBINE(IVP_ST_EDGE,IVP_ST_EDGE):{
		ret_val = mms->p_minimize_KK(e0, e1, &m_cache_0, &m_cache_1);
		break;
	    }
      default:{
	ret_val = mms->p_minimize_FF(e0, e1, &m_cache_0, &m_cache_1);
      }
    }
    
    m_cache_0.remove_reference();
    m_cache_1.remove_reference();

    //mms->proove_polypoly();

    IVP_IF(ivp_check_debug_mindist(mms->mindist)){
      	printf("%32s statii: %i:%i len %f\n", " '' ", syn0->get_status(), syn1->get_status(), mms->mindist->get_length());
    }

    return ret_val;
}

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_PB(IVP_Mindist_Minimize_Solver *mms){
    CORE;
    mms->swap_synapses();
    return minimize_B_POLY(mms);
}

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_B_POLY(IVP_Mindist_Minimize_Solver *mms){
    IVP_Synapse_Real *syn_B = mms->mindist->get_synapse(0);
    IVP_Ball *ball = syn_B->get_object()->to_ball();
    IVP_Cache_Ball	m_cache_B;
    m_cache_B.object = ball;
    m_cache_B.cache_object = ball->get_cache_object();
    m_cache_B.tmp.synapse = syn_B;	// remember order of synapses

    IVP_Synapse_Real *syn_P = mms->mindist->get_synapse(1);
    IVP_Polygon *poly_P = syn_P->get_object()->to_poly();
    const IVP_Compact_Edge *P = syn_P->edge;
    
    IVP_Cache_Ledge_Point m_cache_P(poly_P,P->get_compact_ledge());
    m_cache_P.tmp.synapse = syn_P;

    IVP_IF(ivp_check_debug_mindist(mms->mindist)){
	printf("%32s statii: %i \n","minimize_default_ball_poly", syn_P->get_status());
    }

    IVP_MRC_TYPE ret_val;
    switch(syn_P->get_status()){  
    case IVP_ST_POINT:
	ret_val = mms->p_minimize_BP( &m_cache_B, P, &m_cache_P );
	break;
	
    case IVP_ST_EDGE:
	ret_val = mms->p_minimize_BK( &m_cache_B, P, &m_cache_P );
	break;
    case IVP_ST_TRIANGLE:
	ret_val = mms->p_minimize_BF( &m_cache_B, P, &m_cache_P );
	break;
    default:
      ret_val = IVP_MRC_UNINITIALIZED;
	CORE;
    };
    m_cache_P.remove_reference();
    m_cache_B.cache_object->remove_reference();
    //   mms->proove_ballpoly();
    return ret_val;
}


IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_KB(IVP_Mindist_Minimize_Solver *mms){
    CORE;
    mms->swap_synapses();
    return minimize_B_POLY(mms);
}

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_FB(IVP_Mindist_Minimize_Solver *mms){
    CORE;
    mms->swap_synapses();
    return minimize_B_POLY(mms);
}

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_swapped_poly_poly(IVP_Mindist_Minimize_Solver *mms){
    mms->swap_synapses();
    return minimize_default_poly_poly(mms);
}


IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_BB(IVP_Mindist_Minimize_Solver *mms){
    IVP_Synapse_Real *syn_A = mms->mindist->get_synapse(0);
    IVP_Ball *ball_A = syn_A->get_object()->to_ball();
    IVP_Cache_Object *m_cache_A;
    m_cache_A = ball_A->get_cache_object();

    IVP_Synapse_Real *syn_B = mms->mindist->get_synapse(1);
    IVP_Ball *ball_B = syn_B->get_object()->to_ball();
    IVP_Cache_Object *m_cache_B;
    m_cache_B = ball_B->get_cache_object();

    IVP_U_Point *A = m_cache_A->m_world_f_object.get_position();
    IVP_U_Point *B = m_cache_B->m_world_f_object.get_position();

    mms->mindist->contact_plane.subtract(A,B);
    IVP_DOUBLE qlen = mms->mindist->contact_plane.quad_length();
    IVP_DOUBLE inv_len;
	if( IVP_Inline_Math::fabsd(qlen)>P_DOUBLE_EPS ) { // used to be fabs, which was a sml call
        inv_len = IVP_Fast_Math::isqrt(qlen,3);
	} else {
		inv_len=1.0f;
	}
    mms->mindist->len_numerator = qlen * inv_len - mms->mindist->sum_extra_radius;
    mms->mindist->contact_plane.mult(inv_len);

#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED	
    IVP_U_Float_Point diff_center;
    diff_center.subtract( &m_cache_A->core_pos, &m_cache_B->core_pos );
    mms->mindist->contact_dot_diff_center = diff_center.dot_product(&mms->mindist->contact_plane);
#endif

    m_cache_A->remove_reference();
    m_cache_B->remove_reference();
    return IVP_MRC_OK;
}


///////////////////////////////////////////
// Pierce through convex object and
// traverse triangles until valid q/r values (or valid term.len?).
// Checks for valid termination_len.
// Returned edge represents the triangle found.
///////////////////////////////////////////

const IVP_Compact_Edge *IVP_Compact_Ledge_Solver::minimize_on_other_side(
    const IVP_Compact_Edge *edge,    const IVP_U_Point *partner_os)
{    // manage pierce count
    const IVP_Compact_Ledge *c_ledge = edge->get_compact_ledge();

    int n_triangles = c_ledge->get_n_triangles();
#if defined(IVP_NO_ALLOCA)
    uchar pierce_visited_array[IVP_MAX_TRIANGLES_PER_LEDGE];
#else
    uchar *pierce_visited_array = (uchar *)alloca(n_triangles);
#endif
    
    memset(pierce_visited_array,0,n_triangles);        // @@@OG could be optimized, depending on how often piercing takes place
    

    int pierce_idx = edge->get_triangle()->get_pierce_index();
    const IVP_Compact_Triangle *pierced_tri = &c_ledge->get_first_triangle()[pierce_idx];
    
    const IVP_Compact_Edge *F = pierced_tri->get_first_edge(); // precalculated piercing

    while(1){
	pierce_visited_array[F->get_triangle()->get_tri_index()] = 1; // tag triangle as visited
	
	IVP_Unscaled_QR_Result qr;
	IVP_CLS.calc_unscaled_qr_vals_F_space(c_ledge, F, partner_os, &qr);

	// search best edge and take corresponding neighbor triangle
	const IVP_Compact_Edge *e;
	int j;
	int moved = 0;
	for (e=F,j=0;j<3;e=e->get_next(),j++){
	    if (qr.checks[j] > 0.0f ) continue;  // inside triangle
	    int tri_idx = e->get_opposite()->get_triangle()->get_tri_index();
	    if(pierce_visited_array[tri_idx]) continue; // already visited

	    F = e->get_opposite();
	    moved = 1;
	    break;
	}
	if(!moved){
	    // no movement possible anymore, take this triangle as final triangle
	    break;
	}
    }
    return F;
}



IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::minimize_illegal(IVP_Mindist_Minimize_Solver *){
    CORE;
    return IVP_MRC_ILLEGAL;
}

IVP_MRC_TYPE (*IVP_Mindist_Minimize_Solver::mms_function_table[IVP_ST_MAX_LEGAL][IVP_ST_MAX_LEGAL])(IVP_Mindist_Minimize_Solver *mms);

void IVP_Mindist_Minimize_Solver::init_mms_function_table(){
    for (int i=0; i< IVP_ST_MAX_LEGAL; i++){
	for (int j=0; j< IVP_ST_MAX_LEGAL; j++){
	    mms_function_table[i][j] = minimize_illegal;
	}
    }
    mms_function_table[IVP_ST_POINT] [IVP_ST_POINT]    = minimize_default_poly_poly;
    mms_function_table[IVP_ST_POINT] [IVP_ST_EDGE]     = minimize_default_poly_poly;
    mms_function_table[IVP_ST_POINT] [IVP_ST_TRIANGLE] = minimize_default_poly_poly;
    mms_function_table[IVP_ST_POINT] [IVP_ST_BALL]     = minimize_PB;

    mms_function_table[IVP_ST_EDGE] [IVP_ST_POINT]    = minimize_swapped_poly_poly;
    mms_function_table[IVP_ST_EDGE] [IVP_ST_EDGE]     = minimize_default_poly_poly;
    mms_function_table[IVP_ST_EDGE] [IVP_ST_TRIANGLE] = minimize_illegal;
    mms_function_table[IVP_ST_EDGE] [IVP_ST_BALL]     = minimize_KB;

    mms_function_table[IVP_ST_TRIANGLE] [IVP_ST_POINT]    = minimize_swapped_poly_poly;
    mms_function_table[IVP_ST_TRIANGLE] [IVP_ST_EDGE]     = minimize_illegal;
    mms_function_table[IVP_ST_TRIANGLE] [IVP_ST_TRIANGLE] = minimize_default_poly_poly;
    mms_function_table[IVP_ST_TRIANGLE] [IVP_ST_BALL]     = minimize_FB;

    mms_function_table[IVP_ST_BALL] [IVP_ST_POINT]    = minimize_B_POLY;
    mms_function_table[IVP_ST_BALL] [IVP_ST_EDGE]     = minimize_B_POLY;
    mms_function_table[IVP_ST_BALL] [IVP_ST_TRIANGLE] = minimize_B_POLY;
    mms_function_table[IVP_ST_BALL] [IVP_ST_BALL]     = minimize_BB;
}