ivp_mindist_event.cxx

hl2 source code. Do not use it illegal.

						    IVP_Cache_Object *m_cache_B,
						    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_B);
    IVP_U_Matrix_Cache cache_F(m_cache_F->clp_cache_object);

    IVP_3D_Solver_PF_COLL pf_coll_solver;
    {
	pf_coll_solver.point_object.set_to_zero();
	pf_coll_solver.point_of_area_object.set( IVP_CLS.give_object_coords(F,m_cache_F));
	
	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;  // allow penetration
	IVP_DOUBLE now_len = mindist->len_numerator + radius;

	//@@@ optimization for static reference object (linear movement of ball)
	IVP_BOOL result = pf_coll_solver.find_first_t_for_value_coll( sec_dist,real_coll,   t_now, t_max,
									&cache_P, &cache_F, &now_len, &event_time_out );
	if (result){
	    event_type_out = IVP_COLL_PF_COLL;
	}
    }
}


class IVP_3D_Solver_KK_COLL: public IVP_3D_Solver {
public:
    // object A
    IVP_U_Point k_point_object;	// a point of K
    IVP_U_Point k_vec_object;		// k direction

    IVP_U_Point l_point_object;
    IVP_U_Point l_vec_object;		// l direction

    IVP_DOUBLE side;		// direction of H

    // object B
    IVP_DOUBLE get_value(IVP_U_Matrix *K_w_f_c,IVP_U_Matrix *L_w_f_c);
};

IVP_DOUBLE IVP_3D_Solver_KK_COLL::get_value(IVP_U_Matrix *K_w_f_c,IVP_U_Matrix *L_w_f_c){
    IVP_U_Point k_vec_world;
    IVP_U_Point l_vec_world;
    
    IVP_U_Point k_point_world;
    IVP_U_Point l_point_world;

    K_w_f_c->vmult3(&k_vec_object,&k_vec_world);
    K_w_f_c->vmult4(&k_point_object,&k_point_world);
    
    L_w_f_c->vmult3(&l_vec_object,&l_vec_world);
    L_w_f_c->vmult4(&l_point_object,&l_point_world);

    IVP_U_Point H;
    H.calc_cross_product(&k_vec_world, &l_vec_world);	// unscaled normal

    IVP_DOUBLE res = H.dot_product(&k_point_world) - H.dot_product(&l_point_world);
    IVP_DOUBLE iH_len = IVP_Inline_Math::isqrt_float(H.quad_length());			// bad, but allows good estimate for first deviation
    res *= iH_len;
    return res * side;
}

class IVP_3D_Solver_KK_PARALLEL: public IVP_3D_Solver {
public:
    // object A
    IVP_U_Point k_vec_object;		// normized k direction
    IVP_U_Point l_vec_object;		// normized l direction

    // object B
    IVP_DOUBLE get_value(IVP_U_Matrix *K_w_f_c,IVP_U_Matrix *L_w_f_c);
};

IVP_DOUBLE IVP_3D_Solver_KK_PARALLEL::get_value(IVP_U_Matrix *K_w_f_c,IVP_U_Matrix *L_w_f_c){
    IVP_U_Point k_vec_world;
    IVP_U_Point l_vec_world;
    
    K_w_f_c->vmult3(&k_vec_object,&k_vec_world);
    L_w_f_c->vmult3(&l_vec_object,&l_vec_world);

    IVP_U_Point H;
    H.calc_cross_product(&k_vec_world, &l_vec_world);	// unscaled normal
    IVP_DOUBLE res = H.quad_length();	// see comment on calc max_deviation
    return res;
}



void IVP_Mindist_Event_Solver::calc_next_event_KK(const IVP_Compact_Edge *K,
				     const IVP_Compact_Edge *L,
				     IVP_Cache_Ledge_Point *m_cache_K,
				     IVP_Cache_Ledge_Point *m_cache_L)
{
    // returns the delta time till the next event
    // if real_event_occurred is false, no event until max_delta_time occurred 
    // requires non-'parallel' K and L
    IVP_U_Matrix_Cache cache_K(m_cache_K->clp_cache_object);
    IVP_U_Matrix_Cache cache_L(m_cache_L->clp_cache_object);
    

    IVP_3D_Solver_KK_COLL solver_kk_coll;
    IVP_DOUBLE sum_abs_omega;
    {
	IVP_Core *solver_K = m_cache_K->get_core();
	IVP_Core *solver_L = m_cache_L->get_core();
	sum_abs_omega = solver_K->abs_omega + solver_L->abs_omega;
    }

    event_time_out =  t_max;
    // which direction does H have ? (we want to know for which point to check collision with which surface)    
    int side;
    
    /** detect collision time, first **/    
    {
	const IVP_U_Float_Point *k =  IVP_CLS.give_object_coords( K, m_cache_K );
	const IVP_U_Float_Point *kn = IVP_CLS.give_object_coords( K->get_next(), m_cache_K );
	const IVP_U_Float_Point *l =  IVP_CLS.give_object_coords( L, m_cache_L );
	const IVP_U_Float_Point *ln = IVP_CLS.give_object_coords( L->get_next(), m_cache_L );
      
	solver_kk_coll.k_vec_object.subtract( kn, k ); // only rough estimate for KK parallel
	solver_kk_coll.k_vec_object.fast_normize();
	solver_kk_coll.l_vec_object.subtract( ln, l );
	solver_kk_coll.l_vec_object.fast_normize();

	solver_kk_coll.k_point_object.set(k);
	solver_kk_coll.l_point_object.set(l);
	solver_kk_coll.set_max_deviation(this->max_coll_speed);	// thats not correct, but ensures no collision !!!

	// check side
	{
	    IVP_U_Point k_vec_world_now, l_vec_world_now;
	    m_cache_L->clp_cache_object->m_world_f_object.vmult3(&solver_kk_coll.l_vec_object, &l_vec_world_now);
	    m_cache_K->clp_cache_object->m_world_f_object.vmult3(&solver_kk_coll.k_vec_object, &k_vec_world_now);

	    IVP_U_Point H;
	    H.calc_cross_product(&k_vec_world_now, &l_vec_world_now);
	    IVP_DOUBLE val  = -H.dot_product(&mindist->contact_plane);
#if 0	    
	    IVP_IF(1){
		IVP_U_Point l_startp_world_now, k_startp_world_now;
		IVP_CLS.give_world_coords_AT(L, m_cache_L,&l_startp_world_now);
		IVP_CLS.give_world_coords_AT(K, m_cache_K,&k_startp_world_now);

		IVP_U_Point diff_l_k;
		diff_l_k.subtract(&l_startp_world_now, &k_startp_world_now);
		IVP_DOUBLE val2 = diff_l_k.dot_product(&H);
		IVP_ASSERT( val * val2 >= 0);
	    }
#endif


	    if(val>0.0f){   //
		side = 0; // H points from K to L
		solver_kk_coll.side = -1.0f;
	    }else{
		side = 1; // H points from L to K
		solver_kk_coll.side = 1.0f;
	    }
	}
	
	IVP_DOUBLE found =
	    solver_kk_coll.find_first_t_for_value_coll(   mindist->get_coll_dist() ,ivp_mindist_settings.real_coll_dist, 
							   t_now, event_time_out,  
							   &cache_K, &cache_L, 0, &event_time_out );
	if (found){
	    event_type_out = IVP_COLL_KK_COLL;
	}
    }

 
    
    IVP_3D_Solver_KK_PARALLEL solver_kk_parallel;
    {
	solver_kk_parallel.k_vec_object.set(&solver_kk_coll.k_vec_object);
	solver_kk_parallel.l_vec_object.set(&solver_kk_coll.l_vec_object);
	// deviation == max omega * max_len_of_H
	IVP_DOUBLE dev = sum_abs_omega + sum_abs_omega; // as kk parallel calcs the quadratic length we have to adjust dev:
	// max (f^2') = max ( 2f * f') = max ( 2* 1 * f') 
	solver_kk_parallel.set_max_deviation(dev + P_DOUBLE_EPS);
	IVP_DOUBLE found =
	    solver_kk_parallel.find_first_t_for_value_max_dev( P_DOUBLE_EPS, 
							       t_now, event_time_out, 0, 
							       &cache_K, &cache_L , 0, &event_time_out );
	if (found){
	    event_type_out = IVP_COLL_KK_PARALLEL;
	}
    }

    
    /*** find next sector change ***/
    
    // when do PF-situations occur ?


    
    const IVP_Compact_Edge *F[4];	// areas to check
    F[0] = K->get_opposite();
    F[1] = K;
    F[2] = L->get_opposite();
    F[3] = L;

    IVP_U_Point *edge_object[4];		// edges to check, see P array at p_minimize_Leave_KK
    edge_object[0] = &solver_kk_coll.l_vec_object;		// normized
    edge_object[1] = &solver_kk_coll.l_vec_object;
    edge_object[2] = &solver_kk_coll.k_vec_object;
    edge_object[3] = &solver_kk_coll.k_vec_object;

    int reversed[4];			// edge_object is meant reverse
    reversed[0] = side;
    reversed[1] = 1-side;
    reversed[2] = side;
    reversed[3] = 1-side;

    IVP_U_Matrix_Cache *cache_tab[4];
    cache_tab[0] = &cache_L;
    cache_tab[1] = &cache_L;
    cache_tab[2] = &cache_K;
    cache_tab[3] = &cache_K;

    IVP_Cache_Ledge_Point *m_cache_tab[4];
    m_cache_tab[0] = m_cache_L;
    m_cache_tab[1] = m_cache_L;
    m_cache_tab[2] = m_cache_K;
    m_cache_tab[3] = m_cache_K;

    // check for new PF cases:
    // check scalarproduct of surface normal with dist_plane_normal

    IVP_3D_Solver_VEC_PARALLEL_AREA solver_KK_PF;
    // all lengths of hesse and edges are normized, so max_dev is easy 
    solver_KK_PF.set_max_deviation(sum_abs_omega + P_DOUBLE_EPS);
    
    for(int i=0; i<=3; i++){
	if ( !reversed[i]){
	    solver_KK_PF.vec_object.set_multiple( edge_object[i],-1.0f);		// cache_e
	}else{
	    solver_KK_PF.vec_object.set( edge_object[i]);
	}
	const IVP_Compact_Edge *f = F[i];
	IVP_CLS.calc_hesse_vec_object_not_normized(f, m_cache_tab[3-i]->get_compact_ledge(), &solver_KK_PF.hesse_of_area_object);
	solver_KK_PF.hesse_of_area_object.fast_normize();
	
	IVP_ASSERT(m_cache_tab[3-i]->get_compact_ledge() == f->get_compact_ledge());
	
	IVP_U_Matrix_Cache *cache_e = cache_tab[i];
	IVP_U_Matrix_Cache *cache_F = cache_tab[3-i];

	// when does point P 'collide' with plane F (no borders!) ? 
	// consider extensions of edge
       	IVP_DOUBLE sec_dist = -ivp_mindist_settings.mindist_change_force_dist * cache_e->core->inv_object_diameter;
	ivp_u_bool found = solver_KK_PF.find_first_t_for_value_max_dev( sec_dist, 
									t_now, event_time_out, 0, 
									cache_e, cache_F , 0, &event_time_out );
	if (found){
	    event_type_out = IVP_COLL_KK_PF;
	}
    }

}


/***************************** PP ******************************************/
/***************************** PP ******************************************/
/***************************** PP ******************************************/
void IVP_Mindist_Event_Solver::calc_next_event_PP(const IVP_Compact_Edge *P,
				     const IVP_Compact_Edge *P2,
				     IVP_Cache_Ledge_Point *m_cache_P,
				     IVP_Cache_Ledge_Point *m_cache_P2)
{
    // returns the time till the next event
    // if real_event_occurred is false, no event until max_delta_time occurred 
    IVP_U_Matrix_Cache cache_P(m_cache_P->clp_cache_object);
    IVP_U_Matrix_Cache cache_P2(m_cache_P2->clp_cache_object);
    

    IVP_DOUBLE sum_abs_omega;
    IVP_Core *solver_P = m_cache_P->get_core();
    IVP_Core *solver_P2 = m_cache_P2->get_core();
    sum_abs_omega = solver_P->abs_omega + solver_P2->abs_omega;

    event_time_out =  t_max;
    const IVP_U_Float_Point *points_object[2];
    const IVP_U_Float_Point *p_object = IVP_CLS.give_object_coords(P,m_cache_P);
    const IVP_U_Float_Point *p2_object = IVP_CLS.give_object_coords(P2,m_cache_P2);
    points_object[0] = p_object;
    points_object[1] = p2_object;
    // check collision of both points
    {
	IVP_3D_Solver_DISTANCE_OF_TWO_POINTS solver_pp_coll;
	solver_pp_coll.A_object.set( p_object );
	solver_pp_coll.B_object.set( p2_object);

	solver_pp_coll.normized_direction_world_at_t0.set_multiple( & mindist->contact_plane, -1);
#if 0
	IVP_IF(1){
	  IVP_U_Point check_dir;
	  const IVP_U_Point       p_world; IVP_CLS.give_world_coords_AT(P,m_cache_P,&p_world);
	  const IVP_U_Point       p2_world;IVP_CLS.give_world_coords_AT(P2,m_cache_P2,&p2_world);
	  check_dir.subtract(&p2_world, &p_world);
	  check_dir.normize();
	  IVP_DOUBLE val = -mindist->contact_plane.dot_product( &check_dir );
	  IVP_ASSERT(  IVP_Inline_Math::fabsd(val - 1.0f) < 0.0001f );
	}
#endif	
	solver_pp_coll.set_max_deviation(this->max_coll_speed);
	IVP_FLOAT radius = mindist->sum_extra_radius;
	ivp_u_bool found =
	    solver_pp_coll.find_first_t_for_value_coll( mindist->get_coll_dist() + radius,
							ivp_mindist_settings.real_coll_dist + radius * 0.5f, 
							t_now, t_max,  
							&cache_P, &cache_P2, 0, &event_time_out );
	if (found){
	    event_type_out = IVP_COLL_PP_COLL;