ivp_mindist_minimize.cxx

hl2 source code. Do not use it illegal.

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

#include <ivp_physics.hxx>

#if defined(LINUX) || defined(SUN) || (__MWERKS__ && __POWERPC__)
#   include <alloca.h>
#endif

// IVP_EXPORT_PRIVATE

#include <ivu_float.hxx>
#include <ivp_debug_manager.hxx>

#include <ivu_hash.hxx>

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

#include <ivp_debug.hxx>

#include <ivp_anomaly_manager.hxx>

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


// for english speakers:
// the collision files sometimes make use of abbreviations
// that have got german roots:

// P means Point or Vertex
// F means Face
// K means Edge (german: Kante)
// B means Ball


struct IVP_Loop_Key_Struct
{
    IVP_SYNAPSE_POLYGON_STATUS s0;
    IVP_SYNAPSE_POLYGON_STATUS s1;
    const IVP_Compact_Edge *e0;
    const IVP_Compact_Edge *e1;
};

#if !defined(IVP_LOOP_LIST_SIZE)
IVP_BOOL IVP_Mindist_Minimize_Solver::check_loop_hash(IVP_SYNAPSE_POLYGON_STATUS i_s0,
				      const IVP_Compact_Edge *i_e0,
				      IVP_SYNAPSE_POLYGON_STATUS i_s1,
				      const IVP_Compact_Edge *i_e1)
{
    if (! loop_hash ){
#ifdef	IVP_MINDIST_BEHAVIOUR_DEBUG
	printf("start loop check (max passes exceeded).\n");
#endif	
	this->init_loop_hash();
    }
    IVP_Loop_Key_Struct key_struct;

    if ( i_e0 < i_e1){
	key_struct.s0 = i_s0;
	key_struct.s1 = i_s1;
	key_struct.e0 = i_e0;
	key_struct.e1 = i_e1;
    }else{
	key_struct.s0 = i_s1;
	key_struct.s1 = i_s0;
	key_struct.e0 = i_e1;
	key_struct.e1 = i_e0;
    }
    void *found = loop_hash->find((char *)&key_struct);
    if(found){
	// we had this situation before
	return IVP_TRUE;
    }
    loop_hash->add((char *)&key_struct, (void *)1);
    return IVP_FALSE;
}


void IVP_Mindist_Minimize_Solver::init_loop_hash()
{    
    P_DELETE(loop_hash);
    loop_hash = new IVP_Hash(IVP_LOOP_HASH_SIZE/*size*/, sizeof(IVP_Loop_Key_Struct) );
}
#else
void IVP_Mindist_Minimize_Solver::init_loop_hash(){;};

IVP_BOOL IVP_Mindist_Minimize_Solver::check_loop_hash(IVP_SYNAPSE_POLYGON_STATUS i_s0,     const IVP_Compact_Edge *i_e0,
						      IVP_SYNAPSE_POLYGON_STATUS i_s1,     const IVP_Compact_Edge *i_e1){
    IVP_ASSERT( i_s0 < 4);
    IVP_ASSERT( i_s1 < 4);

    int x0 = int(i_e0) | i_s0;
    int x1 = int(i_e1) | i_s1;

    if (x0 < x1) {
	int h = x0; x0 = x1; x1 = h;
    }

    IVP_MM_Loop_Hash_Struct *s = & loop_hash[loop_hash_size-1];
    for(int i = loop_hash_size-1; i>=0; i--){
	if ( s->a == x0 && s->b == x1){
	    return IVP_TRUE;
	}
	s--;
    }
    if ( loop_hash_size >= IVP_LOOP_LIST_SIZE){
	return IVP_TRUE;
    }
    loop_hash[ loop_hash_size ].a = x0;
    loop_hash[ loop_hash_size ].b = x1;
    loop_hash_size++;
    return IVP_FALSE;
}
#endif

///////////////////////////////////////////
///////////////////////////////////////////
void IVP_Mindist::mindist_rescue_push(){
    // This method is called whenever the simualtion finds interpenetrating
    // moveable objects.

    // The mass centers of the objects are then pushed apart - to
    // hopefully separate them und thus bring them into a sane state.
    
    IVP_Real_Object *obj0 = get_synapse(0)->get_object();
    IVP_Real_Object *obj1 = get_synapse(1)->get_object();

    obj0->get_environment()->get_anomaly_manager()->inter_penetration( this, obj0, obj1 );
}


void IVP_Mindist_Minimize_Solver::pierce_mindist(){

    // Find out the best place to continue on the
    // opposite side of the object
    
    // calc world coords of syn1
    IVP_Synapse_Real *syn_pierce;
    IVP_Synapse_Real    *syn_other = mindist->get_sorted_synapse(0);
    if (syn_other->get_status() != IVP_ST_BACKSIDE){
	syn_pierce = mindist->get_sorted_synapse(1);
    }else{ //@@@ This actually should not happen
	//CORE;
	syn_pierce = syn_other;
	syn_other = mindist->get_sorted_synapse(1);
    }
    IVP_ASSERT(syn_pierce->get_status() == IVP_ST_BACKSIDE);
    
    IVP_U_Point &syn_other_Fos = pos_opposite_BacksideOs;

    // syn0
    const IVP_Compact_Edge *F = syn_pierce->edge;
    const IVP_Compact_Edge *pierced_tri;

    pierced_tri = IVP_CLS.minimize_on_other_side(F, &syn_other_Fos);

    // minimize again with this new start situation
    syn_pierce->update_synapse(pierced_tri, IVP_ST_TRIANGLE);
}
////////////////////////////////////////
////////////////////////////////////////



/** Recalculation of mindist,
  requires:	calc_next_PSI_matrix called
  result:	updates synapses
  */
IVP_MRC_TYPE IVP_Mindist::recalc_invalid_mindist()
{
    IVP_Time_CODE tc = this->get_environment()->get_current_time_code();
    if(recalc_time_stamp==tc) {
	return IVP_MRC_ALREADY_CALCULATED;
    }
    recalc_time_stamp=tc;

    IVP_Mindist_Minimize_Solver mms(this);
    mms.P_Finish_Counter = 0;   // create hash instantly
    
    while(1){
	IVP_MRC_TYPE res = mms.recalc_mindist_sub();
	if (res == IVP_MRC_OK){  // fast return if ok
	  recalc_result = IVP_MDRR_OK;
	  return res;
	}
	recalc_result = IVP_MDRR_INTRUSION;
	
	switch(res){
	case IVP_MRC_BACKSIDE:{
	    // find best triangle on opposite of convex object
	    mms.pierce_mindist();
	    // case ENDLESS must follow!
	}
	case IVP_MRC_ENDLESS_LOOP:{
	  if (this->mindist_function == IVP_MF_PHANTOM ||
	      this->mindist_status == IVP_MD_HULL_RECURSIVE ){
	    return res;
	  }
	  // MINDIST RESCUE PUSH
	  mindist_rescue_push();
	  return res;
	}
	default:
	    CORE;
	    break;
	}
	CORE;	
    }	// while
    CORE;
}


IVP_MRC_TYPE IVP_Mindist::recalc_mindist()
{
    IVP_Time_CODE tc = this->get_environment()->get_current_time_code();
    if(recalc_time_stamp==tc) {
	return IVP_MRC_ALREADY_CALCULATED;
    }

    recalc_time_stamp=tc;

    IVP_Mindist_Minimize_Solver mms(this);
    int pierce_counter = 0;
    while(1){
	IVP_MRC_TYPE res = mms.recalc_mindist_sub();
	if (res == IVP_MRC_OK){  // fast return if ok
	  recalc_result = IVP_MDRR_OK;
	  return res;
	}
	recalc_result = IVP_MDRR_INTRUSION;
	
	switch(res){
	case IVP_MRC_BACKSIDE:{
#ifdef IVP_MINDIST_BEHAVIOUR_DEBUG	    
	    IVP_ASSERT(this->detect_collision(synapse[0]->to_poly()->get_ivp_polygon()->tetras,
					    synapse[1]->to_poly()->get_ivp_polygon()->tetras) == IVP_FALSE);
#endif	    
	    // find best triangle on opposite of convex object
	    mms.pierce_mindist();	    
	    if (++pierce_counter < IVP_MAX_PIERCINGS){
	      continue;
	    }
	    // case ENDLESS must follow!
	}
	case IVP_MRC_ENDLESS_LOOP:{
	  if (this->mindist_function == IVP_MF_PHANTOM ||
	      this->mindist_status == IVP_MD_HULL_RECURSIVE ){
	    return res;
	  }
	  IVP_IF(1){
		const char *name0 = get_synapse(0)->get_object()->get_name();
		if (!name0) name0 = "(null)";
		const char *name1 = get_synapse(1)->get_object()->get_name();
		if (!name1) name1 = "(null)";
		printf("recalc_mindist: Endless Loop without collision or termination problem.%s %s\n",
		       name0,name1);
	    }
	    // MINDIST RESCUE PUSH
	    mindist_rescue_push();

#ifdef IVP_MINDIST_BEHAVIOUR_DEBUG	    
	    if(detect_collision(psyn_0->get_ivp_polygon()->tetras,
				psyn_1->get_ivp_polygon()->tetras)){
		CORE;
	    }else{
		IVP_IF(1){ mms.termination_len = P_DOUBLE_MAX; }
		P_Finish_Counter = 10; // debug purposes
		IVP_IF(1) {
		    printf("recalc_mindist : Endless Loop without collision or termination problem.\n");
		}
		continue; // helps debugging
//		CORE;
		return res;
	    }
#endif
	    return res;
	}
	default:
	    CORE;
	    break;
	}
	CORE;	
    }	// while
    CORE;
}

IVP_MRC_TYPE IVP_Mindist_Minimize_Solver::p_minimize_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)
{

    // Case: Surface - Surface
    
    IVP_DOUBLE min_qdist = P_DOUBLE_MAX;

    // 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){
		    m_cache_A->tmp.synapse->update_synapse(pA, IVP_ST_POINT);
		    m_cache_B->tmp.synapse->update_synapse(pB, IVP_ST_POINT);
		    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){
		  cc_A[i]->tmp.synapse->update_synapse(pA, IVP_ST_POINT);
		  cc_B[i]->tmp.synapse->update_synapse(pB, IVP_ST_TRIANGLE);
		  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;