ivp_sphere_solver.cxx

hl2 source code. Do not use it illegal.

#include <ivp_physics.hxx>
#include <ivp_clustering_longrange.hxx>
#include <ivp_cache_object.hxx>
#include <ivp_surman_polygon.hxx>
#include <ivp_compact_surface.hxx>
#include <ivp_compact_ledge.hxx>

#include <ivp_sphere_solver.hxx>

/////////////////////////////////////////////////////////////////////
// start of file local helpers
/////////////////////////////////////////////////////////////////////

static inline bool AABB_overlaps_AABB_d(IVP_U_Float_Point* rlb1, 
											IVP_U_Float_Point* pos1,
											IVP_U_Float_Point* rlb2, 
											IVP_U_Float_Point* pos2)
{
	if(IVP_Inline_Math::fabsd(pos1->k[0] - pos2->k[0]) > (rlb1->k[0] + rlb2->k[0]))
		return false;

	if(IVP_Inline_Math::fabsd(pos1->k[1] - pos2->k[1]) > (rlb1->k[1] + rlb2->k[1]))
		return false;

	if(IVP_Inline_Math::fabsd(pos1->k[2] - pos2->k[2]) > (rlb1->k[2] + rlb2->k[2]))
		return false;

	return true;
}

static inline IVP_BOOL sphere_overlaps_sphere_d(IVP_U_Point* pos1, 
												IVP_DOUBLE rad1,
												const IVP_U_Point* pos2, 
												IVP_FLOAT rad2)
{
	IVP_U_Float_Point center_distance_vec;
	center_distance_vec.subtract(pos1, pos2);

	IVP_FLOAT center_distance = center_distance_vec.quad_length();

	if( center_distance > (rad1 + rad2) * (rad1 + rad2))
		return IVP_FALSE;
	else
		return IVP_TRUE;
}

static IVP_BOOL found_valid_sphere_node = IVP_FALSE;

static inline void sphere_against_ledgetree_node(const IVP_Compact_Ledgetree_Node* cln, IVP_U_Point* query_pos, IVP_DOUBLE query_rad, int current_depth, int max_depth)
{
	IVP_U_Point node_pos(cln->center.k[0], cln->center.k[1], cln->center.k[2]);
	IVP_FLOAT node_rad = cln->radius;

	if(sphere_overlaps_sphere_d(query_pos, query_rad, &node_pos, node_rad))
	{
		if(current_depth < max_depth && !cln->is_terminal())
		{
			sphere_against_ledgetree_node(cln->left_son(), query_pos, query_rad, current_depth + 1, max_depth);
			sphere_against_ledgetree_node(cln->right_son(), query_pos, query_rad, current_depth + 1, max_depth);
		}
		else
		{
			found_valid_sphere_node = IVP_TRUE;
		}
	}

	return;
}

/**
 * If obj has more than one convex part, sphere_against_object will traverse
 * the obj sphere tree down as far as max_depth, and use that set of spheres
 * for culling against the query sphere, rather than just use the bounding sphere
 * (i.e. sphere at the root of the sphere tree). This should give a better
 * approximation of the object being queried.
 */
static inline IVP_BOOL sphere_against_object(IVP_Real_Object* obj, IVP_U_Point* query_pos, IVP_DOUBLE query_rad, int max_depth)
{
	found_valid_sphere_node = IVP_FALSE;

	if(obj->get_type() == IVP_POLYGON)
	{
		const IVP_Compact_Surface* cs = static_cast<IVP_SurfaceManager_Polygon*>(obj->get_surface_manager())->get_compact_surface();
		const IVP_Compact_Ledgetree_Node* cln = cs->get_compact_ledge_tree_root();

		sphere_against_ledgetree_node(cln, query_pos, query_rad, 0, max_depth);

		return found_valid_sphere_node;
	}
	else if(obj->get_type() == IVP_BALL)
	{
		return sphere_overlaps_sphere_d(query_pos, query_rad, obj->get_core()->get_position_PSI(), obj->get_core()->upper_limit_radius);
	}

	return IVP_FALSE;
}

/////////////////////////////////////////////////////////////////////
// end of file local helpers
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
// Implementation of IVP_Sphere_Solver begins
/////////////////////////////////////////////////////////////////////

IVP_BOOL IVP_Sphere_Solver::check_sphere_against_object(const IVP_Sphere_Solver_Template* tmpl, IVP_Real_Object* obj)
{
	IVP_U_Point obj_center;

	m_center.set(&tmpl->center);
	m_radius = tmpl->radius;
	m_max_traversal_depth = tmpl->max_traversal_depth;

	IVP_Cache_Object* co = obj->get_cache_object();
	co->transform_position_to_object_coords(&m_center, &obj_center);
	co->remove_reference();

	return sphere_against_object(obj, &obj_center, m_radius, m_max_traversal_depth);
}

void IVP_Sphere_Solver::check_sphere_against_environment(const IVP_Sphere_Solver_Template* tmpl, IVP_Environment* env)
{
	m_center.set(&tmpl->center);
	m_radius = tmpl->radius;
	m_max_traversal_depth = tmpl->max_traversal_depth;

	m_tree_manager = env->get_ov_tree_manager();
	IVP_OV_Node* root = m_tree_manager->root;

	IVP_U_Float_Point sphere_bb_rlb, sphere_bb_pos;
	
	sphere_bb_rlb.set(m_radius, m_radius, m_radius);
	sphere_bb_pos.set(m_center.k[0], m_center.k[1], m_center.k[2]);

	recursively_collect_intruding_objects(root, &sphere_bb_rlb, &sphere_bb_pos);
	return;
}

void IVP_Sphere_Solver::recursively_collect_intruding_objects(IVP_OV_Node* node, IVP_U_Float_Point* query_rlb, IVP_U_Float_Point* query_pos)
{
	IVP_U_Float_Point node_luf, node_rlb, node_pos;
	IVP_FLOAT node_size;

	m_tree_manager->get_luf_coordinates_ws(node, &node_luf, &node_size);
	node_rlb.set(node_size / 2.0f, node_size / 2.0f, node_size / 2.0f);
	node_pos.set(node_luf.k[0] + (node_size / 2.0f), node_luf.k[1] + (node_size / 2.0f), node_luf.k[2] + (node_size / 2.0f));

	{
		if(AABB_overlaps_AABB_d(&node_rlb, &node_pos, query_rlb, query_pos))
		{
			for(int i = 0; i < node->elements.len(); i++)
			{
				IVP_Real_Object* query_object = node->elements.element_at(i)->real_object;

				const IVP_U_Point* pos = query_object->get_core()->get_position_PSI();
				IVP_FLOAT rad = query_object->get_core()->upper_limit_radius;

				// check object bounding sphere against sphere query
				if(sphere_overlaps_sphere_d(&m_center, m_radius, pos, rad))
				{
					IVP_U_Point query_center_object_space;

					// transform the query sphere center into object local space
					IVP_Cache_Object* co = query_object->get_cache_object();
					co->transform_position_to_object_coords(&m_center, &query_center_object_space);

					if(sphere_against_object(query_object, &query_center_object_space, m_radius, m_max_traversal_depth))
					{
						m_intruding_objects.add(query_object);
					}

					co->remove_reference();
				}
			}

			{
				for(int i = 0; i < node->children.len(); i++)
				{
					recursively_collect_intruding_objects(node->children.element_at(i), query_rlb, query_pos);
				}
			}
		}
	}

	return;
}

/////////////////////////////////////////////////////////////////////
// Implementation of IVP_Sphere_Solver ends
/////////////////////////////////////////////////////////////////////