ivp_compact_ledge_solver.cxx

hl2 source code. Do not use it illegal.

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

/********************************************************************************
 *	File:	       	ivp_compact_ledge_solver.cxx	
 *	Description:	function collection
 ********************************************************************************/

#include <ivp_physics.hxx>
#include <ivu_float.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_compact_surface.hxx>
#ifdef HAVOK_MOPP
#include <hk_mopp/ivp_compact_mopp.hxx>
#endif // HAVOK_MOPP
 
IVP_Compact_Ledge_Solver IVP_CLS;

///////////////////////////////////////////////////////////////////////////////////////


/********************************************************************************
 *	Names:	       	Bounding box and radius calculation 
 ********************************************************************************/

void IVP_Compact_Ledge_Solver::calc_bounding_box(    const IVP_Compact_Ledge *c_ledge_in,
						     IVP_U_Point *min_extents_out,
						     IVP_U_Point *max_extents_out)
{
    // calc object axis aligned bounding box
    
    // @@@ for point array might be shared, we walk through all points of all triangles!
    // @@@ this is trice as much as optimum...

    const IVP_Compact_Triangle *tri = c_ledge_in->get_first_triangle();
    IVP_U_Float_Point min_extents, max_extents; // to avoid many casts

    {	// set min max to any point
	const IVP_Compact_Edge *first_edge = tri->get_first_edge();
	const IVP_U_Float_Point *any_ledge_point = give_object_coords( first_edge, c_ledge_in);
	min_extents.set(any_ledge_point);
	max_extents.set(any_ledge_point);
    }

    int tri_cnt = c_ledge_in->get_n_triangles()-1;
    for( ; tri_cnt>=0; tri_cnt--,tri=tri->get_next_tri()){
	int edge_cnt;
	const IVP_Compact_Edge *edge;
	for( edge_cnt = 2, edge = tri->get_first_edge();
	     edge_cnt>=0;
	     edge_cnt--,edge=edge->get_next()){
	    const IVP_U_Float_Point *p = give_object_coords(edge,c_ledge_in);

	    if(p->k[0] < min_extents.k[0]){
		min_extents.k[0] = p->k[0];
	    }else if (p->k[0] > max_extents.k[0])
		max_extents.k[0] = p->k[0];
	    
	    if(p->k[1] < min_extents.k[1]){
		min_extents.k[1] = p->k[1];
	    }else if (p->k[1] > max_extents.k[1])
		max_extents.k[1] = p->k[1];

	    if(p->k[2] < min_extents.k[2]){
		min_extents.k[2] = p->k[2];
	    }else if (p->k[2] > max_extents.k[2])
		max_extents.k[2] = p->k[2];
	}
    }
    min_extents_out->set(&min_extents);
    max_extents_out->set(&max_extents);
}


void IVP_Compact_Ledge_Solver::get_all_ledges(const IVP_Compact_Ledgetree_Node *node, IVP_U_BigVector<IVP_Compact_Ledge> *ledges_out)
{
    if (node->is_terminal()) {
	const IVP_Compact_Ledge *ledge = node->get_compact_ledge();
	ledges_out->add((IVP_Compact_Ledge *)ledge );
    }
    else{
	const IVP_Compact_Ledgetree_Node *ls = node->left_son();
	const IVP_Compact_Ledgetree_Node *rs = node->right_son();
	get_all_ledges(ls,ledges_out);
	get_all_ledges(rs,ledges_out);
    }
}

/********************************************************************************
 *	Names:	       	calc_geom_center_and_radius
 *	Description:	calc the center based on the bounding box
 ********************************************************************************/

void IVP_Compact_Ledge_Solver::calc_geom_center_and_radius( const IVP_Compact_Ledge *c_ledge_in, IVP_U_Point *geom_center_out, IVP_DOUBLE *geom_radius_out)
{
    // simply calc bounding box and take center and radius from there
    
    IVP_U_Point min_extents, max_extents;
    IVP_CLS.calc_bounding_box(c_ledge_in, &min_extents, &max_extents);

    geom_center_out->set_interpolate(&min_extents, &max_extents, 0.5f);
    *geom_radius_out = 0.5f * IVP_Inline_Math::sqrtd(min_extents.quad_distance_to(&max_extents));
};


void IVP_Compact_Ledge_Solver::get_all_ledges( const IVP_Compact_Surface* surface, IVP_U_BigVector<IVP_Compact_Ledge> *all_ledges_out )
{
    get_all_ledges( surface->get_compact_ledge_tree_root(), all_ledges_out);
}



#ifdef HAVOK_MOPP
void IVP_Compact_Ledge_Solver::get_all_ledges( const IVP_Compact_Mopp* mopp, IVP_U_BigVector<IVP_Compact_Ledge>* all_ledges_out)
{
	char* ledge = (char*)mopp + mopp->offset_ledges + mopp->size_convex_hull;

	char* point0 = (char*)(((IVP_Compact_Ledge*)ledge)->get_point_array());

	do
	{
		IVP_ASSERT(((unsigned int)(ledge) & 0xf) == 0);

		all_ledges_out->add((IVP_Compact_Ledge*)ledge);

		ledge += ((IVP_Compact_Ledge*)ledge)->get_size();

//		ledge = (IVP_Compact_Ledge*)((char*)ledge + 48); // @@CB big hack !! only supports triangles !!
	} while(ledge != point0);

	return;
}
#endif // HAVOK_MOPP



void IVP_Compact_Ledge_Solver::calc_radius_to_given_center( const IVP_Compact_Ledge *ledge,
							    const IVP_U_Point *center_in,
							    IVP_DOUBLE *radius_out,
							    IVP_DOUBLE *radius_dev_out)
{    
    IVP_DOUBLE q_max = 0.0f;
    IVP_DOUBLE dev_max = 0.0f;

    const IVP_Compact_Triangle *tri;
    int t;
    for (t = ledge->get_n_triangles()-1, tri = ledge->get_first_triangle();
	 t >= 0 ;
	 t--, tri= tri->get_next_tri() ){
	
	const IVP_Compact_Edge *e = tri->get_first_edge();
	IVP_U_Point hesse_vec;
	IVP_CLS.calc_hesse_vec_object_not_normized(e, ledge, &hesse_vec);
	IVP_DOUBLE i_hv_len_squared = 1.0f / hesse_vec.quad_length();
	
	int j;
	for(j=0; j<3; j++, e++){
	    IVP_U_Point p;
	    const IVP_U_Float_Point *p_orig = IVP_CLS.give_object_coords(e, ledge);
	    p.subtract( p_orig , center_in );
	    {	// radius
		IVP_DOUBLE q = p.quad_length();
		if ( q > q_max) q_max = q;
	    }
	    {
		IVP_U_Point cp;
		cp.inline_calc_cross_product(&p, &hesse_vec);
		IVP_DOUBLE dev = cp.quad_length() * i_hv_len_squared;
		if (dev > dev_max){
		    dev_max = dev;
		}
	    }
	}
    }
	 q_max = IVP_Inline_Math::sqrtd(q_max);
    if (q_max > *radius_out) {
	*radius_out = q_max;
    }
    if (radius_dev_out){
	dev_max = IVP_Inline_Math::sqrtd(dev_max);
	if (dev_max > *radius_dev_out){
	    *radius_dev_out = dev_max;
	}
    }
}

#ifdef HAVOK_MOPP
void IVP_Compact_Ledge_Solver::calc_radius_to_given_center(const IVP_Compact_Mopp* c_mopp_in,
	const IVP_U_Point* center_in,
	IVP_DOUBLE* radius_out,
	IVP_DOUBLE* radius_dev_out)
{    
	*radius_out = 0;
	*radius_dev_out = 0;
	IVP_U_BigVector<IVP_Compact_Ledge> all_ledges(1024);
	get_all_ledges(c_mopp_in, &all_ledges );

	for (int l = all_ledges.len()-1; l>=0;l--)
	{
		const IVP_Compact_Ledge *ledge = all_ledges.element_at(l);
		calc_radius_to_given_center(ledge,center_in,radius_out,radius_dev_out);
	}
}
#endif // HAVOK_MOPP

void IVP_Compact_Ledge_Solver::calc_radius_to_given_center( const IVP_Compact_Surface *c_surface_in,
							    const IVP_U_Point *center_in,
							    IVP_DOUBLE *radius_out,
							    IVP_DOUBLE *radius_dev_out)
{    
    *radius_out = 0;
    *radius_dev_out = 0;
    IVP_U_BigVector<IVP_Compact_Ledge> all_ledges(1024);
    get_all_ledges(c_surface_in, &all_ledges );
    
    for (int l = all_ledges.len()-1; l>=0;l--){
	const IVP_Compact_Ledge *ledge = all_ledges.element_at(l);
	calc_radius_to_given_center(ledge,center_in,radius_out,radius_dev_out);
    }
}

void IVP_Compact_Ledge_Solver::calc_pos_other_space(const IVP_Compact_Edge *P,IVP_Cache_Ledge_Point *m_cache_P,
						       IVP_Cache_Ledge_Point *m_cache_other_space, IVP_U_Point *res){
    const IVP_U_Float_Point *p_os = IVP_CLS.give_object_coords( P, m_cache_P );
    IVP_U_Point p_ws;
    m_cache_P->get_object_cache()->m_world_f_object.inline_vmult4(p_os, &p_ws);
    m_cache_other_space->clp_cache_object->m_world_f_object.inline_vimult4(&p_ws, res);
}

void IVP_Compact_Ledge_Solver::transform_vec_other_space(const IVP_U_Point *dir_os, IVP_Cache_Ledge_Point *m_cache_dir,
						       IVP_Cache_Ledge_Point *m_cache_other_space, IVP_U_Point *res){
  IVP_U_Point dir_ws;
  m_cache_dir->clp_cache_object->m_world_f_object.inline_vmult3(dir_os, &dir_ws);
  m_cache_other_space->clp_cache_object->m_world_f_object.inline_vimult3(&dir_ws, res);
}

void IVP_Compact_Ledge_Solver::transform_pos_other_space(const IVP_U_Float_Point *pos_in_os, IVP_Cache_Ledge_Point *m_cache_dir,
						       IVP_Cache_Ledge_Point *m_cache_other_space, IVP_U_Point *res){
  IVP_U_Point dir_ws;
  IVP_U_Point pos_os(pos_in_os);
  m_cache_dir->clp_cache_object->m_world_f_object.inline_vmult4(&pos_os, &dir_ws);
  m_cache_other_space->clp_cache_object->m_world_f_object.inline_vimult4(&dir_ws, res);
}
#ifdef DEBUG
/********************************************************************************
 *	Names:	       	calc s_vals
 *	Description:	find the point on an edge with the shortest distance to a given other point
 *			and return the position of that point on the edge as a relative
 *			value between 0 and 1:   0 means projected point is identical to first point of edge
 *						1 means projected point is identical to second point od the edge
 ********************************************************************************/
IVP_DOUBLE IVP_Compact_Ledge_Solver::calc_s_val(const IVP_Compact_Edge *edge, const IVP_U_Point *p_world, IVP_Cache_Ledge_Point *m_cache_edge)
{
     // calcs intersect pos
     // von lot von p auf this (rel. zu this)
    IVP_U_Point tp;  give_world_coords_AT(edge, m_cache_edge, &tp);
    IVP_U_Point tp_next; give_world_coords_AT(edge->get_next(), m_cache_edge, &tp_next);
    
    IVP_U_Point vec1, vec2;
    vec1.subtract(&tp_next, &tp);
    vec2.subtract(p_world, &tp);
    IVP_DOUBLE i_qlen = 1.0f / vec1.quad_length();
    IVP_DOUBLE s = vec1.dot_product(&vec2);
    s *= i_qlen;
    return s;
}
#endif

void IVP_Compact_Ledge_Solver::calc_unscaled_s_val_K_space(const IVP_Compact_Ledge *c_ledge, const IVP_Compact_Edge *edge, const IVP_U_Point *p_object,IVP_Unscaled_S_Result *result)
{
     // calcs intersect pos
     // von lot von p auf this (rel. zu this)
 
    const IVP_U_Float_Point *p0 = give_object_coords(edge, c_ledge);
    const IVP_U_Float_Point *p1 = give_object_coords(edge->get_next(), c_ledge);
    IVP_U_Point d; d.subtract( p1, p0 );
    IVP_U_Point d0; d0.subtract( p0, p_object );
    IVP_U_Point d1; d1.subtract( p1, p_object );
    IVP_DOUBLE a = -d.dot_product( &d0 );
    IVP_DOUBLE b = d.dot_product( &d1 );
    result->checks[0] = a;
    result->checks[1] = b;
}



// calcs pos of lot von p auf
// plane spanned by -e_tri (=Q) and this->next (=R)

// q, r relate to:
//    origin = e_tri->next
//    Q = e_tri - e_tri->next (opposite to e_tri!)
//    R = e_tri->prev - e_tri->next
// for debugging only
#ifdef DEBUG
void IVP_Compact_Ledge_Solver::calc_qr_vals(const IVP_Compact_Edge *e_tri,const  IVP_U_Point *p_world,
					    IVP_DOUBLE *out_q, IVP_DOUBLE *out_r,
					    IVP_Cache_Ledge_Point *m_cache_e_tri)
{
    IVP_U_Point tp; IVP_CLS.give_world_coords_AT(e_tri, m_cache_e_tri, &tp);
    IVP_U_Point tp_next; IVP_CLS.give_world_coords_AT(e_tri->get_next(), m_cache_e_tri, &tp_next);
    IVP_U_Point tp_prev; IVP_CLS.give_world_coords_AT(e_tri->get_prev(), m_cache_e_tri, &tp_prev);
    
    IVP_U_Point R, Q, Pvec;
    Q.subtract(&tp, &tp_next); // other dir
    R.subtract(&tp_prev, &tp_next);
    
    IVP_DOUBLE QQ = Q.quad_length();
    IVP_DOUBLE RR = R.quad_length();
       
    IVP_DOUBLE QR = R.dot_product(&Q);

    IVP_DOUBLE QQRR = QQ * RR;
    IVP_DOUBLE QRQR = QR * QR;
    IVP_DOUBLE i_det = 1.0f / (QQRR - QRQR);

    Pvec.subtract( p_world, &tp_next);
    IVP_DOUBLE sq = Pvec.dot_product(&Q);
    IVP_DOUBLE sr = Pvec.dot_product(&R);
    
    *out_q = i_det * ( RR * sq - sr * QR );
    *out_r = i_det * ( QQ * sr - sq * QR );
}
#endif

#ifdef DEBUG
void IVP_Compact_Ledge_Solver::calc_qr_vals(const IVP_Compact_Edge *e_tri,const  IVP_Compact_Edge *p,
			     IVP_DOUBLE *out_q, IVP_DOUBLE *out_r,
			     IVP_Cache_Ledge_Point *m_cache_e_tri, IVP_Cache_Ledge_Point *m_cache_p)
{
    IVP_U_Point pp; IVP_CLS.give_world_coords_AT(p, m_cache_p, &pp);
    calc_qr_vals( e_tri, &pp, out_q, out_r, m_cache_e_tri);
}
#endif

/* rounding error of calc_unscaled_qr_vals_F_space:
   input:  p_object    error (P_MAX_WORLD_DOUBLE * P_DOUBLE_RES * ( 1 + x ) || inf)
           tp, tp_next, tp_prv 0 epsilon
 */

static char ivp_uqr_mod_table[] = { 0,2 * sizeof(IVP_FLOAT),1 * sizeof(IVP_FLOAT),0,2 * sizeof(IVP_FLOAT) };

void IVP_Compact_Ledge_Solver::calc_unscaled_qr_vals_F_space(const IVP_Compact_Ledge *c_ledge,
							     const IVP_Compact_Edge *tri, const IVP_U_Point *p_object,
							     IVP_Unscaled_QR_Result *result){
    // like calc_qr_vals(), all in F coord space
  const int this_edge_index = tri->get_edge_index();
  
  const IVP_Compact_Triangle *triangle = tri->get_triangle();
  
    const IVP_U_Float_Point *tp = give_object_coords( triangle->get_edge(0), c_ledge);
    const IVP_U_Float_Point *tp_next = give_object_coords(triangle->get_edge(1), c_ledge);
    const IVP_U_Float_Point *tp_prev = give_object_coords(triangle->get_edge(2), c_ledge);
    
    IVP_U_Point R, Q, Pvec;
    Q.subtract(tp, tp_next);      
    R.subtract(tp_prev, tp_next);

    IVP_DOUBLE QQ = Q.quad_length();
    IVP_DOUBLE RR = R.quad_length();
    
    Pvec.subtract( p_object, tp_next);
    
    IVP_DOUBLE QR = R.dot_product(&Q);

    IVP_DOUBLE QQRR = QQ * RR;
    IVP_DOUBLE QRQR = QR * QR;
    IVP_DOUBLE Det = (QQRR - QRQR);