ivp_mindist_event.cxx

hl2 source code. Do not use it illegal.

	}    
    }
    // check s vals for sector change
    {
	IVP_DOUBLE deviation[2];	// dev[0] when Point[0] is point and Poit[1] is edge
	IVP_DOUBLE neg_maximal_safe_s_value;	// needed to calc exact sector change value
	{ // deviation calculation 
	    // deviation for s:
	    // speed P + speed P2 + speed of projection (= abs_omega_z * dist P P2 )
	    IVP_DOUBLE max_dist_PP = mindist->get_length();
	    max_dist_PP += this->worst_case_speed * (event_time_out-t_now);
	    
	    IVP_DOUBLE speed_P = p_object->fast_real_length() * solver_P->abs_omega + solver_P->current_speed;
	    IVP_DOUBLE speed_P2 = p2_object->fast_real_length() * solver_P2->abs_omega + solver_P2->current_speed;
	    deviation[0] = speed_P + speed_P2 + max_dist_PP * solver_P2->abs_omega;
	    deviation[1] = speed_P + speed_P2 + max_dist_PP * solver_P->abs_omega;
	    
	    IVP_DOUBLE object_radius = P_MAX(m_cache_P->get_core()->upper_limit_radius, m_cache_P2->get_core()->upper_limit_radius);
	    IVP_DOUBLE quad_coll_dist = mindist->get_coll_dist();
	    quad_coll_dist *= quad_coll_dist;
	    neg_maximal_safe_s_value = - P_MIN(max_dist_PP*max_dist_PP, quad_coll_dist) * 0.5f / object_radius ;
	}

	IVP_U_Matrix_Cache *cache[2];
	cache[0] = &cache_P;
	cache[1] = &cache_P2;

	IVP_Cache_Ledge_Point *mcache[2];
	mcache[0] = m_cache_P;
	mcache[1] = m_cache_P2;
	
	const IVP_Compact_Edge *Point[2];
	Point[0] = P;
	Point[1] = P2;

	
	for (int j=0;j<2;j++){
	  //	    IVP_Compact_Edge *point = Point[j];
	    const IVP_Compact_Edge *point2 = Point[1-j];

	    const IVP_U_Float_Point *p0 = points_object[j];
	    const IVP_U_Float_Point *p1 = points_object[1-j];

	    IVP_3D_Solver_S_VALS solver_pp_pk;
	    solver_pp_pk.P_object.set(p0);
	    solver_pp_pk.K_object.set(p1);
	    solver_pp_pk.set_max_deviation(deviation[j]);

	    for(const IVP_Compact_Edge *e = point2->get_prev()->get_opposite(); 1; e=e->get_prev()->get_opposite()){
	      const IVP_U_Float_Point *epn = IVP_CLS.give_object_coords(e->get_next(), mcache[1-j]);
	      
		solver_pp_pk.K_vec_object.subtract( epn,  p1 );
		IVP_DOUBLE qlen = solver_pp_pk.K_vec_object.quad_length();
		IVP_DOUBLE inv_len = IVP_Inline_Math::isqrt_float(qlen);
		solver_pp_pk.K_vec_object.mult(inv_len);

		IVP_DOUBLE sec_dist = neg_maximal_safe_s_value  * qlen * inv_len;

		if (	solver_pp_pk.find_first_t_for_value_max_dev( sec_dist,   t_now, event_time_out, 0, 
								     cache[j], cache[1-j], 0 , &event_time_out ))
		{
		    event_type_out = IVP_COLL_PP_PK;
		}	
		if(e==point2) break;
	    }
	}
    }
}


/***************************** PP ******************************************/
/***************************** PP ******************************************/
/***************************** PP ******************************************/
void IVP_Mindist_Event_Solver::calc_next_event_BP(IVP_Ball * ball,const  IVP_Compact_Edge *P2,
						    IVP_Cache_Object *m_cache_B,
						    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_B);
    IVP_U_Matrix_Cache cache_P2(m_cache_P2->clp_cache_object);
    
    IVP_DOUBLE sum_abs_omega;
    IVP_Core *solver_P = ball->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;

    IVP_U_Point p_object; p_object.set_to_zero();
    const IVP_U_Float_Point *p2_object = IVP_CLS.give_object_coords(P2,m_cache_P2);

    
    // 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  =  m_cache_B->m_world_f_object.get_position();
	  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;
	    }    
    }
    
    // check s vals for sector change
    {
	IVP_DOUBLE deviation;	// dev[0] when Point[0] is point and Poit[1] is edge
	IVP_DOUBLE neg_maximal_safe_s_value;	// needed to calc exact sector change value
	{ // deviation calculation 
	    // deviation for s:
	    // speed P + speed P2 + speed of projection (= abs_omega_z * dist P P2 )
	    IVP_DOUBLE max_dist_PP = mindist->get_length();
	    max_dist_PP += this->worst_case_speed * (event_time_out-t_now);
	    
	    IVP_DOUBLE speed_P = solver_P->current_speed;
	    IVP_DOUBLE speed_P2 = p2_object->fast_real_length() * solver_P2->abs_omega + solver_P2->current_speed;
	    deviation = speed_P + speed_P2 + max_dist_PP * solver_P2->abs_omega;
	    
	    IVP_DOUBLE inv_object_diameter = m_cache_P2->get_core()->inv_object_diameter;
	    IVP_DOUBLE quad_coll_dist = mindist->get_coll_dist();
	    quad_coll_dist *= quad_coll_dist;
	    neg_maximal_safe_s_value = - P_MIN(max_dist_PP*max_dist_PP, quad_coll_dist) * inv_object_diameter;
	}

	{
	    IVP_3D_Solver_S_VALS solver_pp_pk;
	    solver_pp_pk.P_object.set(&p_object);
	    solver_pp_pk.K_object.set(p2_object);
	    solver_pp_pk.set_max_deviation(deviation);

	    for(const IVP_Compact_Edge *e = P2->get_prev()->get_opposite(); 1; e=e->get_prev()->get_opposite()){
		const IVP_U_Float_Point *epn = IVP_CLS.give_object_coords(e->get_next(), m_cache_P2);
	      
		solver_pp_pk.K_vec_object.subtract( epn,  p2_object );
		IVP_DOUBLE qlen = solver_pp_pk.K_vec_object.quad_length();
		IVP_DOUBLE inv_len = IVP_Inline_Math::isqrt_float(qlen);
		solver_pp_pk.K_vec_object.mult(inv_len);
	      IVP_DOUBLE sec_dist = neg_maximal_safe_s_value * qlen * inv_len;

	      if (	solver_pp_pk.find_first_t_for_value_max_dev( sec_dist,   t_now, event_time_out, 0, 
								     &cache_P, &cache_P2, 0 , &event_time_out ))
		{
		    event_type_out = IVP_COLL_PP_PK;
		}	
		if(e==P2) break;
	    }
	}
    }
}


/***************************** BB ******************************************/
/***************************** BB ******************************************/
/***************************** BB ******************************************/
void IVP_Mindist_Event_Solver::calc_next_event_BB(IVP_Cache_Object *m_cache_A,
						    IVP_Cache_Object *m_cache_B)
{
    // 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_A(m_cache_A);
    IVP_U_Matrix_Cache cache_B(m_cache_B);
    
    event_time_out =  t_max;

    const IVP_U_Point       *A_world  =  m_cache_A->m_world_f_object.get_position();
    const IVP_U_Point       *B_world  =  m_cache_B->m_world_f_object.get_position();
    
    // check collision of both points
    {
	// @@@ optimize !!!!!
	IVP_3D_Solver_DISTANCE_OF_TWO_POINTS solver_pp_coll;
	solver_pp_coll.A_object.set_to_zero();
	solver_pp_coll.B_object.set_to_zero();
	solver_pp_coll.normized_direction_world_at_t0.subtract(B_world, A_world);
	solver_pp_coll.normized_direction_world_at_t0.normize();
	
	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_A, &cache_B, 0, &event_time_out );
	    if (found){
		event_type_out = IVP_COLL_PP_COLL;
	    }    
    }    
}



/***************************** PK ******************************************/
/***************************** PK ******************************************/
void IVP_Mindist_Event_Solver::calc_next_event_PK(const IVP_Compact_Edge *P,
				     const IVP_Compact_Edge *K,
				     IVP_Cache_Ledge_Point *m_cache_P,
				     IVP_Cache_Ledge_Point *m_cache_K)
{


    IVP_U_Matrix_Cache cache_P(m_cache_P->clp_cache_object);
    IVP_U_Matrix_Cache cache_K(m_cache_K->clp_cache_object);
    

    IVP_DOUBLE sum_abs_omega;
    IVP_Core *solver_P = m_cache_P->get_core();
    IVP_Core *solver_K = m_cache_K->get_core();
    sum_abs_omega = solver_P->abs_omega + solver_K->abs_omega;

    event_time_out =  t_max;

    const IVP_U_Float_Point *p_object = IVP_CLS.give_object_coords(P,m_cache_P);
    
    // check collision of point and edge
    {
	IVP_3D_Solver_PK_COLL solver_pk_coll;
	const IVP_U_Float_Point *k_object =  IVP_CLS.give_object_coords(K,m_cache_K);
	const IVP_U_Float_Point *kn_object =  IVP_CLS.give_object_coords(K->get_next(),m_cache_K);

	solver_pk_coll.P_object.set( p_object );
	solver_pk_coll.K_object.set(k_object);
	solver_pk_coll.K_vec_object.subtract( kn_object,  k_object);
	solver_pk_coll.K_vec_object.fast_normize();
	solver_pk_coll.set_max_deviation(this->max_coll_speed);
	IVP_FLOAT radius = mindist->sum_extra_radius;
	solver_pk_coll.plane_offset = 0.5f * (mindist->get_coll_dist() + radius);

	// find initial virtual collision plane
	{
	    IVP_U_Point P_world;
	    IVP_U_Point K_world;
	    IVP_U_Point K_vec_world;	// normized direction
	    m_cache_P->get_object_cache()->transform_position_to_world_coords( &solver_pk_coll.P_object, &P_world);
	    m_cache_K->get_object_cache()->transform_position_to_world_coords( &solver_pk_coll.K_object, &K_world);
	    m_cache_K->get_object_cache()->transform_vector_to_world_coords( &solver_pk_coll.K_vec_object, &K_vec_world);

	    IVP_U_Point PK;
	    PK.subtract(&K_world,&P_world);
	    IVP_U_Point H;
	    H.calc_cross_product( &PK, &K_vec_world);

	    m_cache_K->get_object_cache()->transform_vector_to_object_coords( &H, &solver_pk_coll.K_Lot_object);
	    solver_pk_coll.K_Lot_object.normize();
	}
	
	ivp_u_bool found =
	    solver_pk_coll.find_first_t_for_value_coll( mindist->get_coll_dist() + radius,
							ivp_mindist_settings.real_coll_dist + radius * 0.9f, 
							t_now, t_max,  
							&cache_P, &cache_K, 0, &event_time_out );
	    if (found){
		event_type_out = IVP_COLL_PK_COLL;
	    }    
    }


    /***** find next sector change *****/

    /** first: when does case 'P over F0/F1 of K' occur? **/
    
    // both areas F0/F1
    {
	IVP_3D_Solver_PF_COLL solver_pk_pf;
	solver_pk_pf.set_max_deviation(this->worst_case_speed);	// when does point P move through lot area

	solver_pk_pf.point_object.set(p_object);
	
	const IVP_Compact_Edge *FF[2];
	FF[0] = K;
	FF[1] = K->get_opposite();
    
	int i;
	for(i=0; i<=1; i++){
	    // Generation of  collision surface for PF sector  lot = (hesse) x edge 0
	    const IVP_Compact_Edge *F = FF[i]; // area to consider
	    IVP_U_Point v0;
	    const IVP_U_Float_Point *f_object = IVP_CLS.give_object_coords(F,m_cache_K);
	    const IVP_U_Float_Point *fn_object = IVP_CLS.give_object_coords(F->get_next(),m_cache_K);
	    const IVP_U_Float_Point *fp_object = IVP_CLS.give_object_coords(F->get_prev(),m_cache_K);
	    v0.subtract( fn_object, f_object );// @@@ already calculated	    
	    IVP_U_Point fhesse_vec;
	    fhesse_vec.inline_set_vert_to_area_defined_by_three_points(f_object, fp_object, fn_object);
	    solver_pk_pf.hesse_of_area_object.calc_cross_product(&v0, &fhesse_vec);
	    solver_pk_pf.hesse_of_area_object.normize();	// @@@ fast normize 
	    
	    solver_pk_pf.point_of_area_object.set(f_object);
	    
	    IVP_DOUBLE sec_dist = -ivp_mindist_settings.mindist_change_force_dist;
	    ivp_u_bool found_flag =
		solver_pk_pf.find_first_t_for_value_max_dev( sec_dist,   t_now, event_time_out, 0, 
							     &cache_P, &cache_K, 0 , &event_time_out );
	    if (found_flag){
		event_type_out = IVP_COLL_PK_PF;
	    }	
	}
    }
    
    // check s vals for sector change

    // idea: project P on K -> Q
    // calc s_values of Q on neighbouring edges of P
    // advantage: max_deviation can be calculated