ivp_mindist_mcases.cxx

hl2 source code. Do not use it illegal.

    m_cache[0] = m_cache_A; m_cache[1] = m_cache_B;
    IVP_DOUBLE s_grad_max = 0.0f;

    const IVP_U_Point *P_Pmos_max = NULL;
    const IVP_Compact_Edge *Kmax = NULL;
    const IVP_Compact_Edge *Pmax = NULL;
    IVP_Cache_Ledge_Point *m_cache_K_max = NULL;
    IVP_Cache_Ledge_Point *m_cache_P_max = NULL;
    

    // for each point, all neighbouring edges of Pm (=opposite point) are checked
    for(int ip=1; ip>=0; ip--){
      //IVP_Cache_Ledge_Point *m_cache_P = m_cache[ip];
      IVP_Cache_Ledge_Point *m_cache_Pm = m_cache[1-ip];

	const IVP_U_Point *P_Pmos = &point_other_os[ip];
	const IVP_Compact_Edge *Pm = Point[1-ip];
	const IVP_U_Float_Point *Pm_Pmos = IVP_CLS.give_object_coords(Pm, m_cache_Pm);
	IVP_U_Point Pm_P_Pmos;
	Pm_P_Pmos.subtract( P_Pmos, Pm_Pmos);
		
	// each edge
	IVP_DOUBLE dist = Pm_P_Pmos.dot_product(Pm_Pmos);

	Pm = Pm->get_prev();

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

	for( ; 1; e = e->get_opposite()->get_prev()){ 
	    const IVP_U_Float_Point *tp_next_os = IVP_CLS.give_object_coords(e,m_cache_Pm);
	    IVP_DOUBLE grad = Pm_P_Pmos.dot_product(tp_next_os) - dist;
	    if (grad > 0) {
		IVP_DOUBLE i_len = IVP_Inline_Math::isqrt_float( tp_next_os->quad_distance_to(Pm_Pmos));
		grad *= i_len;
		if(grad > s_grad_max){	// search for greatest step to walk
			// now we found a new smax _value, check for espilon
		  P_Pmos_max = P_Pmos;
		    s_grad_max = grad;
		    Kmax = e;
		    Pmax = Point[ip];
		    m_cache_K_max = m_cache_Pm;
		    m_cache_P_max = m_cache[ip];
		}
	    }
	    if(e==Pm) break;
	}
    }

    if(Kmax == NULL){
	// END AB
    end_PP:
	m_cache_A->tmp.synapse->update_synapse(A, IVP_ST_POINT);
	m_cache_B->tmp.synapse->update_synapse(B, IVP_ST_POINT);
	
	return IVP_MRC_OK;
    }

    IVP_Unscaled_S_Result sr;
    IVP_CLS.calc_unscaled_s_val_K_space(m_cache_K_max->get_compact_ledge(), Kmax, P_Pmos_max, &sr); //
    if (sr.checks[1] <= 0){
      IVP_IF(1){
	printf("PP epsilon problem\n");
      }
      goto end_PP;
    }
    
    // let's walk, check PP case first
    if(sr.checks[0] < 0.0f){
	sort_synapses(m_cache_K_max->tmp.synapse,m_cache_P_max->tmp.synapse);
	return p_minimize_PP(Kmax, Pmax, m_cache_K_max, m_cache_P_max);
    }

    sort_synapses(m_cache_P_max->tmp.synapse,m_cache_K_max->tmp.synapse);
    return p_minimize_Leave_PK(Pmax, Kmax, m_cache_P_max, m_cache_K_max);
}

/***************************** BK ******************************************/
/***************************** BK ******************************************/
/***************************** BK ******************************************/
// Do not use this class yet.
//template<class T_Base, class T_Offset, int N> class IVP_Matrix_Ortho3;

// Case: Ball - Edge

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_BK(IVP_Cache_Ball *m_cache_B, const IVP_Compact_Edge *K,
							IVP_Cache_Ledge_Point *m_cache_K     )
{
    IVP_U_Point P_Kos;
    {
      IVP_U_Point *wP = m_cache_B->cache_object->m_world_f_object.get_position();
      m_cache_K->get_object_cache()->transform_position_to_object_coords( wP, &P_Kos);
    }
    
    IVP_Unscaled_S_Result sr;
    IVP_CLS.calc_unscaled_s_val_K_space( m_cache_K->get_compact_ledge(), K, &P_Kos, &sr);    

    if(sr.checks[0] < 0.0f){
	return p_minimize_BP(m_cache_B, K, m_cache_K);
    }
    
    if(sr.checks[1] < 0.0f){
	return p_minimize_BP(m_cache_B, K->get_next(), m_cache_K);
    }
    return p_minimize_Leave_BK(m_cache_B, K, m_cache_K);
}


IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_Leave_BK(IVP_Cache_Ball *m_cache_ball, const IVP_Compact_Edge *K,
							      IVP_Cache_Ledge_Point *m_cache_K)
{
    if(--P_Finish_Counter < 0){
	if( check_loop_hash(IVP_ST_BALL, NULL, IVP_ST_EDGE, K) ){
	    return IVP_MRC_ENDLESS_LOOP;
	}
    }

    // ********** first step, check for PF cases
    IVP_U_Point p_ks; // point p in K space
    {
      IVP_U_Point *p_ws = m_cache_ball->cache_object->m_world_f_object.get_position();
      m_cache_K->get_object_cache()->transform_position_to_object_coords( p_ws, &p_ks);
    }
    
    // get direction of edge K
    const IVP_U_Float_Point *k_start_os    = IVP_CLS.give_object_coords(K, m_cache_K);		// a point on an edge
    const IVP_U_Float_Point *k_next_os     = IVP_CLS.give_object_coords(K->get_next(), m_cache_K);	// next point
    const IVP_U_Float_Point *k_tri_os      = IVP_CLS.give_object_coords(K->get_prev(), m_cache_K);	// prev point == other point of this triange
    const IVP_U_Float_Point *k_oppo_tri_os = IVP_CLS.give_object_coords(K->get_opposite()->get_prev(), m_cache_K);	// point of opposite triangle
    
    IVP_U_Point vec_K_ks;    	vec_K_ks.subtract( k_next_os, k_start_os);	// not normized direction yet
    
    IVP_U_Point vec_K_P;	vec_K_P.subtract( &p_ks, k_start_os);

    IVP_U_Point vec_K_tri;   	vec_K_tri.subtract( k_tri_os, k_start_os);
    IVP_U_Point vec_K_oppo_tri; vec_K_oppo_tri.subtract( k_oppo_tri_os, k_start_os);
    
    IVP_U_Point hesse_tri_os;      hesse_tri_os.calc_cross_product( &vec_K_ks, &vec_K_tri);		// vertical to triangle
    IVP_U_Point hesse_oppo_tri_os; hesse_oppo_tri_os.calc_cross_product( &vec_K_oppo_tri, &vec_K_ks); // v. to oppo tri

    IVP_DOUBLE vert[2];	// dist between point and triangle, oppo tri
    vert[0] = vec_K_P.dot_product( &hesse_tri_os);
    vert[1] = vec_K_P.dot_product( &hesse_oppo_tri_os);
    
     // new verion using qr
    IVP_Unscaled_QR_Result qr0,qr1;
    IVP_CLS.calc_unscaled_qr_vals_F_space(m_cache_K->get_compact_ledge(), K, &p_ks, &qr0);
    IVP_CLS.calc_unscaled_qr_vals_F_space(m_cache_K->get_compact_ledge(), K->get_opposite(), &p_ks, &qr1);

	
    if ( qr0.checks[0] > 0.0f){	// check projected distance
	if (qr1.checks[0] > 0.0f){	// both planes better,
	    if (vert[1] > 0){		// plane visible, if true than it has priority
		return p_minimize_BF(m_cache_ball, K->get_opposite(), m_cache_K);
	    }
	}
	return p_minimize_BF(m_cache_ball, K, m_cache_K);
    }
    
    if (qr1.checks[0] > 0.0f){
	return p_minimize_BF(m_cache_ball, K->get_opposite(), m_cache_K);
    }
    
    // check for backside !!
    if ( vert[0] < -P_DOUBLE_EPS && vert[1] < -P_DOUBLE_EPS){	// both from behind
      this->pos_opposite_BacksideOs.set(&p_ks);
      m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_BACKSIDE);
      return IVP_MRC_BACKSIDE;
    }

    IVP_U_Point Lot2; // unnormized Lot
    Lot2.calc_cross_product( &vec_K_ks, &vec_K_P);

    IVP_DOUBLE iqK_len = 1.0f / vec_K_ks.quad_length();
    IVP_DOUBLE qdist = Lot2.quad_length() * iqK_len;
    IVP_DOUBLE inv_len = IVP_Inline_Math::isqrt_double(qdist);
    mindist->len_numerator = qdist * inv_len - mindist->sum_extra_radius;

    Lot2.calc_cross_product( &vec_K_ks, &Lot2 ); // does change the length !!!

    m_cache_K->get_object_cache()->transform_vector_to_world_coords( &Lot2, &Lot2);

    mindist->contact_plane.set_multiple( &Lot2,  - inv_len * iqK_len);

#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED	
    IVP_U_Float_Point diff_center;
    diff_center.subtract( &m_cache_ball->cache_object->core_pos, &m_cache_K->get_object_cache()->core_pos );
    mindist->contact_dot_diff_center = diff_center.dot_product(&mindist->contact_plane);
#endif
    
    
    m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_EDGE);
    
    return IVP_MRC_OK;
}


/***************************** PK ******************************************/
/***************************** PK ******************************************/
/***************************** PK ******************************************/

// Case: Vertex - Edge

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_PK(const IVP_Compact_Edge *P, const IVP_Compact_Edge *K,
				      IVP_Cache_Ledge_Point *m_cache_P,
				      IVP_Cache_Ledge_Point *m_cache_K)
{
    // Checks wether we can switch to the case PP (vertex - vertex)
    // If not: take appropriate branch
    IVP_ASSERT( m_cache_P->tmp.synapse == mindist->get_sorted_synapse(0));
#ifdef DEBUG_CHECK_LEN
    check_len_PK(P, K, m_cache_P, m_cache_K);
#endif
    IVP_U_Point P_Kos;
    IVP_CLS.calc_pos_other_space(P, m_cache_P, m_cache_K,&P_Kos );

    IVP_Unscaled_S_Result sr;
    IVP_CLS.calc_unscaled_s_val_K_space( m_cache_K->get_compact_ledge(), K, &P_Kos, &sr);

    if(sr.checks[0] < 0.0f){
	return p_minimize_PP(P, K, m_cache_P, m_cache_K);
    }
    if(sr.checks[1] < 0.0f){
	return p_minimize_PP(P, K->get_next(), m_cache_P, m_cache_K);
    }
    return p_minimize_Leave_PK(P, K, m_cache_P, m_cache_K);
}

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_Leave_PK(const IVP_Compact_Edge *P,const IVP_Compact_Edge *K,
					    IVP_Cache_Ledge_Point *m_cache_P,
					    IVP_Cache_Ledge_Point *m_cache_K)
{
    if(--P_Finish_Counter < 0){
	if( check_loop_hash(IVP_ST_POINT, P, IVP_ST_EDGE, K) ){
	    // had this situation before: end
	    return IVP_MRC_ENDLESS_LOOP;
	}
    }
    
#ifdef DEBUG_CHECK_LEN
    check_len_PK(P, K, m_cache_P, m_cache_K);
#endif
    // ********** first step, check for PF cases
    IVP_U_Point p_ks; // point p in K space
    IVP_CLS.calc_pos_other_space( P, m_cache_P, m_cache_K,&p_ks);
 
    // get direction of edge K
    const IVP_U_Float_Point *k_start_os    = IVP_CLS.give_object_coords(K, m_cache_K);		        // a point on an edge
    const IVP_U_Float_Point *k_next_os     = IVP_CLS.give_object_coords(K->get_next(), m_cache_K);	// next point
    const IVP_U_Float_Point *k_tri_os      = IVP_CLS.give_object_coords(K->get_prev(), m_cache_K);	// prev point == other point of this triange
    const IVP_U_Float_Point *k_oppo_tri_os = IVP_CLS.give_object_coords(K->get_opposite()->get_prev(), m_cache_K);	// point of opposite triangle
    

    IVP_U_Point vec_K_ks;    	vec_K_ks.subtract( k_next_os, k_start_os);	// unnormized direction
    
    IVP_U_Point vec_K_P;	vec_K_P.subtract( &p_ks, k_start_os);

    IVP_U_Point vec_K_tri;   	vec_K_tri.subtract( k_tri_os, k_start_os);
    IVP_U_Point vec_K_oppo_tri; vec_K_oppo_tri.subtract( k_oppo_tri_os, k_start_os);

    {
	//IVP_DOUBLE K_ks_len = vec_K_ks.real_length();
	//IVP_DOUBLE i_K_ks_len = 1.0f / K_ks_len;
	//vec_K_ks.mult(i_K_ks_len); // normize K_ks
    }
    //vec_K_ks.fast_normize();
    
    IVP_U_Point hesse_tri_os;      hesse_tri_os.calc_cross_product( &vec_K_ks, &vec_K_tri);		// vertical to triangle
    IVP_U_Point hesse_oppo_tri_os; hesse_oppo_tri_os.calc_cross_product( &vec_K_oppo_tri, &vec_K_ks); // v. to oppo tri
    
    IVP_DOUBLE vert[2];	// dist between point and triangle, oppo tri
    vert[0] = vec_K_P.dot_product( &hesse_tri_os);
    vert[1] = vec_K_P.dot_product( &hesse_oppo_tri_os);

     // new verion using qr
    IVP_Unscaled_QR_Result qr0,qr1;
    IVP_CLS.calc_unscaled_qr_vals_F_space(m_cache_K->get_compact_ledge(), K, &p_ks, &qr0);
    IVP_CLS.calc_unscaled_qr_vals_F_space(m_cache_K->get_compact_ledge(), K->get_opposite(), &p_ks, &qr1);
    
    if ( qr0.checks[0] > 0.0f){	// check projected distance
	if (qr1.checks[0] > 0.0f){	// both planes better,
	    if (vert[1] > 0){		// plane visible, if true than it has priority
		return p_minimize_PF(P, K->get_opposite(), m_cache_P, m_cache_K);
	    }
	}
	return p_minimize_PF(P, K, m_cache_P, m_cache_K);
    }
    
    if (qr1.checks[0] > 0.0f){
	return p_minimize_PF(P, K->get_opposite(), m_cache_P, m_cache_K);
    }
    
    // check for backside !!
    if ( vert[0] < 0 && vert[1] < 0){	// both from behind
	m_cache_P->tmp.synapse->update_synapse(P, IVP_ST_POINT);
	m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_BACKSIDE);
	this->pos_opposite_BacksideOs.set(&p_ks);
	return IVP_MRC_BACKSIDE;
    }


    // ********* next step, find KK cases

    // each edge: find minimal angle between P and Lot( P, K)


    IVP_U_Point Lot2; // unnormized Lot
    IVP_DOUBLE qlen;
    {	// Lot _ws
	
	Lot2.calc_cross_product( &vec_K_ks, &vec_K_P);

	IVP_DOUBLE iqK_len = 1.0f / vec_K_ks.quad_length();
	qlen = Lot2.quad_length() * iqK_len;
	if (qlen > P_DOUBLE_EPS){
	    IVP_DOUBLE inv_len = IVP_Inline_Math::isqrt_double(qlen);
	    mindist->len_numerator = qlen * inv_len - mindist->sum_extra_radius;
	    Lot2.calc_cross_product( &vec_K_ks, &Lot2 ); // does not change the length !!!
	    m_cache_K->get_object_cache()->transform_vector_to_world_coords( &Lot2, &Lot2);
	    mindist->contact_plane.set_multiple( &Lot2, - inv_len * iqK_len);
	}else{
	    mindist->len_numerator = - mindist->sum_extra_radius;
	    IVP_U_Point orth; orth.calc_an_orthogonal( &vec_K_ks );
	    orth.normize();
	    mindist->contact_plane.set(&orth);
	}
	//IVP_ASSERT( mindist->contact_plane.real_length() < 1.001f);

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

    // Lot _point_space
    m_cache_P->get_object_cache()->transform_vector_to_object_coords( &Lot2, &Lot2);

    const IVP_Compact_Edge *NKmax = NULL;    
    IVP_DOUBLE max_s_val = P_DOUBLE_RES * qlen;

    const IVP_U_Float_Point *p_os = 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()){ 
	IVP_U_Point e_vec_Pos;
	e_vec_Pos.subtract( IVP_CLS.give_object_coords(e, m_cache_P), p_os);
	IVP_DOUBLE angle = e_vec_Pos.dot_product(&Lot2);
	if (angle > 0){
	    angle *= IVP_Inline_Math::isqrt_float( e_vec_Pos.quad_length());
	    if(angle > max_s_val){
		max_s_val = angle;
		NKmax = e;
	    }
	}
	if(e==Pm) break;
    }

    while(NKmax != NULL){
	if (max_s_val < P_RES_EPS){	// maybe an epsilon problem
	  IVP_KK_Input kkin( K, NKmax, m_cache_K, m_cache_P);
	  IVP_Unscaled_KK_Result kkr;
	  IVP_RETURN_TYPE check = IVP_CLS.calc_unscaled_KK_vals(kkin, &kkr);
	  if (check == IVP_FAULT || kkr.checks_L[0] < 0.0f){
	    IVP_IF(0) {
	      printf("PK_KK epsilon problem\n");
	    }
	    break;
	  }
	}
	return p_minimize_KK(NKmax, K, m_cache_P, m_cache_K);
    }

    // END PK
    m_cache_P->tmp.synapse->update_synapse(P, IVP_ST_POINT);
    m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_EDGE);

    return IVP_MRC_OK;
} //p_minimize_Leave_PK