text:
#ifndef GIM_BOX_SET_H_INCLUDED
#define GIM_BOX_SET_H_INCLUDED

/*! \file gim_box_set.h
\author Francisco Leon Najera
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.
   (3) The zlib/libpng license that is included with this library in
       the file GIMPACT-LICENSE-ZLIB.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/

#include "gim_array.h"
#include "gim_radixsort.h"
#include "gim_box_collision.h"
#include "gim_tri_collision.h"

//! Overlapping pair
struct GIM_PAIR
{
    GUINT m_index1;
    GUINT m_index2;
    GIM_PAIR()
    {}

    GIM_PAIR(const GIM_PAIR & p)
    {
    	m_index1 = p.m_index1;
    	m_index2 = p.m_index2;
	}

	GIM_PAIR(GUINT index1, GUINT index2)
    {
    	m_index1 = index1;
    	m_index2 = index2;
	}
};

//! A pairset array
class gim_pair_set: public gim_array<GIM_PAIR>
{
public:
	gim_pair_set():gim_array<GIM_PAIR>(32)
	{
	}
	inline void push_pair(GUINT index1,GUINT index2)
	{
		push_back(GIM_PAIR(index1,index2));
	}

	inline void push_pair_inv(GUINT index1,GUINT index2)
	{
		push_back(GIM_PAIR(index2,index1));
	}
};

//! Prototype Base class for primitive classification
/*!
This class is a wrapper for primitive collections.
This tells relevant info for the Bounding Box set classes, which take care of space classification.
This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the  Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
*/
class GIM_PRIMITIVE_MANAGER_PROTOTYPE
{
public:

	virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {}
	//! determines if this manager consist on only triangles, which special case will be optimized
	virtual bool is_trimesh() = 0;
	virtual GUINT get_primitive_count() = 0;
	virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0;
	virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0;
};

struct GIM_AABB_DATA
{
	GIM_AABB m_bound;
	GUINT m_data;
};

//! Node Structure for trees
struct GIM_BOX_TREE_NODE
{
	GIM_AABB m_bound;
	GUINT m_left;//!< Left subtree
	GUINT m_right;//!< Right subtree
	GUINT m_escapeIndex;//!< Scape index for traversing
	GUINT m_data;//!< primitive index if apply

	GIM_BOX_TREE_NODE()
	{
	    m_left = 0;
	    m_right = 0;
	    m_escapeIndex = 0;
	    m_data = 0;
	}

	SIMD_FORCE_INLINE bool is_leaf_node() const
	{
	    return  (!m_left && !m_right);
	}
};

//! Basic Box tree structure
class GIM_BOX_TREE
{
protected:
	GUINT m_num_nodes;
	gim_array<GIM_BOX_TREE_NODE> m_node_array;
protected:
	GUINT _sort_and_calc_splitting_index(
		gim_array<GIM_AABB_DATA> & primitive_boxes,
		 GUINT startIndex,  GUINT endIndex, GUINT splitAxis);

	GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);

	void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,  GUINT endIndex);
public:
	GIM_BOX_TREE()
	{
		m_num_nodes = 0;
	}

	//! prototype functions for box tree management
	//!@{
	void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes);

	SIMD_FORCE_INLINE void clearNodes()
	{
		m_node_array.clear();
		m_num_nodes = 0;
	}

	//! node count
	SIMD_FORCE_INLINE GUINT getNodeCount() const
	{
		return m_num_nodes;
	}

	//! tells if the node is a leaf
	SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
	{
		return m_node_array[nodeindex].is_leaf_node();
	}

	SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
	{
		return m_node_array[nodeindex].m_data;
	}

	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
	{
		bound = m_node_array[nodeindex].m_bound;
	}

	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
	{
		m_node_array[nodeindex].m_bound = bound;
	}

	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex)  const
	{
		return m_node_array[nodeindex].m_left;
	}

	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex)  const
	{
		return m_node_array[nodeindex].m_right;
	}

	SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
	{
		return m_node_array[nodeindex].m_escapeIndex;
	}

	//!@}
};

//! Generic Box Tree Template
/*!
This class offers an structure for managing a box tree of primitives.
Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and
a Box tree structure ( like GIM_BOX_TREE).
*/
template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
class GIM_BOX_TREE_TEMPLATE_SET
{
protected:
	_GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager;
	_GIM_BOX_TREE_PROTOTYPE m_box_tree;
protected:
	//stackless refit
	SIMD_FORCE_INLINE void refit()
	{
		GUINT nodecount = getNodeCount();
		while(nodecount--)
		{
			if(isLeafNode(nodecount))
			{
				GIM_AABB leafbox;
				m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox);
				setNodeBound(nodecount,leafbox);
			}
			else
			{
				//get left bound
				GUINT childindex = getLeftNodeIndex(nodecount);
				GIM_AABB bound;
				getNodeBound(childindex,bound);
				//get right bound
				childindex = getRightNodeIndex(nodecount);
				GIM_AABB bound2;
				getNodeBound(childindex,bound2);
				bound.merge(bound2);

				setNodeBound(nodecount,bound);
			}
		}
	}
public:

	GIM_BOX_TREE_TEMPLATE_SET()
	{
	}

	SIMD_FORCE_INLINE GIM_AABB getGlobalBox()  const
	{
		GIM_AABB totalbox;
		getNodeBound(0, totalbox);
		return totalbox;
	}

	SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager)
	{
		m_primitive_manager = primitive_manager;
	}

	const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const
	{
		return m_primitive_manager;
	}

	_GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager()
	{
		return m_primitive_manager;
	}

//! node manager prototype functions
///@{

	//! this attemps to refit the box set.
	SIMD_FORCE_INLINE void update()
	{
		refit();
	}

	//! this rebuild the entire set
	SIMD_FORCE_INLINE void buildSet()
	{
		//obtain primitive boxes
		gim_array<GIM_AABB_DATA> primitive_boxes;
		primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false);

		for (GUINT i = 0;i<primitive_boxes.size() ;i++ )
		{
			 m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound);
			 primitive_boxes[i].m_data = i;
		}

		m_box_tree.build_tree(primitive_boxes);
	}

	//! returns the indices of the primitives in the m_primitive_manager
	SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const
	{
		GUINT curIndex = 0;
		GUINT numNodes = getNodeCount();

		while (curIndex < numNodes)
		{
			GIM_AABB bound;
			getNodeBound(curIndex,bound);

			//catch bugs in tree data

			bool aabbOverlap = bound.has_collision(box);
			bool isleafnode = isLeafNode(curIndex);

			if (isleafnode && aabbOverlap)
			{
				collided_results.push_back(getNodeData(curIndex));
			}

			if (aabbOverlap || isleafnode)
			{
				//next subnode
				curIndex++;
			}
			else
			{
				//skip node
				curIndex+= getScapeNodeIndex(curIndex);
			}
		}
		if(collided_results.size()>0) return true;
		return false;
	}

	//! returns the indices of the primitives in the m_primitive_manager
	SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box,
		 const btTransform & transform, gim_array<GUINT> & collided_results) const
	{
		GIM_AABB transbox=box;
		transbox.appy_transform(transform);
		return boxQuery(transbox,collided_results);
	}

	//! returns the indices of the primitives in the m_primitive_manager
	SIMD_FORCE_INLINE bool rayQuery(
		const btVector3 & ray_dir,const btVector3 & ray_origin ,
		gim_array<GUINT> & collided_results) const
	{
		GUINT curIndex = 0;
		GUINT numNodes = getNodeCount();

		while (curIndex < numNodes)
		{
			GIM_AABB bound;
			getNodeBound(curIndex,bound);

			//catch bugs in tree data

			bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
			bool isleafnode = isLeafNode(curIndex);

			if (isleafnode && aabbOverlap)
			{
				collided_results.push_back(getNodeData( curIndex));
			}

			if (aabbOverlap || isleafnode)
			{
				//next subnode
				curIndex++;
			}
			else
			{
				//skip node
				curIndex+= getScapeNodeIndex(curIndex);
			}
		}
		if(collided_results.size()>0) return true;
		return false;
	}

	//! tells if this set has hierarcht
	SIMD_FORCE_INLINE bool hasHierarchy() const
	{
		return true;
	}

	//! tells if this set is a trimesh
	SIMD_FORCE_INLINE bool isTrimesh()  const
	{
		return m_primitive_manager.is_trimesh();
	}

	//! node count
	SIMD_FORCE_INLINE GUINT getNodeCount() const
	{
		return m_box_tree.getNodeCount();
	}

	//! tells if the node is a leaf
	SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
	{
		return m_box_tree.isLeafNode(nodeindex);
	}

	SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
	{
		return m_box_tree.getNodeData(nodeindex);
	}

	SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound)  const
	{
		m_box_tree.getNodeBound(nodeindex, bound);
	}

	SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
	{
		m_box_tree.setNodeBound(nodeindex, bound);
	}

	SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
	{
		return m_box_tree.getLeftNodeIndex(nodeindex);
	}

	SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
	{
		return m_box_tree.getRightNodeIndex(nodeindex);
	}

	SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
	{
		return m_box_tree.getScapeNodeIndex(nodeindex);
	}

	SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const
	{
		m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle);
	}

};

//! Class for Box Tree Sets
/*!
this has the GIM_BOX_TREE implementation for bounding boxes.
*/
template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
{
public:

};

/// GIM_BOX_SET collision methods
template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1>
class GIM_TREE_TREE_COLLIDER
{
public:
	gim_pair_set * m_collision_pairs;
	BOX_SET_CLASS0 * m_boxset0;
	BOX_SET_CLASS1 * m_boxset1;
	GUINT current_node0;
	GUINT current_node1;
	bool node0_is_leaf;
	bool node1_is_leaf;
	bool t0_is_trimesh;
	bool t1_is_trimesh;
	bool node0_has_triangle;
	bool node1_has_triangle;
	GIM_AABB m_box0;
	GIM_AABB m_box1;
	GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
	btTransform trans_cache_0to1;
	GIM_TRIANGLE m_tri0;
	btVector4 m_tri0_plane;
	GIM_TRIANGLE m_tri1;
	btVector4 m_tri1_plane;

public:
	GIM_TREE_TREE_COLLIDER()
	{
		current_node0 = G_UINT_INFINITY;
		current_node1 = G_UINT_INFINITY;
	}
protected:
	SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0)
	{
		if(node0_has_triangle) return;
		m_boxset0->getNodeTriangle(node0,m_tri0);
		//transform triangle
		m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]);
		m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]);
		m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]);
		m_tri0.get_plane(m_tri0_plane);

		node0_has_triangle = true;
	}

	SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1)
	{
		if(node1_has_triangle) return;
		m_boxset1->getNodeTriangle(node1,m_tri1);
		//transform triangle
		m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]);
		m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]);
		m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]);
		m_tri1.get_plane(m_tri1_plane);

		node1_has_triangle = true;
	}

	SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0)
	{
		if(node0 == current_node0) return;
		m_boxset0->getNodeBound(node0,m_box0);
		node0_is_leaf = m_boxset0->isLeafNode(node0);
		node0_has_triangle = false;
		current_node0 = node0;
	}

	SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1)
	{
		if(node1 == current_node1) return;
		m_boxset1->getNodeBound(node1,m_box1);
		node1_is_leaf = m_boxset1->isLeafNode(node1);
		node1_has_triangle = false;
		current_node1 = node1;
	}

	SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1)
	{
		retrieve_node0_info(node0);
		retrieve_node1_info(node1);
		bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true);
		if(!result) return false;

		if(t0_is_trimesh && node0_is_leaf)
		{
			//perform primitive vs box collision
			retrieve_node0_triangle(node0);
			//do triangle vs box collision
			m_box1.increment_margin(m_tri0.m_margin);

			result = m_box1.collide_triangle_exact(
				m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane);

			m_box1.increment_margin(-m_tri0.m_margin);

			if(!result) return false;
			return true;
		}
		else if(t1_is_trimesh && node1_is_leaf)
		{
			//perform primitive vs box collision
			retrieve_node1_triangle(node1);
			//do triangle vs box collision
			m_box0.increment_margin(m_tri1.m_margin);

			result = m_box0.collide_triangle_exact(
				m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane);

			m_box0.increment_margin(-m_tri1.m_margin);

			if(!result) return false;
			return true;
		}
		return true;
	}

	//stackless collision routine
	void find_collision_pairs()
	{
		gim_pair_set stack_collisions;
		stack_collisions.reserve(32);

		//add the first pair
		stack_collisions.push_pair(0,0);

		while(stack_collisions.size())
		{
			//retrieve the last pair and pop
			GUINT node0 = stack_collisions.back().m_index1;
			GUINT node1 = stack_collisions.back().m_index2;
			stack_collisions.pop_back();
			if(node_collision(node0,node1)) // a collision is found
			{
				if(node0_is_leaf)
				{
					if(node1_is_leaf)
					{
						m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1));
					}
					else
					{
						//collide left
						stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1));

						//collide right
						stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1));
					}
				}
				else
				{
					if(node1_is_leaf)
					{
						//collide left
						stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1);
						//collide right
						stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1);
					}
					else
					{
						GUINT left0 = m_boxset0->getLeftNodeIndex(node0);
						GUINT right0 = m_boxset0->getRightNodeIndex(node0);
						GUINT left1 = m_boxset1->getLeftNodeIndex(node1);
						GUINT right1 = m_boxset1->getRightNodeIndex(node1);
						//collide left
						stack_collisions.push_pair(left0,left1);
						//collide right
						stack_collisions.push_pair(left0,right1);
						//collide left
						stack_collisions.push_pair(right0,left1);
						//collide right
						stack_collisions.push_pair(right0,right1);

					}// else if node1 is not a leaf
				}// else if node0 is not a leaf

			}// if(node_collision(node0,node1))
		}//while(stack_collisions.size())
	}
public:
	void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1,
		BOX_SET_CLASS1 * boxset2, const btTransform & trans2,
		gim_pair_set & collision_pairs, bool complete_primitive_tests = true)
	{
		m_collision_pairs = &collision_pairs;
		m_boxset0 = boxset1;
		m_boxset1 = boxset2;

		trans_cache_1to0.calc_from_homogenic(trans1,trans2);

		trans_cache_0to1 =  trans2.inverse();
		trans_cache_0to1 *= trans1;

		if(complete_primitive_tests)
		{
			t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh();
			t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh();
		}
		else
		{
			t0_is_trimesh = false;
			t1_is_trimesh = false;
		}

		find_collision_pairs();
	}
};

#endif // GIM_BOXPRUNING_H_INCLUDED


text:
#ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
#define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED

/*! \file btGeometryOperations.h
*\author Francisco Leon Najera

*/
/*
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "btBoxCollision.h"

#define PLANEDIREPSILON 0.0000001f
#define PARALELENORMALS 0.000001f

#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))

/// Calc a plane from a triangle edge an a normal. plane is a vec4f
SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 &  e2, const btVector3 & normal,btVector4 & plane)
{
	btVector3 planenormal = (e2-e1).cross(normal);
	planenormal.normalize();
	plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
}

//***************** SEGMENT and LINE FUNCTIONS **********************************///

/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
 */
SIMD_FORCE_INLINE void bt_closest_point_on_segment(
	btVector3 & cp, const btVector3 & v,
	const btVector3  &e1,const btVector3 &e2)
{
    btVector3 n = e2-e1;
    cp = v - e1;
	btScalar _scalar = cp.dot(n)/n.dot(n);
	if(_scalar <0.0f)
	{
	    cp = e1;
	}
	else if(_scalar >1.0f)
	{
	    cp = e2;
	}
	else
	{
		cp = _scalar*n + e1;
	}
}

//! line plane collision
/*!
*\return
	-0  if the ray never intersects
	-1 if the ray collides in front
	-2 if the ray collides in back
*/

SIMD_FORCE_INLINE int bt_line_plane_collision(
	const btVector4 & plane,
	const btVector3 & vDir,
	const btVector3 & vPoint,
	btVector3 & pout,
	btScalar &tparam,
	btScalar tmin, btScalar tmax)
{

	btScalar _dotdir = vDir.dot(plane);

	if(btFabs(_dotdir)<PLANEDIREPSILON)
	{
		tparam = tmax;
	    return 0;
	}

	btScalar _dis = bt_distance_point_plane(plane,vPoint);
	char returnvalue = _dis<0.0f? 2:1;
	tparam = -_dis/_dotdir;

	if(tparam<tmin)
	{
		returnvalue = 0;
		tparam = tmin;
	}
	else if(tparam>tmax)
	{
		returnvalue = 0;
		tparam = tmax;
	}
	pout = tparam*vDir + vPoint;
	return returnvalue;
}

//! Find closest points on segments
SIMD_FORCE_INLINE void bt_segment_collision(
	const btVector3 & vA1,
	const btVector3 & vA2,
	const btVector3 & vB1,
	const btVector3 & vB2,
	btVector3 & vPointA,
	btVector3 & vPointB)
{
    btVector3 AD = vA2 - vA1;
    btVector3 BD = vB2 - vB1;
    btVector3 N = AD.cross(BD);
    btScalar tp = N.length2();

    btVector4 _M;//plane

    if(tp<SIMD_EPSILON)//ARE PARALELE
    {
    	//project B over A
    	bool invert_b_order = false;
    	_M[0] = vB1.dot(AD);
    	_M[1] = vB2.dot(AD);

    	if(_M[0]>_M[1])
    	{
    		invert_b_order  = true;
    		BT_SWAP_NUMBERS(_M[0],_M[1]);
    	}
    	_M[2] = vA1.dot(AD);
    	_M[3] = vA2.dot(AD);
    	//mid points
    	N[0] = (_M[0]+_M[1])*0.5f;
    	N[1] = (_M[2]+_M[3])*0.5f;

    	if(N[0]<N[1])
    	{
    		if(_M[1]<_M[2])
    		{
    			vPointB = invert_b_order?vB1:vB2;
    			vPointA = vA1;
    		}
    		else if(_M[1]<_M[3])
    		{
    			vPointB = invert_b_order?vB1:vB2;
    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
    		}
    		else
    		{
    			vPointA = vA2;
    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
    		}
    	}
    	else
    	{
    		if(_M[3]<_M[0])
    		{
    			vPointB = invert_b_order?vB2:vB1;
    			vPointA = vA2;
    		}
    		else if(_M[3]<_M[1])
    		{
    			vPointA = vA2;
    			bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
    		}
    		else
    		{
    			vPointB = invert_b_order?vB1:vB2;
    			bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
    		}
    	}
    	return;
    }

    N = N.cross(BD);
    _M.setValue(N[0],N[1],N[2],vB1.dot(N));

	// get point A as the plane collision point
    bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));

    /*Closest point on segment*/
    vPointB = vPointA - vB1;
	tp = vPointB.dot(BD);
	tp/= BD.dot(BD);
	tp = BT_CLAMP(tp,0.0f,1.0f);

	vPointB = tp*BD + vB1;
}

#endif // GIM_VECTOR_H_INCLUDED

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


text:
#ifndef GIM_RADIXSORT_H_INCLUDED
#define GIM_RADIXSORT_H_INCLUDED
/*! \file gim_radixsort.h
\author Francisco Leon Najera.
Based on the work of Michael Herf : "fast floating-point radix sort"
Avaliable on http://www.stereopsis.com/radix.html
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.
   (3) The zlib/libpng license that is included with this library in
       the file GIMPACT-LICENSE-ZLIB.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/

#include "gim_memory.h"

///Macros for sorting.
//! Prototype for comparators
class less_comparator
{
	public:

	template<class T,class Z>
	inline int operator() ( const T& a, const Z& b )
	{
		return ( a<b?-1:(a>b?1:0));
	}
};

//! Prototype for comparators
class integer_comparator
{
	public:

	template<class T>
	inline int operator() ( const T& a, const T& b )
	{
		return (int)(a-b);
	}
};

//!Prototype for getting the integer representation of an object
class uint_key_func
{
public:
	template<class T>
	inline GUINT operator()( const T& a)
	{
		return (GUINT)a;
	}
};

//!Prototype for copying elements
class copy_elements_func
{
public:
	template<class T>
	inline void operator()(T& a,T& b)
	{
		a = b;
	}
};

//!Prototype for copying elements
class memcopy_elements_func
{
public:
	template<class T>
	inline void operator()(T& a,T& b)
	{
		gim_simd_memcpy(&a,&b,sizeof(T));
	}
};

//! @{
struct GIM_RSORT_TOKEN
{
    GUINT m_key;
    GUINT m_value;
    GIM_RSORT_TOKEN()
    {
    }
    GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
    {
    	m_key = rtoken.m_key;
    	m_value = rtoken.m_value;
    }

    inline bool operator <(const GIM_RSORT_TOKEN& other) const
	{
		return (m_key < other.m_key);
	}

	inline bool operator >(const GIM_RSORT_TOKEN& other) const
	{
		return (m_key > other.m_key);
	}
};

//! Prototype for comparators
class GIM_RSORT_TOKEN_COMPARATOR
{
	public:

	inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b )
	{
		return (int)((a.m_key) - (b.m_key));
	}
};

#define kHist 2048
// ---- utils for accessing 11-bit quantities
#define D11_0(x)	(x & 0x7FF)
#define D11_1(x)	(x >> 11 & 0x7FF)
#define D11_2(x)	(x >> 22 )

///Radix sort for unsigned integer keys
inline void gim_radix_sort_rtokens(
				GIM_RSORT_TOKEN * array,
				GIM_RSORT_TOKEN * sorted, GUINT element_count)
{
	GUINT i;
	GUINT b0[kHist * 3];
	GUINT *b1 = b0 + kHist;
	GUINT *b2 = b1 + kHist;
	for (i = 0; i < kHist * 3; ++i)
	{
		b0[i] = 0;
	}
	GUINT fi;
	GUINT pos;
	for (i = 0; i < element_count; ++i)
	{
	    fi = array[i].m_key;
		b0[D11_0(fi)] ++;
		b1[D11_1(fi)] ++;
		b2[D11_2(fi)] ++;
	}
	{
		GUINT sum0 = 0, sum1 = 0, sum2 = 0;
		GUINT tsum;
		for (i = 0; i < kHist; ++i)
		{
			tsum = b0[i] + sum0;
			b0[i] = sum0 - 1;
			sum0 = tsum;
			tsum = b1[i] + sum1;
			b1[i] = sum1 - 1;
			sum1 = tsum;
			tsum = b2[i] + sum2;
			b2[i] = sum2 - 1;
			sum2 = tsum;
		}
	}
	for (i = 0; i < element_count; ++i)
	{
        fi = array[i].m_key;
		pos = D11_0(fi);
		pos = ++b0[pos];
		sorted[pos].m_key = array[i].m_key;
		sorted[pos].m_value = array[i].m_value;
	}
	for (i = 0; i < element_count; ++i)
	{
        fi = sorted[i].m_key;
		pos = D11_1(fi);
		pos = ++b1[pos];
		array[pos].m_key = sorted[i].m_key;
		array[pos].m_value = sorted[i].m_value;
	}
	for (i = 0; i < element_count; ++i)
	{
        fi = array[i].m_key;
		pos = D11_2(fi);
		pos = ++b2[pos];
		sorted[pos].m_key = array[i].m_key;
		sorted[pos].m_value = array[i].m_value;
	}
}

/// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
/*!
*\param array Array of elements to sort
*\param sorted_tokens Tokens of sorted elements
*\param element_count element count
*\param uintkey_macro Functor which retrieves the integer representation of an array element
*/
template<typename T, class GETKEY_CLASS>
void gim_radix_sort_array_tokens(
			T* array ,
			GIM_RSORT_TOKEN * sorted_tokens,
			GUINT element_count,GETKEY_CLASS uintkey_macro)
{
	GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
    for (GUINT _i=0;_i<element_count;++_i)
    {
        _unsorted[_i].m_key = uintkey_macro(array[_i]);
        _unsorted[_i].m_value = _i;
    }
    gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count);
    gim_free(_unsorted);
    gim_free(_unsorted);
}

/// Sorts array in place. For generic use
/*!
\param type Type of the array
\param array
\param element_count
\param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
\param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
*/
template<typename T, class GETKEY_CLASS, class COPY_CLASS>
void gim_radix_sort(
	T * array, GUINT element_count,
	GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
{
	GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN  *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
    gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro);
    T * _original_array = (T *) gim_alloc(sizeof(T)*element_count);
    gim_simd_memcpy(_original_array,array,sizeof(T)*element_count);
    for (GUINT _i=0;_i<element_count;++_i)
    {
        copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]);
    }
    gim_free(_original_array);
    gim_free(_sorted);
}

//! Failsafe Iterative binary search,
/*!
If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
\param _array
\param _start_i the beginning of the array
\param _end_i the ending  index of the array
\param _search_key Value to find
\param _comp_macro macro for comparing elements
\param _found If true the value has found. Boolean
\param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
*/
template<class T, typename KEYCLASS, typename COMP_CLASS>
bool  gim_binary_search_ex(
		const T* _array, GUINT _start_i,
		GUINT _end_i,GUINT & _result_index,
		const KEYCLASS & _search_key,
		COMP_CLASS _comp_macro)
{
	GUINT _k;
	int _comp_result;
	GUINT _i = _start_i;
	GUINT _j = _end_i+1;
	while (_i < _j)
	{
		_k = (_j+_i-1)/2;
		_comp_result = _comp_macro(_array[_k], _search_key);
		if (_comp_result == 0)
		{
			_result_index = _k;
			return true;
		}
		else if (_comp_result < 0)
		{
			_i = _k+1;
		}
		else
		{
			_j = _k;
		}
	}
	_result_index = _i;
	return false;
}

//! Failsafe Iterative binary search,Template version
/*!
If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
\param _array
\param _start_i the beginning of the array
\param _end_i the ending  index of the array
\param _search_key Value to find
\param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
\return true if found, else false
*/
template<class T>
bool gim_binary_search(
	const T*_array,GUINT _start_i,
	GUINT _end_i,const T & _search_key,
	GUINT & _result_index)
{
	GUINT _i = _start_i;
	GUINT _j = _end_i+1;
	GUINT _k;
	while(_i < _j)
	{
		_k = (_j+_i-1)/2;
		if(_array[_k]==_search_key)
		{
			_result_index = _k;
			return true;
		}
		else if (_array[_k]<_search_key)
		{
			_i = _k+1;
		}
		else
		{
			_j = _k;
		}
	}
	_result_index = _i;
	return false;
}

///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
template <typename T, typename COMP_CLASS>
void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
{
	/*  PRE: a[k+1..N] is a heap */
	/* POST:  a[k..N]  is a heap */

	T temp = pArr[k - 1];
	/* k has child(s) */
	while (k <= n/2)
	{
		int child = 2*k;

		if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0)
		{
			child++;
		}
		/* pick larger child */
		if (CompareFunc(temp , pArr[child - 1])<0)
		{
			/* move child up */
			pArr[k - 1] = pArr[child - 1];
			k = child;
		}
		else
		{
			break;
		}
	}
	pArr[k - 1] = temp;
} /*downHeap*/

template <typename T, typename COMP_CLASS>
void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc)
{
	/* sort a[0..N-1],  N.B. 0 to N-1 */
	GUINT k;
	GUINT n = element_count;
	for (k = n/2; k > 0; k--)
	{
		gim_down_heap(pArr, k, n, CompareFunc);
	}

	/* a[1..N] is now a heap */
	while ( n>=2 )
	{
		gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */
		--n;
		/* restore a[1..i-1] heap */
		gim_down_heap(pArr, 1, n, CompareFunc);
	}
}

#endif // GIM_RADIXSORT_H_INCLUDED


text:
#ifndef GIM_TRI_COLLISION_H_INCLUDED
#define GIM_TRI_COLLISION_H_INCLUDED

/*! \file gim_tri_collision.h
\author Francisco Leon Najera
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.
   (3) The zlib/libpng license that is included with this library in
       the file GIMPACT-LICENSE-ZLIB.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/

#include "gim_box_collision.h"
#include "gim_clip_polygon.h"

#ifndef MAX_TRI_CLIPPING
#define MAX_TRI_CLIPPING 16
#endif

//! Structure for collision
struct GIM_TRIANGLE_CONTACT_DATA
{
    GREAL m_penetration_depth;
    GUINT m_point_count;
    btVector4 m_separating_normal;
    btVector3 m_points[MAX_TRI_CLIPPING];

	SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other)
	{
		m_penetration_depth = other.m_penetration_depth;
		m_separating_normal = other.m_separating_normal;
		m_point_count = other.m_point_count;
		GUINT i = m_point_count;
		while(i--)
		{
			m_points[i] = other.m_points[i];
		}
	}

	GIM_TRIANGLE_CONTACT_DATA()
	{
	}

	GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other)
	{
		copy_from(other);
	}

    //! classify points that are closer
    template<typename DISTANCE_FUNC,typename CLASS_PLANE>
    SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane,
    				GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func)
    {
    	m_point_count = 0;
    	m_penetration_depth= -1000.0f;

		GUINT point_indices[MAX_TRI_CLIPPING];

		GUINT _k;

		for(_k=0;_k<point_count;_k++)
		{
			GREAL _dist = -distance_func(plane,points[_k]) + margin;

			if(_dist>=0.0f)
			{
				if(_dist>m_penetration_depth)
				{
					m_penetration_depth = _dist;
					point_indices[0] = _k;
					m_point_count=1;
				}
				else if((_dist+G_EPSILON)>=m_penetration_depth)
				{
					point_indices[m_point_count] = _k;
					m_point_count++;
				}
			}
		}

		for( _k=0;_k<m_point_count;_k++)
		{
			m_points[_k] = points[point_indices[_k]];
		}
	}

	//! classify points that are closer
	SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin,
										 const btVector3 * points, GUINT point_count)
	{
		m_separating_normal = plane;
		mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC());
	}
};

//! Class for colliding triangles
class GIM_TRIANGLE
{
public:
	btScalar m_margin;
    btVector3 m_vertices[3];

    GIM_TRIANGLE():m_margin(0.1f)
    {
    }

    SIMD_FORCE_INLINE GIM_AABB get_box()  const
    {
    	return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
    }

    SIMD_FORCE_INLINE void get_normal(btVector3 &normal)  const
    {
    	TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal);
    }

    SIMD_FORCE_INLINE void get_plane(btVector4 &plane)  const
    {
    	TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);;
    }

    SIMD_FORCE_INLINE void apply_transform(const btTransform & trans)
    {
    	m_vertices[0] = trans(m_vertices[0]);
    	m_vertices[1] = trans(m_vertices[1]);
    	m_vertices[2] = trans(m_vertices[2]);
    }

    SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane)  const
    {
		const btVector3 & e0 = m_vertices[edge_index];
		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
		EDGE_PLANE(e0,e1,triangle_normal,plane);
    }

    //! Gets the relative transformation of this triangle
    /*!
    The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0:
    - triangle normal corresponds to Z axis.
    - 1st normalized edge corresponds to X axis,

    */
    SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform)  const
    {
    	btMatrix3x3 & matrix = triangle_transform.getBasis();

    	btVector3 zaxis;
    	get_normal(zaxis);
    	MAT_SET_Z(matrix,zaxis);

    	btVector3 xaxis = m_vertices[1] - m_vertices[0];
    	VEC_NORMALIZE(xaxis);
    	MAT_SET_X(matrix,xaxis);

    	//y axis
    	xaxis = zaxis.cross(xaxis);
    	MAT_SET_Y(matrix,xaxis);

    	triangle_transform.setOrigin(m_vertices[0]);
    }

	//! Test triangles by finding separating axis
	/*!
	\param other Triangle for collide
	\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
	*/
	bool collide_triangle_hard_test(
		const GIM_TRIANGLE & other,
		GIM_TRIANGLE_CONTACT_DATA & contact_data) const;

	//! Test boxes before doing hard test
	/*!
	\param other Triangle for collide
	\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
	\
	*/
	SIMD_FORCE_INLINE bool collide_triangle(
		const GIM_TRIANGLE & other,
		GIM_TRIANGLE_CONTACT_DATA & contact_data) const
	{
		//test box collisioin
		GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
		GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin);
		if(!boxu.has_collision(boxv)) return false;

		//do hard test
		return collide_triangle_hard_test(other,contact_data);
	}

	/*!

	Solve the System for u,v parameters:

	u*axe1[i1] + v*axe2[i1] = vecproj[i1]
	u*axe1[i2] + v*axe2[i2] = vecproj[i2]

	sustitute:
	v = (vecproj[i2] - u*axe1[i2])/axe2[i2]

	then the first equation in terms of 'u':

	--> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1]

	--> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1]

	--> u*(axe1[i1]  - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2]

	--> u*((axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2]

	--> u*(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]

	--> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2]  - axe1[i2]*axe2[i1])

if 0.0<= u+v <=1.0 then they are inside of triangle

	\return false if the point is outside of triangle.This function  doesn't take the margin
	*/
	SIMD_FORCE_INLINE bool get_uv_parameters(
			const btVector3 & point,
			const btVector3 & tri_plane,
			GREAL & u, GREAL & v) const
	{
		btVector3 _axe1 = m_vertices[1]-m_vertices[0];
		btVector3 _axe2 = m_vertices[2]-m_vertices[0];
		btVector3 _vecproj = point - m_vertices[0];
		GUINT _i1 = (tri_plane.closestAxis()+1)%3;
		GUINT _i2 = (_i1+1)%3;
		if(btFabs(_axe2[_i2])<G_EPSILON)
		{
			u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1]  - _axe1[_i1]*_axe2[_i2]);
			v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1];
		}
		else
		{
			u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2]  - _axe1[_i2]*_axe2[_i1]);
			v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2];
		}

		if(u<-G_EPSILON)
		{
			return false;
		}
		else if(v<-G_EPSILON)
		{
			return false;
		}
		else
		{
			btScalar sumuv;
			sumuv = u+v;
			if(sumuv<-G_EPSILON)
			{
				return false;
			}
			else if(sumuv-1.0f>G_EPSILON)
			{
				return false;
			}
		}
		return true;
	}

	//! is point in triangle beam?
	/*!
	Test if point is in triangle, with m_margin tolerance
	*/
	SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const
	{
		//Test with edge 0
		btVector4 edge_plane;
		this->get_edge_plane(0,tri_normal,edge_plane);
		GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point);
		if(dist-m_margin>0.0f) return false; // outside plane

		this->get_edge_plane(1,tri_normal,edge_plane);
		dist = DISTANCE_PLANE_POINT(edge_plane,point);
		if(dist-m_margin>0.0f) return false; // outside plane

		this->get_edge_plane(2,tri_normal,edge_plane);
		dist = DISTANCE_PLANE_POINT(edge_plane,point);
		if(dist-m_margin>0.0f) return false; // outside plane
		return true;
	}

	//! Bidireccional ray collision
	SIMD_FORCE_INLINE bool ray_collision(
		const btVector3 & vPoint,
		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
	{
		btVector4 faceplane;
		{
			btVector3 dif1 = m_vertices[1] - m_vertices[0];
			btVector3 dif2 = m_vertices[2] - m_vertices[0];
    		VEC_CROSS(faceplane,dif1,dif2);
    		faceplane[3] = m_vertices[0].dot(faceplane);
		}

		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
		if(res == 0) return false;
		if(! is_point_inside(pout,faceplane)) return false;

		if(res==2) //invert normal
		{
			triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]);
		}
		else
		{
			triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
		}

		VEC_NORMALIZE(triangle_normal);

		return true;
	}

	//! one direccion ray collision
	SIMD_FORCE_INLINE bool ray_collision_front_side(
		const btVector3 & vPoint,
		const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
		GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
	{
		btVector4 faceplane;
		{
			btVector3 dif1 = m_vertices[1] - m_vertices[0];
			btVector3 dif2 = m_vertices[2] - m_vertices[0];
    		VEC_CROSS(faceplane,dif1,dif2);
    		faceplane[3] = m_vertices[0].dot(faceplane);
		}

		GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
		if(res != 1) return false;

		if(!is_point_inside(pout,faceplane)) return false;

		triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);

		VEC_NORMALIZE(triangle_normal);

		return true;
	}

};

#endif // GIM_TRI_COLLISION_H_INCLUDED


text:
#ifndef GIM_BOX_SET_STRUCT_H_INCLUDED
#define GIM_BOX_SET_STRUCT_H_INCLUDED

/*! \file gim_box_set.h
\author Francisco Leon Najera
*/
/*
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "LinearMath/btAlignedObjectArray.h"

#include "btBoxCollision.h"
#include "btTriangleShapeEx.h"

//! Overlapping pair
struct GIM_PAIR
{
    int m_index1;
    int m_index2;
    GIM_PAIR()
    {}

    GIM_PAIR(const GIM_PAIR & p)
    {
    	m_index1 = p.m_index1;
    	m_index2 = p.m_index2;
	}

	GIM_PAIR(int index1, int index2)
    {
    	m_index1 = index1;
    	m_index2 = index2;
	}
};

///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
struct GIM_BVH_DATA
{
	btAABB m_bound;
	int m_data;
};

//! Node Structure for trees
class GIM_BVH_TREE_NODE
{
public:
	btAABB m_bound;
protected:
	int	m_escapeIndexOrDataIndex;
public:
	GIM_BVH_TREE_NODE()
	{
		m_escapeIndexOrDataIndex = 0;
	}

	SIMD_FORCE_INLINE bool isLeafNode() const
	{
		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
		return (m_escapeIndexOrDataIndex>=0);
	}

	SIMD_FORCE_INLINE int getEscapeIndex() const
	{
		//btAssert(m_escapeIndexOrDataIndex < 0);
		return -m_escapeIndexOrDataIndex;
	}

	SIMD_FORCE_INLINE void setEscapeIndex(int index)
	{
		m_escapeIndexOrDataIndex = -index;
	}

	SIMD_FORCE_INLINE int getDataIndex() const
	{
		//btAssert(m_escapeIndexOrDataIndex >= 0);

		return m_escapeIndexOrDataIndex;
	}

	SIMD_FORCE_INLINE void setDataIndex(int index)
	{
		m_escapeIndexOrDataIndex = index;
	}

};

#endif // GIM_BOXPRUNING_H_INCLUDED


text:
#ifndef BT_CLIP_POLYGON_H_INCLUDED
#define BT_CLIP_POLYGON_H_INCLUDED

/*! \file btClipPolygon.h
\author Francisco Leon Najera
*/
/*
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "LinearMath/btTransform.h"
#include "LinearMath/btGeometryUtil.h"

SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point)
{
	return point.dot(plane) - plane[3];
}

/*! Vector blending
Takes two vectors a, b, blends them together*/
SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor)
{
	vr = (1-blend_factor)*va + blend_factor*vb;
}

//! This function calcs the distance from a 3D plane
SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect(
						const btVector3 & point0,
						const btVector3 & point1,
						btScalar dist0,
						btScalar dist1,
						btVector3 * clipped,
						int & clipped_count)
{
	bool _prevclassif = (dist0>SIMD_EPSILON);
	bool _classif = (dist1>SIMD_EPSILON);
	if(_classif!=_prevclassif)
	{
		btScalar blendfactor = -dist0/(dist1-dist0);
		bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor);
		clipped_count++;
	}
	if(!_classif)
	{
		clipped[clipped_count] = point1;
		clipped_count++;
	}
}

//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
SIMD_FORCE_INLINE int bt_plane_clip_polygon(
						const btVector4 & plane,
						const btVector3 * polygon_points,
						int polygon_point_count,
						btVector3 * clipped)
{
    int clipped_count = 0;

    //clip first point
	btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);;
	if(!(firstdist>SIMD_EPSILON))
	{
		clipped[clipped_count] = polygon_points[0];
		clipped_count++;
	}

	btScalar olddist = firstdist;
	for(int i=1;i<polygon_point_count;i++)
	{
		btScalar dist = bt_distance_point_plane(plane,polygon_points[i]);

		bt_plane_clip_polygon_collect(
						polygon_points[i-1],polygon_points[i],
						olddist,
						dist,
						clipped,
						clipped_count);

		olddist = dist;
	}

	//RETURN TO FIRST  point

	bt_plane_clip_polygon_collect(
					polygon_points[polygon_point_count-1],polygon_points[0],
					olddist,
					firstdist,
					clipped,
					clipped_count);

	return clipped_count;
}

//! Clips a polygon by a plane
/*!
*\param clipped must be an array of 16 points.
*\return The count of the clipped counts
*/
SIMD_FORCE_INLINE int bt_plane_clip_triangle(
						const btVector4 & plane,
						const btVector3 & point0,
						const btVector3 & point1,
						const btVector3& point2,
						btVector3 * clipped // an allocated array of 16 points at least
						)
{
    int clipped_count = 0;

    //clip first point0
	btScalar firstdist = bt_distance_point_plane(plane,point0);;
	if(!(firstdist>SIMD_EPSILON))
	{
		clipped[clipped_count] = point0;
		clipped_count++;
	}

	// point 1
	btScalar olddist = firstdist;
	btScalar dist = bt_distance_point_plane(plane,point1);

	bt_plane_clip_polygon_collect(
					point0,point1,
					olddist,
					dist,
					clipped,
					clipped_count);

	olddist = dist;

	// point 2
	dist = bt_distance_point_plane(plane,point2);

	bt_plane_clip_polygon_collect(
					point1,point2,
					olddist,
					dist,
					clipped,
					clipped_count);
	olddist = dist;

	//RETURN TO FIRST  point0
	bt_plane_clip_polygon_collect(
					point2,point0,
					olddist,
					firstdist,
					clipped,
					clipped_count);

	return clipped_count;
}

#endif // GIM_TRI_COLLISION_H_INCLUDED


text:
#ifndef GIM_ARRAY_H_INCLUDED
#define GIM_ARRAY_H_INCLUDED
/*! \file gim_array.h
\author Francisco Leon Najera
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.
   (3) The zlib/libpng license that is included with this library in
       the file GIMPACT-LICENSE-ZLIB.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/

#include "gim_memory.h"

#define GIM_ARRAY_GROW_INCREMENT 2
#define GIM_ARRAY_GROW_FACTOR 2

//!	Very simple array container with fast access and simd memory
template<typename T>
class gim_array
{
public:
//! properties
//!@{
    T *m_data;
    GUINT m_size;
    GUINT m_allocated_size;
//!@}
//! protected operations
//!@{

    inline void destroyData()
	{
	    m_allocated_size = 0;
		if(m_data==NULL) return;
		gim_free(m_data);
		m_data = NULL;
	}

	inline bool resizeData(GUINT newsize)
	{
		if(newsize==0)
		{
			destroyData();
			return true;
		}

		if(m_size>0)
		{
            m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T));
		}
		else
		{
		    m_data = (T*)gim_alloc(newsize*sizeof(T));
		}
		m_allocated_size = newsize;
		return true;
	}

	inline bool growingCheck()
	{
		if(m_allocated_size<=m_size)
		{
		    GUINT requestsize = m_size;
		    m_size = m_allocated_size;
			if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false;
		}
		return true;
	}

//!@}
//! public operations
//!@{
    inline  bool reserve(GUINT size)
    {
        if(m_allocated_size>=size) return false;
        return resizeData(size);
    }

    inline void clear_range(GUINT start_range)
    {
        while(m_size>start_range)
        {
            m_data[--m_size].~T();
        }
    }

    inline void clear()
    {
        if(m_size==0)return;
        clear_range(0);
    }

    inline void clear_memory()
    {
        clear();
        destroyData();
    }

    gim_array()
    {
        m_data = 0;
        m_size = 0;
        m_allocated_size = 0;
    }

    gim_array(GUINT reservesize)
    {
        m_data = 0;
        m_size = 0;

        m_allocated_size = 0;
        reserve(reservesize);
    }

    ~gim_array()
    {
        clear_memory();
    }

    inline GUINT size() const
    {
        return m_size;
    }

    inline GUINT max_size() const
    {
        return m_allocated_size;
    }

    inline T & operator[](size_t i)
	{
		return m_data[i];
	}
	inline  const T & operator[](size_t i) const
	{
		return m_data[i];
	}

    inline T * pointer(){ return m_data;}
    inline const T * pointer() const
    { return m_data;}

    inline T * get_pointer_at(GUINT i)
	{
		return m_data + i;
	}

	inline const T * get_pointer_at(GUINT i) const
	{
		return m_data + i;
	}

	inline T & at(GUINT i)
	{
		return m_data[i];
	}

	inline const T & at(GUINT i) const
	{
		return m_data[i];
	}

	inline T & front()
	{
		return *m_data;
	}

	inline const T & front() const
	{
		return *m_data;
	}

	inline T & back()
	{
		return m_data[m_size-1];
	}

	inline const T & back() const
	{
		return m_data[m_size-1];
	}

	inline void swap(GUINT i, GUINT j)
	{
	    gim_swap_elements(m_data,i,j);
	}

	inline void push_back(const T & obj)
	{
	    this->growingCheck();
	    m_data[m_size] = obj;
	    m_size++;
	}

	//!Simply increase the m_size, doesn't call the new element constructor
	inline void push_back_mem()
	{
	    this->growingCheck();
	    m_size++;
	}

	inline void push_back_memcpy(const T & obj)
	{
	    this->growingCheck();
	    gim_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
	    m_size++;
	}

	inline void pop_back()
	{
	    m_size--;
        m_data[m_size].~T();
	}

	//!Simply decrease the m_size, doesn't call the deleted element destructor
	inline void pop_back_mem()
	{
	    m_size--;
	}

    //! fast erase
	inline void erase(GUINT index)
	{
	    if(index<m_size-1)
	    {
	        swap(index,m_size-1);
	    }
	    pop_back();
	}

	inline void erase_sorted_mem(GUINT index)
	{
	    m_size--;
	    for(GUINT i = index;i<m_size;i++)
	    {
	        gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T));
	    }
	}

	inline void erase_sorted(GUINT index)
	{
	    m_data[index].~T();
	    erase_sorted_mem(index);
	}

	inline void insert_mem(GUINT index)
	{
	    this->growingCheck();
	    for(GUINT i = m_size;i>index;i--)
	    {
	        gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T));
	    }
	    m_size++;
	}

	inline void insert(const T & obj,GUINT index)
	{
	    insert_mem(index);
	    m_data[index] = obj;
	}

	inline void resize(GUINT size, bool call_constructor = true, const T& fillData=T())
	{
	    if(size>m_size)
	    {
            reserve(size);
            if(call_constructor)
            {
                while(m_size<size)
                {
                    m_data[m_size] = fillData;
                    m_size++;
                }
            }
            else
            {
            	m_size = size;
            }
	    }
	    else if(size<m_size)
	    {
	        if(call_constructor) clear_range(size);
	        m_size = size;
	    }
	}

	inline void refit()
	{
	    resizeData(m_size);
	}

};

#endif // GIM_CONTAINERS_H_INCLUDED


text:
#ifndef GIM_GEOM_TYPES_H_INCLUDED
#define GIM_GEOM_TYPES_H_INCLUDED

/*! \file gim_geom_types.h
\author Francisco Leon Najera
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.
   (3) The zlib/libpng license that is included with this library in
       the file GIMPACT-LICENSE-ZLIB.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/

#include "gim_math.h"

//! Short Integer vector 2D
typedef GSHORT vec2s[2];
//! Integer vector 3D
typedef GSHORT vec3s[3];
//! Integer vector 4D
typedef GSHORT vec4s[4];

//! Short Integer vector 2D
typedef GUSHORT vec2us[2];
//! Integer vector 3D
typedef GUSHORT vec3us[3];
//! Integer vector 4D
typedef GUSHORT vec4us[4];

//! Integer vector 2D
typedef GINT vec2i[2];
//! Integer vector 3D
typedef GINT vec3i[3];
//! Integer vector 4D
typedef GINT vec4i[4];

//! Unsigned Integer vector 2D
typedef GUINT vec2ui[2];
//! Unsigned Integer vector 3D
typedef GUINT vec3ui[3];
//! Unsigned Integer vector 4D
typedef GUINT vec4ui[4];

//! Float vector 2D
typedef GREAL vec2f[2];
//! Float vector 3D
typedef GREAL vec3f[3];
//! Float vector 4D
typedef GREAL vec4f[4];

//! Double vector 2D
typedef GREAL2 vec2d[2];
//! Float vector 3D
typedef GREAL2 vec3d[3];
//! Float vector 4D
typedef GREAL2 vec4d[4];

//! Matrix 2D, row ordered
typedef GREAL mat2f[2][2];
//! Matrix 3D, row ordered
typedef GREAL mat3f[3][3];
//! Matrix 4D, row ordered
typedef GREAL mat4f[4][4];

//! Quaternion
typedef GREAL quatf[4];

//typedef struct _aabb3f aabb3f;

#endif // GIM_GEOM_TYPES_H_INCLUDED


text:
#ifndef BT_BOX_COLLISION_H_INCLUDED
#define BT_BOX_COLLISION_H_INCLUDED

/*! \file gim_box_collision.h
\author Francisco Leon Najera
*/
/*
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "LinearMath/btTransform.h"

///Swap numbers
#define BT_SWAP_NUMBERS(a,b){ \
    a = a+b; \
    b = a-b; \
    a = a-b; \
}\

#define BT_MAX(a,b) (a<b?b:a)
#define BT_MIN(a,b) (a>b?b:a)

#define BT_GREATER(x, y)	btFabs(x) > (y)

#define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
#define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))

enum eBT_PLANE_INTERSECTION_TYPE
{
	BT_CONST_BACK_PLANE = 0,
	BT_CONST_COLLIDE_PLANE,
	BT_CONST_FRONT_PLANE
};

//SIMD_FORCE_INLINE bool test_cross_edge_box(
//	const btVector3 & edge,
//	const btVector3 & absolute_edge,
//	const btVector3 & pointa,
//	const btVector3 & pointb, const btVector3 & extend,
//	int dir_index0,
//	int dir_index1
//	int component_index0,
//	int component_index1)
//{
//	// dir coords are -z and y
//
//	const btScalar dir0 = -edge[dir_index0];
//	const btScalar dir1 = edge[dir_index1];
//	btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
//	btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
//	//find minmax
//	if(pmin>pmax)
//	{
//		BT_SWAP_NUMBERS(pmin,pmax);
//	}
//	//find extends
//	const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
//					extend[component_index1] * absolute_edge[dir_index1];
//
//	if(pmin>rad || -rad>pmax) return false;
//	return true;
//}
//
//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
//	const btVector3 & edge,
//	const btVector3 & absolute_edge,
//	const btVector3 & pointa,
//	const btVector3 & pointb, btVector3 & extend)
//{
//
//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
//}
//
//
//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
//	const btVector3 & edge,
//	const btVector3 & absolute_edge,
//	const btVector3 & pointa,
//	const btVector3 & pointb, btVector3 & extend)
//{
//
//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
//}
//
//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
//	const btVector3 & edge,
//	const btVector3 & absolute_edge,
//	const btVector3 & pointa,
//	const btVector3 & pointb, btVector3 & extend)
//{
//
//	return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
//}

#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
{\
	const btScalar dir0 = -edge[i_dir_0];\
	const btScalar dir1 = edge[i_dir_1];\
	btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
	btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
	if(pmin>pmax)\
	{\
		BT_SWAP_NUMBERS(pmin,pmax); \
	}\
	const btScalar abs_dir0 = absolute_edge[i_dir_0];\
	const btScalar abs_dir1 = absolute_edge[i_dir_1];\
	const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
	if(pmin>rad || -rad>pmax) return false;\
}\

#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
{\
	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
}\

#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
{\
	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
}\

#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
{\
	TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
}\

//! Returns the dot product between a vec3f and the col of a matrix
SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
{
	return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
}

//!  Class for transforming a model1 to the space of model0
ATTRIBUTE_ALIGNED16	(class) BT_BOX_BOX_TRANSFORM_CACHE
{
public:
    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
	btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
	btMatrix3x3 m_AR;//!< Absolute value of m_R1to0

	SIMD_FORCE_INLINE void calc_absolute_matrix()
	{
//		static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
//		m_AR[0] = vepsi + m_R1to0[0].absolute();
//		m_AR[1] = vepsi + m_R1to0[1].absolute();
//		m_AR[2] = vepsi + m_R1to0[2].absolute();

		int i,j;

        for(i=0;i<3;i++)
        {
            for(j=0;j<3;j++ )
            {
            	m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]);
            }
        }

	}

	BT_BOX_BOX_TRANSFORM_CACHE()
	{
	}

	//! Calc the transformation relative  1 to 0. Inverts matrics by transposing
	SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
	{

		btTransform temp_trans = trans0.inverse();
		temp_trans = temp_trans * trans1;

		m_T1to0 = temp_trans.getOrigin();
		m_R1to0 = temp_trans.getBasis();

		calc_absolute_matrix();
	}

	//! Calcs the full invertion of the matrices. Useful for scaling matrices
	SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
	{
		m_R1to0 = trans0.getBasis().inverse();
		m_T1to0 = m_R1to0 * (-trans0.getOrigin());

		m_T1to0 += m_R1to0*trans1.getOrigin();
		m_R1to0 *= trans1.getBasis();

		calc_absolute_matrix();
	}

	SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
	{
        return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0;
	}
};

#define BOX_PLANE_EPSILON 0.000001f

//! Axis aligned box
ATTRIBUTE_ALIGNED16	(class) btAABB
{
public:
	btVector3 m_min;
	btVector3 m_max;

	btAABB()
	{}

	btAABB(const btVector3 & V1,
			 const btVector3 & V2,
			 const btVector3 & V3)
	{
		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);

		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
	}

	btAABB(const btVector3 & V1,
			 const btVector3 & V2,
			 const btVector3 & V3,
			 btScalar margin)
	{
		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);

		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);

		m_min[0] -= margin;
		m_min[1] -= margin;
		m_min[2] -= margin;
		m_max[0] += margin;
		m_max[1] += margin;
		m_max[2] += margin;
	}

	btAABB(const btAABB &other):
		m_min(other.m_min),m_max(other.m_max)
	{
	}

	btAABB(const btAABB &other,btScalar margin ):
		m_min(other.m_min),m_max(other.m_max)
	{
		m_min[0] -= margin;
		m_min[1] -= margin;
		m_min[2] -= margin;
		m_max[0] += margin;
		m_max[1] += margin;
		m_max[2] += margin;
	}

	SIMD_FORCE_INLINE void invalidate()
	{
		m_min[0] = SIMD_INFINITY;
		m_min[1] = SIMD_INFINITY;
		m_min[2] = SIMD_INFINITY;
		m_max[0] = -SIMD_INFINITY;
		m_max[1] = -SIMD_INFINITY;
		m_max[2] = -SIMD_INFINITY;
	}

	SIMD_FORCE_INLINE void increment_margin(btScalar margin)
	{
		m_min[0] -= margin;
		m_min[1] -= margin;
		m_min[2] -= margin;
		m_max[0] += margin;
		m_max[1] += margin;
		m_max[2] += margin;
	}

	SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin)
	{
		m_min[0] = other.m_min[0] - margin;
		m_min[1] = other.m_min[1] - margin;
		m_min[2] = other.m_min[2] - margin;

		m_max[0] = other.m_max[0] + margin;
		m_max[1] = other.m_max[1] + margin;
		m_max[2] = other.m_max[2] + margin;
	}

	template<typename CLASS_POINT>
	SIMD_FORCE_INLINE void calc_from_triangle(
							const CLASS_POINT & V1,
							const CLASS_POINT & V2,
							const CLASS_POINT & V3)
	{
		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);

		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
	}

	template<typename CLASS_POINT>
	SIMD_FORCE_INLINE void calc_from_triangle_margin(
							const CLASS_POINT & V1,
							const CLASS_POINT & V2,
							const CLASS_POINT & V3, btScalar margin)
	{
		m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
		m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
		m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);

		m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
		m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
		m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);

		m_min[0] -= margin;
		m_min[1] -= margin;
		m_min[2] -= margin;
		m_max[0] += margin;
		m_max[1] += margin;
		m_max[2] += margin;
	}

	//! Apply a transform to an AABB
	SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
	{
		btVector3 center = (m_max+m_min)*0.5f;
		btVector3 extends = m_max - center;
		// Compute new center
		center = trans(center);

        btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
                                          trans.getBasis().getRow(1).absolute(),
                                          trans.getBasis().getRow(2).absolute());

		m_min = center - textends;
		m_max = center + textends;
	}

	//! Apply a transform to an AABB
	SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
	{
		btVector3 center = (m_max+m_min)*0.5f;
		btVector3 extends = m_max - center;
		// Compute new center
		center = trans.transform(center);

        btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(),
                                          trans.m_R1to0.getRow(1).absolute(),
                                          trans.m_R1to0.getRow(2).absolute());

		m_min = center - textends;
		m_max = center + textends;
	}

	//! Merges a Box
	SIMD_FORCE_INLINE void merge(const btAABB & box)
	{
		m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
		m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
		m_min[2] = BT_MIN(m_min[2],box.m_min[2]);

		m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
		m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
		m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
	}

	//! Merges a point
	template<typename CLASS_POINT>
	SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
	{
		m_min[0] = BT_MIN(m_min[0],point[0]);
		m_min[1] = BT_MIN(m_min[1],point[1]);
		m_min[2] = BT_MIN(m_min[2],point[2]);

		m_max[0] = BT_MAX(m_max[0],point[0]);
		m_max[1] = BT_MAX(m_max[1],point[1]);
		m_max[2] = BT_MAX(m_max[2],point[2]);
	}

	//! Gets the extend and center
	SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
	{
		center = (m_max+m_min)*0.5f;
		extend = m_max - center;
	}

	//! Finds the intersecting box between this box and the other.
	SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection)  const
	{
		intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
		intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
		intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);

		intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
		intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
		intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
	}

	SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
	{
		if(m_min[0] > other.m_max[0] ||
		   m_max[0] < other.m_min[0] ||
		   m_min[1] > other.m_max[1] ||
		   m_max[1] < other.m_min[1] ||
		   m_min[2] > other.m_max[2] ||
		   m_max[2] < other.m_min[2])
		{
			return false;
		}
		return true;
	}

	/*! \brief Finds the Ray intersection parameter.
	\param aabb Aligned box
	\param vorigin A vec3f with the origin of the ray
	\param vdir A vec3f with the direction of the ray
	*/
	SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)  const
	{
		btVector3 extents,center;
		this->get_center_extend(center,extents);;

		btScalar Dx = vorigin[0] - center[0];
		if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
		btScalar Dy = vorigin[1] - center[1];
		if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
		btScalar Dz = vorigin[2] - center[2];
		if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)	return false;

		btScalar f = vdir[1] * Dz - vdir[2] * Dy;
		if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
		f = vdir[2] * Dx - vdir[0] * Dz;
		if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
		f = vdir[0] * Dy - vdir[1] * Dx;
		if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
		return true;
	}

	SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
	{
		btVector3 center = (m_max+m_min)*0.5f;
		btVector3 extend = m_max-center;

		btScalar _fOrigin =  direction.dot(center);
		btScalar _fMaximumExtent = extend.dot(direction.absolute());
		vmin = _fOrigin - _fMaximumExtent;
		vmax = _fOrigin + _fMaximumExtent;
	}

	SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
	{
		btScalar _fmin,_fmax;
		this->projection_interval(plane,_fmin,_fmax);

		if(plane[3] > _fmax + BOX_PLANE_EPSILON)
		{
			return BT_CONST_BACK_PLANE; // 0
		}

		if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
		{
			return BT_CONST_COLLIDE_PLANE; //1
		}
		return BT_CONST_FRONT_PLANE;//2
	}

	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
	{
		btAABB tbox = box;
		tbox.appy_transform(trans1_to_0);
		return has_collision(tbox);
	}

	SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
		const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
	{
		btAABB tbox = box;
		tbox.appy_transform_trans_cache(trans1_to_0);
		return has_collision(tbox);
	}

	//! transcache is the transformation cache from box to this AABB
	SIMD_FORCE_INLINE bool overlapping_trans_cache(
		const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
	{

		//Taken from OPCODE
		btVector3 ea,eb;//extends
		btVector3 ca,cb;//extends
		get_center_extend(ca,ea);
		box.get_center_extend(cb,eb);

		btVector3 T;
		btScalar t,t2;
		int i;

		// Class I : A's basis vectors
		for(i=0;i<3;i++)
		{
			T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
			t = transcache.m_AR[i].dot(eb) + ea[i];
			if(BT_GREATER(T[i], t))	return false;
		}
		// Class II : B's basis vectors
		for(i=0;i<3;i++)
		{
			t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
			t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
			if(BT_GREATER(t,t2))	return false;
		}
		// Class III : 9 cross products
		if(fulltest)
		{
			int j,m,n,o,p,q,r;
			for(i=0;i<3;i++)
			{
				m = (i+1)%3;
				n = (i+2)%3;
				o = i==0?1:0;
				p = i==2?1:2;
				for(j=0;j<3;j++)
				{
					q = j==2?1:2;
					r = j==0?1:0;
					t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
					t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
						eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
					if(BT_GREATER(t,t2))	return false;
				}
			}
		}
		return true;
	}

	//! Simple test for planes.
	SIMD_FORCE_INLINE bool collide_plane(
		const btVector4 & plane) const
	{
		eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
		return (classify == BT_CONST_COLLIDE_PLANE);
	}

	//! test for a triangle, with edges
	SIMD_FORCE_INLINE bool collide_triangle_exact(
		const btVector3 & p1,
		const btVector3 & p2,
		const btVector3 & p3,
		const btVector4 & triangle_plane) const
	{
		if(!collide_plane(triangle_plane)) return false;

		btVector3 center,extends;
		this->get_center_extend(center,extends);

		const btVector3 v1(p1 - center);
		const btVector3 v2(p2 - center);
		const btVector3 v3(p3 - center);

		//First axis
		btVector3 diff(v2 - v1);
		btVector3 abs_diff = diff.absolute();
		//Test With X axis
		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
		//Test With Y axis
		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
		//Test With Z axis
		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);

		diff = v3 - v2;
		abs_diff = diff.absolute();
		//Test With X axis
		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
		//Test With Y axis
		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
		//Test With Z axis
		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);

		diff = v1 - v3;
		abs_diff = diff.absolute();
		//Test With X axis
		TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
		//Test With Y axis
		TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
		//Test With Z axis
		TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);

		return true;
	}
};

//! Compairison of transformation objects
SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
{
	if(!(t1.getOrigin() == t2.getOrigin()) ) return false;

	if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
	if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
	if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
	return true;
}

#endif // GIM_BOX_COLLISION_H_INCLUDED


text:
/*! \file btGenericPoolAllocator.h
\author Francisco Leon Najera. email projectileman@yahoo.com

General purpose allocator class
*/
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef BT_GENERIC_POOL_ALLOCATOR_H
#define BT_GENERIC_POOL_ALLOCATOR_H

#include <limits.h>
#include <stdio.h>
#include <string.h>
#include "LinearMath/btAlignedAllocator.h"

#define BT_UINT_MAX UINT_MAX
#define BT_DEFAULT_MAX_POOLS 16

//! Generic Pool class
class btGenericMemoryPool
{
public:
	unsigned char * m_pool; //[m_element_size*m_max_element_count];
	size_t * m_free_nodes; //[m_max_element_count];//! free nodes
	size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node
	size_t m_allocated_count;
	size_t m_free_nodes_count;
protected:
	size_t m_element_size;
	size_t m_max_element_count;

	size_t allocate_from_free_nodes(size_t num_elements);
	size_t allocate_from_pool(size_t num_elements);

public:

	void init_pool(size_t element_size, size_t element_count);

	void end_pool();

	btGenericMemoryPool(size_t element_size, size_t element_count)
	{
		init_pool(element_size, element_count);
	}

	~btGenericMemoryPool()
	{
		end_pool();
	}

	inline size_t get_pool_capacity()
	{
		return m_element_size*m_max_element_count;
	}

	inline size_t gem_element_size()
	{
		return m_element_size;
	}

	inline size_t get_max_element_count()
	{
		return m_max_element_count;
	}

	inline size_t get_allocated_count()
	{
		return m_allocated_count;
	}

	inline size_t get_free_positions_count()
	{
		return m_free_nodes_count;
	}

	inline void * get_element_data(size_t element_index)
	{
		return &m_pool[element_index*m_element_size];
	}

	//! Allocates memory in pool
	/*!
	\param size_bytes size in bytes of the buffer
	*/
	void * allocate(size_t size_bytes);

	bool freeMemory(void * pointer);
};

//! Generic Allocator with pools
/*!
General purpose Allocator which can create Memory Pools dynamiacally as needed.
*/
class btGenericPoolAllocator
{
protected:
	size_t m_pool_element_size;
	size_t m_pool_element_count;
public:
	btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS];
	size_t m_pool_count;

	inline size_t get_pool_capacity()
	{
		return m_pool_element_size*m_pool_element_count;
	}

protected:
	// creates a pool
	btGenericMemoryPool * push_new_pool();

	void * failback_alloc(size_t size_bytes);

	bool failback_free(void * pointer);
public:

	btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count)
	{
		m_pool_count = 0;
		m_pool_element_size = pool_element_size;
		m_pool_element_count = pool_element_count;
	}

	virtual ~btGenericPoolAllocator();

	//! Allocates memory in pool
	/*!
	\param size_bytes size in bytes of the buffer
	*/
	void * allocate(size_t size_bytes);

	bool freeMemory(void * pointer);
};

void * btPoolAlloc(size_t size);
void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
void btPoolFree(void *ptr);

#endif


text:
/*! \file btGImpactShape.h
\author Francisco Leon Najera
*/
/*
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef GIMPACT_TRIANGLE_SHAPE_EX_H
#define GIMPACT_TRIANGLE_SHAPE_EX_H

#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "btBoxCollision.h"
#include "btClipPolygon.h"
#include "btGeometryOperations.h"

#define MAX_TRI_CLIPPING 16

//! Structure for collision
struct GIM_TRIANGLE_CONTACT
{
    btScalar m_penetration_depth;
    int m_point_count;
    btVector4 m_separating_normal;
    btVector3 m_points[MAX_TRI_CLIPPING];

	SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other)
	{
		m_penetration_depth = other.m_penetration_depth;
		m_separating_normal = other.m_separating_normal;
		m_point_count = other.m_point_count;
		int i = m_point_count;
		while(i--)
		{
			m_points[i] = other.m_points[i];
		}
	}

	GIM_TRIANGLE_CONTACT()
	{
	}

	GIM_TRIANGLE_CONTACT(const GIM_TRIANGLE_CONTACT& other)
	{
		copy_from(other);
	}

    //! classify points that are closer
    void merge_points(const btVector4 & plane,
    				btScalar margin, const btVector3 * points, int point_count);

};

class btPrimitiveTriangle
{
public:
	btVector3 m_vertices[3];
	btVector4 m_plane;
	btScalar m_margin;
	btScalar m_dummy;
	btPrimitiveTriangle():m_margin(0.01f)
	{

	}

	SIMD_FORCE_INLINE void buildTriPlane()
	{
		btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]);
		normal.normalize();
		m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal));
	}

	//! Test if triangles could collide
	bool overlap_test_conservative(const btPrimitiveTriangle& other);

	//! Calcs the plane which is paralele to the edge and perpendicular to the triangle plane
	/*!
	\pre this triangle must have its plane calculated.
	*/
	SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane)  const
    {
		const btVector3 & e0 = m_vertices[edge_index];
		const btVector3 & e1 = m_vertices[(edge_index+1)%3];
		bt_edge_plane(e0,e1,m_plane,plane);
    }

    void applyTransform(const btTransform& t)
	{
		m_vertices[0] = t(m_vertices[0]);
		m_vertices[1] = t(m_vertices[1]);
		m_vertices[2] = t(m_vertices[2]);
	}

	//! Clips the triangle against this
	/*!
	\pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated.
	\return the number of clipped points
	*/
    int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points );

	//! Find collision using the clipping method
	/*!
	\pre this triangle and other must have their triangles calculated
	*/
    bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts);
};

//! Helper class for colliding Bullet Triangle Shapes
/*!
This class implements a better getAabb method than the previous btTriangleShape class
*/
class btTriangleShapeEx: public btTriangleShape
{
public:

	btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0))
	{
	}

	btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2):	btTriangleShape(p0,p1,p2)
	{
	}

	btTriangleShapeEx(const btTriangleShapeEx & other):	btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2])
	{
	}

	virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
	{
		btVector3 tv0 = t(m_vertices1[0]);
		btVector3 tv1 = t(m_vertices1[1]);
		btVector3 tv2 = t(m_vertices1[2]);

		btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin);
		aabbMin = trianglebox.m_min;
		aabbMax = trianglebox.m_max;
	}

	void applyTransform(const btTransform& t)
	{
		m_vertices1[0] = t(m_vertices1[0]);
		m_vertices1[1] = t(m_vertices1[1]);
		m_vertices1[2] = t(m_vertices1[2]);
	}

	SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const
	{
		btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
		normal.normalize();
		plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal));
	}

	bool overlap_test_conservative(const btTriangleShapeEx& other);
};

#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H


text:
#ifndef GIM_MEMORY_H_INCLUDED
#define GIM_MEMORY_H_INCLUDED
/*! \file gim_memory.h
\author Francisco Leon Najera
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.
   (3) The zlib/libpng license that is included with this library in
       the file GIMPACT-LICENSE-ZLIB.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/

#include "gim_math.h"
#include <string.h>

#ifdef PREFETCH
#include <xmmintrin.h>	// for prefetch
#define pfval	64
#define pfval2	128
//! Prefetch 64
#define pf(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval), 0)
//! Prefetch 128
#define pf2(_x,_i)	_mm_prefetch((void *)(_x + _i + pfval2), 0)
#else
//! Prefetch 64
#define pf(_x,_i)
//! Prefetch 128
#define pf2(_x,_i)
#endif

///Functions for manip packed arrays of numbers
#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\
{\
    for (GUINT _i_=0;_i_<element_count ;++_i_)\
    {\
    	dest_array[_i_] = source_array[_i_];\
    }\
}\

#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\
{\
    for (GUINT _i_=0;_i_<element_count ;++_i_)\
    {\
    	copy_macro(dest_array[_i_],source_array[_i_]);\
    }\
}\

#define GIM_ZERO_ARRAY(array,element_count)\
{\
    for (GUINT _i_=0;_i_<element_count ;++_i_)\
    {\
    	array[_i_] = 0;\
    }\
}\

#define GIM_CONSTANT_ARRAY(array,element_count,constant)\
{\
    for (GUINT _i_=0;_i_<element_count ;++_i_)\
    {\
    	array[_i_] = constant;\
    }\
}\

///Function prototypes to allocate and free memory.
typedef void * gim_alloc_function (size_t size);
typedef void * gim_alloca_function (size_t size);//Allocs on the heap
typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize);
typedef void gim_free_function (void *ptr);

///Memory Function Handlers
///set new memory management functions. if fn is 0, the default handlers are used.
void gim_set_alloc_handler (gim_alloc_function *fn);
void gim_set_alloca_handler (gim_alloca_function *fn);
void gim_set_realloc_handler (gim_realloc_function *fn);
void gim_set_free_handler (gim_free_function *fn);

///get current memory management functions.
gim_alloc_function *gim_get_alloc_handler (void);
gim_alloca_function *gim_get_alloca_handler(void);
gim_realloc_function *gim_get_realloc_handler (void);
gim_free_function  *gim_get_free_handler (void);

///Standar Memory functions
void * gim_alloc(size_t size);
void * gim_alloca(size_t size);
void * gim_realloc(void *ptr, size_t oldsize, size_t newsize);
void gim_free(void *ptr);

#if defined (_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
    #define GIM_SIMD_MEMORY 1
#endif

//! SIMD POINTER INTEGER
#define SIMD_T GUINT64
//! SIMD INTEGER SIZE
#define SIMD_T_SIZE sizeof(SIMD_T)

inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
{
#ifdef GIM_SIMD_MEMORY
/*
//'long long int' is incompatible with visual studio 6...
    //copy words
    SIMD_T * ui_src_ptr = (SIMD_T *)src;
    SIMD_T * ui_dst_ptr = (SIMD_T *)dst;
    while(copysize>=SIMD_T_SIZE)
    {
        *(ui_dst_ptr++) = *(ui_src_ptr++);
        copysize-=SIMD_T_SIZE;
    }
    if(copysize==0) return;
*/

    char * c_src_ptr = (char *)src;
    char * c_dst_ptr = (char *)dst;
    while(copysize>0)
    {
        *(c_dst_ptr++) = *(c_src_ptr++);
        copysize--;
    }
    return;
#else
    memcpy(dst,src,copysize);
#endif
}

template<class T>
inline void gim_swap_elements(T* _array,size_t _i,size_t _j)
{
	T _e_tmp_ = _array[_i];
	_array[_i] = _array[_j];
	_array[_j] = _e_tmp_;
}

template<class T>
inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j)
{
	char _e_tmp_[sizeof(T)];
	gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T));
	gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T));
	gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T));
}

template <int SIZE>
inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j)
{
	char _e_tmp_[SIZE];
	_i*=SIZE;
	_j*=SIZE;
	gim_simd_memcpy(_e_tmp_,_array+_i,SIZE);
	gim_simd_memcpy(_array+_i,_array+_j,SIZE);
	gim_simd_memcpy(_array+_j,_e_tmp_,SIZE);
}

#endif // GIM_MEMORY_H_INCLUDED


text:
#ifndef BT_CONTACT_H_INCLUDED
#define BT_CONTACT_H_INCLUDED

/*! \file gim_contact.h
\author Francisco Leon Najera
*/
/*
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "LinearMath/btTransform.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btTriangleShapeEx.h"
#include "btContactProcessingStructs.h"

class btContactArray:public btAlignedObjectArray<GIM_CONTACT>
{
public:
	btContactArray()
	{
		reserve(64);
	}

	SIMD_FORCE_INLINE void push_contact(
		const btVector3 &point,const btVector3 & normal,
		btScalar depth, int feature1, int feature2)
	{
		push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) );
	}

	SIMD_FORCE_INLINE void push_triangle_contacts(
		const GIM_TRIANGLE_CONTACT & tricontact,
		int feature1,int feature2)
	{
		for(int i = 0;i<tricontact.m_point_count ;i++ )
		{
			push_contact(
				tricontact.m_points[i],
				tricontact.m_separating_normal,
				tricontact.m_penetration_depth,feature1,feature2);
		}
	}

	void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true);

	void merge_contacts_unique(const btContactArray & contacts);
};

#endif // GIM_CONTACT_H_INCLUDED


text:
#ifndef GIM_CLIP_POLYGON_H_INCLUDED
#define GIM_CLIP_POLYGON_H_INCLUDED

/*! \file gim_tri_collision.h
\author Francisco Leon Najera
*/
/*
-----------------------------------------------------------------------------
This source file is part of GIMPACT Library.

For the latest info, see http://gimpact.sourceforge.net/

Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
email: projectileman@yahoo.com

 This library is free software; you can redistribute it and/or
 modify it under the terms of EITHER:
   (1) The GNU Lesser General Public License as published by the Free
       Software Foundation; either version 2.1 of the License, or (at
       your option) any later version. The text of the GNU Lesser
       General Public License is included with this library in the
       file GIMPACT-LICENSE-LGPL.TXT.
   (2) The BSD-style license that is included with this library in
       the file GIMPACT-LICENSE-BSD.TXT.
   (3) The zlib/libpng license that is included with this library in
       the file GIMPACT-LICENSE-ZLIB.TXT.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.

-----------------------------------------------------------------------------
*/

//! This function calcs the distance from a 3D plane
class DISTANCE_PLANE_3D_FUNC
{
public:
	template<typename CLASS_POINT,typename CLASS_PLANE>
	inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point)
	{
		return DISTANCE_PLANE_POINT(plane, point);
	}
};

template<typename CLASS_POINT>
SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT(
						const CLASS_POINT & point0,
						const CLASS_POINT & point1,
						GREAL dist0,
						GREAL dist1,
						CLASS_POINT * clipped,
						GUINT & clipped_count)
{
	GUINT _prevclassif = (dist0>G_EPSILON);
	GUINT _classif = (dist1>G_EPSILON);
	if(_classif!=_prevclassif)
	{
		GREAL blendfactor = -dist0/(dist1-dist0);
		VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor);
		clipped_count++;
	}
	if(!_classif)
	{
		VEC_COPY(clipped[clipped_count],point1);
		clipped_count++;
	}
}

//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
						const CLASS_PLANE & plane,
						const CLASS_POINT * polygon_points,
						GUINT polygon_point_count,
						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
{
    GUINT clipped_count = 0;

    //clip first point
	GREAL firstdist = distance_func(plane,polygon_points[0]);;
	if(!(firstdist>G_EPSILON))
	{
		VEC_COPY(clipped[clipped_count],polygon_points[0]);
		clipped_count++;
	}

	GREAL olddist = firstdist;
	for(GUINT _i=1;_i<polygon_point_count;_i++)
	{
		GREAL dist = distance_func(plane,polygon_points[_i]);

		PLANE_CLIP_POLYGON_COLLECT(
						polygon_points[_i-1],polygon_points[_i],
						olddist,
						dist,
						clipped,
						clipped_count);

		olddist = dist;
	}

	//RETURN TO FIRST  point

	PLANE_CLIP_POLYGON_COLLECT(
					polygon_points[polygon_point_count-1],polygon_points[0],
					olddist,
					firstdist,
					clipped,
					clipped_count);

	return clipped_count;
}

//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC(
						const CLASS_PLANE & plane,
						const CLASS_POINT & point0,
						const CLASS_POINT & point1,
						const CLASS_POINT & point2,
						CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
{
    GUINT clipped_count = 0;

    //clip first point
	GREAL firstdist = distance_func(plane,point0);;
	if(!(firstdist>G_EPSILON))
	{
		VEC_COPY(clipped[clipped_count],point0);
		clipped_count++;
	}

	// point 1
	GREAL olddist = firstdist;
	GREAL dist = distance_func(plane,point1);

	PLANE_CLIP_POLYGON_COLLECT(
					point0,point1,
					olddist,
					dist,
					clipped,
					clipped_count);

	olddist = dist;

	// point 2
	dist = distance_func(plane,point2);

	PLANE_CLIP_POLYGON_COLLECT(
					point1,point2,
					olddist,
					dist,
					clipped,
					clipped_count);
	olddist = dist;

	//RETURN TO FIRST  point
	PLANE_CLIP_POLYGON_COLLECT(
					point2,point0,
					olddist,
					firstdist,
					clipped,
					clipped_count);

	return clipped_count;
}

template<typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D(
						const CLASS_PLANE & plane,
						const CLASS_POINT * polygon_points,
						GUINT polygon_point_count,
						CLASS_POINT * clipped)
{
	return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC());
}

template<typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D(
						const CLASS_PLANE & plane,
						const CLASS_POINT & point0,
						const CLASS_POINT & point1,
						const CLASS_POINT & point2,
						CLASS_POINT * clipped)
{
	return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC());
}

#endif // GIM_TRI_COLLISION_H_INCLUDED