ivp_mindist_debug.cxx

hl2 source code. Do not use it illegal.

// Copyright (C) Ipion Software GmbH 1999-2000. All rights reserved.


#include <ivp_physics.hxx>
#include <ivu_geometry.hxx>

#include <ivp_debug.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>


/************************************************************/
/************************************************************/
/************************************************************/
/************************************************************/
/************************************************************/

// NOT YET FULLY TRANSFORMED TO COMPACT LEDGE STYLE !!! -OG

/************************************************************/
/************************************************************/
/************************************************************/
/************************************************************/
/************************************************************/


IVP_BOOL ivp_check_debug_mindist( IVP_Mindist *md){
    if (! IVP_DEBUG_OBJECT0 ) return IVP_FALSE;
    if (! IVP_DEBUG_OBJECT1 ) return IVP_FALSE;
    const char *search0 = IVP_DEBUG_OBJECT0;
    const char *search1 = IVP_DEBUG_OBJECT1;

    const char *name0 = md->get_synapse(0)->get_object()->get_name();
    const char *name1 = md->get_synapse(1)->get_object()->get_name();
    if (	!P_String::string_cmp(name0, search0, IVP_FALSE) && !P_String::string_cmp(name1, search1, IVP_FALSE) ||
		!P_String::string_cmp(name0, search1, IVP_FALSE) && !P_String::string_cmp(name1, search0, IVP_FALSE)){
	const IVP_Time &time = md->get_synapse(0)->l_obj->get_environment()->get_current_time();
	if ( time - IVP_DEBUG_TIME >= 0.0f){
	    printf("%10s %10s %8f-%X: ", name0, name1, time.get_time(), (int)md & 0xffff );
	    return IVP_TRUE;
	}
    }
    return IVP_FALSE;
}


int p_sqrt( IVP_DOUBLE val) // for debugger
{
    printf("%g\n", IVP_Inline_Math::sqrtd(val));
    return 0;
}


#ifdef DEBUG
// copy-paste from p_minimize_FF for proove_polypoly()
IVP_DOUBLE IVP_Mindist_Minimize_Solver::p_optimize_FF( const IVP_Compact_Edge *A, const IVP_Compact_Edge *B,
			  IVP_Cache_Ledge_Point *m_cache_A,  IVP_Cache_Ledge_Point *m_cache_B,
			  IVP_DOUBLE min_qdist){
    // Case: Surface - Surface
    
    // check all 9 point point combinations first:
    {
	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_U_Point pos_B_ws; IVP_CLS.give_world_coords_AT(pB, m_cache_B, &pos_B_ws);
		IVP_U_Point pos_A_ws; IVP_CLS.give_world_coords_AT(pA, m_cache_A, &pos_A_ws);

		IVP_DOUBLE qdist = pos_A_ws.quad_distance_to(&pos_B_ws);
		if (qdist < min_qdist){
		    min_qdist = qdist;
		}
	    }
	}
    }
    // check all 6 point area combinations
    IVP_Cache_Ledge_Point *cc_A[2]; cc_A[0] = m_cache_A; cc_A[1] = m_cache_B;
    IVP_Cache_Ledge_Point *cc_B[2]; cc_B[0] = m_cache_B; cc_B[1] = m_cache_A;
    const IVP_Compact_Edge *tabA[2]; tabA[0] = A; tabA[1] = B;
    const IVP_Compact_Edge *tabB[2]; tabB[0] = B; tabB[1] = A;
    {
	for (int i=0;i<2;i++){
	    const IVP_Compact_Edge *pA;
	    const IVP_Compact_Edge *pB= tabB[i];
	    int a;
	    IVP_U_Hesse hesse_B;
	    IVP_CLS.calc_hesse_object(pB, cc_B[i]->get_compact_ledge(), &hesse_B);

	    for ( pA=tabA[i],a=0; a<3; pA=pA->get_next(),a++){
	      IVP_U_Point pA_Bos;
	      IVP_CLS.calc_pos_other_space( pA, cc_A[i], cc_B[i], &pA_Bos );
	      IVP_Unscaled_QR_Result qr;
	      IVP_CLS.calc_unscaled_qr_vals_F_space(cc_B[i]->get_compact_ledge(), pB, &pA_Bos, &qr);
	      if ( !qr.is_outside()){
		IVP_DOUBLE qdist = hesse_B.get_dist( &pA_Bos );
		qdist *= qdist;
		if ( qdist * (1+P_DOUBLE_RES) < min_qdist){
		  min_qdist = qdist;
		}
	      }
	    }
	}
    }

    // check all 18 point edge combinations
    {
	int i;
	for (i=0; i<2; i++){
	    const IVP_Compact_Edge *pA, *pB;
	    int a, b;
	    for (pB=tabB[i],b=0; b<3; pB=pB->get_next(),b++){
	      IVP_U_Point pB_Aos;
	      IVP_CLS.calc_pos_other_space( pB, cc_B[i], cc_A[i], &pB_Aos );
	      
	      for (pA=tabA[i],a=0; a<3; pA=pA->get_next(),a++){
		IVP_Unscaled_S_Result sr;
		IVP_CLS.calc_unscaled_s_val_K_space(  cc_A[i]->get_compact_ledge(), pA, &pB_Aos, &sr);
		if (sr.is_outside()) continue;
		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){
		  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_DOUBLE quad_dist = kkin.cross_KL_Los.quad_length();
	      if(quad_dist <= (P_DOUBLE_RES * P_DOUBLE_RES * P_MAX_OBJECT_SIZE * P_MAX_OBJECT_SIZE)){ // not parallel
		  continue;
	      }
	      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){
		    min_qdist = qdist;
		}
	    }
	}
    }
    return min_qdist;
}
#endif


#ifdef DEBUG
IVP_RETURN_TYPE IVP_Mindist_Minimize_Solver::proove_polypoly()
{
    // does a REAL SLOW (!!!) brute force recalculation and check.

    // requires collision free situation
    IVP_Synapse_Real *syn0, *syn1;
    
    syn0 = mindist->get_synapse(0);
    syn1 = mindist->get_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;
    
    const IVP_Compact_Ledge *ledge0 = e0->get_compact_ledge();
    const IVP_Compact_Ledge *ledge1 = e1->get_compact_ledge();

    IVP_Cache_Ledge_Point m_cache_0; m_cache_0.init_cache_ledge_point(poly_0,ledge0);
    IVP_Cache_Ledge_Point m_cache_1; m_cache_1.init_cache_ledge_point(poly_1,ledge1);
    
    m_cache_0.tmp.synapse = syn0;	// remember order of synapses
    m_cache_1.tmp.synapse = syn1;
    
    IVP_DOUBLE min_qdist = P_DOUBLE_MAX;

    const IVP_Compact_Triangle *best_t0;
    const IVP_Compact_Triangle *best_t1;
   
    // each area of the first object
    for (int l0 = ledge0->get_n_triangles()-1; l0>=0;l0--){
	for (int l1 = ledge1->get_n_triangles()-1; l1>=0;l1--){
	    const IVP_Compact_Triangle *t0 = &ledge0->get_first_triangle()[l0];
	    const IVP_Compact_Triangle *t1 = &ledge1->get_first_triangle()[l1];
	    
	    IVP_DOUBLE qd = p_optimize_FF(t0->get_first_edge(), t1->get_first_edge(), &m_cache_0, &m_cache_1, min_qdist);
	    if(qd < min_qdist){
		// found better situation, synapses are already updated
		best_t0 = t0;
		best_t1 = t1;
		min_qdist = qd;
	    }
	}	
    }
    m_cache_0.remove_reference();
    m_cache_1.remove_reference();
    
    // compare brute force result with original mindist state
	IVP_DOUBLE dist_0 = sqrt(min_qdist);
	IVP_DOUBLE dist_1 = mindist->get_length();
	IVP_DOUBLE diff = IVP_Inline_Math::fabsd(dist_0 - dist_1) + P_DOUBLE_EPS;
	
	IVP_DOUBLE perc = diff * 100.0f / dist_1;
        // printf("\nBrute Force Recalculation of Mindist shows differences:\n");
	// printf("Distances: md %f, bf %f, diff: %f percent: %f\n", dist_0, dist_1, diff, perc);

	// check if difference is neglectible
	if(perc < 50.0f){
	    return IVP_OK;
	}
	
	// somewhat greater differences ...
	printf("\nBrute Force Recalculation of Mindist shows differences:\n");
	printf("Dsts: md %f, bf %f, diff: %f percent: %f\n", dist_0, dist_1, diff, perc);
	printf("Coords of synapses (original 0/1, brute force 0/1):\n");

	//	IVP_DOUBLE qd0 = p_optimize_FF(best_t0->get_first_edge(), best_t1->get_first_edge(), &m_cache_0, &m_cache_1, min_qdist + P_DOUBLE_RES);
	//IVP_DOUBLE qd1 = p_optimize_FF(best_t0->get_first_edge(), best_t1->get_first_edge(), &m_cache_0, &m_cache_1, min_qdist + P_DOUBLE_RES);

	CORE;
	return IVP_FAULT;
}
#endif 

#ifdef DEBUG
IVP_RETURN_TYPE IVP_Mindist_Minimize_Solver::proove_ballpoly()
{
    // does a REAL SLOW (!!!) brute force recalculation and check.

    // requires collision free situation
    IVP_Synapse_Real  *syn1;
    IVP_Synapse_Real *syn0;
    
    syn0 = mindist->get_synapse(0);
    syn1 = mindist->get_synapse(1);

    // get caches
    IVP_Real_Object *poly_0 = syn0->get_object();
    IVP_Polygon *poly_1 = syn1->get_object()->to_poly();
    
    
    const IVP_Compact_Edge *e1 = syn1->edge;
    const IVP_Compact_Ledge *ledge1 = e1->get_compact_ledge();

    IVP_Cache_Object *m_cache_0 = poly_0->get_cache_object();
    IVP_Cache_Object *m_cache_1 = poly_1->get_cache_object();
       
    IVP_DOUBLE min_qdist = P_DOUBLE_MAX;

    const IVP_Compact_Triangle *best_t1;
    IVP_U_Point *ball_ws;
    IVP_U_Point ball_Fos;
    ball_ws = m_cache_0->m_world_f_object.get_position();
    m_cache_1->transform_position_to_object_coords( ball_ws, &ball_Fos);
   
    for (int l1 = ledge1->get_n_triangles()-1; l1>=0;l1--){
	const IVP_Compact_Triangle *t1 = &ledge1->get_first_triangle()[l1];

	IVP_DOUBLE qd = IVP_CLS.calc_qlen_PF_F_space(ledge1, t1, &ball_Fos);
	if(qd < min_qdist){
	    // found better situation, synapses are already updated
	    best_t1 = t1;
	    min_qdist = qd;
	}
    }