ivp_mindist_mcases.cxx

hl2 source code. Do not use it illegal.

    if( sr_lK.checks[0] * kkr.checks_K[0] < 0.0f){
	sort_synapses(m_cache_L->tmp.synapse,m_cache_K->tmp.synapse);
	return p_minimize_PP(plp, pkm, m_cache_L, m_cache_K);
    }
    
    return p_minimize_PP(pkp, plp, m_cache_K, m_cache_L);
}

class IVP_Leave_KK_Case {
public:
  const IVP_Compact_Edge *F;
  const IVP_Compact_Edge *P;
  const IVP_U_Point *P_Fos;
  IVP_Cache_Ledge_Point *m_cache_P;
  IVP_Cache_Ledge_Point *m_cache_F;
  const IVP_U_Point *H_Fos;
  IVP_U_Point hesse_Fos;
};

// prerequisites:
// synapses are set correctly
// world_coords of points of KK are already set
IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_Leave_KK(const IVP_Compact_Edge *K, const IVP_Compact_Edge *L,
							      const IVP_KK_Input &kkin, const IVP_Unscaled_KK_Result *kkr,
							      IVP_Cache_Ledge_Point *m_cache_K,
							      IVP_Cache_Ledge_Point *m_cache_L)
{
    if(--P_Finish_Counter < 0){
	if( check_loop_hash(IVP_ST_EDGE, K, IVP_ST_EDGE, L) ){
	    // we had this situation before: end
	    return IVP_MRC_ENDLESS_LOOP;
	}
    }
    
    // create plane to determine on which side of K the start point of L can be found
    IVP_U_Point H_Kos;
    IVP_U_Point H_Los;
    IVP_U_Point H_ws;

    int side;// OSR
    {
	IVP_U_Point diff_l_k;
	diff_l_k.subtract(kkin.L_Los[0], &kkin.K_Los[0]);

	IVP_FLOAT val = diff_l_k.dot_product(&kkin.cross_KL_Los);
	IVP_DOUBLE HH = kkin.cross_KL_Los.quad_length();

	side = (*( unsigned int *)&val)>>31;

	IVP_DOUBLE iHQ = IVP_Fast_Math::isqrt(HH,3);

	
	H_Los.set(&kkin.cross_KL_Los);

	m_cache_L->get_object_cache()->transform_vector_to_world_coords( & H_Los, & H_ws);
	m_cache_K->get_object_cache()->transform_vector_to_object_coords( & H_ws, & H_Kos);

	mindist->len_numerator = IVP_Inline_Math::fabsd(val * iHQ);	// no direction of KK
	mindist->len_numerator -= mindist->sum_extra_radius;
	mindist->contact_plane.set_multiple ( &H_ws,( side - 0.5f ) * 2.0f * iHQ);

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

    IVP_BOOL          reverse_side_check[4];	// IVP_TRUE when point sees side from wrong direction
    IVP_Leave_KK_Case cases[4];
    IVP_U_Point L_Kos[2];
    IVP_CLS.transform_pos_other_space( kkin.L_Los[0], m_cache_L, m_cache_K, &L_Kos[0]);
    IVP_CLS.transform_pos_other_space( kkin.L_Los[1], m_cache_L, m_cache_K, &L_Kos[1]);
    {
      cases[0].F          = K->get_opposite();
      cases[0].P          = kkin.L;
      cases[0].P_Fos      = &L_Kos[0];
      cases[0].m_cache_F  = m_cache_K;
      cases[0].m_cache_P  = m_cache_L;
      cases[0].H_Fos      = &H_Kos;

      cases[1].F          = K;
      cases[1].P          = kkin.L->get_opposite();
      cases[1].P_Fos      = &L_Kos[1];
      cases[1].m_cache_F  = m_cache_K;
      cases[1].m_cache_P  = m_cache_L;
      cases[1].H_Fos      = &H_Kos;

      cases[2].F          = L->get_opposite();
      cases[2].P          = kkin.K;
      cases[2].P_Fos      = &kkin.K_Los[0];
      cases[2].m_cache_F  = m_cache_L;
      cases[2].m_cache_P  = m_cache_K;
      cases[2].H_Fos      = &H_Los;
      
      cases[3].F          = L;
      cases[3].P          = kkin.K->get_opposite();
      cases[3].P_Fos      = &kkin.K_Los[1];
      cases[3].m_cache_F  = m_cache_L;
      cases[3].m_cache_P  = m_cache_K;
      cases[3].H_Fos      = &H_Los;
    }
    {
      IVP_Leave_KK_Case *c = &cases[0];
	for(int i=0; i<=3; i++){
	      IVP_CLS.calc_hesse_vec_object_not_normized( c->F, c->F->get_compact_ledge(), &c->hesse_Fos);
	      IVP_FLOAT side_check_dist = c->H_Fos->dot_product(&c->hesse_Fos);
	      int reverse = (*(unsigned int *)&side_check_dist)>>31;
	      reverse ^=  (i>>1) ^ side;
	      reverse_side_check[i] = (IVP_BOOL)reverse;
	      c++;
	}
    }
    
    IVP_DOUBLE min_grad_pos = -P_DOUBLE_RES * 4; // max_double
    const IVP_Compact_Edge *min_edge = NULL;
    const IVP_Compact_Edge *min_plane = NULL;
    IVP_Cache_Ledge_Point *min_edge_m_cache = NULL;
    IVP_Cache_Ledge_Point *min_plane_m_cache = NULL;
    IVP_Unscaled_QR_Result min_qr;
    {

    // check all neigbouring areas !!
      IVP_Leave_KK_Case *cF = &cases[0];
    for(int i=0; i<=3; i++,cF++){
	IVP_Leave_KK_Case *ce = &cases[(i^side) ^ (int)reverse_side_check[i]];
	IVP_DOUBLE delta_h2;
        IVP_U_Point e_dir_Fos;
	{
	    IVP_Leave_KK_Case *ce2 = &cases[i^side];
	    IVP_Leave_KK_Case *ce2_next = &cases[1^(i^side)];	    
	    
	    e_dir_Fos.subtract( ce2->P_Fos, ce2_next->P_Fos );
	    delta_h2 = e_dir_Fos.dot_product(&cF->hesse_Fos);
	}
	
	if (delta_h2 >= 0.0f) continue;

	// calc gradient as real sin(alpha)
	IVP_DOUBLE inv_hesse_real_length = IVP_Inline_Math::isqrt_float(cF->hesse_Fos.quad_length());
	IVP_DOUBLE grad = delta_h2 * IVP_Inline_Math::isqrt_float(e_dir_Fos.quad_length());
	grad *= inv_hesse_real_length;
	if(grad < min_grad_pos){
	    // check side of triangle

	  IVP_Unscaled_QR_Result qr;
	  IVP_CLS.calc_unscaled_qr_vals_F_space( cF->m_cache_F->get_compact_ledge(), cF->F, ce->P_Fos, &qr);
	  
	    if (qr.checks[0] > 0.0f){
		// we now remember plane with best gradient
		min_grad_pos = grad;
		min_edge = ce->P;
		min_edge_m_cache = ce->m_cache_P;
		min_plane = cF->F;
		min_plane_m_cache = cF->m_cache_F;
		min_qr = qr;
	    }else{
		IVP_IF(1) {
		    printf("negative r value in KK: %g\n",qr.checks[0]);
		}
	    }
	}
    }
    }
    if(min_edge == 0){
	m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_EDGE);
	m_cache_L->tmp.synapse->update_synapse(L, IVP_ST_EDGE);

	// End KL	// sk and sl are already set by master
	if (reverse_side_check[0] + reverse_side_check[1] == IVP_TRUE + IVP_TRUE){
	    m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_BACKSIDE);
	    m_cache_L->tmp.synapse->update_synapse(L, IVP_ST_TRIANGLE);
	    IVP_DOUBLE sl = kkr->checks_L[0] / ( kkr->checks_L[0] + kkr->checks_L[1] );
	    this->pos_opposite_BacksideOs.set_interpolate(&L_Kos[0], &L_Kos[1], sl);
	    return IVP_MRC_BACKSIDE;
	}
	if (reverse_side_check[2] + reverse_side_check[3] == IVP_TRUE + IVP_TRUE){
	    m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_TRIANGLE);
	    IVP_DOUBLE sk = kkr->checks_K[0] / ( kkr->checks_K[0] + kkr->checks_K[1] );
	    this->pos_opposite_BacksideOs.set_interpolate( &kkin.K_Los[0], &kkin.K_Los[1], sk );
	    m_cache_L->tmp.synapse->update_synapse(L, IVP_ST_BACKSIDE);
	    return IVP_MRC_BACKSIDE;
	}
	return IVP_MRC_OK;
    }
    
    /** Now we found a good plane,
	  check for a KK case of neighbouring edges */
    sort_synapses(min_edge_m_cache->tmp.synapse,min_plane_m_cache->tmp.synapse);

    {
	if (!min_qr.is_outside()){// nearer point is inside new area->PF
	    IVP_U_Point edge_Fos;
	    IVP_CLS.calc_pos_other_space( min_edge, min_edge_m_cache, min_plane_m_cache,&edge_Fos);
	    return p_minimize_Leave_PF(min_edge,&edge_Fos,  min_plane, min_edge_m_cache, min_plane_m_cache);
	}

	if (min_qr.checks[2] >= 0.0f){	// other side is inside
	    return p_minimize_KK(min_edge, min_plane->get_next(), min_edge_m_cache, min_plane_m_cache);	// no ideas about backsides !!!
	}

	if ( min_qr.checks[1] >= 0.0f){
	    return p_minimize_KK(min_edge, min_plane->get_prev(), min_edge_m_cache, min_plane_m_cache);	// no ideas about backsides !!!
	}
    }
    // now we are outside, find new KK s vales of other edges of new area
    // which edge to take? calc and compare dists...
    {
	IVP_DOUBLE dist_next = IVP_CLS.calc_qlen_KK(min_edge, min_plane->get_next(),min_edge_m_cache, min_plane_m_cache);
	IVP_DOUBLE dist_prev = IVP_CLS.calc_qlen_KK(min_edge, min_plane->get_prev()->get_opposite(),min_edge_m_cache, min_plane_m_cache);

	// @@@ slow solution: compare all 3 dists...
#ifdef IVP_MINDIST_BEHAVIOUR_DEBUG
	IVP_DOUBLE dist_plane = calc_qlen_KK(min_edge, min_plane,min_edge_m_cache, min_plane_m_cache);
	if( (dist_plane < dist_next) && (dist_plane < dist_prev) ){
	    CORE;
	    // How could this have happened?
	    // No improvement seems possible, though grad says so...
	    // End KL	// sk and sl are already set by master
	    m_cache_K->tmp.synapse->update_synapse(K, IVP_ST_EDGE);
	    m_cache_L->tmp.synapse->update_synapse(L, IVP_ST_EDGE);
	    return IVP_MRC_OK;
	
	}
#endif
	if(dist_next > dist_prev){
	    return p_minimize_KK(min_edge, min_plane->get_prev(), min_edge_m_cache, min_plane_m_cache);	// no ideas about backsides !!!
	}else{				       
	    return p_minimize_KK(min_edge, min_plane->get_next(), min_edge_m_cache, min_plane_m_cache);	// no ideas about backsides !!!
	}
    }
} // p_minimize_Leave_KK

/***************************** BP ******************************************/
/***************************** BP ******************************************/
/***************************** BP ******************************************/

// Case: Ball - Vertex

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_BP(IVP_Cache_Ball *m_cache_ball, const IVP_Compact_Edge *P, IVP_Cache_Ledge_Point *m_cache_P){

    if(--P_Finish_Counter < 0){
	if( check_loop_hash(IVP_ST_BALL, NULL, IVP_ST_POINT, P) ){
	    // we had this situation before: end
	    return IVP_MRC_ENDLESS_LOOP;
	}
    }
    

    const IVP_U_Point *center_b_ws;
    IVP_U_Point center_ball_Pmos;
    
    IVP_U_Point p_ws; IVP_CLS.give_world_coords_AT(P, m_cache_P, &p_ws);
    center_b_ws = m_cache_ball->cache_object->m_world_f_object.get_position();
    m_cache_P->get_object_cache()->transform_position_to_object_coords(center_b_ws, &center_ball_Pmos);

    IVP_U_Point contact_plane;
    contact_plane.subtract( center_b_ws, &p_ws );
    IVP_DOUBLE dist_plus_radius =  contact_plane.real_length_plus_normize();
    mindist->contact_plane.set(&contact_plane);
    
    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_ball->cache_object->core_pos, &m_cache_P->get_object_cache()->core_pos );
    mindist->contact_dot_diff_center = diff_center.dot_product(&mindist->contact_plane);
#endif    
    IVP_DOUBLE s_grad_max = 0; 
    
    const IVP_Compact_Edge *Kmax = NULL;

    // each vertex P is position, all neighbouring edges of Pm checked
    {
      const IVP_Compact_Edge *Pm = P; // alias
	const IVP_U_Float_Point *Pm_Pmos = IVP_CLS.give_object_coords(Pm, m_cache_P);

	IVP_U_Point Pm_P_Pmos;
	Pm_P_Pmos.subtract( &center_ball_Pmos, Pm_Pmos);
	
	IVP_DOUBLE dist = Pm_P_Pmos.dot_product(Pm_Pmos);

	// each edge
	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_P);
	    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 reverse areas
			// check whether new point on edge still could see me 
			s_grad_max = grad;
			Kmax = e;
		}
	    }
	    if(e==Pm) break;
	}
    }

    if(Kmax == NULL){
	// END AB
      end_BP:
	m_cache_P->tmp.synapse->update_synapse(P, IVP_ST_POINT);
	return IVP_MRC_OK;
    }
    IVP_Unscaled_S_Result sr;
    IVP_CLS.calc_unscaled_s_val_K_space(m_cache_P->get_compact_ledge(), Kmax, &center_ball_Pmos, &sr); //
    if (sr.checks[1] <= 0){
      IVP_IF(1){
	printf("BP epsilon problem\n");
      }
      goto end_BP;
    }

    // let's walk, check PP case first
    if(sr.checks[0] < 0 ){
	return p_minimize_BP(m_cache_ball, Kmax, m_cache_P);
    }
    return p_minimize_BK(m_cache_ball, Kmax, m_cache_P);
}


/***************************** PP ******************************************/
/***************************** PP ******************************************/
/***************************** PP ******************************************/


IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_PP(const IVP_Compact_Edge *A, const IVP_Compact_Edge *B,
				    IVP_Cache_Ledge_Point *m_cache_A,
				    IVP_Cache_Ledge_Point *m_cache_B)
{
    // Checks for a better edge, i.e. an edge that points in the right direction
    // (maybe the other vertex of the edge is the next vertex to consider)

    if(--P_Finish_Counter < 0){
	if( check_loop_hash(IVP_ST_POINT, A, IVP_ST_POINT, B) ){
	    // had this situation before: end
	    return IVP_MRC_ENDLESS_LOOP;
	}
    }
    
    IVP_ASSERT(m_cache_A->tmp.synapse == mindist->get_sorted_synapse(0));
#ifdef DEBUG_CHECK_LEN
    check_len_PP(A, B, m_cache_A, m_cache_B);
#endif

    

    const IVP_Compact_Edge *Point[2];
    Point[0] = A; Point[1] = B;

    IVP_U_Point points_ws[2];
    IVP_CLS.give_world_coords_AT(A, m_cache_A, &points_ws[0]);
    IVP_CLS.give_world_coords_AT(B, m_cache_B, &points_ws[1]);
    
    IVP_U_Point point_other_os[2];
    m_cache_B->get_object_cache()->transform_position_to_object_coords(&points_ws[0], &point_other_os[0]);
    m_cache_A->get_object_cache()->transform_position_to_object_coords(&points_ws[1], &point_other_os[1]);

    {
	IVP_DOUBLE qlen = points_ws[0].quad_distance_to(&points_ws[1]);
	if (qlen > P_DOUBLE_RES){
		IVP_DOUBLE inv_len = IVP_Fast_Math::isqrt(qlen,3);
		mindist->len_numerator = qlen * inv_len - mindist->sum_extra_radius;
		mindist->contact_plane.inline_subtract_and_mult( &points_ws[0], &points_ws[1], inv_len);
	}else{
	    return IVP_MRC_ENDLESS_LOOP;
	}

#ifdef IVP_HALFSPACE_OPTIMIZATION_ENABLED	
	IVP_U_Float_Point diff_center;
	diff_center.subtract( &m_cache_A->get_object_cache()->core_pos, &m_cache_B->get_object_cache()->core_pos );
	mindist->contact_dot_diff_center = diff_center.dot_product(&mindist->contact_plane);
#endif
    }    
    
    IVP_Cache_Ledge_Point *m_cache[2];