text:
#ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
#define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED

/*! \file gim_basic_geometry_operations.h
*\author Francisco Leon Najera
type independant geometry routines

*/
/*
-----------------------------------------------------------------------------
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_linear_math.h"

#ifndef PLANEDIREPSILON
#define PLANEDIREPSILON 0.0000001f
#endif

#ifndef PARALELENORMALS
#define PARALELENORMALS 0.000001f
#endif

#define TRIANGLE_NORMAL(v1,v2,v3,n)\
{\
	vec3f _dif1,_dif2;\
    VEC_DIFF(_dif1,v2,v1);\
    VEC_DIFF(_dif2,v3,v1);\
    VEC_CROSS(n,_dif1,_dif2);\
    VEC_NORMALIZE(n);\
}\

#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
    vec3f _dif1,_dif2; \
    VEC_DIFF(_dif1,v2,v1); \
    VEC_DIFF(_dif2,v3,v1); \
    VEC_CROSS(n,_dif1,_dif2); \
}\

/// plane is a vec4f
#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
    TRIANGLE_NORMAL(v1,v2,v3,plane);\
    plane[3] = VEC_DOT(v1,plane);\
}\

/// plane is a vec4f
#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
    TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
    plane[3] = VEC_DOT(v1,plane);\
}\

/// Calc a plane from an edge an a normal. plane is a vec4f
#define EDGE_PLANE(e1,e2,n,plane) {\
    vec3f _dif; \
    VEC_DIFF(_dif,e2,e1); \
    VEC_CROSS(plane,_dif,n); \
    VEC_NORMALIZE(plane); \
    plane[3] = VEC_DOT(e1,plane);\
}\

#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])

#define PROJECT_POINT_PLANE(point,plane,projected) {\
	GREAL _dis;\
	_dis = DISTANCE_PLANE_POINT(plane,point);\
	VEC_SCALE(projected,-_dis,plane);\
	VEC_SUM(projected,projected,point);	\
}\

//! Verifies if a point is in the plane hull
template<typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE bool POINT_IN_HULL(
	const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
{
	GREAL _dis;
	for (GUINT _i = 0;_i< plane_count;++_i)
	{
		_dis = DISTANCE_PLANE_POINT(planes[_i],point);
	    if(_dis>0.0f) return false;
	}
	return true;
}

template<typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
	const CLASS_POINT& s1,
	const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
{
	GREAL _dis1,_dis2;
	_dis1 = DISTANCE_PLANE_POINT(plane,s1);
	VEC_DIFF(clipped,s2,s1);
	_dis2 = VEC_DOT(clipped,plane);
	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
	VEC_SUM(clipped,clipped,s1);
}

enum ePLANE_INTERSECTION_TYPE
{
	G_BACK_PLANE = 0,
	G_COLLIDE_PLANE,
	G_FRONT_PLANE
};

enum eLINE_PLANE_INTERSECTION_TYPE
{
	G_FRONT_PLANE_S1 = 0,
	G_FRONT_PLANE_S2,
	G_BACK_PLANE_S1,
	G_BACK_PLANE_S2,
	G_COLLIDE_PLANE_S1,
	G_COLLIDE_PLANE_S2
};

//! Confirms if the plane intersect the edge or nor
/*!
intersection type must have the following values

 0 : Segment in front of plane, s1 closest

 1 : Segment in front of plane, s2 closest

 2 : Segment in back of plane, s1 closest

 3 : Segment in back of plane, s2 closest

 4 : Segment collides plane, s1 in back

 5 : Segment collides plane, s2 in back

*/

template<typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
	const CLASS_POINT& s1,
	const CLASS_POINT &s2,
	const CLASS_PLANE &plane,CLASS_POINT &clipped)
{
	GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
	GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
	if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
	{
	    if(_dis1<_dis2) return G_FRONT_PLANE_S1;
	    return G_FRONT_PLANE_S2;
	}
	else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
	{
	    if(_dis1>_dis2) return G_BACK_PLANE_S1;
	    return G_BACK_PLANE_S2;
	}

	VEC_DIFF(clipped,s2,s1);
	_dis2 = VEC_DOT(clipped,plane);
	VEC_SCALE(clipped,-_dis1/_dis2,clipped);
	VEC_SUM(clipped,clipped,s1);
	if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
	return G_COLLIDE_PLANE_S2;
}

//! Confirms if the plane intersect the edge or not
/*!
clipped1 and clipped2 are the vertices behind the plane.
clipped1 is the closest

intersection_type must have the following values

 0 : Segment in front of plane, s1 closest

 1 : Segment in front of plane, s2 closest

 2 : Segment in back of plane, s1 closest

 3 : Segment in back of plane, s2 closest

 4 : Segment collides plane, s1 in back

 5 : Segment collides plane, s2 in back

*/
template<typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
	const CLASS_POINT& s1,
	const CLASS_POINT &s2,
	const CLASS_PLANE &plane,
	CLASS_POINT &clipped1,CLASS_POINT &clipped2)
{
	eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
	switch(intersection_type)
	{
	case G_FRONT_PLANE_S1:
		VEC_COPY(clipped1,s1);
	    VEC_COPY(clipped2,s2);
		break;
	case G_FRONT_PLANE_S2:
		VEC_COPY(clipped1,s2);
	    VEC_COPY(clipped2,s1);
		break;
	case G_BACK_PLANE_S1:
		VEC_COPY(clipped1,s1);
	    VEC_COPY(clipped2,s2);
		break;
	case G_BACK_PLANE_S2:
		VEC_COPY(clipped1,s2);
	    VEC_COPY(clipped2,s1);
		break;
	case G_COLLIDE_PLANE_S1:
		VEC_COPY(clipped2,s1);
		break;
	case G_COLLIDE_PLANE_S2:
		VEC_COPY(clipped2,s2);
		break;
	}
	return intersection_type;
}

//! Finds the 2 smallest cartesian coordinates of a plane normal
#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)

//! Ray plane collision in one way
/*!
Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).

It uses the PLANEDIREPSILON constant.
*/
template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
	const CLASS_PLANE & plane,
	const CLASS_POINT & vDir,
	const CLASS_POINT & vPoint,
	CLASS_POINT & pout,T &tparam)
{
	GREAL _dis,_dotdir;
	_dotdir = VEC_DOT(plane,vDir);
	if(_dotdir<PLANEDIREPSILON)
	{
	    return false;
	}
	_dis = DISTANCE_PLANE_POINT(plane,vPoint);
	tparam = -_dis/_dotdir;
	VEC_SCALE(pout,tparam,vDir);
	VEC_SUM(pout,vPoint,pout);
	return true;
}

//! line collision
/*!
*\return
	-0  if the ray never intersects
	-1 if the ray collides in front
	-2 if the ray collides in back
*/
template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
	const CLASS_PLANE & plane,
	const CLASS_POINT & vDir,
	const CLASS_POINT & vPoint,
	CLASS_POINT & pout,
	T &tparam,
	T tmin, T tmax)
{
	GREAL _dis,_dotdir;
	_dotdir = VEC_DOT(plane,vDir);
	if(btFabs(_dotdir)<PLANEDIREPSILON)
	{
		tparam = tmax;
	    return 0;
	}
	_dis = DISTANCE_PLANE_POINT(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;
	}

	VEC_SCALE(pout,tparam,vDir);
	VEC_SUM(pout,vPoint,pout);
	return returnvalue;
}

/*! \brief Returns the Ray on which 2 planes intersect if they do.
    Written by Rodrigo Hernandez on ODE convex collision

  \param p1 Plane 1
  \param p2 Plane 2
  \param p Contains the origin of the ray upon returning if planes intersect
  \param d Contains the direction of the ray upon returning if planes intersect
  \return true if the planes intersect, 0 if paralell.

*/
template<typename CLASS_POINT,typename CLASS_PLANE>
SIMD_FORCE_INLINE bool INTERSECT_PLANES(
		const CLASS_PLANE &p1,
		const CLASS_PLANE &p2,
		CLASS_POINT &p,
		CLASS_POINT &d)
{
	VEC_CROSS(d,p1,p2);
  	GREAL denom = VEC_DOT(d, d);
  	if(GIM_IS_ZERO(denom)) return false;
	vec3f _n;
	_n[0]=p1[3]*p2[0] - p2[3]*p1[0];
	_n[1]=p1[3]*p2[1] - p2[3]*p1[1];
	_n[2]=p1[3]*p2[2] - p2[3]*p1[2];
	VEC_CROSS(p,_n,d);
	p[0]/=denom;
	p[1]/=denom;
	p[2]/=denom;
	return true;
}

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

/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
 */
template<typename CLASS_POINT>
SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
	CLASS_POINT & cp, const CLASS_POINT & v,
	const CLASS_POINT &e1,const CLASS_POINT &e2)
{
    vec3f _n;
    VEC_DIFF(_n,e2,e1);
    VEC_DIFF(cp,v,e1);
	GREAL _scalar = VEC_DOT(cp, _n);
	_scalar/= VEC_DOT(_n, _n);
	if(_scalar <0.0f)
	{
	    VEC_COPY(cp,e1);
	}
	else if(_scalar >1.0f)
	{
	    VEC_COPY(cp,e2);
	}
	else
	{
        VEC_SCALE(cp,_scalar,_n);
        VEC_SUM(cp,cp,e1);
	}
}

/*! \brief Finds the line params where these lines intersect.

\param dir1 Direction of line 1
\param point1 Point of line 1
\param dir2 Direction of line 2
\param point2 Point of line 2
\param t1 Result Parameter for line 1
\param t2 Result Parameter for line 2
\param dointersect  0  if the lines won't intersect, else 1

*/
template<typename T,typename CLASS_POINT>
SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
	const CLASS_POINT & dir1,
	CLASS_POINT & point1,
	const CLASS_POINT & dir2,
	CLASS_POINT &  point2,
	T& t1,T& t2)
{
    GREAL det;
	GREAL e1e1 = VEC_DOT(dir1,dir1);
	GREAL e1e2 = VEC_DOT(dir1,dir2);
	GREAL e2e2 = VEC_DOT(dir2,dir2);
	vec3f p1p2;
    VEC_DIFF(p1p2,point1,point2);
    GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
	GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
	det = e1e2*e1e2 - e1e1*e2e2;
	if(GIM_IS_ZERO(det)) return false;
	t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
	t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
	return true;
}

//! Find closest points on segments
template<typename CLASS_POINT>
SIMD_FORCE_INLINE void SEGMENT_COLLISION(
	const CLASS_POINT & vA1,
	const CLASS_POINT & vA2,
	const CLASS_POINT & vB1,
	const CLASS_POINT & vB2,
	CLASS_POINT & vPointA,
	CLASS_POINT & vPointB)
{
    CLASS_POINT _AD,_BD,n;
    vec4f _M;//plane
    VEC_DIFF(_AD,vA2,vA1);
    VEC_DIFF(_BD,vB2,vB1);
    VEC_CROSS(n,_AD,_BD);
    GREAL _tp = VEC_DOT(n,n);
    if(_tp<G_EPSILON)//ARE PARALELE
    {
    	//project B over A
    	bool invert_b_order = false;
    	_M[0] = VEC_DOT(vB1,_AD);
    	_M[1] = VEC_DOT(vB2,_AD);
    	if(_M[0]>_M[1])
    	{
    		invert_b_order  = true;
    		GIM_SWAP_NUMBERS(_M[0],_M[1]);
    	}
    	_M[2] = VEC_DOT(vA1,_AD);
    	_M[3] = VEC_DOT(vA2,_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;
    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
    		}
    		else
    		{
    			vPointA = vA2;
    			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;
    			CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
    		}
    		else
    		{
    			vPointB = invert_b_order?vB1:vB2;
    			CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
    		}
    	}
    	return;
    }

    VEC_CROSS(_M,n,_BD);
    _M[3] = VEC_DOT(_M,vB1);

    LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
    /*Closest point on segment*/
    VEC_DIFF(vPointB,vPointA,vB1);
	_tp = VEC_DOT(vPointB, _BD);
	_tp/= VEC_DOT(_BD, _BD);
	_tp = GIM_CLAMP(_tp,0.0f,1.0f);
    VEC_SCALE(vPointB,_tp,_BD);
    VEC_SUM(vPointB,vPointB,vB1);
}

//! Line box intersection in one dimension
/*!

*\param pos Position of the ray
*\param dir Projection of the Direction of the ray
*\param bmin Minimum bound of the box
*\param bmax Maximum bound of the box
*\param tfirst the minimum projection. Assign to 0 at first.
*\param tlast the maximum projection. Assign to INFINITY at first.
*\return true if there is an intersection.
*/
template<typename T>
SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
{
	if(GIM_IS_ZERO(dir))
	{
        return !(pos < bmin || pos > bmax);
	}
	GREAL a0 = (bmin - pos) / dir;
	GREAL a1 = (bmax - pos) / dir;
	if(a0 > a1)   GIM_SWAP_NUMBERS(a0, a1);
	tfirst = GIM_MAX(a0, tfirst);
	tlast = GIM_MIN(a1, tlast);
	if (tlast < tfirst) return false;
	return true;
}

//! Sorts 3 componets
template<typename T>
SIMD_FORCE_INLINE void SORT_3_INDICES(
		const T * values,
		GUINT * order_indices)
{
	//get minimum
	order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);

	//get second and third
	GUINT i0 = (order_indices[0] + 1)%3;
	GUINT i1 = (i0 + 1)%3;

	if(values[i0] < values[i1])
	{
		order_indices[1] = i0;
		order_indices[2] = i1;
	}
	else
	{
		order_indices[1] = i1;
		order_indices[2] = i0;
	}
}

#endif // GIM_VECTOR_H_INCLUDED


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) 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"
#include "btGImpactBvhStructs.h"

//! A pairset array
class btPairSet: public btAlignedObjectArray<GIM_PAIR>
{
public:
	btPairSet()
	{
		reserve(32);
	}
	inline void push_pair(int index1,int index2)
	{
		push_back(GIM_PAIR(index1,index2));
	}

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

class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA>
{
};

class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<GIM_BVH_TREE_NODE>
{
};

//! Basic Box tree structure
class btBvhTree
{
protected:
	int m_num_nodes;
	GIM_BVH_TREE_NODE_ARRAY m_node_array;
protected:
	int _sort_and_calc_splitting_index(
		GIM_BVH_DATA_ARRAY & primitive_boxes,
		 int startIndex,  int endIndex, int splitAxis);

	int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);

	void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
public:
	btBvhTree()
	{
		m_num_nodes = 0;
	}

	//! prototype functions for box tree management
	//!@{
	void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);

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

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

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

	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
	{
		return m_node_array[nodeindex].getDataIndex();
	}

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

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

	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
	{
		return nodeindex+1;
	}

	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
	{
		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
	}

	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
	{
		return m_node_array[nodeindex].getEscapeIndex();
	}

	SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
	{
		return &m_node_array[index];
	}

	//!@}
};

//! 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 btPrimitiveManagerBase
{
public:

	virtual ~btPrimitiveManagerBase() {}

	//! determines if this manager consist on only triangles, which special case will be optimized
	virtual bool is_trimesh() const = 0;
	virtual int get_primitive_count() const = 0;
	virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0;
	//! retrieves only the points of the triangle, and the collision margin
	virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0;
};

//! Structure for containing Boxes
/*!
This class offers an structure for managing a box tree of primitives.
Requires a Primitive prototype (like btPrimitiveManagerBase )
*/
class btGImpactBvh
{
protected:
	btBvhTree m_box_tree;
	btPrimitiveManagerBase * m_primitive_manager;

protected:
	//stackless refit
	void refit();
public:

	//! this constructor doesn't build the tree. you must call	buildSet
	btGImpactBvh()
	{
		m_primitive_manager = NULL;
	}

	//! this constructor doesn't build the tree. you must call	buildSet
	btGImpactBvh(btPrimitiveManagerBase * primitive_manager)
	{
		m_primitive_manager = primitive_manager;
	}

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

	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
	{
		m_primitive_manager = primitive_manager;
	}

	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
	{
		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
	void buildSet();

	//! returns the indices of the primitives in the m_primitive_manager
	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;

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

	//! returns the indices of the primitives in the m_primitive_manager
	bool rayQuery(
		const btVector3 & ray_dir,const btVector3 & ray_origin ,
		btAlignedObjectArray<int> & collided_results) const;

	//! 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 int getNodeCount() const
	{
		return m_box_tree.getNodeCount();
	}

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

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

	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
	{
		m_box_tree.getNodeBound(nodeindex, bound);
	}

	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
	{
		m_box_tree.setNodeBound(nodeindex, bound);
	}

	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
	{
		return m_box_tree.getLeftNode(nodeindex);
	}

	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
	{
		return m_box_tree.getRightNode(nodeindex);
	}

	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
	{
		return m_box_tree.getEscapeNodeIndex(nodeindex);
	}

	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
	{
		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
	}

	SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
	{
		return m_box_tree.get_node_pointer(index);
	}

#ifdef TRI_COLLISION_PROFILING
	static float getAverageTreeCollisionTime();
#endif //TRI_COLLISION_PROFILING

	static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1,
		btGImpactBvh * boxset2, const btTransform & trans2,
		btPairSet & collision_pairs);
};

#endif // GIM_BOXPRUNING_H_INCLUDED

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


text:
#ifndef GIM_CONTACT_H_INCLUDED
#define GIM_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) 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_geometry.h"
#include "gim_radixsort.h"
#include "gim_array.h"

/**
Configuration var for applying interpolation of  contact normals
*/
#ifndef NORMAL_CONTACT_AVERAGE
#define NORMAL_CONTACT_AVERAGE 1
#endif

#ifndef CONTACT_DIFF_EPSILON
#define CONTACT_DIFF_EPSILON 0.00001f
#endif

#ifndef BT_CONTACT_H_STRUCTS_INCLUDED

/// Structure for collision results
///Functions for managing and sorting contacts resulting from a collision query.
///Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST
///After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY
///Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts
class GIM_CONTACT
{
public:
    btVector3 m_point;
    btVector3 m_normal;
    GREAL m_depth;//Positive value indicates interpenetration
    GREAL m_distance;//Padding not for use
    GUINT m_feature1;//Face number
    GUINT m_feature2;//Face number
public:
    GIM_CONTACT()
    {
    }

    GIM_CONTACT(const GIM_CONTACT & contact):
				m_point(contact.m_point),
				m_normal(contact.m_normal),
				m_depth(contact.m_depth),
				m_feature1(contact.m_feature1),
				m_feature2(contact.m_feature2)
    {
    	m_point = contact.m_point;
    	m_normal = contact.m_normal;
    	m_depth = contact.m_depth;
    	m_feature1 = contact.m_feature1;
    	m_feature2 = contact.m_feature2;
    }

    GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
    	 			GREAL depth, GUINT feature1, GUINT feature2):
				m_point(point),
				m_normal(normal),
				m_depth(depth),
				m_feature1(feature1),
				m_feature2(feature2)
    {
    }

	//! Calcs key for coord classification
    SIMD_FORCE_INLINE GUINT calc_key_contact() const
    {
    	GINT _coords[] = {
    		(GINT)(m_point[0]*1000.0f+1.0f),
    		(GINT)(m_point[1]*1333.0f),
    		(GINT)(m_point[2]*2133.0f+3.0f)};
		GUINT _hash=0;
		GUINT *_uitmp = (GUINT *)(&_coords[0]);
		_hash = *_uitmp;
		_uitmp++;
		_hash += (*_uitmp)<<4;
		_uitmp++;
		_hash += (*_uitmp)<<8;
		return _hash;
    }

    SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count)
    {
    	btVector3 vec_sum(m_normal);
		for(GUINT i=0;i<normal_count;i++)
		{
			vec_sum += normals[i];
		}

		GREAL vec_sum_len = vec_sum.length2();
		if(vec_sum_len <CONTACT_DIFF_EPSILON) return;

		GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)

		m_normal = vec_sum*vec_sum_len;
    }

};

#endif

class gim_contact_array:public gim_array<GIM_CONTACT>
{
public:
	gim_contact_array():gim_array<GIM_CONTACT>(64)
	{
	}

	SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal,
    	 			GREAL depth, GUINT feature1, GUINT feature2)
	{
		push_back_mem();
		GIM_CONTACT & newele = back();
		newele.m_point = point;
		newele.m_normal = normal;
		newele.m_depth = depth;
		newele.m_feature1 = feature1;
		newele.m_feature2 = feature2;
	}

	SIMD_FORCE_INLINE void push_triangle_contacts(
		const GIM_TRIANGLE_CONTACT_DATA & tricontact,
		GUINT feature1,GUINT feature2)
	{
		for(GUINT i = 0;i<tricontact.m_point_count ;i++ )
		{
			push_back_mem();
			GIM_CONTACT & newele = back();
			newele.m_point = tricontact.m_points[i];
			newele.m_normal = tricontact.m_separating_normal;
			newele.m_depth = tricontact.m_penetration_depth;
			newele.m_feature1 = feature1;
			newele.m_feature2 = feature2;
		}
	}

	void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true);
	void merge_contacts_unique(const gim_contact_array & contacts);
};

#endif // GIM_CONTACT_H_INCLUDED


text:
#ifndef GIM_QUANTIZED_SET_H_INCLUDED
#define GIM_QUANTIZED_SET_H_INCLUDED

/*! \file btGImpactQuantizedBvh.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 "btGImpactBvh.h"
#include "btQuantization.h"
#include "btGImpactQuantizedBvhStructs.h"

class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
{
};

//! Basic Box tree structure
class btQuantizedBvhTree
{
protected:
	int m_num_nodes;
	GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array;
	btAABB m_global_bound;
	btVector3 m_bvhQuantization;
protected:
	void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) );

	int _sort_and_calc_splitting_index(
		GIM_BVH_DATA_ARRAY & primitive_boxes,
		 int startIndex,  int endIndex, int splitAxis);

	int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);

	void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex);
public:
	btQuantizedBvhTree()
	{
		m_num_nodes = 0;
	}

	//! prototype functions for box tree management
	//!@{
	void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);

	SIMD_FORCE_INLINE void quantizePoint(
		unsigned short * quantizedpoint, const btVector3 & point) const
	{
		bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization);
	}

	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
		int node_index,
		unsigned short * quantizedMin,unsigned short * quantizedMax) const
	{
		return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax);
	}

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

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

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

	SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
	{
		return m_node_array[nodeindex].getDataIndex();
	}

	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
	{
		bound.m_min = bt_unquantize(
			m_node_array[nodeindex].m_quantizedAabbMin,
			m_global_bound.m_min,m_bvhQuantization);

		bound.m_max = bt_unquantize(
			m_node_array[nodeindex].m_quantizedAabbMax,
			m_global_bound.m_min,m_bvhQuantization);
	}

	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
	{
		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMin,
							bound.m_min,
							m_global_bound.m_min,
							m_global_bound.m_max,
							m_bvhQuantization);

		bt_quantize_clamp(	m_node_array[nodeindex].m_quantizedAabbMax,
							bound.m_max,
							m_global_bound.m_min,
							m_global_bound.m_max,
							m_bvhQuantization);
	}

	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
	{
		return nodeindex+1;
	}

	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
	{
		if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
		return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
	}

	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
	{
		return m_node_array[nodeindex].getEscapeIndex();
	}

	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
	{
		return &m_node_array[index];
	}

	//!@}
};

//! Structure for containing Boxes
/*!
This class offers an structure for managing a box tree of primitives.
Requires a Primitive prototype (like btPrimitiveManagerBase )
*/
class btGImpactQuantizedBvh
{
protected:
	btQuantizedBvhTree m_box_tree;
	btPrimitiveManagerBase * m_primitive_manager;

protected:
	//stackless refit
	void refit();
public:

	//! this constructor doesn't build the tree. you must call	buildSet
	btGImpactQuantizedBvh()
	{
		m_primitive_manager = NULL;
	}

	//! this constructor doesn't build the tree. you must call	buildSet
	btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager)
	{
		m_primitive_manager = primitive_manager;
	}

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

	SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
	{
		m_primitive_manager = primitive_manager;
	}

	SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
	{
		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
	void buildSet();

	//! returns the indices of the primitives in the m_primitive_manager
	bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;

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

	//! returns the indices of the primitives in the m_primitive_manager
	bool rayQuery(
		const btVector3 & ray_dir,const btVector3 & ray_origin ,
		btAlignedObjectArray<int> & collided_results) const;

	//! 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 int getNodeCount() const
	{
		return m_box_tree.getNodeCount();
	}

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

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

	SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound)  const
	{
		m_box_tree.getNodeBound(nodeindex, bound);
	}

	SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
	{
		m_box_tree.setNodeBound(nodeindex, bound);
	}

	SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
	{
		return m_box_tree.getLeftNode(nodeindex);
	}

	SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
	{
		return m_box_tree.getRightNode(nodeindex);
	}

	SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
	{
		return m_box_tree.getEscapeNodeIndex(nodeindex);
	}

	SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
	{
		m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
	}

	SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
	{
		return m_box_tree.get_node_pointer(index);
	}

#ifdef TRI_COLLISION_PROFILING
	static float getAverageTreeCollisionTime();
#endif //TRI_COLLISION_PROFILING

	static void find_collision(const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
		const btGImpactQuantizedBvh * boxset2, const btTransform & trans2,
		btPairSet & collision_pairs);
};

#endif // GIM_BOXPRUNING_H_INCLUDED


text:
#ifndef GIM_BITSET_H_INCLUDED
#define GIM_BITSET_H_INCLUDED
/*! \file gim_bitset.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"

#define GUINT_BIT_COUNT 32
#define GUINT_EXPONENT 5

class gim_bitset
{
public:
    gim_array<GUINT> m_container;

    gim_bitset()
    {

    }

    gim_bitset(GUINT bits_count)
    {
        resize(bits_count);
    }

    ~gim_bitset()
    {
    }

	inline bool resize(GUINT newsize)
	{
		GUINT oldsize = m_container.size();
		m_container.resize(newsize/GUINT_BIT_COUNT + 1,false);
		while(oldsize<m_container.size())
		{
			m_container[oldsize] = 0;
		}
		return true;
	}

	inline GUINT size()
	{
		return m_container.size()*GUINT_BIT_COUNT;
	}

	inline void set_all()
	{
		for(GUINT i = 0;i<m_container.size();++i)
		{
			m_container[i] = 0xffffffff;
		}
	}

	inline void clear_all()
	{
	    for(GUINT i = 0;i<m_container.size();++i)
		{
			m_container[i] = 0;
		}
	}

	inline void set(GUINT bit_index)
	{
		if(bit_index>=size())
		{
			resize(bit_index);
		}
		m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1)));
	}

	///Return 0 or 1
	inline char get(GUINT bit_index)
	{
		if(bit_index>=size())
		{
			return 0;
		}
		char value = m_container[bit_index >> GUINT_EXPONENT] &
					 (1 << (bit_index & (GUINT_BIT_COUNT-1)));
		return value;
	}

	inline void clear(GUINT bit_index)
	{
	    m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1)));
	}
};

#endif // GIM_CONTAINERS_H_INCLUDED


text:
#ifndef GIM_LINEAR_H_INCLUDED
#define GIM_LINEAR_H_INCLUDED

/*! \file gim_linear_math.h
*\author Francisco Leon Najera
Type Independant Vector and matrix operations.
*/
/*
-----------------------------------------------------------------------------
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 "gim_geom_types.h"

//! Zero out a 2D vector
#define VEC_ZERO_2(a)				\
{						\
   (a)[0] = (a)[1] = 0.0f;			\
}\

//! Zero out a 3D vector
#define VEC_ZERO(a)				\
{						\
   (a)[0] = (a)[1] = (a)[2] = 0.0f;		\
}\

/// Zero out a 4D vector
#define VEC_ZERO_4(a)				\
{						\
   (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f;	\
}\

/// Vector copy
#define VEC_COPY_2(b,a)				\
{						\
   (b)[0] = (a)[0];				\
   (b)[1] = (a)[1];				\
}\

/// Copy 3D vector
#define VEC_COPY(b,a)				\
{						\
   (b)[0] = (a)[0];				\
   (b)[1] = (a)[1];				\
   (b)[2] = (a)[2];				\
}\

/// Copy 4D vector
#define VEC_COPY_4(b,a)				\
{						\
   (b)[0] = (a)[0];				\
   (b)[1] = (a)[1];				\
   (b)[2] = (a)[2];				\
   (b)[3] = (a)[3];				\
}\

/// VECTOR SWAP
#define VEC_SWAP(b,a)				\
{  \
    GIM_SWAP_NUMBERS((b)[0],(a)[0]);\
    GIM_SWAP_NUMBERS((b)[1],(a)[1]);\
    GIM_SWAP_NUMBERS((b)[2],(a)[2]);\
}\

/// Vector difference
#define VEC_DIFF_2(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] - (v1)[0];		\
   (v21)[1] = (v2)[1] - (v1)[1];		\
}\

/// Vector difference
#define VEC_DIFF(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] - (v1)[0];		\
   (v21)[1] = (v2)[1] - (v1)[1];		\
   (v21)[2] = (v2)[2] - (v1)[2];		\
}\

/// Vector difference
#define VEC_DIFF_4(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] - (v1)[0];		\
   (v21)[1] = (v2)[1] - (v1)[1];		\
   (v21)[2] = (v2)[2] - (v1)[2];		\
   (v21)[3] = (v2)[3] - (v1)[3];		\
}\

/// Vector sum
#define VEC_SUM_2(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] + (v1)[0];		\
   (v21)[1] = (v2)[1] + (v1)[1];		\
}\

/// Vector sum
#define VEC_SUM(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] + (v1)[0];		\
   (v21)[1] = (v2)[1] + (v1)[1];		\
   (v21)[2] = (v2)[2] + (v1)[2];		\
}\

/// Vector sum
#define VEC_SUM_4(v21,v2,v1)			\
{						\
   (v21)[0] = (v2)[0] + (v1)[0];		\
   (v21)[1] = (v2)[1] + (v1)[1];		\
   (v21)[2] = (v2)[2] + (v1)[2];		\
   (v21)[3] = (v2)[3] + (v1)[3];		\
}\

/// scalar times vector
#define VEC_SCALE_2(c,a,b)			\
{						\
   (c)[0] = (a)*(b)[0];				\
   (c)[1] = (a)*(b)[1];				\
}\

/// scalar times vector
#define VEC_SCALE(c,a,b)			\
{						\
   (c)[0] = (a)*(b)[0];				\
   (c)[1] = (a)*(b)[1];				\
   (c)[2] = (a)*(b)[2];				\
}\

/// scalar times vector
#define VEC_SCALE_4(c,a,b)			\
{						\
   (c)[0] = (a)*(b)[0];				\
   (c)[1] = (a)*(b)[1];				\
   (c)[2] = (a)*(b)[2];				\
   (c)[3] = (a)*(b)[3];				\
}\

/// accumulate scaled vector
#define VEC_ACCUM_2(c,a,b)			\
{						\
   (c)[0] += (a)*(b)[0];			\
   (c)[1] += (a)*(b)[1];			\
}\

/// accumulate scaled vector
#define VEC_ACCUM(c,a,b)			\
{						\
   (c)[0] += (a)*(b)[0];			\
   (c)[1] += (a)*(b)[1];			\
   (c)[2] += (a)*(b)[2];			\
}\

/// accumulate scaled vector
#define VEC_ACCUM_4(c,a,b)			\
{						\
   (c)[0] += (a)*(b)[0];			\
   (c)[1] += (a)*(b)[1];			\
   (c)[2] += (a)*(b)[2];			\
   (c)[3] += (a)*(b)[3];			\
}\

/// Vector dot product
#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1])

/// Vector dot product
#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])

/// Vector dot product
#define VEC_DOT_4(a,b)	((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])

/// vector impact parameter (squared)
#define VEC_IMPACT_SQ(bsq,direction,position) {\
   GREAL _llel_ = VEC_DOT(direction, position);\
   bsq = VEC_DOT(position, position) - _llel_*_llel_;\
}\

/// vector impact parameter
#define VEC_IMPACT(bsq,direction,position)	{\
   VEC_IMPACT_SQ(bsq,direction,position);		\
   GIM_SQRT(bsq,bsq);					\
}\

/// Vector length
#define VEC_LENGTH_2(a,l)\
{\
    GREAL _pp = VEC_DOT_2(a,a);\
    GIM_SQRT(_pp,l);\
}\

/// Vector length
#define VEC_LENGTH(a,l)\
{\
    GREAL _pp = VEC_DOT(a,a);\
    GIM_SQRT(_pp,l);\
}\

/// Vector length
#define VEC_LENGTH_4(a,l)\
{\
    GREAL _pp = VEC_DOT_4(a,a);\
    GIM_SQRT(_pp,l);\
}\

/// Vector inv length
#define VEC_INV_LENGTH_2(a,l)\
{\
    GREAL _pp = VEC_DOT_2(a,a);\
    GIM_INV_SQRT(_pp,l);\
}\

/// Vector inv length
#define VEC_INV_LENGTH(a,l)\
{\
    GREAL _pp = VEC_DOT(a,a);\
    GIM_INV_SQRT(_pp,l);\
}\

/// Vector inv length
#define VEC_INV_LENGTH_4(a,l)\
{\
    GREAL _pp = VEC_DOT_4(a,a);\
    GIM_INV_SQRT(_pp,l);\
}\

/// distance between two points
#define VEC_DISTANCE(_len,_va,_vb) {\
    vec3f _tmp_;				\
    VEC_DIFF(_tmp_, _vb, _va);			\
    VEC_LENGTH(_tmp_,_len);			\
}\

/// Vector length
#define VEC_CONJUGATE_LENGTH(a,l)\
{\
    GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\
    GIM_SQRT(_pp,l);\
}\

/// Vector length
#define VEC_NORMALIZE(a) {	\
    GREAL len;\
    VEC_INV_LENGTH(a,len); \
    if(len<G_REAL_INFINITY)\
    {\
        a[0] *= len;				\
        a[1] *= len;				\
        a[2] *= len;				\
    }						\
}\

/// Set Vector size
#define VEC_RENORMALIZE(a,newlen) {	\
    GREAL len;\
    VEC_INV_LENGTH(a,len); \
    if(len<G_REAL_INFINITY)\
    {\
        len *= newlen;\
        a[0] *= len;				\
        a[1] *= len;				\
        a[2] *= len;				\
    }						\
}\

/// Vector cross
#define VEC_CROSS(c,a,b)		\
{						\
   c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1];	\
   c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2];	\
   c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0];	\
}\

/*! Vector perp -- assumes that n is of unit length
 * accepts vector v, subtracts out any component parallel to n */
#define VEC_PERPENDICULAR(vp,v,n)			\
{						\
   GREAL dot = VEC_DOT(v, n);			\
   vp[0] = (v)[0] - dot*(n)[0];		\
   vp[1] = (v)[1] - dot*(n)[1];		\
   vp[2] = (v)[2] - dot*(n)[2];		\
}\

/*! Vector parallel -- assumes that n is of unit length */
#define VEC_PARALLEL(vp,v,n)			\
{						\
   GREAL dot = VEC_DOT(v, n);			\
   vp[0] = (dot) * (n)[0];			\
   vp[1] = (dot) * (n)[1];			\
   vp[2] = (dot) * (n)[2];			\
}\

/*! Same as Vector parallel --  n can have any length
 * accepts vector v, subtracts out any component perpendicular to n */
#define VEC_PROJECT(vp,v,n)			\
{ \
	GREAL scalar = VEC_DOT(v, n);			\
	scalar/= VEC_DOT(n, n); \
	vp[0] = (scalar) * (n)[0];			\
    vp[1] = (scalar) * (n)[1];			\
    vp[2] = (scalar) * (n)[2];			\
}\

/*! accepts vector v*/
#define VEC_UNPROJECT(vp,v,n)			\
{ \
	GREAL scalar = VEC_DOT(v, n);			\
	scalar = VEC_DOT(n, n)/scalar; \
	vp[0] = (scalar) * (n)[0];			\
    vp[1] = (scalar) * (n)[1];			\
    vp[2] = (scalar) * (n)[2];			\
}\

/*! Vector reflection -- assumes n is of unit length
 Takes vector v, reflects it against reflector n, and returns vr */
#define VEC_REFLECT(vr,v,n)			\
{						\
   GREAL dot = VEC_DOT(v, n);			\
   vr[0] = (v)[0] - 2.0 * (dot) * (n)[0];	\
   vr[1] = (v)[1] - 2.0 * (dot) * (n)[1];	\
   vr[2] = (v)[2] - 2.0 * (dot) * (n)[2];	\
}\

/*! Vector blending
Takes two vectors a, b, blends them together with two scalars */
#define VEC_BLEND_AB(vr,sa,a,sb,b)			\
{						\
   vr[0] = (sa) * (a)[0] + (sb) * (b)[0];	\
   vr[1] = (sa) * (a)[1] + (sb) * (b)[1];	\
   vr[2] = (sa) * (a)[2] + (sb) * (b)[2];	\
}\

/*! Vector blending
Takes two vectors a, b, blends them together with s <=1 */
#define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b)

#define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];

//! Finds the bigger cartesian coordinate from a vector
#define VEC_MAYOR_COORD(vec, maxc)\
{\
	GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\
    maxc =  A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\
}\

//! Finds the 2 smallest cartesian coordinates from a vector
#define VEC_MINOR_AXES(vec, i0, i1)\
{\
	VEC_MAYOR_COORD(vec,i0);\
	i0 = (i0+1)%3;\
	i1 = (i0+1)%3;\
}\

#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2])

#define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2]))

/// Vector cross
#define X_AXIS_CROSS_VEC(dst,src)\
{					   \
	dst[0] = 0.0f;     \
	dst[1] = -src[2];  \
	dst[2] = src[1];  \
}\

#define Y_AXIS_CROSS_VEC(dst,src)\
{					   \
	dst[0] = src[2];     \
	dst[1] = 0.0f;  \
	dst[2] = -src[0];  \
}\

#define Z_AXIS_CROSS_VEC(dst,src)\
{					   \
	dst[0] = -src[1];     \
	dst[1] = src[0];  \
	dst[2] = 0.0f;  \
}\

/// initialize matrix
#define IDENTIFY_MATRIX_3X3(m)			\
{						\
   m[0][0] = 1.0;				\
   m[0][1] = 0.0;				\
   m[0][2] = 0.0;				\
						\
   m[1][0] = 0.0;				\
   m[1][1] = 1.0;				\
   m[1][2] = 0.0;				\
						\
   m[2][0] = 0.0;				\
   m[2][1] = 0.0;				\
   m[2][2] = 1.0;				\
}\

/*! initialize matrix */
#define IDENTIFY_MATRIX_4X4(m)			\
{						\
   m[0][0] = 1.0;				\
   m[0][1] = 0.0;				\
   m[0][2] = 0.0;				\
   m[0][3] = 0.0;				\
						\
   m[1][0] = 0.0;				\
   m[1][1] = 1.0;				\
   m[1][2] = 0.0;				\
   m[1][3] = 0.0;				\
						\
   m[2][0] = 0.0;				\
   m[2][1] = 0.0;				\
   m[2][2] = 1.0;				\
   m[2][3] = 0.0;				\
						\
   m[3][0] = 0.0;				\
   m[3][1] = 0.0;				\
   m[3][2] = 0.0;				\
   m[3][3] = 1.0;				\
}\

/*! initialize matrix */
#define ZERO_MATRIX_4X4(m)			\
{						\
   m[0][0] = 0.0;				\
   m[0][1] = 0.0;				\
   m[0][2] = 0.0;				\
   m[0][3] = 0.0;				\
						\
   m[1][0] = 0.0;				\
   m[1][1] = 0.0;				\
   m[1][2] = 0.0;				\
   m[1][3] = 0.0;				\
						\
   m[2][0] = 0.0;				\
   m[2][1] = 0.0;				\
   m[2][2] = 0.0;				\
   m[2][3] = 0.0;				\
						\
   m[3][0] = 0.0;				\
   m[3][1] = 0.0;				\
   m[3][2] = 0.0;				\
   m[3][3] = 0.0;				\
}\

/*! matrix rotation  X */
#define ROTX_CS(m,cosine,sine)		\
{					\
   /* rotation about the x-axis */	\
					\
   m[0][0] = 1.0;			\
   m[0][1] = 0.0;			\
   m[0][2] = 0.0;			\
   m[0][3] = 0.0;			\
					\
   m[1][0] = 0.0;			\
   m[1][1] = (cosine);			\
   m[1][2] = (sine);			\
   m[1][3] = 0.0;			\
					\
   m[2][0] = 0.0;			\
   m[2][1] = -(sine);			\
   m[2][2] = (cosine);			\
   m[2][3] = 0.0;			\
					\
   m[3][0] = 0.0;			\
   m[3][1] = 0.0;			\
   m[3][2] = 0.0;			\
   m[3][3] = 1.0;			\
}\

/*! matrix rotation  Y */
#define ROTY_CS(m,cosine,sine)		\
{					\
   /* rotation about the y-axis */	\
					\
   m[0][0] = (cosine);			\
   m[0][1] = 0.0;			\
   m[0][2] = -(sine);			\
   m[0][3] = 0.0;			\
					\
   m[1][0] = 0.0;			\
   m[1][1] = 1.0;			\
   m[1][2] = 0.0;			\
   m[1][3] = 0.0;			\
					\
   m[2][0] = (sine);			\
   m[2][1] = 0.0;			\
   m[2][2] = (cosine);			\
   m[2][3] = 0.0;			\
					\
   m[3][0] = 0.0;			\
   m[3][1] = 0.0;			\
   m[3][2] = 0.0;			\
   m[3][3] = 1.0;			\
}\

/*! matrix rotation  Z */
#define ROTZ_CS(m,cosine,sine)		\
{					\
   /* rotation about the z-axis */	\
					\
   m[0][0] = (cosine);			\
   m[0][1] = (sine);			\
   m[0][2] = 0.0;			\
   m[0][3] = 0.0;			\
					\
   m[1][0] = -(sine);			\
   m[1][1] = (cosine);			\
   m[1][2] = 0.0;			\
   m[1][3] = 0.0;			\
					\
   m[2][0] = 0.0;			\
   m[2][1] = 0.0;			\
   m[2][2] = 1.0;			\
   m[2][3] = 0.0;			\
					\
   m[3][0] = 0.0;			\
   m[3][1] = 0.0;			\
   m[3][2] = 0.0;			\
   m[3][3] = 1.0;			\
}\

/*! matrix copy */
#define COPY_MATRIX_2X2(b,a)	\
{				\
   b[0][0] = a[0][0];		\
   b[0][1] = a[0][1];		\
				\
   b[1][0] = a[1][0];		\
   b[1][1] = a[1][1];		\
				\
}\

/*! matrix copy */
#define COPY_MATRIX_2X3(b,a)	\
{				\
   b[0][0] = a[0][0];		\
   b[0][1] = a[0][1];		\
   b[0][2] = a[0][2];		\
				\
   b[1][0] = a[1][0];		\
   b[1][1] = a[1][1];		\
   b[1][2] = a[1][2];		\
}\

/*! matrix copy */
#define COPY_MATRIX_3X3(b,a)	\
{				\
   b[0][0] = a[0][0];		\
   b[0][1] = a[0][1];		\
   b[0][2] = a[0][2];		\
				\
   b[1][0] = a[1][0];		\
   b[1][1] = a[1][1];		\
   b[1][2] = a[1][2];		\
				\
   b[2][0] = a[2][0];		\
   b[2][1] = a[2][1];		\
   b[2][2] = a[2][2];		\
}\

/*! matrix copy */
#define COPY_MATRIX_4X4(b,a)	\
{				\
   b[0][0] = a[0][0];		\
   b[0][1] = a[0][1];		\
   b[0][2] = a[0][2];		\
   b[0][3] = a[0][3];		\
				\
   b[1][0] = a[1][0];		\
   b[1][1] = a[1][1];		\
   b[1][2] = a[1][2];		\
   b[1][3] = a[1][3];		\
				\
   b[2][0] = a[2][0];		\
   b[2][1] = a[2][1];		\
   b[2][2] = a[2][2];		\
   b[2][3] = a[2][3];		\
				\
   b[3][0] = a[3][0];		\
   b[3][1] = a[3][1];		\
   b[3][2] = a[3][2];		\
   b[3][3] = a[3][3];		\
}\

/*! matrix transpose */
#define TRANSPOSE_MATRIX_2X2(b,a)	\
{				\
   b[0][0] = a[0][0];		\
   b[0][1] = a[1][0];		\
				\
   b[1][0] = a[0][1];		\
   b[1][1] = a[1][1];		\
}\

/*! matrix transpose */
#define TRANSPOSE_MATRIX_3X3(b,a)	\
{				\
   b[0][0] = a[0][0];		\
   b[0][1] = a[1][0];		\
   b[0][2] = a[2][0];		\
				\
   b[1][0] = a[0][1];		\
   b[1][1] = a[1][1];		\
   b[1][2] = a[2][1];		\
				\
   b[2][0] = a[0][2];		\
   b[2][1] = a[1][2];		\
   b[2][2] = a[2][2];		\
}\

/*! matrix transpose */
#define TRANSPOSE_MATRIX_4X4(b,a)	\
{				\
   b[0][0] = a[0][0];		\
   b[0][1] = a[1][0];		\
   b[0][2] = a[2][0];		\
   b[0][3] = a[3][0];		\
				\
   b[1][0] = a[0][1];		\
   b[1][1] = a[1][1];		\
   b[1][2] = a[2][1];		\
   b[1][3] = a[3][1];		\
				\
   b[2][0] = a[0][2];		\
   b[2][1] = a[1][2];		\
   b[2][2] = a[2][2];		\
   b[2][3] = a[3][2];		\
				\
   b[3][0] = a[0][3];		\
   b[3][1] = a[1][3];		\
   b[3][2] = a[2][3];		\
   b[3][3] = a[3][3];		\
}\

/*! multiply matrix by scalar */
#define SCALE_MATRIX_2X2(b,s,a)		\
{					\
   b[0][0] = (s) * a[0][0];		\
   b[0][1] = (s) * a[0][1];		\
					\
   b[1][0] = (s) * a[1][0];		\
   b[1][1] = (s) * a[1][1];		\
}\

/*! multiply matrix by scalar */
#define SCALE_MATRIX_3X3(b,s,a)		\
{					\
   b[0][0] = (s) * a[0][0];		\
   b[0][1] = (s) * a[0][1];		\
   b[0][2] = (s) * a[0][2];		\
					\
   b[1][0] = (s) * a[1][0];		\
   b[1][1] = (s) * a[1][1];		\
   b[1][2] = (s) * a[1][2];		\
					\
   b[2][0] = (s) * a[2][0];		\
   b[2][1] = (s) * a[2][1];		\
   b[2][2] = (s) * a[2][2];		\
}\

/*! multiply matrix by scalar */
#define SCALE_MATRIX_4X4(b,s,a)		\
{					\
   b[0][0] = (s) * a[0][0];		\
   b[0][1] = (s) * a[0][1];		\
   b[0][2] = (s) * a[0][2];		\
   b[0][3] = (s) * a[0][3];		\
					\
   b[1][0] = (s) * a[1][0];		\
   b[1][1] = (s) * a[1][1];		\
   b[1][2] = (s) * a[1][2];		\
   b[1][3] = (s) * a[1][3];		\
					\
   b[2][0] = (s) * a[2][0];		\
   b[2][1] = (s) * a[2][1];		\
   b[2][2] = (s) * a[2][2];		\
   b[2][3] = (s) * a[2][3];		\
					\
   b[3][0] = s * a[3][0];		\
   b[3][1] = s * a[3][1];		\
   b[3][2] = s * a[3][2];		\
   b[3][3] = s * a[3][3];		\
}\

/*! multiply matrix by scalar */
#define SCALE_VEC_MATRIX_2X2(b,svec,a)		\
{					\
   b[0][0] = svec[0] * a[0][0];		\
   b[1][0] = svec[0] * a[1][0];		\
					\
   b[0][1] = svec[1] * a[0][1];		\
   b[1][1] = svec[1] * a[1][1];		\
}\

/*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */
#define SCALE_VEC_MATRIX_3X3(b,svec,a)		\
{					\
   b[0][0] = svec[0] * a[0][0];		\
   b[1][0] = svec[0] * a[1][0];		\
   b[2][0] = svec[0] * a[2][0];		\
					\
   b[0][1] = svec[1] * a[0][1];		\
   b[1][1] = svec[1] * a[1][1];		\
   b[2][1] = svec[1] * a[2][1];		\
					\
   b[0][2] = svec[2] * a[0][2];		\
   b[1][2] = svec[2] * a[1][2];		\
   b[2][2] = svec[2] * a[2][2];		\
}\

/*! multiply matrix by scalar */
#define SCALE_VEC_MATRIX_4X4(b,svec,a)		\
{					\
   b[0][0] = svec[0] * a[0][0];		\
   b[1][0] = svec[0] * a[1][0];		\
   b[2][0] = svec[0] * a[2][0];		\
   b[3][0] = svec[0] * a[3][0];		\
					\
   b[0][1] = svec[1] * a[0][1];		\
   b[1][1] = svec[1] * a[1][1];		\
   b[2][1] = svec[1] * a[2][1];		\
   b[3][1] = svec[1] * a[3][1];		\
					\
   b[0][2] = svec[2] * a[0][2];		\
   b[1][2] = svec[2] * a[1][2];		\
   b[2][2] = svec[2] * a[2][2];		\
   b[3][2] = svec[2] * a[3][2];		\
   \
   b[0][3] = svec[3] * a[0][3];		\
   b[1][3] = svec[3] * a[1][3];		\
   b[2][3] = svec[3] * a[2][3];		\
   b[3][3] = svec[3] * a[3][3];		\
}\

/*! multiply matrix by scalar */
#define ACCUM_SCALE_MATRIX_2X2(b,s,a)		\
{					\
   b[0][0] += (s) * a[0][0];		\
   b[0][1] += (s) * a[0][1];		\
					\
   b[1][0] += (s) * a[1][0];		\
   b[1][1] += (s) * a[1][1];		\
}\

/*! multiply matrix by scalar */
#define ACCUM_SCALE_MATRIX_3X3(b,s,a)		\
{					\
   b[0][0] += (s) * a[0][0];		\
   b[0][1] += (s) * a[0][1];		\
   b[0][2] += (s) * a[0][2];		\
					\
   b[1][0] += (s) * a[1][0];		\
   b[1][1] += (s) * a[1][1];		\
   b[1][2] += (s) * a[1][2];		\
					\
   b[2][0] += (s) * a[2][0];		\
   b[2][1] += (s) * a[2][1];		\
   b[2][2] += (s) * a[2][2];		\
}\

/*! multiply matrix by scalar */
#define ACCUM_SCALE_MATRIX_4X4(b,s,a)		\
{					\
   b[0][0] += (s) * a[0][0];		\
   b[0][1] += (s) * a[0][1];		\
   b[0][2] += (s) * a[0][2];		\
   b[0][3] += (s) * a[0][3];		\
					\
   b[1][0] += (s) * a[1][0];		\
   b[1][1] += (s) * a[1][1];		\
   b[1][2] += (s) * a[1][2];		\
   b[1][3] += (s) * a[1][3];		\
					\
   b[2][0] += (s) * a[2][0];		\
   b[2][1] += (s) * a[2][1];		\
   b[2][2] += (s) * a[2][2];		\
   b[2][3] += (s) * a[2][3];		\
					\
   b[3][0] += (s) * a[3][0];		\
   b[3][1] += (s) * a[3][1];		\
   b[3][2] += (s) * a[3][2];		\
   b[3][3] += (s) * a[3][3];		\
}\

/*! matrix product */
/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
#define MATRIX_PRODUCT_2X2(c,a,b)		\
{						\
   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0];	\
   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1];	\
						\
   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0];	\
   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1];	\
						\
}\

/*! matrix product */
/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
#define MATRIX_PRODUCT_3X3(c,a,b)				\
{								\
   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];	\
   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];	\
   c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];	\
								\
   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];	\
   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];	\
   c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];	\
								\
   c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];	\
   c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];	\
   c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];	\
}\

/*! matrix product */
/*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/
#define MATRIX_PRODUCT_4X4(c,a,b)		\
{						\
   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\
   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\
   c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\
   c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\
						\
   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\
   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\
   c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\
   c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\
						\
   c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\
   c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\
   c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\
   c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\
						\
   c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\
   c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\
   c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\
   c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\
}\

/*! matrix times vector */
#define MAT_DOT_VEC_2X2(p,m,v)					\
{								\
   p[0] = m[0][0]*v[0] + m[0][1]*v[1];				\
   p[1] = m[1][0]*v[0] + m[1][1]*v[1];				\
}\

/*! matrix times vector */
#define MAT_DOT_VEC_3X3(p,m,v)					\
{								\
   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];		\
   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];		\
   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];		\
}\

/*! matrix times vector
v is a vec4f
*/
#define MAT_DOT_VEC_4X4(p,m,v)					\
{								\
   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];	\
   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];	\
   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];	\
   p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];	\
}\

/*! matrix times vector
v is a vec3f
and m is a mat4f<br>
Last column is added as the position
*/
#define MAT_DOT_VEC_3X4(p,m,v)					\
{								\
   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3];	\
   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3];	\
   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3];	\
}\

/*! vector transpose times matrix */
/*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */
#define VEC_DOT_MAT_3X3(p,v,m)					\
{								\
   p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0];		\
   p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1];		\
   p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2];		\
}\

/*! affine matrix times vector */
/** The matrix is assumed to be an affine matrix, with last two
 * entries representing a translation */
#define MAT_DOT_VEC_2X3(p,m,v)					\
{								\
   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2];		\
   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2];		\
}\

//! Transform a plane
#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\
{								\
   pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1]  + m[0][2]*plane[2];\
   pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1]  + m[1][2]*plane[2];\
   pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1]  + m[2][2]*plane[2];\
   pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1]  + m[2][3]*pout[2] + plane[3];\
}\

/** inverse transpose of matrix times vector
 *
 * This macro computes inverse transpose of matrix m,
 * and multiplies vector v into it, to yeild vector p
 *
 * DANGER !!! Do Not use this on normal vectors!!!
 * It will leave normals the wrong length !!!
 * See macro below for use on normals.
 */
#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v)			\
{								\
   GREAL det;						\
								\
   det = m[0][0]*m[1][1] - m[0][1]*m[1][0];			\
   p[0] = m[1][1]*v[0] - m[1][0]*v[1];				\
   p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
								\
   /* if matrix not singular, and not orthonormal, then renormalize */ \
   if ((det!=1.0f) && (det != 0.0f)) {				\
      det = 1.0f / det;						\
      p[0] *= det;						\
      p[1] *= det;						\
   }								\
}\

/** transform normal vector by inverse transpose of matrix
 * and then renormalize the vector
 *
 * This macro computes inverse transpose of matrix m,
 * and multiplies vector v into it, to yeild vector p
 * Vector p is then normalized.
 */
#define NORM_XFORM_2X2(p,m,v)					\
{								\
   GREAL len;							\
								\
   /* do nothing if off-diagonals are zero and diagonals are 	\
    * equal */							\
   if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \
      p[0] = m[1][1]*v[0] - m[1][0]*v[1];			\
      p[1] = - m[0][1]*v[0] + m[0][0]*v[1];			\
								\
      len = p[0]*p[0] + p[1]*p[1];				\
      GIM_INV_SQRT(len,len);					\
      p[0] *= len;						\
      p[1] *= len;						\
   } else {							\
      VEC_COPY_2 (p, v);					\
   }								\
}\

/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define OUTER_PRODUCT_2X2(m,v,t)				\
{								\
   m[0][0] = v[0] * t[0];					\
   m[0][1] = v[0] * t[1];					\
								\
   m[1][0] = v[1] * t[0];					\
   m[1][1] = v[1] * t[1];					\
}\

/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define OUTER_PRODUCT_3X3(m,v,t)				\
{								\
   m[0][0] = v[0] * t[0];					\
   m[0][1] = v[0] * t[1];					\
   m[0][2] = v[0] * t[2];					\
								\
   m[1][0] = v[1] * t[0];					\
   m[1][1] = v[1] * t[1];					\
   m[1][2] = v[1] * t[2];					\
								\
   m[2][0] = v[2] * t[0];					\
   m[2][1] = v[2] * t[1];					\
   m[2][2] = v[2] * t[2];					\
}\

/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define OUTER_PRODUCT_4X4(m,v,t)				\
{								\
   m[0][0] = v[0] * t[0];					\
   m[0][1] = v[0] * t[1];					\
   m[0][2] = v[0] * t[2];					\
   m[0][3] = v[0] * t[3];					\
								\
   m[1][0] = v[1] * t[0];					\
   m[1][1] = v[1] * t[1];					\
   m[1][2] = v[1] * t[2];					\
   m[1][3] = v[1] * t[3];					\
								\
   m[2][0] = v[2] * t[0];					\
   m[2][1] = v[2] * t[1];					\
   m[2][2] = v[2] * t[2];					\
   m[2][3] = v[2] * t[3];					\
								\
   m[3][0] = v[3] * t[0];					\
   m[3][1] = v[3] * t[1];					\
   m[3][2] = v[3] * t[2];					\
   m[3][3] = v[3] * t[3];					\
}\

/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define ACCUM_OUTER_PRODUCT_2X2(m,v,t)				\
{								\
   m[0][0] += v[0] * t[0];					\
   m[0][1] += v[0] * t[1];					\
								\
   m[1][0] += v[1] * t[0];					\
   m[1][1] += v[1] * t[1];					\
}\

/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define ACCUM_OUTER_PRODUCT_3X3(m,v,t)				\
{								\
   m[0][0] += v[0] * t[0];					\
   m[0][1] += v[0] * t[1];					\
   m[0][2] += v[0] * t[2];					\
								\
   m[1][0] += v[1] * t[0];					\
   m[1][1] += v[1] * t[1];					\
   m[1][2] += v[1] * t[2];					\
								\
   m[2][0] += v[2] * t[0];					\
   m[2][1] += v[2] * t[1];					\
   m[2][2] += v[2] * t[2];					\
}\

/** outer product of vector times vector transpose
 *
 * The outer product of vector v and vector transpose t yeilds
 * dyadic matrix m.
 */
#define ACCUM_OUTER_PRODUCT_4X4(m,v,t)				\
{								\
   m[0][0] += v[0] * t[0];					\
   m[0][1] += v[0] * t[1];					\
   m[0][2] += v[0] * t[2];					\
   m[0][3] += v[0] * t[3];					\
								\
   m[1][0] += v[1] * t[0];					\
   m[1][1] += v[1] * t[1];					\
   m[1][2] += v[1] * t[2];					\
   m[1][3] += v[1] * t[3];					\
								\
   m[2][0] += v[2] * t[0];					\
   m[2][1] += v[2] * t[1];					\
   m[2][2] += v[2] * t[2];					\
   m[2][3] += v[2] * t[3];					\
								\
   m[3][0] += v[3] * t[0];					\
   m[3][1] += v[3] * t[1];					\
   m[3][2] += v[3] * t[2];					\
   m[3][3] += v[3] * t[3];					\
}\

/** determinant of matrix
 *
 * Computes determinant of matrix m, returning d
 */
#define DETERMINANT_2X2(d,m)					\
{								\
   d = m[0][0] * m[1][1] - m[0][1] * m[1][0];			\
}\

/** determinant of matrix
 *
 * Computes determinant of matrix m, returning d
 */
#define DETERMINANT_3X3(d,m)					\
{								\
   d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);		\
   d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);	\
   d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);	\
}\

/** i,j,th cofactor of a 4x4 matrix
 *
 */
#define COFACTOR_4X4_IJ(fac,m,i,j) 				\
{								\
   GUINT __ii[4], __jj[4], __k;						\
								\
   for (__k=0; __k<i; __k++) __ii[__k] = __k;				\
   for (__k=i; __k<3; __k++) __ii[__k] = __k+1;				\
   for (__k=0; __k<j; __k++) __jj[__k] = __k;				\
   for (__k=j; __k<3; __k++) __jj[__k] = __k+1;				\
								\
   (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] 	\
                            - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \
   (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]]	\
                             - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\
   (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]]	\
                             - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\
								\
   __k = i+j;							\
   if ( __k != (__k/2)*2) {						\
      (fac) = -(fac);						\
   }								\
}\

/** determinant of matrix
 *
 * Computes determinant of matrix m, returning d
 */
#define DETERMINANT_4X4(d,m)					\
{								\
   GREAL cofac;						\
   COFACTOR_4X4_IJ (cofac, m, 0, 0);				\
   d = m[0][0] * cofac;						\
   COFACTOR_4X4_IJ (cofac, m, 0, 1);				\
   d += m[0][1] * cofac;					\
   COFACTOR_4X4_IJ (cofac, m, 0, 2);				\
   d += m[0][2] * cofac;					\
   COFACTOR_4X4_IJ (cofac, m, 0, 3);				\
   d += m[0][3] * cofac;					\
}\

/** cofactor of matrix
 *
 * Computes cofactor of matrix m, returning a
 */
#define COFACTOR_2X2(a,m)					\
{								\
   a[0][0] = (m)[1][1];						\
   a[0][1] = - (m)[1][0];						\
   a[1][0] = - (m)[0][1];						\
   a[1][1] = (m)[0][0];						\
}\

/** cofactor of matrix
 *
 * Computes cofactor of matrix m, returning a
 */
#define COFACTOR_3X3(a,m)					\
{								\
   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
   a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
   a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
   a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
   a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
   a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
   a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
}\

/** cofactor of matrix
 *
 * Computes cofactor of matrix m, returning a
 */
#define COFACTOR_4X4(a,m)					\
{								\
   int i,j;							\
								\
   for (i=0; i<4; i++) {					\
      for (j=0; j<4; j++) {					\
         COFACTOR_4X4_IJ (a[i][j], m, i, j);			\
      }								\
   }								\
}\

/** adjoint of matrix
 *
 * Computes adjoint of matrix m, returning a
 * (Note that adjoint is just the transpose of the cofactor matrix)
 */
#define ADJOINT_2X2(a,m)					\
{								\
   a[0][0] = (m)[1][1];						\
   a[1][0] = - (m)[1][0];						\
   a[0][1] = - (m)[0][1];						\
   a[1][1] = (m)[0][0];						\
}\

/** adjoint of matrix
 *
 * Computes adjoint of matrix m, returning a
 * (Note that adjoint is just the transpose of the cofactor matrix)
 */
#define ADJOINT_3X3(a,m)					\
{								\
   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];			\
   a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);		\
   a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];			\
   a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);		\
   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];			\
   a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);		\
   a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];			\
   a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);		\
   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]);		\
}\

/** adjoint of matrix
 *
 * Computes adjoint of matrix m, returning a
 * (Note that adjoint is just the transpose of the cofactor matrix)
 */
#define ADJOINT_4X4(a,m)					\
{								\
   char _i_,_j_;							\
								\
   for (_i_=0; _i_<4; _i_++) {					\
      for (_j_=0; _j_<4; _j_++) {					\
         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
      }								\
   }								\
}\

/** compute adjoint of matrix and scale
 *
 * Computes adjoint of matrix m, scales it by s, returning a
 */
#define SCALE_ADJOINT_2X2(a,s,m)				\
{								\
   a[0][0] = (s) * m[1][1];					\
   a[1][0] = - (s) * m[1][0];					\
   a[0][1] = - (s) * m[0][1];					\
   a[1][1] = (s) * m[0][0];					\
}\

/** compute adjoint of matrix and scale
 *
 * Computes adjoint of matrix m, scales it by s, returning a
 */
#define SCALE_ADJOINT_3X3(a,s,m)				\
{								\
   a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);	\
   a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);	\
   a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);	\
								\
   a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);	\
   a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);	\
   a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);	\
								\
   a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);	\
   a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);	\
   a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);	\
}\

/** compute adjoint of matrix and scale
 *
 * Computes adjoint of matrix m, scales it by s, returning a
 */
#define SCALE_ADJOINT_4X4(a,s,m)				\
{								\
   char _i_,_j_; \
   for (_i_=0; _i_<4; _i_++) {					\
      for (_j_=0; _j_<4; _j_++) {					\
         COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_);			\
         a[_j_][_i_] *= s;						\
      }								\
   }								\
}\

/** inverse of matrix
 *
 * Compute inverse of matrix a, returning determinant m and
 * inverse b
 */
#define INVERT_2X2(b,det,a)			\
{						\
   GREAL _tmp_;					\
   DETERMINANT_2X2 (det, a);			\
   _tmp_ = 1.0 / (det);				\
   SCALE_ADJOINT_2X2 (b, _tmp_, a);		\
}\

/** inverse of matrix
 *
 * Compute inverse of matrix a, returning determinant m and
 * inverse b
 */
#define INVERT_3X3(b,det,a)			\
{						\
   GREAL _tmp_;					\
   DETERMINANT_3X3 (det, a);			\
   _tmp_ = 1.0 / (det);				\
   SCALE_ADJOINT_3X3 (b, _tmp_, a);		\
}\

/** inverse of matrix
 *
 * Compute inverse of matrix a, returning determinant m and
 * inverse b
 */
#define INVERT_4X4(b,det,a)			\
{						\
   GREAL _tmp_;					\
   DETERMINANT_4X4 (det, a);			\
   _tmp_ = 1.0 / (det);				\
   SCALE_ADJOINT_4X4 (b, _tmp_, a);		\
}\

//! Get the triple(3) row of a transform matrix
#define MAT_GET_ROW(mat,vec3,rowindex)\
{\
    vec3[0] = mat[rowindex][0];\
    vec3[1] = mat[rowindex][1];\
    vec3[2] = mat[rowindex][2]; \
}\

//! Get the tuple(2) row of a transform matrix
#define MAT_GET_ROW2(mat,vec2,rowindex)\
{\
    vec2[0] = mat[rowindex][0];\
    vec2[1] = mat[rowindex][1];\
}\

//! Get the quad (4) row of a transform matrix
#define MAT_GET_ROW4(mat,vec4,rowindex)\
{\
    vec4[0] = mat[rowindex][0];\
    vec4[1] = mat[rowindex][1];\
    vec4[2] = mat[rowindex][2];\
    vec4[3] = mat[rowindex][3];\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_GET_COL(mat,vec3,colindex)\
{\
    vec3[0] = mat[0][colindex];\
    vec3[1] = mat[1][colindex];\
    vec3[2] = mat[2][colindex]; \
}\

//! Get the tuple(2) col of a transform matrix
#define MAT_GET_COL2(mat,vec2,colindex)\
{\
    vec2[0] = mat[0][colindex];\
    vec2[1] = mat[1][colindex];\
}\

//! Get the quad (4) col of a transform matrix
#define MAT_GET_COL4(mat,vec4,colindex)\
{\
    vec4[0] = mat[0][colindex];\
    vec4[1] = mat[1][colindex];\
    vec4[2] = mat[2][colindex];\
    vec4[3] = mat[3][colindex];\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_GET_X(mat,vec3)\
{\
    MAT_GET_COL(mat,vec3,0);\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_GET_Y(mat,vec3)\
{\
    MAT_GET_COL(mat,vec3,1);\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_GET_Z(mat,vec3)\
{\
    MAT_GET_COL(mat,vec3,2);\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_SET_X(mat,vec3)\
{\
    mat[0][0] = vec3[0];\
    mat[1][0] = vec3[1];\
    mat[2][0] = vec3[2];\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_SET_Y(mat,vec3)\
{\
    mat[0][1] = vec3[0];\
    mat[1][1] = vec3[1];\
    mat[2][1] = vec3[2];\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_SET_Z(mat,vec3)\
{\
    mat[0][2] = vec3[0];\
    mat[1][2] = vec3[1];\
    mat[2][2] = vec3[2];\
}\

//! Get the triple(3) col of a transform matrix
#define MAT_GET_TRANSLATION(mat,vec3)\
{\
    vec3[0] = mat[0][3];\
    vec3[1] = mat[1][3];\
    vec3[2] = mat[2][3]; \
}\

//! Set the triple(3) col of a transform matrix
#define MAT_SET_TRANSLATION(mat,vec3)\
{\
    mat[0][3] = vec3[0];\
    mat[1][3] = vec3[1];\
    mat[2][3] = vec3[2]; \
}\

//! Returns the dot product between a vec3f and the row of a matrix
#define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2])

//! Returns the dot product between a vec2f and the row of a matrix
#define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1])

//! Returns the dot product between a vec4f and the row of a matrix
#define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3])

//! Returns the dot product between a vec3f and the col of a matrix
#define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex])

//! Returns the dot product between a vec2f and the col of a matrix
#define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex])

//! Returns the dot product between a vec4f and the col of a matrix
#define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex])

/*!Transpose matrix times vector
v is a vec3f
and m is a mat4f<br>
*/
#define INV_MAT_DOT_VEC_3X3(p,m,v)					\
{								\
   p[0] = MAT_DOT_COL(m,v,0); \
   p[1] = MAT_DOT_COL(m,v,1);	\
   p[2] = MAT_DOT_COL(m,v,2);	\
}\

#endif // GIM_VECTOR_H_INCLUDED


text:
/*
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_SIMPLEX_SOLVER_INTERFACE_H
#define BT_SIMPLEX_SOLVER_INTERFACE_H

#include "LinearMath/btVector3.h"

#define NO_VIRTUAL_INTERFACE 1
#ifdef NO_VIRTUAL_INTERFACE
#include "btVoronoiSimplexSolver.h"
#define btSimplexSolverInterface btVoronoiSimplexSolver
#else

/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
/// voronoi regions or barycentric coordinates
class btSimplexSolverInterface
{
	public:
		virtual ~btSimplexSolverInterface() {};

	virtual void reset() = 0;

	virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0;

	virtual bool closest(btVector3& v) = 0;

	virtual btScalar maxVertex() = 0;

	virtual bool fullSimplex() const = 0;

	virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0;

	virtual bool inSimplex(const btVector3& w) = 0;

	virtual void backup_closest(btVector3& v) = 0;

	virtual bool emptySimplex() const = 0;

	virtual void compute_points(btVector3& p1, btVector3& p2) = 0;

	virtual int numVertices() const =0;

};
#endif
#endif //BT_SIMPLEX_SOLVER_INTERFACE_H


text:
/*
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_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
#define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H

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

/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
/// This interface allows to query for closest points and penetration depth between two (convex) objects
/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
struct btDiscreteCollisionDetectorInterface
{

	struct Result
	{

		virtual ~Result(){}

		///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner
		virtual void setShapeIdentifiersA(int partId0,int index0)=0;
		virtual void setShapeIdentifiersB(int partId1,int index1)=0;
		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0;
	};

	struct ClosestPointInput
	{
		ClosestPointInput()
			:m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT))
		{
		}

		btTransform m_transformA;
		btTransform m_transformB;
		btScalar	m_maximumDistanceSquared;
	};

	virtual ~btDiscreteCollisionDetectorInterface() {};

	//
	// give either closest points (distance > 0) or penetration (distance)
	// the normal always points from B towards A
	//
	virtual void	getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0;

};

struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
{
		btVector3	m_normalOnSurfaceB;
		btVector3	m_closestPointInB;
		btScalar	m_distance; //negative means penetration !

    protected:
		btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
		{
		}

	public:
		virtual ~btStorageResult() {};

		virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
		{
			if (depth < m_distance)
			{
				m_normalOnSurfaceB = normalOnBInWorld;
				m_closestPointInB = pointInWorld;
				m_distance = depth;
			}
		}
};

#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H


text:
/*
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_SUBSIMPLEX_CONVEX_CAST_H
#define BT_SUBSIMPLEX_CONVEX_CAST_H

#include "btConvexCast.h"
#include "btSimplexSolverInterface.h"
class btConvexShape;

/// btSubsimplexConvexCast implements Gino van den Bergens' paper
///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
/// GJK based Ray Cast, optimized version
/// Objects should not start in overlap, otherwise results are not defined.
class btSubsimplexConvexCast : public btConvexCast
{
	btSimplexSolverInterface* m_simplexSolver;
	const btConvexShape*	m_convexA;
	const btConvexShape*	m_convexB;

public:

	btSubsimplexConvexCast (const btConvexShape*	shapeA,const btConvexShape*	shapeB,btSimplexSolverInterface* simplexSolver);

	//virtual ~btSubsimplexConvexCast();
	///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
	///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
	virtual bool	calcTimeOfImpact(
			const btTransform& fromA,
			const btTransform& toA,
			const btTransform& fromB,
			const btTransform& toB,
			CastResult& result);

};

#endif //BT_SUBSIMPLEX_CONVEX_CAST_H


text:
/*
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_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
#define BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H

#include "btConvexPenetrationDepthSolver.h"

///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{
protected:

	static btVector3*	getPenetrationDirections();

public:

	virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
	const btConvexShape* convexA,const btConvexShape* convexB,
				const btTransform& transA,const btTransform& transB,
			btVector3& v, btVector3& pa, btVector3& pb,
			class btIDebugDraw* debugDraw
			);
};

#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H