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_COLLISION_OBJECT_H
#define BT_COLLISION_OBJECT_H

#include "LinearMath/btTransform.h"

// island management, m_activationState1
#define ACTIVE_TAG 1
#define ISLAND_SLEEPING 2
#define WANTS_DEACTIVATION 3
#define DISABLE_DEACTIVATION 4
#define DISABLE_SIMULATION 5

struct btBroadphaseProxy;
class btCollisionShape;
struct btCollisionShapeData;
#include "LinearMath/btMotionState.h"
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btAlignedObjectArray.h"

typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;

#ifdef BT_USE_DOUBLE_PRECISION
#	define btCollisionObjectData btCollisionObjectDoubleData
#	define btCollisionObjectDataName "btCollisionObjectDoubleData"
#else
#	define btCollisionObjectData btCollisionObjectFloatData
#	define btCollisionObjectDataName "btCollisionObjectFloatData"
#endif

/// btCollisionObject can be used to manage collision detection objects.
/// btCollisionObject maintains all information that is needed for a collision
/// detection: Shape, Transform and AABB proxy. They can be added to the
/// btCollisionWorld.
ATTRIBUTE_ALIGNED16(class) btCollisionObject
{
protected:
	btTransform m_worldTransform;

	/// m_interpolationWorldTransform is used for CCD and interpolation
	/// it can be either previous or future (predicted) transform
	btTransform m_interpolationWorldTransform;

	// those two are experimental: just added for bullet time effect, so you can
	// still apply impulses (directly modifying velocities) without destroying the
	// continuous interpolated motion (which uses this interpolation velocities)
	btVector3 m_interpolationLinearVelocity;
	btVector3 m_interpolationAngularVelocity;

	btVector3 m_anisotropicFriction;
	int       m_hasAnisotropicFriction;
	btScalar  m_contactProcessingThreshold;

	btBroadphaseProxy* m_broadphaseHandle;
	btCollisionShape*  m_collisionShape;

	/// m_extensionPointer is used by some internal low-level Bullet extensions.
	void* m_extensionPointer;

	/// m_rootCollisionShape is temporarily used to store the original collision
	/// shape The m_collisionShape might be temporarily replaced by a child
	/// collision shape during collision detection purposes If it is NULL, the
	/// m_collisionShape is not temporarily replaced.
	btCollisionShape* m_rootCollisionShape;

	int m_collisionFlags;
	int m_islandTag1;
	int m_companionId;

	/// index of object in world's collisionObjects array
	int m_worldArrayIndex;

	mutable int      m_activationState1;
	mutable btScalar m_deactivationTime;

	btScalar m_friction;
	btScalar m_restitution;

	/// torsional friction orthogonal to contact normal (useful to stop spheres
	/// rolling forever)
	btScalar m_rollingFriction;

	/// torsional friction around the contact normal (useful for grasping)
	btScalar m_spinningFriction;
	btScalar m_contactDamping;
	btScalar m_contactStiffness;

	/// m_internalType is reserved to distinguish Bullet's btCollisionObject,
	/// btRigidBody, btSoftBody, btGhostObject etc. do not assign your own
	/// m_internalType unless you write a new dynamics object class.
	int m_internalType;

	/// users can point to their objects, m_userPointer is not used by Bullet,
	/// see setUserPointer/getUserPointer
	void* m_userObjectPointer;
	int m_userIndex2;
	int m_userIndex;

	/// time of impact calculation
	btScalar m_hitFraction;

	/// Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
	btScalar m_ccdSweptSphereRadius;

	/// Don't do continuous collision detection if the motion (in one step) is
	/// less then m_ccdMotionThreshold
	btScalar m_ccdMotionThreshold;

	/// If some object should have elaborate collision filtering by sub-classes
	int m_checkCollideWith;

	btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck;

	/// internal update revision number. It will be increased when the object
	/// changes. This allows some subsystems to perform lazy evaluation.
	int m_updateRevision;

	btVector3 m_customDebugColorRGB;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	enum CollisionFlags
	{
		CF_STATIC_OBJECT                    = 1,
		CF_KINEMATIC_OBJECT                 = 2,
		CF_NO_CONTACT_RESPONSE              = 4,
		CF_CUSTOM_MATERIAL_CALLBACK         = 8,  // this allows per-triangle material (friction/restitution)
		CF_CHARACTER_OBJECT                 = 16,
		CF_DISABLE_VISUALIZE_OBJECT         = 32, // disable debug drawing
		CF_DISABLE_SPU_COLLISION_PROCESSING = 64, // disable parallel/SPU processing
		CF_HAS_CONTACT_STIFFNESS_DAMPING    = 128,
		CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
		CF_HAS_FRICTION_ANCHOR              = 512,
		CF_HAS_COLLISION_SOUND_TRIGGER      = 1024
	};

	enum CollisionObjectTypes
	{
		CO_COLLISION_OBJECT = 1,
		CO_RIGID_BODY       = 2,
		/// CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and
		/// that pass its collision filter It is useful for collision sensors,
		/// explosion objects, character controller etc.
		CO_GHOST_OBJECT      = 4,
		CO_SOFT_BODY         = 8,
		CO_HF_FLUID          = 16,
		CO_USER_TYPE         = 32,
		CO_FEATHERSTONE_LINK = 64
	};

	enum AnisotropicFrictionFlags
	{
		CF_ANISOTROPIC_FRICTION_DISABLED = 0,
		CF_ANISOTROPIC_FRICTION          = 1,
		CF_ANISOTROPIC_ROLLING_FRICTION  = 2
	};

	/// static objects, kinematic and object without contact response don't merge islands
	SIMD_FORCE_INLINE bool mergesSimulationIslands() const { return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE)) == 0); }
	const btVector3& getAnisotropicFriction() const { return m_anisotropicFriction; }

	void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION)
	{
		m_anisotropicFriction = anisotropicFriction;
		bool isUnity = (anisotropicFriction[0] != 1.f) || (anisotropicFriction[1] != 1.f) || (anisotropicFriction[2] != 1.f);
		m_hasAnisotropicFriction = isUnity ? frictionMode : 0;
	}
	bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const { return (m_hasAnisotropicFriction & frictionMode) != 0; }

	/// the constraint solver can discard solving contacts, if the distance is
	/// above this threshold. 0 by default. Note that using contacts with positive
	/// distance can improve stability. It increases, however, the chance of colliding
	/// with degerate contacts, such as 'interior' triangle edges
	void setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; }
	btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; }
	SIMD_FORCE_INLINE bool isStaticObject() const { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; }
	SIMD_FORCE_INLINE bool isKinematicObject() const { return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; }
	SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const { return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0; }
	SIMD_FORCE_INLINE bool hasContactResponse() const { return (m_collisionFlags & CF_NO_CONTACT_RESPONSE) == 0; }

	btCollisionObject();
	virtual ~btCollisionObject();

	virtual void setCollisionShape(btCollisionShape * collisionShape)
	{
		m_updateRevision++;
		m_collisionShape     = collisionShape;
		m_rootCollisionShape = collisionShape;
	}

	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; }
	SIMD_FORCE_INLINE btCollisionShape*       getCollisionShape() { return m_collisionShape; }

	void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck)
	{
		if (ignoreCollisionCheck)
		{
			// We don't check for duplicates. Is it ok to leave that up to the
			// user of this API? int index =
			// m_objectsWithoutCollisionCheck.findLinearSearch(co); if (index ==
			// m_objectsWithoutCollisionCheck.size())
			//{
				m_objectsWithoutCollisionCheck.push_back(co);
			//}
		}
		else
		{
			m_objectsWithoutCollisionCheck.remove(co);
		}
		m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0;
	}

	virtual bool checkCollideWithOverride(const btCollisionObject* co) const
	{
		int index = m_objectsWithoutCollisionCheck.findLinearSearch(co);
		if (index < m_objectsWithoutCollisionCheck.size())
		{
			return false;
		}
		return true;
	}

	/// Avoid using this internal API call, the extension pointer is used by
	/// some Bullet extensions. If you need to store your own user pointer, use
	/// 'setUserPointer/getUserPointer' instead.
	void* internalGetExtensionPointer() const { return m_extensionPointer; }

	/// Avoid using this internal API call, the extension pointer is used by
	/// some Bullet extensions If you need to store your own user pointer, use
	/// 'setUserPointer/getUserPointer' instead.
	void internalSetExtensionPointer(void* pointer) { m_extensionPointer = pointer; }

	SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1; }
	void                  setActivationState(int newState) const;
	void                  setDeactivationTime(btScalar time) { m_deactivationTime = time; }
	btScalar              getDeactivationTime() const { return m_deactivationTime; }
	void                  forceActivationState(int newState) const;
	void                  activate(bool forceActivation = false) const;

	SIMD_FORCE_INLINE bool isActive() const
	{
		return (
		    (getActivationState() != ISLAND_SLEEPING) &&
		    (getActivationState() != DISABLE_SIMULATION));
	}

	void     setRestitution(btScalar rest) { m_updateRevision++; m_restitution = rest; }
	btScalar getRestitution() const { return m_restitution; }
	void     setFriction(btScalar frict) { m_updateRevision++; m_friction = frict; }
	btScalar getFriction() const { return m_friction; }
	void     setRollingFriction(btScalar frict) { m_updateRevision++; m_rollingFriction = frict; }
	btScalar getRollingFriction() const { return m_rollingFriction; }
	void     setSpinningFriction(btScalar frict) {m_updateRevision++; m_spinningFriction = frict; }
	btScalar getSpinningFriction() const { return m_spinningFriction; }

	void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
	{
		m_updateRevision++;
		m_contactStiffness = stiffness;
		m_contactDamping   = damping;

		m_collisionFlags |= CF_HAS_CONTACT_STIFFNESS_DAMPING;

		// avoid divisions by zero...
		if (m_contactStiffness < SIMD_EPSILON)
		{
			m_contactStiffness = SIMD_EPSILON;
		}
	}

	btScalar getContactStiffness() const { return m_contactStiffness; }
	btScalar getContactDamping() const { return m_contactDamping; }

	/// reserved for Bullet internal usage
	int getInternalType() const { return m_internalType; }

	btTransform&                               getWorldTransform() { return m_worldTransform; }
	const btTransform&                         getWorldTransform() const { return m_worldTransform; }
	void                                       setWorldTransform(const btTransform& worldTrans) { m_updateRevision++; m_worldTransform = worldTrans; }
	SIMD_FORCE_INLINE btBroadphaseProxy*       getBroadphaseHandle() { return m_broadphaseHandle; }
	SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const { return m_broadphaseHandle; }
	void                                       setBroadphaseHandle(btBroadphaseProxy * handle) { m_broadphaseHandle = handle; }
	const btTransform&                         getInterpolationWorldTransform() const { return m_interpolationWorldTransform; }
	btTransform&                               getInterpolationWorldTransform() { return m_interpolationWorldTransform; }
	void                                       setInterpolationWorldTransform(const btTransform& trans) { m_updateRevision++; m_interpolationWorldTransform = trans; }
	void                                       setInterpolationLinearVelocity(const btVector3& linvel) { m_updateRevision++; m_interpolationLinearVelocity = linvel; }
	void                                       setInterpolationAngularVelocity(const btVector3& angvel) { m_updateRevision++; m_interpolationAngularVelocity = angvel; }
	const btVector3&                           getInterpolationLinearVelocity() const { return m_interpolationLinearVelocity; }
	const btVector3&                           getInterpolationAngularVelocity() const { return m_interpolationAngularVelocity; }
	SIMD_FORCE_INLINE int                      getIslandTag() const { return m_islandTag1; }
	void                                       setIslandTag(int tag) { m_islandTag1 = tag; }
	SIMD_FORCE_INLINE int                      getCompanionId() const { return m_companionId; }
	void                                       setCompanionId(int id) { m_companionId = id; }
	SIMD_FORCE_INLINE int                      getWorldArrayIndex() const { return m_worldArrayIndex; }

	// only should be called by CollisionWorld
	void setWorldArrayIndex(int ix) { m_worldArrayIndex = ix; }

	SIMD_FORCE_INLINE btScalar getHitFraction() const { return m_hitFraction; }
	void setHitFraction(btScalar hitFraction) { m_hitFraction = hitFraction; }
	SIMD_FORCE_INLINE int getCollisionFlags() const { return m_collisionFlags; }
	void setCollisionFlags(int flags) { m_collisionFlags = flags; }

	/// Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
	btScalar getCcdSweptSphereRadius() const { return m_ccdSweptSphereRadius; }

	/// Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
	void setCcdSweptSphereRadius(btScalar radius) { m_ccdSweptSphereRadius = radius; }

	btScalar getCcdMotionThreshold() const { return m_ccdMotionThreshold; }
	btScalar getCcdSquareMotionThreshold() const { return m_ccdMotionThreshold * m_ccdMotionThreshold; }

	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
	void setCcdMotionThreshold(btScalar ccdMotionThreshold) { m_ccdMotionThreshold = ccdMotionThreshold; }

	/// users can point to their objects, userPointer is not used by Bullet
	void* getUserPointer() const { return m_userObjectPointer; }

	int getUserIndex() const { return m_userIndex; }
	int getUserIndex2() const { return m_userIndex2; }

	/// users can point to their objects, userPointer is not used by Bullet
	void setUserPointer(void* userPointer) { m_userObjectPointer = userPointer; }

	/// users can point to their objects, userPointer is not used by Bullet
	void setUserIndex(int index) { m_userIndex = index; }

	void setUserIndex2(int index) { m_userIndex2 = index; }
	int getUpdateRevisionInternal() const { return m_updateRevision; }

	void setCustomDebugColor(const btVector3& colorRGB)
	{
		m_customDebugColorRGB = colorRGB;
		m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
	}

	void removeCustomDebugColor()
	{
		m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR;
	}

	bool getCustomDebugColor(btVector3 & colorRGB) const
	{
		bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR));
		if (hasCustomColor)
		{
			colorRGB = m_customDebugColorRGB;
		}
		return hasCustomColor;
	}

	inline bool checkCollideWith(const btCollisionObject* co) const
	{
		if (m_checkCollideWith)
		{
			return checkCollideWithOverride(co);
		}
		return true;
	}

	virtual int calculateSerializeBufferSize() const;

	/// fills the dataBuffer and returns the struct name (and 0 on failure)
	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;

	virtual void serializeSingleObject(class btSerializer * serializer) const;
};

/// do not change those serialization structures, it requires an updated
/// sBulletDNAstr/sBulletDNAstr64
struct btCollisionObjectDoubleData
{
	void*                 m_broadphaseHandle;
	void*                 m_collisionShape;
	btCollisionShapeData* m_rootCollisionShape;
	char*                 m_name;

	btTransformDoubleData m_worldTransform;
	btTransformDoubleData m_interpolationWorldTransform;
	btVector3DoubleData   m_interpolationLinearVelocity;
	btVector3DoubleData   m_interpolationAngularVelocity;
	btVector3DoubleData   m_anisotropicFriction;
	double                m_contactProcessingThreshold;
	double                m_deactivationTime;
	double                m_friction;
	double                m_rollingFriction;
	double                m_contactDamping;
	double                m_contactStiffness;
	double                m_restitution;
	double                m_hitFraction;
	double                m_ccdSweptSphereRadius;
	double                m_ccdMotionThreshold;

	int m_hasAnisotropicFriction;
	int m_collisionFlags;
	int m_islandTag1;
	int m_companionId;
	int m_activationState1;
	int m_internalType;
	int m_checkCollideWith;

	char m_padding[4];
};

/// do not change those serialization structures, it requires an updated
/// sBulletDNAstr/sBulletDNAstr64
struct btCollisionObjectFloatData
{
	void*                 m_broadphaseHandle;
	void*                 m_collisionShape;
	btCollisionShapeData* m_rootCollisionShape;
	char*                 m_name;

	btTransformFloatData m_worldTransform;
	btTransformFloatData m_interpolationWorldTransform;
	btVector3FloatData   m_interpolationLinearVelocity;
	btVector3FloatData   m_interpolationAngularVelocity;
	btVector3FloatData   m_anisotropicFriction;
	float                m_contactProcessingThreshold;
	float                m_deactivationTime;
	float                m_friction;
	float                m_rollingFriction;
	float                m_contactDamping;
	float                m_contactStiffness;
	float                m_restitution;
	float                m_hitFraction;
	float                m_ccdSweptSphereRadius;
	float                m_ccdMotionThreshold;

	int  m_hasAnisotropicFriction;
	int  m_collisionFlags;
	int  m_islandTag1;
	int  m_companionId;
	int  m_activationState1;
	int  m_internalType;
	int  m_checkCollideWith;
	char m_padding[4];
};

SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const { return sizeof(btCollisionObjectData); }

#endif // BT_COLLISION_OBJECT_H


text:
#ifndef GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED
#define GIM_QUANTIZED_SET_STRUCTS_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"

///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
ATTRIBUTE_ALIGNED16	(struct) BT_QUANTIZED_BVH_NODE
{
	//12 bytes
	unsigned short int	m_quantizedAabbMin[3];
	unsigned short int	m_quantizedAabbMax[3];
	//4 bytes
	int	m_escapeIndexOrDataIndex;

	BT_QUANTIZED_BVH_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;
	}

	SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
		unsigned short * quantizedMin,unsigned short * quantizedMax) const
	{
		if(m_quantizedAabbMin[0] > quantizedMax[0] ||
		   m_quantizedAabbMax[0] < quantizedMin[0] ||
		   m_quantizedAabbMin[1] > quantizedMax[1] ||
		   m_quantizedAabbMax[1] < quantizedMin[1] ||
		   m_quantizedAabbMin[2] > quantizedMax[2] ||
		   m_quantizedAabbMax[2] < quantizedMin[2])
		{
			return false;
		}
		return true;
	}

};

#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


text:
#ifndef BT_COMPOUND_FROM_GIMPACT
#define BT_COMPOUND_FROM_GIMPACT

#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "btGImpactShape.h"
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"

ATTRIBUTE_ALIGNED16(class) btCompoundFromGimpactShape	: public btCompoundShape
{
public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	virtual ~btCompoundFromGimpactShape()
	{
		/*delete all the btBU_Simplex1to4 ChildShapes*/
		for (int i = 0; i < m_children.size(); i++)
		{
			delete m_children[i].m_childShape;
		}
	}

};

struct MyCallback : public btTriangleRaycastCallback
		{
			int	m_ignorePart;
			int	m_ignoreTriangleIndex;

			MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex)
			:btTriangleRaycastCallback(from,to),
			m_ignorePart(ignorePart),
			m_ignoreTriangleIndex(ignoreTriangleIndex)
			{

			}
			virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
			{
				if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex)
				{
					if (hitFraction < m_hitFraction)
						return hitFraction;
				}

				return m_hitFraction;
			}
		};
		struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback
		{
			const btGImpactMeshShape*		m_gimpactShape;
			btCompoundShape*			m_colShape;
			btScalar	m_depth;

			MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth)
			:m_colShape(colShape),
			m_gimpactShape(meshShape),
			m_depth(depth)
			{
			}

			virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
			{
				btVector3 scale = m_gimpactShape->getLocalScaling();
				btVector3 v0=triangle[0]*scale;
				btVector3 v1=triangle[1]*scale;
				btVector3 v2=triangle[2]*scale;

				btVector3 centroid = (v0+v1+v2)/3;
				btVector3 normal = (v1-v0).cross(v2-v0);
				normal.normalize();
				btVector3 rayFrom = centroid;
				btVector3 rayTo = centroid-normal*m_depth;

				MyCallback cb(rayFrom,rayTo,partId,triangleIndex);

				m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo);
				if (cb.m_hitFraction<1)
				{
					rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction);
					//rayTo = cb.m_from;
					//rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction);
					//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
				}

				btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo);
				btTransform ident;
				ident.setIdentity();
				m_colShape->addChildShape(ident,tet);
			}
		};

btCompoundShape*	btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth)
{
	btCompoundShape* colShape = new btCompoundFromGimpactShape();

		btTransform tr;
		tr.setIdentity();

		MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth);
		btVector3 aabbMin,aabbMax;
		gimpactMesh->getAabb(tr,aabbMin,aabbMax);
		gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax);

	return colShape;
}

#endif //BT_COMPOUND_FROM_GIMPACT


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 BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H
#define BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H

#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
class btDispatcher;
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"

#include "LinearMath/btAlignedObjectArray.h"

#include "btGImpactShape.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h"
#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"

//! Collision Algorithm for GImpact Shapes
/*!
For register this algorithm in Bullet, proceed as following:
 \code
btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(m_dynamicsWorld ->getDispatcher());
btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
 \endcode
*/
class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
protected:
	btCollisionAlgorithm * m_convex_algorithm;
    btPersistentManifold * m_manifoldPtr;
	btManifoldResult* m_resultOut;
	const btDispatcherInfo * m_dispatchInfo;
	int m_triface0;
	int m_part0;
	int m_triface1;
	int m_part1;

	//! Creates a new contact point
	SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0,const btCollisionObject* body1)
	{
		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
		return m_manifoldPtr;
	}

	SIMD_FORCE_INLINE void destroyConvexAlgorithm()
	{
		if(m_convex_algorithm)
		{
			m_convex_algorithm->~btCollisionAlgorithm();
			m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm);
			m_convex_algorithm = NULL;
		}
	}

	SIMD_FORCE_INLINE void destroyContactManifolds()
	{
		if(m_manifoldPtr == NULL) return;
		m_dispatcher->releaseManifold(m_manifoldPtr);
		m_manifoldPtr = NULL;
	}

	SIMD_FORCE_INLINE void clearCache()
	{
		destroyContactManifolds();
		destroyConvexAlgorithm();

		m_triface0 = -1;
		m_part0 = -1;
		m_triface1 = -1;
		m_part1 = -1;
	}

	SIMD_FORCE_INLINE btPersistentManifold* getLastManifold()
	{
		return m_manifoldPtr;
	}

	// Call before process collision
	SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
	{
		if(getLastManifold() == 0)
		{
			newContactManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
		}

		m_resultOut->setPersistentManifold(getLastManifold());
	}

	// Call before process collision
	SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
	{
		checkManifold(body0Wrap,body1Wrap);

		btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
				body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
		return convex_algorithm ;
	}

	// Call before process collision
	SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
	{
		if(m_convex_algorithm) return;
		m_convex_algorithm = newAlgorithm(body0Wrap,body1Wrap);
	}

	void addContactPoint(const btCollisionObjectWrapper * body0Wrap,
					const btCollisionObjectWrapper * body1Wrap,
					const btVector3 & point,
					const btVector3 & normal,
					btScalar distance);

//! Collision routines
//!@{

	void collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap,
				  const btCollisionObjectWrapper* body1Wrap,
				  const btGImpactMeshShapePart * shape0,
				  const btGImpactMeshShapePart * shape1,
				  const int * pairs, int pair_count);

	void collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
					  const btCollisionObjectWrapper* body1Wrap,
					  const btGImpactMeshShapePart * shape0,
					  const btGImpactMeshShapePart * shape1,
					  const int * pairs, int pair_count);

	void shape_vs_shape_collision(
					  const btCollisionObjectWrapper* body0,
					  const btCollisionObjectWrapper* body1,
					  const btCollisionShape * shape0,
					  const btCollisionShape * shape1);

	void convex_vs_convex_collision(const btCollisionObjectWrapper* body0Wrap,
					  const btCollisionObjectWrapper* body1Wrap,
					  const btCollisionShape* shape0,
					  const btCollisionShape* shape1);

	void gimpact_vs_gimpact_find_pairs(
					  const btTransform & trans0,
					  const btTransform & trans1,
					  const btGImpactShapeInterface * shape0,
					  const btGImpactShapeInterface * shape1,btPairSet & pairset);

	void gimpact_vs_shape_find_pairs(
					  const btTransform & trans0,
					  const btTransform & trans1,
					  const btGImpactShapeInterface * shape0,
					  const btCollisionShape * shape1,
					  btAlignedObjectArray<int> & collided_primitives);

	void gimpacttrimeshpart_vs_plane_collision(
					  const btCollisionObjectWrapper * body0Wrap,
					  const btCollisionObjectWrapper * body1Wrap,
					  const btGImpactMeshShapePart * shape0,
					  const btStaticPlaneShape * shape1,bool swapped);

public:

	btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);

	virtual ~btGImpactCollisionAlgorithm();

	virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);

	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);

	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
	{
		if (m_manifoldPtr)
			manifoldArray.push_back(m_manifoldPtr);
	}

	btManifoldResult*	internalGetResultOut()
	{
		return m_resultOut;
	}

	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
	{
		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
		{
			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm));
			return new(mem) btGImpactCollisionAlgorithm(ci,body0Wrap,body1Wrap);
		}
	};

	//! Use this function for register the algorithm externally
	static void registerAlgorithm(btCollisionDispatcher * dispatcher);
#ifdef TRI_COLLISION_PROFILING
	//! Gets the average time in miliseconds of tree collisions
	static float getAverageTreeCollisionTime();

	//! Gets the average time in miliseconds of triangle collisions
	static float getAverageTriangleCollisionTime();
#endif //TRI_COLLISION_PROFILING

	//! Collides two gimpact shapes
	/*!
	\pre shape0 and shape1 couldn't be btGImpactMeshShape objects
	*/

	void gimpact_vs_gimpact(const btCollisionObjectWrapper* body0Wrap,
					  const btCollisionObjectWrapper * body1Wrap,
					  const btGImpactShapeInterface * shape0,
					  const btGImpactShapeInterface * shape1);

	void gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
					  const btCollisionObjectWrapper* body1Wrap,
					  const btGImpactShapeInterface * shape0,
					  const btCollisionShape * shape1,bool swapped);

	void gimpact_vs_compoundshape(const btCollisionObjectWrapper * body0Wrap,
					  const btCollisionObjectWrapper * body1Wrap,
					  const btGImpactShapeInterface * shape0,
					  const btCompoundShape * shape1,bool swapped);

	void gimpact_vs_concave(
					  const btCollisionObjectWrapper * body0Wrap,
					  const btCollisionObjectWrapper * body1Wrap,
					  const btGImpactShapeInterface * shape0,
					  const btConcaveShape * shape1,bool swapped);

		/// Accessor/Mutator pairs for Part and triangleID
    void 	setFace0(int value)
    {
    	m_triface0 = value;
    }
    int getFace0()
    {
    	return m_triface0;
    }
    void setFace1(int value)
    {
    	m_triface1 = value;
    }
    int getFace1()
    {
    	return m_triface1;
    }
    void setPart0(int value)
    {
    	m_part0 = value;
    }
    int getPart0()
    {
    	return m_part0;
    }
    void setPart1(int value)
    {
    	m_part1 = value;
		}
    int getPart1()
    {
    	return m_part1;
    }

};

//algorithm details
//#define BULLET_TRIANGLE_COLLISION 1
#define GIMPACT_VS_PLANE_COLLISION 1

#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H


text:
#ifndef GIM_BOX_COLLISION_H_INCLUDED
#define GIM_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) 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_basic_geometry_operations.h"
#include "LinearMath/btTransform.h"

//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)
//	{
//		GIM_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);
//}

#ifndef TEST_CROSS_EDGE_BOX_MCR

#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)\
	{\
		GIM_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;\
}\

#endif

#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);\
}\

//!  Class for transforming a model1 to the space of model0
class GIM_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();
	}

	GIM_BOX_BOX_TRANSFORM_CACHE()
	{
	}

	GIM_BOX_BOX_TRANSFORM_CACHE(mat4f  trans1_to_0)
	{
		COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
        MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
		calc_absolute_matrix();
	}

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

		m_R1to0 = trans0.getBasis().transpose();
		m_T1to0 = m_R1to0 * (-trans0.getOrigin());

		m_T1to0 += m_R1to0*trans1.getOrigin();
		m_R1to0 *= trans1.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)
	{
        return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0;
	}
};

#ifndef BOX_PLANE_EPSILON
#define BOX_PLANE_EPSILON 0.000001f
#endif

//! Axis aligned box
class GIM_AABB
{
public:
	btVector3 m_min;
	btVector3 m_max;

	GIM_AABB()
	{}

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

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

	GIM_AABB(const btVector3 & V1,
			 const btVector3 & V2,
			 const btVector3 & V3,
			 GREAL margin)
	{
		m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);

		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
		m_max[2] = GIM_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;
	}

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

	GIM_AABB(const GIM_AABB &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] = G_REAL_INFINITY;
		m_min[1] = G_REAL_INFINITY;
		m_min[2] = G_REAL_INFINITY;
		m_max[0] = -G_REAL_INFINITY;
		m_max[1] = -G_REAL_INFINITY;
		m_max[2] = -G_REAL_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 GIM_AABB &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] = GIM_MIN3(V1[0],V2[0],V3[0]);
		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);

		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
		m_max[2] = GIM_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] = GIM_MIN3(V1[0],V2[0],V3[0]);
		m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
		m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);

		m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
		m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
		m_max[2] = GIM_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;
	}

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

		m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
		m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
		m_max[2] = GIM_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] = GIM_MIN(m_min[0],point[0]);
		m_min[1] = GIM_MIN(m_min[1],point[1]);
		m_min[2] = GIM_MIN(m_min[2],point[2]);

		m_max[0] = GIM_MAX(m_max[0],point[0]);
		m_max[1] = GIM_MAX(m_max[1],point[1]);
		m_max[2] = GIM_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 GIM_AABB & other, GIM_AABB & intersection)  const
	{
		intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
		intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
		intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);

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

	SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & 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)
	{
		btVector3 extents,center;
		this->get_center_extend(center,extents);;

		btScalar Dx = vorigin[0] - center[0];
		if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)	return false;
		btScalar Dy = vorigin[1] - center[1];
		if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)	return false;
		btScalar Dz = vorigin[2] - center[2];
		if(GIM_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 ePLANE_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 G_BACK_PLANE; // 0
		}

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

	SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
	{
		GIM_AABB tbox = box;
		tbox.appy_transform(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 GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
	{

		//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(GIM_GREATER(T[i], t))	return false;
		}
		// Class II : B's basis vectors
		for(i=0;i<3;i++)
		{
			t = MAT_DOT_COL(transcache.m_R1to0,T,i);
			t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
			if(GIM_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(GIM_GREATER(t,t2))	return false;
				}
			}
		}
		return true;
	}

	//! Simple test for planes.
	SIMD_FORCE_INLINE bool collide_plane(
		const btVector4 & plane)
	{
		ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
		return (classify == G_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)
	{
		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;
	}
};

#ifndef BT_BOX_COLLISION_H_INCLUDED
//! 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

#endif // GIM_BOX_COLLISION_H_INCLUDED


text:
/*! \file btGImpactMassUtil.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_MASS_UTIL_H
#define GIMPACT_MASS_UTIL_H

#include "LinearMath/btTransform.h"

SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed(
	const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform)
{
	btMatrix3x3  rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();

	btScalar x2 = transform.getOrigin()[0];
	x2*= x2;
	btScalar y2 = transform.getOrigin()[1];
	y2*= y2;
	btScalar z2 = transform.getOrigin()[2];
	z2*= z2;

	btScalar ix = rotatedTensor[0][0]*(y2+z2);
	btScalar iy = rotatedTensor[1][1]*(x2+z2);
	btScalar iz = rotatedTensor[2][2]*(x2+y2);

	return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
}

SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass)
{
	btScalar x2 = point[0]*point[0];
	btScalar y2 = point[1]*point[1];
	btScalar z2 = point[2]*point[2];
	return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2));
}

#endif //GIMPACT_MESH_SHAPE_H


text:
#ifndef GIM_MATH_H_INCLUDED
#define GIM_MATH_H_INCLUDED
/*! \file gim_math.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 "LinearMath/btScalar.h"

#define GREAL btScalar
#define GREAL2 double
#define GINT int
#define GUINT unsigned int
#define GSHORT short
#define GUSHORT unsigned short
#define GINT64 long long
#define GUINT64 unsigned long long

#define G_PI 3.14159265358979f
#define G_HALF_PI 1.5707963f
//267948966
#define G_TWO_PI 6.28318530f
//71795864
#define G_ROOT3 1.73205f
#define G_ROOT2 1.41421f
#define G_UINT_INFINITY 0xffffffff //!< A very very high value
#define G_REAL_INFINITY FLT_MAX
#define	G_SIGN_BITMASK			0x80000000
#define G_EPSILON SIMD_EPSILON

enum GIM_SCALAR_TYPES
{
	G_STYPE_REAL =0,
	G_STYPE_REAL2,
	G_STYPE_SHORT,
	G_STYPE_USHORT,
	G_STYPE_INT,
	G_STYPE_UINT,
	G_STYPE_INT64,
	G_STYPE_UINT64
};

#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f)
#define G_RADTODEG(X) ((X)*180.0f/3.1415926f)

//! Integer representation of a floating-point value.
#define GIM_IR(x)					((GUINT&)(x))

//! Signed integer representation of a floating-point value.
#define GIM_SIR(x)					((GINT&)(x))

//! Absolute integer representation of a floating-point value
#define GIM_AIR(x)					(GIM_IR(x)&0x7fffffff)

//! Floating-point representation of an integer value.
#define GIM_FR(x)					((GREAL&)(x))

#define GIM_MAX(a,b) (a<b?b:a)
#define GIM_MIN(a,b) (a>b?b:a)

#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c))
#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c))

#define GIM_IS_ZERO(value) (value < G_EPSILON &&  value > -G_EPSILON)

#define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON)

#define GIM_IS_POSISITVE(value) (value >= G_EPSILON)

#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2))

///returns a clamped number
#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))

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

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

#define GIM_INV_SQRT(va,isva)\
{\
    if(va<=0.0000001f)\
    {\
        isva = G_REAL_INFINITY;\
    }\
    else\
    {\
        GREAL _x = va * 0.5f;\
        GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\
        isva = GIM_FR(_y);\
        isva  = isva * ( 1.5f - ( _x * isva * isva ) );\
    }\
}\

#define GIM_SQRT(va,sva)\
{\
    GIM_INV_SQRT(va,sva);\
    sva = 1.0f/sva;\
}\

//! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html
inline GREAL gim_inv_sqrt(GREAL f)
{
    GREAL r;
    GIM_INV_SQRT(f,r);
    return r;
}

inline GREAL gim_sqrt(GREAL f)
{
    GREAL r;
    GIM_SQRT(f,r);
    return r;
}

#endif // GIM_MATH_H_INCLUDED


text:
#ifndef BT_CONTACT_H_STRUCTS_INCLUDED
#define BT_CONTACT_H_STRUCTS_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"

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

#define CONTACT_DIFF_EPSILON 0.00001f

///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint.
///@todo: remove and replace GIM_CONTACT by btManifoldPoint.
class GIM_CONTACT
{
public:
    btVector3 m_point;
    btVector3 m_normal;
    btScalar m_depth;//Positive value indicates interpenetration
    btScalar m_distance;//Padding not for use
    int m_feature1;//Face number
    int 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)
    {
    }

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

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

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

		btScalar 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/btSqrt(vec_sum_len);
    }

};

#endif // BT_CONTACT_H_STRUCTS_INCLUDED


text:
#ifndef GIM_GEOMETRY_H_INCLUDED
#define GIM_GEOMETRY_H_INCLUDED

/*! \file gim_geometry.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.

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

///Additional Headers for Collision
#include "gim_basic_geometry_operations.h"
#include "gim_clip_polygon.h"
#include "gim_box_collision.h"
#include "gim_tri_collision.h"

#endif // GIM_VECTOR_H_INCLUDED


text:
#ifndef GIM_HASH_TABLE_H_INCLUDED
#define GIM_HASH_TABLE_H_INCLUDED
/*! \file gim_trimesh_data.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_radixsort.h"

#define GIM_INVALID_HASH 0xffffffff //!< A very very high value
#define GIM_DEFAULT_HASH_TABLE_SIZE 380
#define GIM_DEFAULT_HASH_TABLE_NODE_SIZE 4
#define GIM_HASH_TABLE_GROW_FACTOR 2

#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII

template<typename T>
struct GIM_HASH_TABLE_NODE
{
    GUINT m_key;
    T m_data;
    GIM_HASH_TABLE_NODE()
    {
    }

    GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value)
    {
        m_key = value.m_key;
        m_data = value.m_data;
    }

    GIM_HASH_TABLE_NODE(GUINT key, const T & data)
    {
        m_key = key;
        m_data = data;
    }

    bool operator <(const GIM_HASH_TABLE_NODE<T> & other) const
	{
		///inverse order, further objects are first
		if(m_key <  other.m_key) return true;
		return false;
	}

	bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const
	{
		///inverse order, further objects are first
		if(m_key >  other.m_key) return true;
		return false;
	}

	bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const
	{
		///inverse order, further objects are first
		if(m_key ==  other.m_key) return true;
		return false;
	}
};

///Macro for getting the key
class GIM_HASH_NODE_GET_KEY
{
public:
	template<class T>
	inline GUINT operator()( const T& a)
	{
		return a.m_key;
	}
};

///Macro for comparing the key and the element
class GIM_HASH_NODE_CMP_KEY_MACRO
{
public:
	template<class T>
	inline int operator() ( const T& a, GUINT key)
	{
		return ((int)(a.m_key - key));
	}
};

///Macro for comparing Hash nodes
class GIM_HASH_NODE_CMP_MACRO
{
public:
	template<class T>
	inline int operator() ( const T& a, const T& b )
	{
		return ((int)(a.m_key - b.m_key));
	}
};

//! Sorting for hash table
/*!
switch automatically between quicksort and radixsort
*/
template<typename T>
void gim_sort_hash_node_array(T * array, GUINT array_count)
{
    if(array_count<GIM_MIN_RADIX_SORT_SIZE)
    {
    	gim_heap_sort(array,array_count,GIM_HASH_NODE_CMP_MACRO());
    }
    else
    {
    	memcopy_elements_func cmpfunc;
    	gim_radix_sort(array,array_count,GIM_HASH_NODE_GET_KEY(),cmpfunc);
    }
}

// Note: assumes long is at least 32 bits.
#define GIM_NUM_PRIME 28

static const GUINT gim_prime_list[GIM_NUM_PRIME] =
{
  53ul,         97ul,         193ul,       389ul,       769ul,
  1543ul,       3079ul,       6151ul,      12289ul,     24593ul,
  49157ul,      98317ul,      196613ul,    393241ul,    786433ul,
  1572869ul,    3145739ul,    6291469ul,   12582917ul,  25165843ul,
  50331653ul,   100663319ul,  201326611ul, 402653189ul, 805306457ul,
  1610612741ul, 3221225473ul, 4294967291ul
};

inline GUINT gim_next_prime(GUINT number)
{
    //Find nearest upper prime
    GUINT result_ind = 0;
    gim_binary_search(gim_prime_list,0,(GIM_NUM_PRIME-2),number,result_ind);

    // inv: result_ind < 28
    return gim_prime_list[result_ind];
}

//! A compact hash table implementation
/*!
A memory aligned compact hash table that coud be treated as an array.
It could be a simple sorted array without the overhead of the hash key bucked, or could
be a formely hash table with an array of keys.
You can use switch_to_hashtable() and switch_to_sorted_array for saving space or increase speed.
</br>

<ul>
<li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
<li> If node_size != 0, then this container becomes a hash table for ever
</ul>

*/
template<class T>
class gim_hash_table
{
protected:
    typedef GIM_HASH_TABLE_NODE<T> _node_type;

    //!The nodes
    //array< _node_type, SuperAllocator<_node_type> > m_nodes;
    gim_array< _node_type > m_nodes;
    //SuperBufferedArray< _node_type > m_nodes;
    bool m_sorted;

    ///Hash table data management. The hash table has the indices to the corresponding m_nodes array
    GUINT * m_hash_table;//!<
    GUINT m_table_size;//!<
    GUINT m_node_size;//!<
    GUINT m_min_hash_table_size;

    //! Returns the cell index
    inline GUINT _find_cell(GUINT hashkey)
    {
        _node_type * nodesptr = m_nodes.pointer();
        GUINT start_index = (hashkey%m_table_size)*m_node_size;
        GUINT end_index = start_index + m_node_size;

        while(start_index<end_index)
        {
            GUINT value = m_hash_table[start_index];
            if(value != GIM_INVALID_HASH)
            {
                if(nodesptr[value].m_key == hashkey) return start_index;
            }
            start_index++;
        }
        return GIM_INVALID_HASH;
    }

    //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key
    inline GUINT _find_avaliable_cell(GUINT hashkey)
    {
        _node_type * nodesptr = m_nodes.pointer();
        GUINT avaliable_index = GIM_INVALID_HASH;
        GUINT start_index = (hashkey%m_table_size)*m_node_size;
        GUINT end_index = start_index + m_node_size;

        while(start_index<end_index)
        {
            GUINT value = m_hash_table[start_index];
            if(value == GIM_INVALID_HASH)
            {
                if(avaliable_index==GIM_INVALID_HASH)
                {
                    avaliable_index = start_index;
                }
            }
            else if(nodesptr[value].m_key == hashkey)
            {
                return start_index;
            }
            start_index++;
        }
        return avaliable_index;
    }

    //! reserves the memory for the hash table.
    /*!
    \pre hash table must be empty
    \post reserves the memory for the hash table, an initializes all elements to GIM_INVALID_HASH.
    */
    inline void _reserve_table_memory(GUINT newtablesize)
    {
        if(newtablesize==0) return;
        if(m_node_size==0) return;

        //Get a Prime size

        m_table_size = gim_next_prime(newtablesize);

        GUINT datasize = m_table_size*m_node_size;
        //Alloc the data buffer
        m_hash_table =  (GUINT *)gim_alloc(datasize*sizeof(GUINT));
    }

    inline void _invalidate_keys()
    {
        GUINT datasize = m_table_size*m_node_size;
        for(GUINT i=0;i<datasize;i++)
        {
            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
        }
    }

    //! Clear all memory for the hash table
    inline void _clear_table_memory()
    {
        if(m_hash_table==NULL) return;
        gim_free(m_hash_table);
        m_hash_table = NULL;
        m_table_size = 0;
    }

    //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys
    inline void _rehash()
    {
        _invalidate_keys();

        _node_type * nodesptr = m_nodes.pointer();
        for(GUINT i=0;i<(GUINT)m_nodes.size();i++)
        {
            GUINT nodekey = nodesptr[i].m_key;
            if(nodekey != GIM_INVALID_HASH)
            {
                //Search for the avaliable cell in buffer
                GUINT index = _find_avaliable_cell(nodekey);

				if(m_hash_table[index]!=GIM_INVALID_HASH)
				{//The new index is alreade used... discard this new incomming object, repeated key
				    btAssert(m_hash_table[index]==nodekey);
					nodesptr[i].m_key = GIM_INVALID_HASH;
				}
				else
				{
					//;
					//Assign the value for alloc
					m_hash_table[index] = i;
				}
            }
        }
    }

    //! Resize hash table indices
    inline void _resize_table(GUINT newsize)
    {
        //Clear memory
        _clear_table_memory();
        //Alloc the data
        _reserve_table_memory(newsize);
        //Invalidate keys and rehash
        _rehash();
    }

    //! Destroy hash table memory
    inline void _destroy()
    {
        if(m_hash_table==NULL) return;
        _clear_table_memory();
    }

    //! Finds an avaliable hash table cell, and resizes the table if there isn't space
    inline GUINT _assign_hash_table_cell(GUINT hashkey)
    {
        GUINT cell_index = _find_avaliable_cell(hashkey);

        if(cell_index==GIM_INVALID_HASH)
        {
            //rehashing
            _resize_table(m_table_size+1);
            GUINT cell_index = _find_avaliable_cell(hashkey);
            btAssert(cell_index!=GIM_INVALID_HASH);
        }
        return cell_index;
    }

    //! erase by index in hash table
    inline bool _erase_by_index_hash_table(GUINT index)
    {
        if(index >= m_nodes.size()) return false;
        if(m_nodes[index].m_key != GIM_INVALID_HASH)
        {
            //Search for the avaliable cell in buffer
            GUINT cell_index = _find_cell(m_nodes[index].m_key);

            btAssert(cell_index!=GIM_INVALID_HASH);
            btAssert(m_hash_table[cell_index]==index);

            m_hash_table[cell_index] = GIM_INVALID_HASH;
        }

        return this->_erase_unsorted(index);
    }

    //! erase by key in hash table
    inline bool _erase_hash_table(GUINT hashkey)
    {
        if(hashkey == GIM_INVALID_HASH) return false;

        //Search for the avaliable cell in buffer
        GUINT cell_index = _find_cell(hashkey);
        if(cell_index ==GIM_INVALID_HASH) return false;

        GUINT index = m_hash_table[cell_index];
        m_hash_table[cell_index] = GIM_INVALID_HASH;

        return this->_erase_unsorted(index);
    }

    //! insert an element in hash table
    /*!
    If the element exists, this won't insert the element
    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
    If so, the element has been inserted at the last position of the array.
    */
    inline GUINT _insert_hash_table(GUINT hashkey, const T & value)
    {
        if(hashkey==GIM_INVALID_HASH)
        {
            //Insert anyway
            _insert_unsorted(hashkey,value);
            return GIM_INVALID_HASH;
        }

        GUINT cell_index = _assign_hash_table_cell(hashkey);

        GUINT value_key = m_hash_table[cell_index];

        if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited

        m_hash_table[cell_index] = m_nodes.size();

        _insert_unsorted(hashkey,value);
        return GIM_INVALID_HASH;
    }

    //! insert an element in hash table.
    /*!
    If the element exists, this replaces the element.
    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
    If so, the element has been inserted at the last position of the array.
    */
    inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value)
    {
        if(hashkey==GIM_INVALID_HASH)
        {
            //Insert anyway
            _insert_unsorted(hashkey,value);
            return GIM_INVALID_HASH;
        }

        GUINT cell_index = _assign_hash_table_cell(hashkey);

        GUINT value_key = m_hash_table[cell_index];

        if(value_key!= GIM_INVALID_HASH)
        {//replaces the existing
            m_nodes[value_key] = _node_type(hashkey,value);
            return value_key;// index of the replaced element
        }

        m_hash_table[cell_index] = m_nodes.size();

        _insert_unsorted(hashkey,value);
        return GIM_INVALID_HASH;

    }

    ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array
    inline bool _erase_sorted(GUINT index)
    {
        if(index>=(GUINT)m_nodes.size()) return false;
        m_nodes.erase_sorted(index);
		if(m_nodes.size()<2) m_sorted = false;
        return true;
    }

    //! faster, but unsorted
    inline bool _erase_unsorted(GUINT index)
    {
        if(index>=m_nodes.size()) return false;

        GUINT lastindex = m_nodes.size()-1;
        if(index<lastindex && m_hash_table!=0)
        {
			GUINT hashkey =  m_nodes[lastindex].m_key;
			if(hashkey!=GIM_INVALID_HASH)
			{
				//update the new position of the last element
				GUINT cell_index = _find_cell(hashkey);
				btAssert(cell_index!=GIM_INVALID_HASH);
				//new position of the last element which will be swaped
				m_hash_table[cell_index] = index;
			}
        }
        m_nodes.erase(index);
        m_sorted = false;
        return true;
    }

    //! Insert in position ordered
    /*!
    Also checks if it is needed to transform this container to a hash table, by calling check_for_switching_to_hashtable
    */
    inline void _insert_in_pos(GUINT hashkey, const T & value, GUINT pos)
    {
        m_nodes.insert(_node_type(hashkey,value),pos);
        this->check_for_switching_to_hashtable();
    }

    //! Insert an element in an ordered array
    inline GUINT _insert_sorted(GUINT hashkey, const T & value)
    {
        if(hashkey==GIM_INVALID_HASH || size()==0)
        {
            m_nodes.push_back(_node_type(hashkey,value));
            return GIM_INVALID_HASH;
        }
        //Insert at last position
        //Sort element

        GUINT result_ind=0;
        GUINT last_index = m_nodes.size()-1;
        _node_type * ptr = m_nodes.pointer();

        bool found = gim_binary_search_ex(
        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());

        //Insert before found index
        if(found)
        {
            return result_ind;
        }
        else
        {
            _insert_in_pos(hashkey, value, result_ind);
        }
        return GIM_INVALID_HASH;
    }

    inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value)
    {
        if(hashkey==GIM_INVALID_HASH || size()==0)
        {
            m_nodes.push_back(_node_type(hashkey,value));
            return GIM_INVALID_HASH;
        }
        //Insert at last position
        //Sort element
        GUINT result_ind;
        GUINT last_index = m_nodes.size()-1;
        _node_type * ptr = m_nodes.pointer();

        bool found = gim_binary_search_ex(
        	ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());

        //Insert before found index
        if(found)
        {
            m_nodes[result_ind] = _node_type(hashkey,value);
        }
        else
        {
            _insert_in_pos(hashkey, value, result_ind);
        }
        return result_ind;
    }

    //! Fast insertion in m_nodes array
    inline GUINT  _insert_unsorted(GUINT hashkey, const T & value)
    {
        m_nodes.push_back(_node_type(hashkey,value));
        m_sorted = false;
        return GIM_INVALID_HASH;
    }

public:

    /*!
        <li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes.
        When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable.
        <li> If node_size != 0, then this container becomes a hash table for ever
        </ul>
    */
    gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE,
                     GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE,
                     GUINT min_hash_table_size = GIM_INVALID_HASH)
    {
        m_hash_table = NULL;
        m_table_size = 0;
        m_sorted = false;
        m_node_size = node_size;
        m_min_hash_table_size = min_hash_table_size;

        if(m_node_size!=0)
        {
            if(reserve_size!=0)
            {
                m_nodes.reserve(reserve_size);
                _reserve_table_memory(reserve_size);
                _invalidate_keys();
            }
            else
            {
                m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE);
                _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE);
                _invalidate_keys();
            }
        }
        else if(reserve_size!=0)
        {
            m_nodes.reserve(reserve_size);
        }

    }

    ~gim_hash_table()
    {
        _destroy();
    }

    inline bool is_hash_table()
    {
        if(m_hash_table) return true;
        return false;
    }

    inline bool is_sorted()
    {
        if(size()<2) return true;
        return m_sorted;
    }

    bool sort()
    {
        if(is_sorted()) return true;
        if(m_nodes.size()<2) return false;

        _node_type * ptr = m_nodes.pointer();
        GUINT siz = m_nodes.size();
        gim_sort_hash_node_array(ptr,siz);
        m_sorted=true;

        if(m_hash_table)
        {
            _rehash();
        }
        return true;
    }

    bool switch_to_hashtable()
    {
        if(m_hash_table) return false;
        if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
        if(m_nodes.size()<GIM_DEFAULT_HASH_TABLE_SIZE)
        {
            _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE);
        }
        else
        {
            _resize_table(m_nodes.size()+1);
        }

        return true;
    }

    bool switch_to_sorted_array()
    {
        if(m_hash_table==NULL) return true;
        _clear_table_memory();
        return sort();
    }

    //!If the container reaches the
    bool check_for_switching_to_hashtable()
    {
        if(this->m_hash_table) return true;

        if(!(m_nodes.size()< m_min_hash_table_size))
        {
            if(m_node_size == 0)
            {
                m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE;
            }

            _resize_table(m_nodes.size()+1);
            return true;
        }
        return false;
    }

    inline void set_sorted(bool value)
    {
    	m_sorted = value;
    }

    //! Retrieves the amount of keys.
    inline GUINT size() const
    {
        return m_nodes.size();
    }

    //! Retrieves the hash key.
    inline GUINT get_key(GUINT index) const
    {
        return m_nodes[index].m_key;
    }

    //! Retrieves the value by index
    /*!
    */
    inline T * get_value_by_index(GUINT index)
    {
        return &m_nodes[index].m_data;
    }

    inline const T& operator[](GUINT index) const
    {
        return m_nodes[index].m_data;
    }

    inline T& operator[](GUINT index)
    {
        return m_nodes[index].m_data;
    }

    //! Finds the index of the element with the key
    /*!
    \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted
    If so, the element has been inserted at the last position of the array.
    */
    inline GUINT find(GUINT hashkey)
    {
        if(m_hash_table)
        {
            GUINT cell_index = _find_cell(hashkey);
            if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH;
            return m_hash_table[cell_index];
        }
		GUINT last_index = m_nodes.size();
        if(last_index<2)
        {
			if(last_index==0) return GIM_INVALID_HASH;
            if(m_nodes[0].m_key == hashkey) return 0;
            return GIM_INVALID_HASH;
        }
        else if(m_sorted)
        {
            //Binary search
            GUINT result_ind = 0;
			last_index--;
            _node_type *  ptr =  m_nodes.pointer();

            bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO());

            if(found) return result_ind;
        }
        return GIM_INVALID_HASH;
    }

    //! Retrieves the value associated with the index
    /*!
    \return the found element, or null
    */
    inline T * get_value(GUINT hashkey)
    {
        GUINT index = find(hashkey);
        if(index == GIM_INVALID_HASH) return NULL;
        return &m_nodes[index].m_data;
    }

    /*!
    */
    inline bool erase_by_index(GUINT index)
    {
        if(index > m_nodes.size()) return false;

        if(m_hash_table == NULL)
        {
            if(is_sorted())
            {
                return this->_erase_sorted(index);
            }
            else
            {
                return this->_erase_unsorted(index);
            }
        }
        else
        {
            return this->_erase_by_index_hash_table(index);
        }
        return false;
    }

    inline bool erase_by_index_unsorted(GUINT index)
    {
        if(index > m_nodes.size()) return false;

        if(m_hash_table == NULL)
        {
            return this->_erase_unsorted(index);
        }
        else
        {
            return this->_erase_by_index_hash_table(index);
        }
        return false;
    }

    /*!

    */
    inline bool erase_by_key(GUINT hashkey)
    {
        if(size()==0) return false;

        if(m_hash_table)
        {
            return this->_erase_hash_table(hashkey);
        }
        //Binary search

        if(is_sorted()==false) return false;

        GUINT result_ind = find(hashkey);
        if(result_ind!= GIM_INVALID_HASH)
        {
            return this->_erase_sorted(result_ind);
        }
        return false;
    }

    void clear()
    {
        m_nodes.clear();

        if(m_hash_table==NULL) return;
        GUINT datasize = m_table_size*m_node_size;
        //Initialize the hashkeys.
        GUINT i;
        for(i=0;i<datasize;i++)
        {
            m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys
        }
		m_sorted = false;
    }

    //! Insert an element into the hash
    /*!
    \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
    of the existing element.
    */
    inline GUINT insert(GUINT hashkey, const T & element)
    {
        if(m_hash_table)
        {
            return this->_insert_hash_table(hashkey,element);
        }
        if(this->is_sorted())
        {
            return this->_insert_sorted(hashkey,element);
        }
        return this->_insert_unsorted(hashkey,element);
    }

    //! Insert an element into the hash, and could overrite an existing object with the same hash.
    /*!
    \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position
    of the replaced element.
    */
    inline GUINT insert_override(GUINT hashkey, const T & element)
    {
        if(m_hash_table)
        {
            return this->_insert_hash_table_replace(hashkey,element);
        }
        if(this->is_sorted())
        {
            return this->_insert_sorted_replace(hashkey,element);
        }
        this->_insert_unsorted(hashkey,element);
        return m_nodes.size();
    }

    //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted
    /*!
    */
    inline GUINT insert_unsorted(GUINT hashkey,const T & element)
    {
        if(m_hash_table)
        {
            return this->_insert_hash_table(hashkey,element);
        }
        return this->_insert_unsorted(hashkey,element);
    }

};

#endif // GIM_CONTAINERS_H_INCLUDED


text:
#ifndef BT_GIMPACT_QUANTIZATION_H_INCLUDED
#define BT_GIMPACT_QUANTIZATION_H_INCLUDED

/*! \file btQuantization.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"

SIMD_FORCE_INLINE void bt_calc_quantization_parameters(
	btVector3 & outMinBound,
	btVector3 & outMaxBound,
	btVector3 & bvhQuantization,
	const btVector3& srcMinBound,const btVector3& srcMaxBound,
	btScalar quantizationMargin)
{
	//enlarge the AABB to avoid division by zero when initializing the quantization values
	btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
	outMinBound = srcMinBound - clampValue;
	outMaxBound = srcMaxBound + clampValue;
	btVector3 aabbSize = outMaxBound - outMinBound;
	bvhQuantization = btVector3(btScalar(65535.0),
								btScalar(65535.0),
								btScalar(65535.0)) / aabbSize;
}

SIMD_FORCE_INLINE void bt_quantize_clamp(
	unsigned short* out,
	const btVector3& point,
	const btVector3 & min_bound,
	const btVector3 & max_bound,
	const btVector3 & bvhQuantization)
{

	btVector3 clampedPoint(point);
	clampedPoint.setMax(min_bound);
	clampedPoint.setMin(max_bound);

	btVector3 v = (clampedPoint - min_bound) * bvhQuantization;
	out[0] = (unsigned short)(v.getX()+0.5f);
	out[1] = (unsigned short)(v.getY()+0.5f);
	out[2] = (unsigned short)(v.getZ()+0.5f);
}

SIMD_FORCE_INLINE btVector3 bt_unquantize(
	const unsigned short* vecIn,
	const btVector3 & offset,
	const btVector3 & bvhQuantization)
{
	btVector3	vecOut;
	vecOut.setValue(
		(btScalar)(vecIn[0]) / (bvhQuantization.getX()),
		(btScalar)(vecIn[1]) / (bvhQuantization.getY()),
		(btScalar)(vecIn[2]) / (bvhQuantization.getZ()));
	vecOut += offset;
	return vecOut;
}

#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED