ivp_mindist_event.cxx

hl2 source code. Do not use it illegal.

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


// IVP_EXPORT_PRIVATE

#include <ivp_physics.hxx>
#include <ivp_debug_manager.hxx>

#include <ivp_mindist_intern.hxx>
#include <ivp_mindist_event.hxx>

#include <ivu_hash.hxx>


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

#include <ivp_3d_solver.hxx>

#define P_MAX(a,b)(((a)>(b))?(a):(b))
#define P_MIN(a,b)(((a)<(b))?(a):(b))



class IVP_3D_Solver_PF_COLL: public IVP_3D_Solver {
public:
    // object A
    IVP_U_Point point_object;

    IVP_U_Point hesse_of_area_object;
    IVP_U_Point point_of_area_object;
    // object B    
    
    IVP_DOUBLE get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c);
};

IVP_DOUBLE IVP_3D_Solver_PF_COLL::get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c){
    IVP_3D_Solver_PF_COLL *t = this;
    IVP_U_Point point_world;
    A_w_f_c->vmult4(&t->point_object,&point_world);

    IVP_U_Point hesse_of_area_world, point_of_area_world;
    B_w_f_c->inline_vmult4(&t->point_of_area_object, &point_of_area_world);
    B_w_f_c->inline_vmult3(&t->hesse_of_area_object, &hesse_of_area_world);

    IVP_U_Point point_minus_area_world;
    point_minus_area_world.subtract(&point_world, &point_of_area_world);
    IVP_DOUBLE res = point_minus_area_world.dot_product(&hesse_of_area_world);
    return res;
}

class IVP_3D_Solver_VEC_PARALLEL_AREA: public IVP_3D_Solver {
public:
    // object A
    IVP_U_Point vec_object;

    // object B
    IVP_U_Point hesse_of_area_object;
    IVP_DOUBLE get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c);
};

IVP_DOUBLE IVP_3D_Solver_VEC_PARALLEL_AREA::get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c){
    IVP_3D_Solver_VEC_PARALLEL_AREA *t = this;
    IVP_U_Point vec_world;
    A_w_f_c->vmult3(&t->vec_object,&vec_world);

    IVP_U_Point hesse_of_area_world;
    B_w_f_c->vmult3(&t->hesse_of_area_object, &hesse_of_area_world);

    IVP_DOUBLE res = vec_world.dot_product(&hesse_of_area_world);
    return res;
}


class IVP_3D_Solver_DISTANCE_OF_TWO_POINTS: public IVP_3D_Solver {
public:
    // object A
    IVP_U_Point A_object;
    IVP_U_Point B_object;
    IVP_U_Point normized_direction_world_at_t0;

    IVP_DOUBLE get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c);
};

IVP_DOUBLE IVP_3D_Solver_DISTANCE_OF_TWO_POINTS::get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c){
    IVP_3D_Solver_DISTANCE_OF_TWO_POINTS *t = this;
    
    IVP_U_Point A_world;
    IVP_U_Point B_world;
    A_w_f_c->vmult4(&t->A_object,&A_world);
    B_w_f_c->vmult4(&t->B_object,&B_world);
    IVP_U_Point dir_world;
    dir_world.subtract(&B_world, &A_world);
    IVP_DOUBLE res = dir_world.quad_length();

    const IVP_DOUBLE linear_factor = 1.2f;
    // check linear
    IVP_DOUBLE linear_dist = dir_world.dot_product(&normized_direction_world_at_t0) * linear_factor;

    if ( linear_dist * IVP_Inline_Math::fabsd(linear_dist) < res  ){
	return linear_dist;
    }
    
    return IVP_Inline_Math::sqrtd(res);	// bad, but allows good estimates for first deviation
}

class IVP_3D_Solver_S_VALS: public IVP_3D_Solver {
public:
    // object A is Point
    IVP_U_Point P_object; 
    // object B is edge
    IVP_U_Point K_object;
    IVP_U_Point K_vec_object;	// normized K_vec
    IVP_DOUBLE get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c);
};

IVP_DOUBLE IVP_3D_Solver_S_VALS::get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c){
    IVP_3D_Solver_S_VALS *t = this;
    
    IVP_U_Point P_world;
    IVP_U_Point K_world;
    IVP_U_Point K_vec_world;
    A_w_f_c->vmult4(&t->P_object,&P_world);
    B_w_f_c->vmult4(&t->K_object,&K_world);
    B_w_f_c->vmult3(&t->K_vec_object,&K_vec_world);

    IVP_U_Point PK;
    PK.subtract(&K_world,&P_world);
    IVP_DOUBLE res = PK.dot_product(&K_vec_world);
    return res;
}

class IVP_3D_Solver_PK_KK: public IVP_3D_Solver {
public:
    // object A is Point
    IVP_U_Point P_object;
    IVP_U_Point P_ne_vec_object;		// neighbour edge of P
    // object B is edge
    IVP_U_Point K_object;
    IVP_U_Point K_vec_object;	// normized K_vec
    IVP_DOUBLE get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c);
};


// when does a PK case lead to KK case
// idea: build the Lot from P to K
// and check angle between Lot and neighbouring edges of P
// Note: to minimize errors, the lot is constructed using calc_hesse twice instead of calc_s_val

IVP_DOUBLE IVP_3D_Solver_PK_KK::get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c){
    IVP_3D_Solver_PK_KK *t = this;
    
    IVP_U_Point P_world;
    IVP_U_Point P_ne_vec_world;
    IVP_U_Point K_world;
    IVP_U_Point K_vec_world;
    A_w_f_c->vmult4(&t->P_object,&P_world);
    A_w_f_c->vmult3(&t->P_ne_vec_object,&P_ne_vec_world);
    B_w_f_c->vmult4(&t->K_object,&K_world);
    B_w_f_c->vmult3(&t->K_vec_object,&K_vec_world);

    IVP_U_Point KP;
    KP.subtract(&P_world,&K_world);

    IVP_U_Point Lot;
    Lot.inline_calc_cross_product(&K_vec_world, &KP);
    Lot.inline_calc_cross_product_and_normize(&Lot, &K_vec_world);
    IVP_DOUBLE res = Lot.dot_product(&P_ne_vec_world);
    
    return res;
}

/* check the distance between point and edge
 * as this distance never gets negative, check the distance
 * to a virtual plane too ( minus plane_offset )
 * and return the minimum of both distances */
class IVP_3D_Solver_PK_COLL: public IVP_3D_Solver {
public:
    IVP_DOUBLE plane_offset;		// negative offset for plane check
    // object A is Point
    IVP_U_Float_Point P_object; 
    // object B is edge
    IVP_U_Float_Point K_object;
    IVP_U_Point K_vec_object;	// normized K_vec
    IVP_U_Point K_Lot_object;	// a normized vertical direction
    IVP_DOUBLE get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c);
};

IVP_DOUBLE IVP_3D_Solver_PK_COLL::get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c){
    IVP_3D_Solver_PK_COLL *t = this;
    
    IVP_U_Point P_world;
    IVP_U_Point K_world;
    IVP_U_Point K_vec_world;	// normized direction
    IVP_U_Point K_Lot_world;
    A_w_f_c->vmult4(&t->P_object,&P_world);
    B_w_f_c->vmult4(&t->K_object,&K_world);
    B_w_f_c->vmult3(&t->K_vec_object,&K_vec_world);
    B_w_f_c->vmult3(&t->K_Lot_object,&K_Lot_world);
    
    IVP_U_Point PK;
    PK.subtract(&K_world,&P_world);
    IVP_U_Point H;
    H.calc_cross_product( &PK, &K_vec_world);
    IVP_DOUBLE res = H.real_length();
    // check distance to plane two
    IVP_DOUBLE res2 = H.dot_product(&K_Lot_world) + plane_offset;
    if (res2 < res) return res2;
    return res;
}

class IVP_3D_Solver_PF_NPF: public IVP_3D_Solver {
public:
    // object A
    IVP_U_Point vec_object;

    // object B
    IVP_U_Point hesse_of_area_object;
    IVP_DOUBLE get_value(IVP_U_Matrix *A_w_f_c, IVP_U_Matrix *B_w_f_c);

};

IVP_DOUBLE IVP_3D_Solver_PF_NPF::get_value(IVP_U_Matrix *A_w_f_c,IVP_U_Matrix *B_w_f_c){
    IVP_U_Point hesse_world;
    IVP_U_Point hesse_other_object;		// fast normized

    B_w_f_c->vmult3(&hesse_of_area_object, &hesse_world);
    A_w_f_c->vimult3(&hesse_world,&hesse_other_object);
    
    IVP_DOUBLE res = hesse_other_object.dot_product(&vec_object);
    return res;
}

void IVP_Mindist_Event_Solver::calc_next_event_PF(const IVP_Compact_Edge *P,const  IVP_Compact_Edge *F,
						    IVP_Cache_Ledge_Point *m_cache_P,
						    IVP_Cache_Ledge_Point *m_cache_F)
{
    // returns the delta time till the next event
    // if real_event_occurred is false, no event until max_delta_time occurred 
    
    /** detect collision time, first **/

    IVP_U_Matrix_Cache cache_P(m_cache_P->clp_cache_object);
    IVP_U_Matrix_Cache cache_F(m_cache_F->clp_cache_object);

    IVP_DOUBLE max_rot_speed = cache_P.core->abs_omega + cache_F.core->abs_omega;
    event_time_out =  t_max;	// flag, set after next psi -> no event within this PSI

    IVP_3D_Solver_PF_COLL pf_coll_solver;

    {
	pf_coll_solver.point_object.set(IVP_CLS.give_object_coords(P,m_cache_P));
	pf_coll_solver.point_of_area_object.set( IVP_CLS.give_object_coords(F,m_cache_F));
	
	// pf_coll_solver.hesse_of_area_object = F->get_triangle()->tmp.gen.hesse;
	IVP_CLS.calc_hesse_vec_object_not_normized(F, m_cache_F->get_compact_ledge(), &pf_coll_solver.hesse_of_area_object);
	pf_coll_solver.hesse_of_area_object.fast_normize();
	
	pf_coll_solver.set_max_deviation(this->max_coll_speed);
	
	// when does point P collide with surface F (no borders!) ?
	IVP_FLOAT radius = mindist->sum_extra_radius;
	IVP_DOUBLE sec_dist = mindist->get_coll_dist() + radius;	// security distance
	IVP_DOUBLE real_coll = ivp_mindist_settings.real_coll_dist + 0.5f * radius;
	IVP_DOUBLE now_len = mindist->len_numerator+radius;	
	IVP_BOOL result = pf_coll_solver.find_first_t_for_value_coll( sec_dist,real_coll,   t_now, event_time_out,
								    &cache_P, &cache_F, &now_len, &event_time_out );
	if (result){
	    event_type_out = IVP_COLL_PF_COLL;
	}
    }


    /** find next sector change **/
    IVP_3D_Solver_PF_NPF pf_npf_solver;
    
    
    // when are neighbor points of P nearer to F than P ?
    
    pf_npf_solver.hesse_of_area_object = pf_coll_solver.hesse_of_area_object;
    IVP_DOUBLE sec_dist;             // gradient, which indicates a possible new collision
    {
	IVP_DOUBLE real_dist = P_MIN( mindist->get_coll_dist(), mindist->len_numerator);
	real_dist += mindist->sum_extra_radius * 0.1f; // lazy sector change for extra radius
	sec_dist = -ivp_mindist_settings.mindist_change_force_dist *  m_cache_F->get_core()->inv_object_diameter;
	sec_dist *= real_dist;
	sec_dist /= mindist->get_coll_dist();
    }
    IVP_U_Point wHesseF;
    IVP_U_Point hesse_in_P_object;
    {
	m_cache_F->get_object_cache()->m_world_f_object.vmult3( &pf_npf_solver.hesse_of_area_object , &wHesseF);
	m_cache_P->get_object_cache()->m_world_f_object.vimult3(&wHesseF, &hesse_in_P_object);
    }
    
    pf_npf_solver.set_max_deviation(max_rot_speed + P_DOUBLE_EPS);
    IVP_DOUBLE max_grad_change = pf_npf_solver.max_deviation * (event_time_out - t_now);
    
    const IVP_U_Float_Point *p_object = IVP_CLS.give_object_coords(P,m_cache_P);
    for(const IVP_Compact_Edge *e = P->get_prev()->get_opposite(); 1; e=e->get_prev()->get_opposite()){
	const IVP_U_Float_Point *p_next_object = IVP_CLS.give_object_coords(e->get_next(),m_cache_P);
	IVP_U_Point vec_object;
	vec_object.subtract( p_next_object, p_object);
	IVP_DOUBLE grad = hesse_in_P_object.dot_product(&vec_object);
	IVP_DOUBLE qlen = vec_object.quad_length();
	IVP_DOUBLE ilen = IVP_Inline_Math::isqrt_float(qlen);
	grad *= ilen;	// search greatest gradient
	if ( grad < max_grad_change ){	// fast check
	    pf_npf_solver.vec_object.set_multiple(&vec_object ,ilen);
	    IVP_BOOL found_flag;
	    found_flag = pf_npf_solver.find_first_t_for_value_max_dev( sec_dist,   t_now, event_time_out, 0,
								       &cache_P, &cache_F, &grad , &event_time_out );
	    if (found_flag){
		event_type_out = IVP_COLL_PF_NPF;
	    }	
	}
	if(e==P) break;
    }
}

void IVP_Mindist_Event_Solver::calc_next_event_BF(const  IVP_Compact_Edge *F,