text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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_IDEBUG_DRAW__H
#define BT_IDEBUG_DRAW__H

#include "btVector3.h"
#include "btTransform.h"

/// The btIDebugDraw interface class allows hooking up a debug renderer to
/// visually debug simulations. Typical use case: create a debug drawer object,
/// and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer
/// and call debugDrawWorld. A class that implements the btIDebugDraw interface
/// has to implement the drawLine method at a minimum. For color arguments the
/// X,Y,Z components refer to Red, Green and Blue each in the range [0..1]
class btIDebugDraw
{
public:
	ATTRIBUTE_ALIGNED16(struct)
	DefaultColors
	{
		btVector3 m_activeObject;
		btVector3 m_deactivatedObject;
		btVector3 m_wantsDeactivationObject;
		btVector3 m_disabledDeactivationObject;
		btVector3 m_disabledSimulationObject;
		btVector3 m_aabb;
		btVector3 m_contactPoint;

		DefaultColors()
		    : m_activeObject(1, 1, 1),
		      m_deactivatedObject(0, 1, 0),
		      m_wantsDeactivationObject(0, 1, 1),
		      m_disabledDeactivationObject(1, 0, 0),
		      m_disabledSimulationObject(1, 1, 0),
		      m_aabb(1, 0, 0),
		      m_contactPoint(1, 1, 0)
		{
		}
	};

	enum DebugDrawModes
	{
		DBG_NoDebug              = 0,
		DBG_DrawWireframe        = 1,
		DBG_DrawAabb             = 2,
		DBG_DrawFeaturesText     = 4,
		DBG_DrawContactPoints    = 8,
		DBG_NoDeactivation       = 16,
		DBG_NoHelpText           = 32,
		DBG_DrawText             = 64,
		DBG_ProfileTimings       = 128,
		DBG_EnableSatComparison  = 256,
		DBG_DisableBulletLCP     = 512,
		DBG_EnableCCD            = 1024,
		DBG_DrawConstraints      = (1 << 11),
		DBG_DrawConstraintLimits = (1 << 12),
		DBG_FastWireframe        = (1 << 13),
		DBG_DrawNormals          = (1 << 14),
		DBG_DrawFrames           = (1 << 15),
		DBG_MAX_DEBUG_DRAW_MODE
	};

	virtual ~btIDebugDraw(){};

	virtual DefaultColors getDefaultColors() const
	{
		DefaultColors colors;
		return colors;
	}

	/// the default implementation for setDefaultColors has no effect. A derived
	/// class can implement it and store the colors.
	virtual void setDefaultColors(const DefaultColors& /*colors*/) {}

	virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& color) = 0;

	virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
	{
		(void) toColor;
		drawLine(from, to, fromColor);
	}

	virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
	{
		btVector3 center      = transform.getOrigin();
		btVector3 up          = transform.getBasis().getColumn(1);
		btVector3 axis        = transform.getBasis().getColumn(0);
		btScalar  minTh       = -SIMD_HALF_PI;
		btScalar  maxTh       = SIMD_HALF_PI;
		btScalar  minPs       = -SIMD_HALF_PI;
		btScalar  maxPs       = SIMD_HALF_PI;
		btScalar  stepDegrees = 30.f;
		drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false);
		drawSpherePatch(center, up, -axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false);
	}

	virtual void drawSphere(const btVector3& p, btScalar radius, const btVector3& color)
	{
		btTransform tr;
		tr.setIdentity();
		tr.setOrigin(p);
		drawSphere(radius, tr, color);
	}

	virtual void drawTriangle(
	    const btVector3& v0,
	    const btVector3& v1,
	    const btVector3& v2,
	    const btVector3& /*n0*/,
	    const btVector3& /*n1*/,
	    const btVector3& /*n2*/,
	    const btVector3& color,
	    btScalar         alpha)
	{
		drawTriangle(v0, v1, v2, color, alpha);
	}

	virtual void drawTriangle(
	    const btVector3& v0,
	    const btVector3& v1,
	    const btVector3& v2,
	    const btVector3& color,
	    btScalar /*alpha*/)
	{
		drawLine(v0, v1, color);
		drawLine(v1, v2, color);
		drawLine(v2, v0, color);
	}

	virtual void drawContactPoint(
	    const btVector3& PointOnB,
	    const btVector3& normalOnB,
	    btScalar         distance,
	    int              lifeTime,
	    const btVector3& color) = 0;

	virtual void reportErrorWarning(const char* warningString)                 = 0;
	virtual void draw3dText(const btVector3& location, const char* textString) = 0;
	virtual void setDebugMode(int debugMode)                                   = 0;
	virtual int  getDebugMode() const                                          = 0;

	virtual void drawAabb(const btVector3& from, const btVector3& to, const btVector3& color)
	{
		btVector3 halfExtents = (to - from) * 0.5f;
		btVector3 center      = (to + from) * 0.5f;
		int i, j;

		btVector3 edgecoord(1.f, 1.f, 1.f), pa, pb;
		for (i = 0; i < 4; i++)
		{
			for (j = 0; j < 3; j++)
			{
				pa = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1], edgecoord[2] * halfExtents[2]);
				pa += center;

				int othercoord = j % 3;
				edgecoord[othercoord] *= -1.f;
				pb = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1], edgecoord[2] * halfExtents[2]);
				pb += center;

				drawLine(pa, pb, color);
			}
			edgecoord = btVector3(-1.f, -1.f, -1.f);
			if (i < 3)
				edgecoord[i] *= -1.f;
		}
	}

	virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
	{
		btVector3 start = transform.getOrigin();
		drawLine(start, start + transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f, 0.3, 0.3));
		drawLine(start, start + transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3, 1.f, 0.3));
		drawLine(start, start + transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3, 1.f));
	}

	virtual void drawArc(
	    const btVector3& center,
	    const btVector3& normal,
	    const btVector3& axis,
	    btScalar         radiusA,
	    btScalar         radiusB,
	    btScalar         minAngle,
	    btScalar         maxAngle,
	    const btVector3& color,
	    bool             drawSect,
	    btScalar         stepDegrees = btScalar(10.f))
	{
		const btVector3& vx     = axis;
		btVector3        vy     = normal.cross(axis);
		btScalar         step   = stepDegrees * SIMD_RADS_PER_DEG;
		int              nSteps = (int) btFabs((maxAngle - minAngle) / step);
		if (!nSteps) nSteps = 1;
		btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
		if (drawSect)
		{
			drawLine(center, prev, color);
		}
		for (int i = 1; i <= nSteps; i++)
		{
			btScalar  angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
			btVector3 next  = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
			drawLine(prev, next, color);
			prev = next;
		}
		if (drawSect)
		{
			drawLine(center, prev, color);
		}
	}

	virtual void drawSpherePatch(
	    const btVector3& center,
	    const btVector3& up,
	    const btVector3& axis,
	    btScalar         radius,
	    btScalar         minTh,
	    btScalar         maxTh,
	    btScalar         minPs,
	    btScalar         maxPs,
	    const btVector3& color,
	    btScalar         stepDegrees = btScalar(10.f),
	    bool             drawCenter  = true)
	{
		btVector3        vA[74];
		btVector3        vB[74];
		btVector3 *      pvA = vA, *pvB = vB, *pT;
		btVector3        npole = center + up * radius;
		btVector3        spole = center - up * radius;
		btVector3        arcStart;
		btScalar         step  = stepDegrees * SIMD_RADS_PER_DEG;
		const btVector3& kv    = up;
		const btVector3& iv    = axis;
		btVector3        jv    = kv.cross(iv);
		bool             drawN = false;
		bool             drawS = false;
		if (minTh <= -SIMD_HALF_PI)
		{
			minTh = -SIMD_HALF_PI + step;
			drawN = true;
		}
		if (maxTh >= SIMD_HALF_PI)
		{
			maxTh = SIMD_HALF_PI - step;
			drawS = true;
		}
		if (minTh > maxTh)
		{
			minTh = -SIMD_HALF_PI + step;
			maxTh = SIMD_HALF_PI - step;
			drawN = drawS = true;
		}
		int n_hor = (int) ((maxTh - minTh) / step) + 1;
		if (n_hor < 2) n_hor = 2;
		btScalar step_h   = (maxTh - minTh) / btScalar(n_hor - 1);
		bool     isClosed = false;
		if (minPs > maxPs)
		{
			minPs    = -SIMD_PI + step;
			maxPs    = SIMD_PI;
			isClosed = true;
		}
		else if ((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
		{
			isClosed = true;
		}
		else
		{
			isClosed = false;
		}
		int n_vert = (int) ((maxPs - minPs) / step) + 1;
		if (n_vert < 2) n_vert = 2;
		btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
		for (int i = 0; i < n_hor; i++)
		{
			btScalar th  = minTh + btScalar(i) * step_h;
			btScalar sth = radius * btSin(th);
			btScalar cth = radius * btCos(th);
			for (int j = 0; j < n_vert; j++)
			{
				btScalar psi = minPs + btScalar(j) * step_v;
				btScalar sps = btSin(psi);
				btScalar cps = btCos(psi);
				pvB[j]       = center + cth * cps * iv + cth * sps * jv + sth * kv;
				if (i)
				{
					drawLine(pvA[j], pvB[j], color);
				}
				else if (drawS)
				{
					drawLine(spole, pvB[j], color);
				}
				if (j)
				{
					drawLine(pvB[j - 1], pvB[j], color);
				}
				else
				{
					arcStart = pvB[j];
				}
				if ((i == (n_hor - 1)) && drawN)
				{
					drawLine(npole, pvB[j], color);
				}

				if (drawCenter)
				{
					if (isClosed)
					{
						if (j == (n_vert - 1))
						{
							drawLine(arcStart, pvB[j], color);
						}
					}
					else
					{
						if (((!i) || (i == (n_hor - 1))) && ((!j) || (j == (n_vert - 1))))
						{
							drawLine(center, pvB[j], color);
						}
					}
				}
			}
			pT  = pvA;
			pvA = pvB;
			pvB = pT;
		}
	}

	virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
	{
		drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
		drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
		drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
		drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
		drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
		drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
	}

	virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
	{
		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
		drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
		drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
		drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
	}

	virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
	{
		int stepDegrees = 30;

		btVector3 capStart(0.f, 0.f, 0.f);
		capStart[upAxis] = -halfHeight;

		btVector3 capEnd(0.f, 0.f, 0.f);
		capEnd[upAxis] = halfHeight;

		// Draw the ends
		{
			btTransform childTransform = transform;
			childTransform.getOrigin() = transform * capStart;
			{
				btVector3 center = childTransform.getOrigin();
				btVector3 up     = childTransform.getBasis().getColumn((upAxis + 1) % 3);
				btVector3 axis   = -childTransform.getBasis().getColumn(upAxis);
				btScalar  minTh  = -SIMD_HALF_PI;
				btScalar  maxTh  = SIMD_HALF_PI;
				btScalar  minPs  = -SIMD_HALF_PI;
				btScalar  maxPs  = SIMD_HALF_PI;

				drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false);
			}
		}

		{
			btTransform childTransform = transform;
			childTransform.getOrigin() = transform * capEnd;
			{
				btVector3 center = childTransform.getOrigin();
				btVector3 up     = childTransform.getBasis().getColumn((upAxis + 1) % 3);
				btVector3 axis   = childTransform.getBasis().getColumn(upAxis);
				btScalar  minTh  = -SIMD_HALF_PI;
				btScalar  maxTh  = SIMD_HALF_PI;
				btScalar  minPs  = -SIMD_HALF_PI;
				btScalar  maxPs  = SIMD_HALF_PI;
				drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false);
			}
		}

		// Draw some additional lines
		btVector3 start = transform.getOrigin();

		for (int i = 0; i < 360; i += stepDegrees)
		{
			capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
			capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
			drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color);
		}
	}

	virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
	{
		btVector3 start = transform.getOrigin();
		btVector3 offsetHeight(0, 0, 0);
		offsetHeight[upAxis]  = halfHeight;
		int       stepDegrees = 30;
		btVector3 capStart(0.f, 0.f, 0.f);
		capStart[upAxis] = -halfHeight;
		btVector3 capEnd(0.f, 0.f, 0.f);
		capEnd[upAxis] = halfHeight;

		for (int i = 0; i < 360; i += stepDegrees)
		{
			capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
			capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
			drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color);
		}
		// Drawing top and bottom caps of the cylinder
		btVector3 yaxis(0, 0, 0);
		yaxis[upAxis] = btScalar(1.0);
		btVector3 xaxis(0, 0, 0);
		xaxis[(upAxis + 1) % 3] = btScalar(1.0);

		drawArc(
			start - transform.getBasis() * (offsetHeight),
			transform.getBasis() * yaxis,
			transform.getBasis() * xaxis,
			radius, radius,
			0, SIMD_2_PI,
			color, false, btScalar(10.0)
		);

		drawArc(
			start + transform.getBasis() * (offsetHeight),
			transform.getBasis() * yaxis,
			transform.getBasis() * xaxis,
			radius, radius,
			0, SIMD_2_PI,
			color, false, btScalar(10.0)
		);
	}

	virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color)
	{
		int       stepDegrees = 30;
		btVector3 start       = transform.getOrigin();

		btVector3 offsetHeight(0, 0, 0);
		btScalar  halfHeight = height * btScalar(0.5);
		offsetHeight[upAxis] = halfHeight;
		btVector3 offsetRadius(0, 0, 0);
		offsetRadius[(upAxis + 1) % 3] = radius;
		btVector3 offset2Radius(0, 0, 0);
		offset2Radius[(upAxis + 2) % 3] = radius;

		btVector3 capEnd(0.f, 0.f, 0.f);
		capEnd[upAxis] = -halfHeight;

		for (int i = 0; i < 360; i += stepDegrees)
		{
			capEnd[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
			capEnd[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
			drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * capEnd, color);
		}

		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offsetRadius), color);
		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offsetRadius), color);
		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offset2Radius), color);
		drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offset2Radius), color);

		// Drawing the base of the cone
		btVector3 yaxis(0, 0, 0);
		yaxis[upAxis] = btScalar(1.0);
		btVector3 xaxis(0, 0, 0);
		xaxis[(upAxis + 1) % 3] = btScalar(1.0);
		drawArc(
			start - transform.getBasis() * (offsetHeight),
			transform.getBasis() * yaxis,
			transform.getBasis() * xaxis,
			radius, radius,
			0, SIMD_2_PI,
			color, false, 10.0
		);
	}

	virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color)
	{
		btVector3 planeOrigin = planeNormal * planeConst;
		btVector3 vec0, vec1;
		btPlaneSpace1(planeNormal, vec0, vec1);
		btScalar  vecLen = 100.f;
		btVector3 pt0    = planeOrigin + vec0 * vecLen;
		btVector3 pt1    = planeOrigin - vec0 * vecLen;
		btVector3 pt2    = planeOrigin + vec1 * vecLen;
		btVector3 pt3    = planeOrigin - vec1 * vecLen;
		drawLine(transform * pt0, transform * pt1, color);
		drawLine(transform * pt2, transform * pt3, color);
	}

	virtual void clearLines() { }
	virtual void flushLines() { }
};

#endif //BT_IDEBUG_DRAW__H


text:
/*
Copyright (c) 2003-2006 Gino van den Bergen / 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_GEN_MINMAX_H
#define BT_GEN_MINMAX_H

#include "btScalar.h"

template <class T>
SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
{
  return a < b ? a : b ;
}

template <class T>
SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
{
  return  a > b ? a : b;
}

template <class T>
SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
{
	return a < lb ? lb : (ub < a ? ub : a);
}

template <class T>
SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
{
    if (b < a)
	{
		a = b;
	}
}

template <class T>
SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
{
    if (a < b)
	{
		a = b;
	}
}

template <class T>
SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
{
	if (a < lb)
	{
		a = lb;
	}
	else if (ub < a)
	{
		a = ub;
	}
}

#endif //BT_GEN_MINMAX_H

repo_name: JoshParnell/libphx

repo_language: C

mime_type: text/x-c++


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_MOTIONSTATE_H
#define BT_MOTIONSTATE_H

#include "btTransform.h"

///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics
///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
class	btMotionState
{
	public:

		virtual ~btMotionState()
		{

		}

		virtual void	getWorldTransform(btTransform& worldTrans ) const =0;

		//Bullet only calls the update of worldtransform for active objects
		virtual void	setWorldTransform(const btTransform& worldTrans)=0;

};

#endif //BT_MOTIONSTATE_H


text:
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org

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_HASH_MAP_H
#define BT_HASH_MAP_H

#include "btAlignedObjectArray.h"

///very basic hashable string implementation, compatible with btHashMap
struct btHashString
{
	const char* m_string;
	unsigned int	m_hash;

	SIMD_FORCE_INLINE	unsigned int getHash()const
	{
		return m_hash;
	}

	btHashString(const char* name)
		:m_string(name)
	{
		/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
		static const unsigned int  InitialFNV = 2166136261u;
		static const unsigned int FNVMultiple = 16777619u;

		/* Fowler / Noll / Vo (FNV) Hash */
		unsigned int hash = InitialFNV;

		for(int i = 0; m_string[i]; i++)
		{
			hash = hash ^ (m_string[i]);       /* xor  the low 8 bits */
			hash = hash * FNVMultiple;  /* multiply by the magic number */
		}
		m_hash = hash;
	}

	int portableStringCompare(const char* src,	const char* dst) const
	{
			int ret = 0 ;

			while( ! (ret = *(const unsigned char *)src - *(const unsigned char *)dst) && *dst)
					++src, ++dst;

			if ( ret < 0 )
					ret = -1 ;
			else if ( ret > 0 )
					ret = 1 ;

			return( ret );
	}

	bool equals(const btHashString& other) const
	{
		return (m_string == other.m_string) ||
			(0==portableStringCompare(m_string,other.m_string));

	}

};

const int BT_HASH_NULL=0xffffffff;

class btHashInt
{
	int	m_uid;
public:

	btHashInt()
	{
	}

	btHashInt(int uid)	:m_uid(uid)
	{
	}

	int	getUid1() const
	{
		return m_uid;
	}

	void	setUid1(int uid)
	{
		m_uid = uid;
	}

	bool equals(const btHashInt& other) const
	{
		return getUid1() == other.getUid1();
	}
	//to our success
	SIMD_FORCE_INLINE	unsigned int getHash()const
	{
		unsigned int key = m_uid;
		// Thomas Wang's hash
		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);

		return key;
	}
};

class btHashPtr
{

	union
	{
		const void*	m_pointer;
		unsigned int	m_hashValues[2];
	};

public:

	btHashPtr(const void* ptr)
		:m_pointer(ptr)
	{
	}

	const void*	getPointer() const
	{
		return m_pointer;
	}

	bool equals(const btHashPtr& other) const
	{
		return getPointer() == other.getPointer();
	}

	//to our success
	SIMD_FORCE_INLINE	unsigned int getHash()const
	{
		const bool VOID_IS_8 = ((sizeof(void*)==8));

		unsigned int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
		// Thomas Wang's hash
		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
		return key;
	}

};

template <class Value>
class btHashKeyPtr
{
        int     m_uid;
public:

        btHashKeyPtr(int uid)    :m_uid(uid)
        {
        }

        int     getUid1() const
        {
                return m_uid;
        }

        bool equals(const btHashKeyPtr<Value>& other) const
        {
                return getUid1() == other.getUid1();
        }

        //to our success
        SIMD_FORCE_INLINE       unsigned int getHash()const
        {
                unsigned int key = m_uid;
                // Thomas Wang's hash
                key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
                return key;
        }

};

template <class Value>
class btHashKey
{
	int	m_uid;
public:

	btHashKey(int uid)	:m_uid(uid)
	{
	}

	int	getUid1() const
	{
		return m_uid;
	}

	bool equals(const btHashKey<Value>& other) const
	{
		return getUid1() == other.getUid1();
	}
	//to our success
	SIMD_FORCE_INLINE	unsigned int getHash()const
	{
		unsigned int key = m_uid;
		// Thomas Wang's hash
		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
		return key;
	}
};

///The btHashMap template class implements a generic and lightweight hashmap.
///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp
template <class Key, class Value>
class btHashMap
{

protected:
	btAlignedObjectArray<int>		m_hashTable;
	btAlignedObjectArray<int>		m_next;

	btAlignedObjectArray<Value>		m_valueArray;
	btAlignedObjectArray<Key>		m_keyArray;

	void	growTables(const Key& /*key*/)
	{
		int newCapacity = m_valueArray.capacity();

		if (m_hashTable.size() < newCapacity)
		{
			//grow hashtable and next table
			int curHashtableSize = m_hashTable.size();

			m_hashTable.resize(newCapacity);
			m_next.resize(newCapacity);

			int i;

			for (i= 0; i < newCapacity; ++i)
			{
				m_hashTable[i] = BT_HASH_NULL;
			}
			for (i = 0; i < newCapacity; ++i)
			{
				m_next[i] = BT_HASH_NULL;
			}

			for(i=0;i<curHashtableSize;i++)
			{
				//const Value& value = m_valueArray[i];
				//const Key& key = m_keyArray[i];

				int	hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1);	// New hash value with new mask
				m_next[i] = m_hashTable[hashValue];
				m_hashTable[hashValue] = i;
			}

		}
	}

	public:

	void insert(const Key& key, const Value& value) {
		int hash = key.getHash() & (m_valueArray.capacity()-1);

		//replace value if the key is already there
		int index = findIndex(key);
		if (index != BT_HASH_NULL)
		{
			m_valueArray[index]=value;
			return;
		}

		int count = m_valueArray.size();
		int oldCapacity = m_valueArray.capacity();
		m_valueArray.push_back(value);
		m_keyArray.push_back(key);

		int newCapacity = m_valueArray.capacity();
		if (oldCapacity < newCapacity)
		{
			growTables(key);
			//hash with new capacity
			hash = key.getHash() & (m_valueArray.capacity()-1);
		}
		m_next[count] = m_hashTable[hash];
		m_hashTable[hash] = count;
	}

	void remove(const Key& key) {

		int hash = key.getHash() & (m_valueArray.capacity()-1);

		int pairIndex = findIndex(key);

		if (pairIndex ==BT_HASH_NULL)
		{
			return;
		}

		// Remove the pair from the hash table.
		int index = m_hashTable[hash];
		btAssert(index != BT_HASH_NULL);

		int previous = BT_HASH_NULL;
		while (index != pairIndex)
		{
			previous = index;
			index = m_next[index];
		}

		if (previous != BT_HASH_NULL)
		{
			btAssert(m_next[previous] == pairIndex);
			m_next[previous] = m_next[pairIndex];
		}
		else
		{
			m_hashTable[hash] = m_next[pairIndex];
		}

		// We now move the last pair into spot of the
		// pair being removed. We need to fix the hash
		// table indices to support the move.

		int lastPairIndex = m_valueArray.size() - 1;

		// If the removed pair is the last pair, we are done.
		if (lastPairIndex == pairIndex)
		{
			m_valueArray.pop_back();
			m_keyArray.pop_back();
			return;
		}

		// Remove the last pair from the hash table.
		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);

		index = m_hashTable[lastHash];
		btAssert(index != BT_HASH_NULL);

		previous = BT_HASH_NULL;
		while (index != lastPairIndex)
		{
			previous = index;
			index = m_next[index];
		}

		if (previous != BT_HASH_NULL)
		{
			btAssert(m_next[previous] == lastPairIndex);
			m_next[previous] = m_next[lastPairIndex];
		}
		else
		{
			m_hashTable[lastHash] = m_next[lastPairIndex];
		}

		// Copy the last pair into the remove pair's spot.
		m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
		m_keyArray[pairIndex] = m_keyArray[lastPairIndex];

		// Insert the last pair into the hash table
		m_next[pairIndex] = m_hashTable[lastHash];
		m_hashTable[lastHash] = pairIndex;

		m_valueArray.pop_back();
		m_keyArray.pop_back();

	}

	int size() const
	{
		return m_valueArray.size();
	}

	const Value* getAtIndex(int index) const
	{
		btAssert(index < m_valueArray.size());
		btAssert(index>=0);
		if (index>=0 && index < m_valueArray.size())
		{
			return &m_valueArray[index];
		}
		return 0;
	}

	Value* getAtIndex(int index)
	{
		btAssert(index < m_valueArray.size());
		btAssert(index>=0);
		if (index>=0 && index < m_valueArray.size())
		{
			return &m_valueArray[index];
		}
		return 0;
	}

    Key getKeyAtIndex(int index)
    {
        btAssert(index < m_keyArray.size());
		btAssert(index>=0);
		return m_keyArray[index];
    }

    const Key getKeyAtIndex(int index) const
    {
        btAssert(index < m_keyArray.size());
		btAssert(index>=0);
		return m_keyArray[index];
   }

	Value* operator[](const Key& key) {
		return find(key);
	}

	const Value* operator[](const Key& key) const {
		return find(key);
	}

	const Value*	find(const Key& key) const
	{
		int index = findIndex(key);
		if (index == BT_HASH_NULL)
		{
			return NULL;
		}
		return &m_valueArray[index];
	}

	Value*	find(const Key& key)
	{
		int index = findIndex(key);
		if (index == BT_HASH_NULL)
		{
			return NULL;
		}
		return &m_valueArray[index];
	}

	int	findIndex(const Key& key) const
	{
		unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);

		if (hash >= (unsigned int)m_hashTable.size())
		{
			return BT_HASH_NULL;
		}

		int index = m_hashTable[hash];
		while ((index != BT_HASH_NULL) && key.equals(m_keyArray[index]) == false)
		{
			index = m_next[index];
		}
		return index;
	}

	void	clear()
	{
		m_hashTable.clear();
		m_next.clear();
		m_valueArray.clear();
		m_keyArray.clear();
	}

};

#endif //BT_HASH_MAP_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_ALIGNED_ALLOCATOR
#define BT_ALIGNED_ALLOCATOR

///we probably replace this with our own aligned memory allocator
///so we replace _aligned_malloc and _aligned_free with our own
///that is better portable and more predictable

#include "btScalar.h"

///BT_DEBUG_MEMORY_ALLOCATIONS preprocessor can be set in build system
///for regression tests to detect memory leaks
///#define BT_DEBUG_MEMORY_ALLOCATIONS 1
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS

int btDumpMemoryLeaks();

#define btAlignedAlloc(a,b) \
		btAlignedAllocInternal(a,b,__LINE__,__FILE__)

#define btAlignedFree(ptr) \
		btAlignedFreeInternal(ptr,__LINE__,__FILE__)

void*	btAlignedAllocInternal	(size_t size, int alignment,int line,char* filename);

void	btAlignedFreeInternal	(void* ptr,int line,char* filename);

#else
	void*	btAlignedAllocInternal	(size_t size, int alignment);
	void	btAlignedFreeInternal	(void* ptr);

	#define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
	#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)

#endif
typedef int	size_type;

typedef void *(btAlignedAllocFunc)(size_t size, int alignment);
typedef void (btAlignedFreeFunc)(void *memblock);
typedef void *(btAllocFunc)(size_t size);
typedef void (btFreeFunc)(void *memblock);

///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);

///The btAlignedAllocator is a portable class for aligned memory allocations.
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
template < typename T , unsigned Alignment >
class btAlignedAllocator {

	typedef btAlignedAllocator< T , Alignment > self_type;

public:

	//just going down a list:
	btAlignedAllocator() {}
	/*
	btAlignedAllocator( const self_type & ) {}
	*/

	template < typename Other >
	btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}

	typedef const T*         const_pointer;
	typedef const T&         const_reference;
	typedef T*               pointer;
	typedef T&               reference;
	typedef T                value_type;

	pointer       address   ( reference        ref ) const                           { return &ref; }
	const_pointer address   ( const_reference  ref ) const                           { return &ref; }
	pointer       allocate  ( size_type        n   , const_pointer *      hint = 0 ) {
		(void)hint;
		return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
	}
	void          construct ( pointer          ptr , const value_type &   value    ) { new (ptr) value_type( value ); }
	void          deallocate( pointer          ptr ) {
		btAlignedFree( reinterpret_cast< void * >( ptr ) );
	}
	void          destroy   ( pointer          ptr )                                 { ptr->~value_type(); }

	template < typename O > struct rebind {
		typedef btAlignedAllocator< O , Alignment > other;
	};
	template < typename O >
	self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }

	friend bool operator==( const self_type & , const self_type & ) { return true; }
};

#endif //BT_ALIGNED_ALLOCATOR


text:
/*
Copyright (c) 2003-2006 Gino van den Bergen / 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.
*/

/*
StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson
Nov.2006
*/

#ifndef BT_STACK_ALLOC
#define BT_STACK_ALLOC

#include "btScalar.h" //for btAssert
#include "btAlignedAllocator.h"

///The btBlock class is an internal structure for the btStackAlloc memory allocator.
struct btBlock
{
	btBlock*			previous;
	unsigned char*		address;
};

///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
class btStackAlloc
{
public:

	btStackAlloc(unsigned int size)	{ ctor();create(size); }
	~btStackAlloc()		{ destroy(); }

	inline void		create(unsigned int size)
	{
		destroy();
		data		=  (unsigned char*) btAlignedAlloc(size,16);
		totalsize	=	size;
	}
	inline void		destroy()
	{
		btAssert(usedsize==0);
		//Raise(L"StackAlloc is still in use");

		if(usedsize==0)
		{
			if(!ischild && data)
				btAlignedFree(data);

			data				=	0;
			usedsize			=	0;
		}

	}

	int	getAvailableMemory() const
	{
		return static_cast<int>(totalsize - usedsize);
	}

	unsigned char*			allocate(unsigned int size)
	{
		const unsigned int	nus(usedsize+size);
		if(nus<totalsize)
		{
			usedsize=nus;
			return(data+(usedsize-size));
		}
		btAssert(0);
		//&& (L"Not enough memory"));

		return(0);
	}
	SIMD_FORCE_INLINE btBlock*		beginBlock()
	{
		btBlock*	pb = (btBlock*)allocate(sizeof(btBlock));
		pb->previous	=	current;
		pb->address		=	data+usedsize;
		current			=	pb;
		return(pb);
	}
	SIMD_FORCE_INLINE void		endBlock(btBlock* block)
	{
		btAssert(block==current);
		//Raise(L"Unmatched blocks");
		if(block==current)
		{
			current		=	block->previous;
			usedsize	=	(unsigned int)((block->address-data)-sizeof(btBlock));
		}
	}

private:
	void		ctor()
	{
		data		=	0;
		totalsize	=	0;
		usedsize	=	0;
		current		=	0;
		ischild		=	false;
	}
	unsigned char*		data;
	unsigned int		totalsize;
	unsigned int		usedsize;
	btBlock*	current;
	bool		ischild;
};

#endif //BT_STACK_ALLOC


text:
/***************************************************************************************************
**
** Real-Time Hierarchical Profiling for Game Programming Gems 3
**
** by Greg Hjelstrom & Byon Garrabrant
**
***************************************************************************************************/

// Credits: The Clock class was inspired by the Timer classes in
// Ogre (www.ogre3d.org).

#ifndef BT_QUICK_PROF_H
#define BT_QUICK_PROF_H

#include "btScalar.h"
#define USE_BT_CLOCK 1

#ifdef USE_BT_CLOCK

///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
class btClock
{
public:
	btClock();

	btClock(const btClock& other);
	btClock& operator=(const btClock& other);

	~btClock();

	/// Resets the initial reference time.
	void reset();

	/// Returns the time in ms since the last call to reset or since
	/// the btClock was created.
	unsigned long long int getTimeMilliseconds();

	/// Returns the time in us since the last call to reset or since
	/// the Clock was created.
	unsigned long long int getTimeMicroseconds();

	unsigned long long int getTimeNanoseconds();

	/// Returns the time in s since the last call to reset or since
	/// the Clock was created.
	btScalar getTimeSeconds();

private:
	struct btClockData* m_data;
};

#endif //USE_BT_CLOCK

typedef void (btEnterProfileZoneFunc)(const char* msg);
typedef void (btLeaveProfileZoneFunc)();

btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc();
btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc();

void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc);
void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);

#ifndef BT_NO_PROFILE // FIX redefinition
//To disable built-in profiling, please comment out next line
//#define BT_NO_PROFILE 1
#endif //BT_NO_PROFILE

#ifndef BT_NO_PROFILE
//btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined,
//otherwise returns thread index in range [0..maxThreads]
unsigned int btQuickprofGetCurrentThreadIndex2();
const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;

#include <stdio.h>//@todo remove this, backwards compatibility

#include "btAlignedAllocator.h"
#include <new>

///A node in the Profile Hierarchy Tree
class	CProfileNode {

public:
	CProfileNode( const char * name, CProfileNode * parent );
	~CProfileNode( void );

	CProfileNode * Get_Sub_Node( const char * name );

	CProfileNode * Get_Parent( void )		{ return Parent; }
	CProfileNode * Get_Sibling( void )		{ return Sibling; }
	CProfileNode * Get_Child( void )			{ return Child; }

	void				CleanupMemory();
	void				Reset( void );
	void				Call( void );
	bool				Return( void );

	const char *	Get_Name( void )				{ return Name; }
	int				Get_Total_Calls( void )		{ return TotalCalls; }
	float				Get_Total_Time( void )		{ return TotalTime; }
	void*			GetUserPointer() const {return m_userPtr;}
	void			SetUserPointer(void* ptr) { m_userPtr = ptr;}
protected:

	const char *	Name;
	int				TotalCalls;
	float				TotalTime;
	unsigned long int			StartTime;
	int				RecursionCounter;

	CProfileNode *	Parent;
	CProfileNode *	Child;
	CProfileNode *	Sibling;
	void*	m_userPtr;
};

///An iterator to navigate through the tree
class CProfileIterator
{
public:
	// Access all the children of the current parent
	void				First(void);
	void				Next(void);
	bool				Is_Done(void);
	bool                Is_Root(void) { return (CurrentParent->Get_Parent() == 0); }

	void				Enter_Child( int index );		// Make the given child the new parent
	void				Enter_Largest_Child( void );	// Make the largest child the new parent
	void				Enter_Parent( void );			// Make the current parent's parent the new parent

	// Access the current child
	const char *	Get_Current_Name( void )			{ return CurrentChild->Get_Name(); }
	int				Get_Current_Total_Calls( void )	{ return CurrentChild->Get_Total_Calls(); }
	float				Get_Current_Total_Time( void )	{ return CurrentChild->Get_Total_Time(); }

	void*	Get_Current_UserPointer( void )			{ return CurrentChild->GetUserPointer(); }
	void	Set_Current_UserPointer(void* ptr) {CurrentChild->SetUserPointer(ptr);}
	// Access the current parent
	const char *	Get_Current_Parent_Name( void )			{ return CurrentParent->Get_Name(); }
	int				Get_Current_Parent_Total_Calls( void )	{ return CurrentParent->Get_Total_Calls(); }
	float				Get_Current_Parent_Total_Time( void )	{ return CurrentParent->Get_Total_Time(); }

protected:

	CProfileNode *	CurrentParent;
	CProfileNode *	CurrentChild;

	CProfileIterator( CProfileNode * start );
	friend	class		CProfileManager;
};

///The Manager for the Profile system
class	CProfileManager {
public:
	static	void						Start_Profile( const char * name );
	static	void						Stop_Profile( void );

	static	void						CleanupMemory(void);
//	{
//		Root.CleanupMemory();
//	}

	static	void						Reset( void );
	static	void						Increment_Frame_Counter( void );
	static	int						Get_Frame_Count_Since_Reset( void )		{ return FrameCounter; }
	static	float						Get_Time_Since_Reset( void );

	static	CProfileIterator *	Get_Iterator( void );
//	{
//
//		return new CProfileIterator( &Root );
//	}
	static	void						Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }

	static void	dumpRecursive(CProfileIterator* profileIterator, int spacing);

	static void	dumpAll();

private:

	static	int						FrameCounter;
	static	unsigned long int					ResetTime;
};

#endif //#ifndef BT_NO_PROFILE

///ProfileSampleClass is a simple way to profile a function's scope
///Use the BT_PROFILE macro at the start of scope to time
class	CProfileSample {
public:
	CProfileSample( const char * name );

	~CProfileSample( void );
};

#define	BT_PROFILE( name )			CProfileSample __profile( name )

#endif //BT_QUICK_PROF_H


text:
/*
Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net

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_CONVEX_HULL_COMPUTER_H
#define BT_CONVEX_HULL_COMPUTER_H

#include "btVector3.h"
#include "btAlignedObjectArray.h"

/// Convex hull implementation based on Preparata and Hong
/// See http://code.google.com/p/bullet/issues/detail?id=275
/// Ole Kniemeyer, MAXON Computer GmbH
class btConvexHullComputer
{
	private:
		btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);

	public:

		class Edge
		{
			private:
				int next;
				int reverse;
				int targetVertex;

				friend class btConvexHullComputer;

			public:
				int getSourceVertex() const
				{
					return (this + reverse)->targetVertex;
				}

				int getTargetVertex() const
				{
					return targetVertex;
				}

				const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
				{
					return this + next;
				}

				const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
				{
					return (this + reverse)->getNextEdgeOfVertex();
				}

				const Edge* getReverseEdge() const
				{
					return this + reverse;
				}
		};

		// Vertices of the output hull
		btAlignedObjectArray<btVector3> vertices;

		// Edges of the output hull
		btAlignedObjectArray<Edge> edges;

		// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
		btAlignedObjectArray<int> faces;

		/*
		Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
		between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
		by that amount (each face is moved by "shrink" length units towards the center along its normal).
		If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius"
		is the minimum distance of a face to the center of the convex hull.

		The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large
		that the resulting convex hull is empty.

		The output convex hull can be found in the member variables "vertices", "edges", "faces".
		*/
		btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
		{
			return compute(coords, false, stride, count, shrink, shrinkClamp);
		}

		// same as above, but double precision
		btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
		{
			return compute(coords, true, stride, count, shrink, shrinkClamp);
		}
};

#endif //BT_CONVEX_HULL_COMPUTER_H


text:
/*
Copyright (c) 2003-2006 Gino van den Bergen / 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_SIMD_QUADWORD_H
#define BT_SIMD_QUADWORD_H

#include "btScalar.h"
#include "btMinMax.h"

#if defined (__CELLOS_LV2) && defined (__SPU__)
#include <altivec.h>
#endif

/**@brief The btQuadWord class is base class for btVector3 and btQuaternion.
 * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
 */
#ifndef USE_LIBSPE2
ATTRIBUTE_ALIGNED16(class) btQuadWord
#else
class btQuadWord
#endif
{
protected:

#if defined (__SPU__) && defined (__CELLOS_LV2__)
	union {
		vec_float4 mVec128;
		btScalar	m_floats[4];
	};
public:
	vec_float4	get128() const
	{
		return mVec128;
	}
protected:
#else //__CELLOS_LV2__ __SPU__

#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
	union {
		btSimdFloat4 mVec128;
		btScalar	m_floats[4];
	};
public:
	SIMD_FORCE_INLINE	btSimdFloat4	get128() const
	{
		return mVec128;
	}
	SIMD_FORCE_INLINE	void	set128(btSimdFloat4 v128)
	{
		mVec128 = v128;
	}
#else
	btScalar	m_floats[4];
#endif // BT_USE_SSE

#endif //__CELLOS_LV2__ __SPU__

	public:

#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)

	// Set Vector
	SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec)
	{
		mVec128 = vec;
	}

	// Copy constructor
	SIMD_FORCE_INLINE btQuadWord(const btQuadWord& rhs)
	{
		mVec128 = rhs.mVec128;
	}

	// Assignment Operator
	SIMD_FORCE_INLINE btQuadWord&
	operator=(const btQuadWord& v)
	{
		mVec128 = v.mVec128;

		return *this;
	}

#endif

  /**@brief Return the x value */
		SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
  /**@brief Return the y value */
		SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
  /**@brief Return the z value */
		SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
  /**@brief Set the x value */
		SIMD_FORCE_INLINE void	setX(btScalar _x) { m_floats[0] = _x;};
  /**@brief Set the y value */
		SIMD_FORCE_INLINE void	setY(btScalar _y) { m_floats[1] = _y;};
  /**@brief Set the z value */
		SIMD_FORCE_INLINE void	setZ(btScalar _z) { m_floats[2] = _z;};
  /**@brief Set the w value */
		SIMD_FORCE_INLINE void	setW(btScalar _w) { m_floats[3] = _w;};
  /**@brief Return the x value */
		SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
  /**@brief Return the y value */
		SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
  /**@brief Return the z value */
		SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
  /**@brief Return the w value */
		SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }

	//SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i];	}
	//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
	///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
	SIMD_FORCE_INLINE	operator       btScalar *()       { return &m_floats[0]; }
	SIMD_FORCE_INLINE	operator const btScalar *() const { return &m_floats[0]; }

	SIMD_FORCE_INLINE	bool	operator==(const btQuadWord& other) const
	{
#ifdef BT_USE_SSE
        return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
		return ((m_floats[3]==other.m_floats[3]) &&
                (m_floats[2]==other.m_floats[2]) &&
                (m_floats[1]==other.m_floats[1]) &&
                (m_floats[0]==other.m_floats[0]));
#endif
	}

	SIMD_FORCE_INLINE	bool	operator!=(const btQuadWord& other) const
	{
		return !(*this == other);
	}

  /**@brief Set x,y,z and zero w
   * @param x Value of x
   * @param y Value of y
   * @param z Value of z
   */
		SIMD_FORCE_INLINE void 	setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
		{
			m_floats[0]=_x;
			m_floats[1]=_y;
			m_floats[2]=_z;
			m_floats[3] = 0.f;
		}

/*		void getValue(btScalar *m) const
		{
			m[0] = m_floats[0];
			m[1] = m_floats[1];
			m[2] = m_floats[2];
		}
*/
/**@brief Set the values
   * @param x Value of x
   * @param y Value of y
   * @param z Value of z
   * @param w Value of w
   */
		SIMD_FORCE_INLINE void	setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
		{
			m_floats[0]=_x;
			m_floats[1]=_y;
			m_floats[2]=_z;
			m_floats[3]=_w;
		}
  /**@brief No initialization constructor */
		SIMD_FORCE_INLINE btQuadWord()
		//	:m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
		{
		}

  /**@brief Three argument constructor (zeros w)
   * @param x Value of x
   * @param y Value of y
   * @param z Value of z
   */
		SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z)
		{
			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
		}

/**@brief Initializing constructor
   * @param x Value of x
   * @param y Value of y
   * @param z Value of z
   * @param w Value of w
   */
		SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
		{
			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
		}

  /**@brief Set each element to the max of the current values and the values of another btQuadWord
   * @param other The other btQuadWord to compare with
   */
		SIMD_FORCE_INLINE void	setMax(const btQuadWord& other)
		{
        #ifdef BT_USE_SSE
            mVec128 = _mm_max_ps(mVec128, other.mVec128);
        #elif defined(BT_USE_NEON)
            mVec128 = vmaxq_f32(mVec128, other.mVec128);
        #else
        	btSetMax(m_floats[0], other.m_floats[0]);
			btSetMax(m_floats[1], other.m_floats[1]);
			btSetMax(m_floats[2], other.m_floats[2]);
			btSetMax(m_floats[3], other.m_floats[3]);
		#endif
        }
  /**@brief Set each element to the min of the current values and the values of another btQuadWord
   * @param other The other btQuadWord to compare with
   */
		SIMD_FORCE_INLINE void	setMin(const btQuadWord& other)
		{
        #ifdef BT_USE_SSE
            mVec128 = _mm_min_ps(mVec128, other.mVec128);
        #elif defined(BT_USE_NEON)
            mVec128 = vminq_f32(mVec128, other.mVec128);
        #else
        	btSetMin(m_floats[0], other.m_floats[0]);
			btSetMin(m_floats[1], other.m_floats[1]);
			btSetMin(m_floats[2], other.m_floats[2]);
			btSetMin(m_floats[3], other.m_floats[3]);
		#endif
        }

};

#endif //BT_SIMD_QUADWORD_H


text:
/*
Copyright (c) 2003-2009 Erwin Coumans  http://bullet.googlecode.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_SCALAR_H
#define BT_SCALAR_H

#ifdef BT_MANAGED_CODE
//Aligned data types not supported in managed code
#pragma unmanaged
#endif

#include <math.h>
#include <stdlib.h>  //size_t for MSVC 6.0
#include <float.h>

/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
#define BT_BULLET_VERSION 287

inline int btGetVersion()
{
	return BT_BULLET_VERSION;
}

// The following macro "BT_NOT_EMPTY_FILE" can be put into a file
// in order suppress the MS Visual C++ Linker warning 4221
//
// warning LNK4221: no public symbols found; archive member will be inaccessible
//
// This warning occurs on PC and XBOX when a file compiles out completely
// has no externally visible symbols which may be dependant on configuration
// #defines and options.
//
// see more https://stackoverflow.com/questions/1822887/what-is-the-best-way-to-eliminate-ms-visual-c-linker-warning-warning-lnk422

#if defined (_MSC_VER)
	#define BT_NOT_EMPTY_FILE_CAT_II(p, res) res
	#define BT_NOT_EMPTY_FILE_CAT_I(a, b) BT_NOT_EMPTY_FILE_CAT_II(~, a ## b)
	#define BT_NOT_EMPTY_FILE_CAT(a, b) BT_NOT_EMPTY_FILE_CAT_I(a, b)
	#define BT_NOT_EMPTY_FILE namespace { char BT_NOT_EMPTY_FILE_CAT(NoEmptyFileDummy, __COUNTER__); }
#else
	#define BT_NOT_EMPTY_FILE
#endif

// clang and most formatting tools don't support indentation of preprocessor guards, so turn it off
// clang-format off
#if defined(DEBUG) || defined (_DEBUG)
	#define BT_DEBUG
#endif

#ifdef _WIN32
	#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
		#define SIMD_FORCE_INLINE inline
		#define ATTRIBUTE_ALIGNED16(a) a
		#define ATTRIBUTE_ALIGNED64(a) a
		#define ATTRIBUTE_ALIGNED128(a) a
	#elif defined(_M_ARM)
		#define SIMD_FORCE_INLINE __forceinline
		#define ATTRIBUTE_ALIGNED16(a) __declspec() a
		#define ATTRIBUTE_ALIGNED64(a) __declspec() a
		#define ATTRIBUTE_ALIGNED128(a) __declspec () a
	#else//__MINGW32__
		//#define BT_HAS_ALIGNED_ALLOCATOR
		#pragma warning(disable : 4324) // disable padding warning
//			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
		#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
//			#pragma warning(disable:4786) // Disable the "debug name too long" warning

		#define SIMD_FORCE_INLINE __forceinline
		#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
		#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
		#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
		#ifdef _XBOX
			#define BT_USE_VMX128

			#include <ppcintrinsics.h>
 			#define BT_HAVE_NATIVE_FSEL
 			#define btFsel(a,b,c) __fsel((a),(b),(c))
		#else

#if defined (_M_ARM)
            //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however)
#elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
			#if _MSC_VER>1400
				#define BT_USE_SIMD_VECTOR3
			#endif

			#define BT_USE_SSE
			#ifdef BT_USE_SSE

#if (_MSC_FULL_VER >= 170050727)//Visual Studio 2012 can compile SSE4/FMA3 (but SSE4/FMA3 is not enabled by default)
			#define BT_ALLOW_SSE4
#endif //(_MSC_FULL_VER >= 160040219)

			//BT_USE_SSE_IN_API is disabled under Windows by default, because
			//it makes it harder to integrate Bullet into your application under Windows
			//(structured embedding Bullet structs/classes need to be 16-byte aligned)
			//with relatively little performance gain
			//If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
			//you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
			//#define BT_USE_SSE_IN_API
			#endif //BT_USE_SSE
			#include <emmintrin.h>
#endif

		#endif//_XBOX

	#endif //__MINGW32__

	#ifdef BT_DEBUG
		#ifdef _MSC_VER
			#include <stdio.h>
			#define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak();	}}
		#else//_MSC_VER
			#include <assert.h>
			#define btAssert assert
		#endif//_MSC_VER
	#else
		#define btAssert(x)
	#endif
		//btFullAssert is optional, slows down a lot
		#define btFullAssert(x)

		#define btLikely(_c)  _c
		#define btUnlikely(_c) _c

#else//_WIN32

	#if defined	(__CELLOS_LV2__)
		#define SIMD_FORCE_INLINE inline __attribute__((always_inline))
		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
		#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
		#ifndef assert
		#include <assert.h>
		#endif
		#ifdef BT_DEBUG
			#ifdef __SPU__
				#include <spu_printf.h>
				#define printf spu_printf
				#define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
			#else
				#define btAssert assert
			#endif

		#else//BT_DEBUG
				#define btAssert(x)
		#endif//BT_DEBUG
		//btFullAssert is optional, slows down a lot
		#define btFullAssert(x)

		#define btLikely(_c)  _c
		#define btUnlikely(_c) _c

	#else//defined	(__CELLOS_LV2__)

		#ifdef USE_LIBSPE2

			#define SIMD_FORCE_INLINE __inline
			#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
			#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
			#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
			#ifndef assert
			#include <assert.h>
			#endif
	#ifdef BT_DEBUG
			#define btAssert assert
	#else
			#define btAssert(x)
	#endif
			//btFullAssert is optional, slows down a lot
			#define btFullAssert(x)

			#define btLikely(_c)   __builtin_expect((_c), 1)
			#define btUnlikely(_c) __builtin_expect((_c), 0)

		#else//USE_LIBSPE2
	//non-windows systems

			#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
				#if defined (__i386__) || defined (__x86_64__)
					#define BT_USE_SIMD_VECTOR3
					#define BT_USE_SSE
					//BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
					//if apps run into issues, we will disable the next line
					#define BT_USE_SSE_IN_API
					#ifdef BT_USE_SSE
						// include appropriate SSE level
						#if defined (__SSE4_1__)
							#include <smmintrin.h>
						#elif defined (__SSSE3__)
							#include <tmmintrin.h>
						#elif defined (__SSE3__)
							#include <pmmintrin.h>
						#else
							#include <emmintrin.h>
						#endif
					#endif //BT_USE_SSE
				#elif defined( __ARM_NEON__ )
					#ifdef __clang__
						#define BT_USE_NEON 1
						#define BT_USE_SIMD_VECTOR3

						#if defined BT_USE_NEON && defined (__clang__)
							#include <arm_neon.h>
						#endif//BT_USE_NEON
				   #endif //__clang__
				#endif//__arm__

				#define SIMD_FORCE_INLINE inline __attribute__ ((always_inline))
			///@todo: check out alignment methods for other platforms/compilers
				#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
				#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
				#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
				#ifndef assert
				#include <assert.h>
				#endif

				#if defined(DEBUG) || defined (_DEBUG)
				 #if defined (__i386__) || defined (__x86_64__)
				#include <stdio.h>
				 #define btAssert(x)\
				{\
				if(!(x))\
				{\
					printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
					asm volatile ("int3");\
				}\
				}
				#else//defined (__i386__) || defined (__x86_64__)
					#define btAssert assert
				#endif//defined (__i386__) || defined (__x86_64__)
				#else//defined(DEBUG) || defined (_DEBUG)
					#define btAssert(x)
				#endif//defined(DEBUG) || defined (_DEBUG)

				//btFullAssert is optional, slows down a lot
				#define btFullAssert(x)
				#define btLikely(_c)  _c
				#define btUnlikely(_c) _c

			#else//__APPLE__

				#define SIMD_FORCE_INLINE inline
				///@todo: check out alignment methods for other platforms/compilers
				///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
				///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
				///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
				#define ATTRIBUTE_ALIGNED16(a) a
				#define ATTRIBUTE_ALIGNED64(a) a
				#define ATTRIBUTE_ALIGNED128(a) a
				#ifndef assert
				#include <assert.h>
				#endif

				#if defined(DEBUG) || defined (_DEBUG)
					#define btAssert assert
				#else
					#define btAssert(x)
				#endif

				//btFullAssert is optional, slows down a lot
				#define btFullAssert(x)
				#define btLikely(_c)  _c
				#define btUnlikely(_c) _c
			#endif //__APPLE__
		#endif // LIBSPE2
	#endif	//__CELLOS_LV2__
#endif//_WIN32

///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
#if defined(BT_USE_DOUBLE_PRECISION)
	typedef double btScalar;
	//this number could be bigger in double precision
	#define BT_LARGE_FLOAT 1e30
#else
	typedef float btScalar;
	//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
	#define BT_LARGE_FLOAT 1e18f
#endif

#ifdef BT_USE_SSE
	typedef __m128 btSimdFloat4;
#endif  //BT_USE_SSE

#if defined(BT_USE_SSE)
	//#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
	#ifdef _WIN32

		#ifndef BT_NAN
			static int btNanMask = 0x7F800001;
			#define BT_NAN (*(float *)&btNanMask)
		#endif

		#ifndef BT_INFINITY
			static int btInfinityMask = 0x7F800000;
			#define BT_INFINITY (*(float *)&btInfinityMask)
			inline int btGetInfinityMask()  //suppress stupid compiler warning
			{
				return btInfinityMask;
			}
		#endif

	//use this, in case there are clashes (such as xnamath.h)
	#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS
	inline __m128 operator+(const __m128 A, const __m128 B)
	{
		return _mm_add_ps(A, B);
	}

	inline __m128 operator-(const __m128 A, const __m128 B)
	{
		return _mm_sub_ps(A, B);
	}

	inline __m128 operator*(const __m128 A, const __m128 B)
	{
		return _mm_mul_ps(A, B);
	}
	#endif  //BT_NO_SIMD_OPERATOR_OVERLOADS

	#define btCastfTo128i(a) (_mm_castps_si128(a))
	#define btCastfTo128d(a) (_mm_castps_pd(a))
	#define btCastiTo128f(a) (_mm_castsi128_ps(a))
	#define btCastdTo128f(a) (_mm_castpd_ps(a))
	#define btCastdTo128i(a) (_mm_castpd_si128(a))
	#define btAssign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3)

	#else  //_WIN32

		#define btCastfTo128i(a) ((__m128i)(a))
		#define btCastfTo128d(a) ((__m128d)(a))
		#define btCastiTo128f(a) ((__m128)(a))
		#define btCastdTo128f(a) ((__m128)(a))
		#define btCastdTo128i(a) ((__m128i)(a))
		#define btAssign128(r0, r1, r2, r3) \
			(__m128) { r0, r1, r2, r3 }
		#define BT_INFINITY INFINITY
		#define BT_NAN NAN
	#endif  //_WIN32
#else//BT_USE_SSE

	#ifdef BT_USE_NEON
	#include <arm_neon.h>

	typedef float32x4_t btSimdFloat4;
	#define BT_INFINITY INFINITY
	#define BT_NAN NAN
	#define btAssign128(r0, r1, r2, r3) \
		(float32x4_t) { r0, r1, r2, r3 }
	#else  //BT_USE_NEON

	#ifndef BT_INFINITY
	struct btInfMaskConverter
	{
		union {
			float mask;
			int intmask;
		};
		btInfMaskConverter(int _mask = 0x7F800000)
			: intmask(_mask)
		{
		}
	};
	static btInfMaskConverter btInfinityMask = 0x7F800000;
	#define BT_INFINITY (btInfinityMask.mask)
	inline int btGetInfinityMask()  //suppress stupid compiler warning
	{
		return btInfinityMask.intmask;
	}
	#endif
	#endif  //BT_USE_NEON

#endif  //BT_USE_SSE

#ifdef BT_USE_NEON
	#include <arm_neon.h>

	typedef float32x4_t btSimdFloat4;
	#define BT_INFINITY INFINITY
	#define BT_NAN NAN
	#define btAssign128(r0, r1, r2, r3) \
		(float32x4_t) { r0, r1, r2, r3 }
#endif//BT_USE_NEON

#define BT_DECLARE_ALIGNED_ALLOCATOR()                                                                     \
	SIMD_FORCE_INLINE void *operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); }   \
	SIMD_FORCE_INLINE void operator delete(void *ptr) { btAlignedFree(ptr); }                              \
	SIMD_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; }                                \
	SIMD_FORCE_INLINE void operator delete(void *, void *) {}                                              \
	SIMD_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \
	SIMD_FORCE_INLINE void operator delete[](void *ptr) { btAlignedFree(ptr); }                            \
	SIMD_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; }                              \
	SIMD_FORCE_INLINE void operator delete[](void *, void *) {}

#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)

	SIMD_FORCE_INLINE btScalar btSqrt(btScalar x)
	{
		return sqrt(x);
	}
	SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }
	SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); }
	SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); }
	SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); }
	SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
	{
		if (x < btScalar(-1)) x = btScalar(-1);
		if (x > btScalar(1)) x = btScalar(1);
		return acos(x);
	}
	SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
	{
		if (x < btScalar(-1)) x = btScalar(-1);
		if (x > btScalar(1)) x = btScalar(1);
		return asin(x);
	}
	SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); }
	SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); }
	SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
	SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
	SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return pow(x, y); }
	SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmod(x, y); }

#else//BT_USE_DOUBLE_PRECISION

	SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
	{
	#ifdef USE_APPROXIMATION
	#ifdef __LP64__
		float xhalf = 0.5f * y;
		int i = *(int *)&y;
		i = 0x5f375a86 - (i >> 1);
		y = *(float *)&i;
		y = y * (1.5f - xhalf * y * y);
		y = y * (1.5f - xhalf * y * y);
		y = y * (1.5f - xhalf * y * y);
		y = 1 / y;
		return y;
	#else
		double x, z, tempf;
		unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
		tempf = y;
		*tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
		x = tempf;
		z = y * btScalar(0.5);
		x = (btScalar(1.5) * x) - (x * x) * (x * z); /* iteration formula     */
		x = (btScalar(1.5) * x) - (x * x) * (x * z);
		x = (btScalar(1.5) * x) - (x * x) * (x * z);
		x = (btScalar(1.5) * x) - (x * x) * (x * z);
		x = (btScalar(1.5) * x) - (x * x) * (x * z);
		return x * y;
	#endif
	#else
		return sqrtf(y);
	#endif
	}
	SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }
	SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); }
	SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); }
	SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); }
	SIMD_FORCE_INLINE btScalar btAcos(btScalar x)
	{
		if (x < btScalar(-1))
			x = btScalar(-1);
		if (x > btScalar(1))
			x = btScalar(1);
		return acosf(x);
	}
	SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
	{
		if (x < btScalar(-1))
			x = btScalar(-1);
		if (x > btScalar(1))
			x = btScalar(1);
		return asinf(x);
	}
	SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
	SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
	SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
	SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
	SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return powf(x, y); }
	SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmodf(x, y); }

#endif//BT_USE_DOUBLE_PRECISION

#define SIMD_PI btScalar(3.1415926535897932384626433832795029)
#define SIMD_2_PI (btScalar(2.0) * SIMD_PI)
#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5))
#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
#define btRecipSqrt(x) ((btScalar)(btScalar(1.0) / btSqrt(btScalar(x)))) /* reciprocal square root */
#define btRecip(x) (btScalar(1.0) / btScalar(x))

#ifdef BT_USE_DOUBLE_PRECISION
	#define SIMD_EPSILON DBL_EPSILON
	#define SIMD_INFINITY DBL_MAX
	#define BT_ONE 1.0
	#define BT_ZERO 0.0
	#define BT_TWO 2.0
	#define BT_HALF 0.5
#else
	#define SIMD_EPSILON FLT_EPSILON
	#define SIMD_INFINITY FLT_MAX
	#define BT_ONE 1.0f
	#define BT_ZERO 0.0f
	#define BT_TWO 2.0f
	#define BT_HALF 0.5f
#endif

// clang-format on

SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
{
	btScalar coeff_1 = SIMD_PI / 4.0f;
	btScalar coeff_2 = 3.0f * coeff_1;
	btScalar abs_y = btFabs(y);
	btScalar angle;
	if (x >= 0.0f)
	{
		btScalar r = (x - abs_y) / (x + abs_y);
		angle = coeff_1 - coeff_1 * r;
	}
	else
	{
		btScalar r = (x + abs_y) / (abs_y - x);
		angle = coeff_2 - coeff_1 * r;
	}
	return (y < 0.0f) ? -angle : angle;
}

SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }

SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps)
{
	return (((a) <= eps) && !((a) < -eps));
}
SIMD_FORCE_INLINE bool btGreaterEqual(btScalar a, btScalar eps)
{
	return (!((a) <= eps));
}

SIMD_FORCE_INLINE int btIsNegative(btScalar x)
{
	return x < btScalar(0.0) ? 1 : 0;
}

SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }

#define BT_DECLARE_HANDLE(name) \
	typedef struct name##__     \
	{                           \
		int unused;             \
	} * name

#ifndef btFsel
SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c)
{
	return a >= 0 ? b : c;
}
#endif
#define btFsels(a, b, c) (btScalar) btFsel(a, b, c)

SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
{
	long int i = 1;
	const char *p = (const char *)&i;
	if (p[0] == 1)  // Lowest address contains the least significant byte
		return true;
	else
		return false;
}

///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
{
	// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
	// Rely on positive value or'ed with its negative having sign bit on
	// and zero value or'ed with its negative (which is still zero) having sign bit off
	// Use arithmetic shift right, shifting the sign bit through all 32 bits
	unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
	unsigned testEqz = ~testNz;
	return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
{
	unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
	unsigned testEqz = ~testNz;
	return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
{
#ifdef BT_HAVE_NATIVE_FSEL
	return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
#else
	return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
#endif
}

template <typename T>
SIMD_FORCE_INLINE void btSwap(T &a, T &b)
{
	T tmp = a;
	a = b;
	b = tmp;
}

//PCK: endian swapping functions
SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
{
	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
}

SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
{
	return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
}

SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
{
	return btSwapEndian((unsigned)val);
}

SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
{
	return btSwapEndian((unsigned short)val);
}

///btSwapFloat uses using char pointers to swap the endianness
////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
///so instead of returning a float/double, we return integer/long long integer
SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
{
	unsigned int a = 0;
	unsigned char *dst = (unsigned char *)&a;
	unsigned char *src = (unsigned char *)&d;

	dst[0] = src[3];
	dst[1] = src[2];
	dst[2] = src[1];
	dst[3] = src[0];
	return a;
}

// unswap using char pointers
SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
{
	float d = 0.0f;
	unsigned char *src = (unsigned char *)&a;
	unsigned char *dst = (unsigned char *)&d;

	dst[0] = src[3];
	dst[1] = src[2];
	dst[2] = src[1];
	dst[3] = src[0];

	return d;
}

// swap using char pointers
SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char *dst)
{
	unsigned char *src = (unsigned char *)&d;

	dst[0] = src[7];
	dst[1] = src[6];
	dst[2] = src[5];
	dst[3] = src[4];
	dst[4] = src[3];
	dst[5] = src[2];
	dst[6] = src[1];
	dst[7] = src[0];
}

// unswap using char pointers
SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
{
	double d = 0.0;
	unsigned char *dst = (unsigned char *)&d;

	dst[0] = src[7];
	dst[1] = src[6];
	dst[2] = src[5];
	dst[3] = src[4];
	dst[4] = src[3];
	dst[5] = src[2];
	dst[6] = src[1];
	dst[7] = src[0];

	return d;
}

template <typename T>
SIMD_FORCE_INLINE void btSetZero(T *a, int n)
{
	T *acurr = a;
	size_t ncurr = n;
	while (ncurr > 0)
	{
		*(acurr++) = 0;
		--ncurr;
	}
}

SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n)
{
	btScalar p0, q0, m0, p1, q1, m1, sum;
	sum = 0;
	n -= 2;
	while (n >= 0)
	{
		p0 = a[0];
		q0 = b[0];
		m0 = p0 * q0;
		p1 = a[1];
		q1 = b[1];
		m1 = p1 * q1;
		sum += m0;
		sum += m1;
		a += 2;
		b += 2;
		n -= 2;
	}
	n += 2;
	while (n > 0)
	{
		sum += (*a) * (*b);
		a++;
		b++;
		n--;
	}
	return sum;
}

// returns normalized value in range [-SIMD_PI, SIMD_PI]
SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
{
	angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
	if (angleInRadians < -SIMD_PI)
	{
		return angleInRadians + SIMD_2_PI;
	}
	else if (angleInRadians > SIMD_PI)
	{
		return angleInRadians - SIMD_2_PI;
	}
	else
	{
		return angleInRadians;
	}
}

///rudimentary class to provide type info
struct btTypedObject
{
	btTypedObject(int objectType)
		: m_objectType(objectType)
	{
	}
	int m_objectType;
	inline int getObjectType() const
	{
		return m_objectType;
	}
};

///align a pointer to the provided alignment, upwards
template <typename T>
T *btAlignPointer(T *unalignedPtr, size_t alignment)
{
	struct btConvertPointerSizeT
	{
		union {
			T *ptr;
			size_t integer;
		};
	};
	btConvertPointerSizeT converter;

	const size_t bit_mask = ~(alignment - 1);
	converter.ptr = unalignedPtr;
	converter.integer += alignment - 1;
	converter.integer &= bit_mask;
	return converter.ptr;
}

#endif  //BT_SCALAR_H


text:
/*
Copyright (c) 2003-2006 Gino van den Bergen / 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_GEN_RANDOM_H
#define BT_GEN_RANDOM_H

#ifdef MT19937

#include <limits.h>
#include <mt19937.h>

#define GEN_RAND_MAX UINT_MAX

SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { init_genrand(seed); }
SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return genrand_int32(); }

#else

#include <stdlib.h>

#define GEN_RAND_MAX RAND_MAX

SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { srand(seed); }
SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return rand(); }

#endif

#endif //BT_GEN_RANDOM_H


text:
/*
Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien

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.
*/

///These spatial algebra classes are used for btMultiBody,
///see BulletDynamics/Featherstone

#ifndef BT_SPATIAL_ALGEBRA_H
#define BT_SPATIAL_ALGEBRA_H

#include "btMatrix3x3.h"

struct btSpatialForceVector
{
	btVector3 m_topVec, m_bottomVec;
	//
	btSpatialForceVector() { setZero(); }
	btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
	btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
	{
		setValue(ax, ay, az, lx, ly, lz);
	}
	//
	void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
	void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
	{
		m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz);
	}
	//
	void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
	void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
	{
		m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az;
		m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz;
	}
	//
	const btVector3 & getLinear()  const { return m_topVec; }
	const btVector3 & getAngular() const { return m_bottomVec; }
	//
	void setLinear(const btVector3 &linear) { m_topVec = linear; }
	void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
	//
	void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
	void addLinear(const btVector3 &linear) { m_topVec += linear; }
	//
	void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
	//
	btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
	btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
	btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
	btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
	btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
	btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
	//btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
};

struct btSpatialMotionVector
{
	btVector3 m_topVec, m_bottomVec;
	//
	btSpatialMotionVector() { setZero(); }
	btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
	//
	void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
	void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
	{
		m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz);
	}
	//
	void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
	void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
	{
		m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az;
		m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz;
	}
	//
	const btVector3 & getAngular() const { return m_topVec; }
	const btVector3 & getLinear() const { return m_bottomVec; }
	//
	void setAngular(const btVector3 &angular) { m_topVec = angular; }
	void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
	//
	void addAngular(const btVector3 &angular) { m_topVec += angular; }
	void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
	//
	void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
	//
	btScalar dot(const btSpatialForceVector &b) const
	{
		return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
	}
	//
	template<typename SpatialVectorType>
	void cross(const SpatialVectorType &b, SpatialVectorType &out) const
	{
		out.m_topVec = m_topVec.cross(b.m_topVec);
		out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
	}
	template<typename SpatialVectorType>
	SpatialVectorType cross(const SpatialVectorType &b) const
	{
		SpatialVectorType out;
		out.m_topVec = m_topVec.cross(b.m_topVec);
		out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
		return out;
	}
	//
	btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
	btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
	btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
	btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
	btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
	btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
	btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
};

struct btSymmetricSpatialDyad
{
	btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
	//
	btSymmetricSpatialDyad() { setIdentity(); }
	btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
	//
	void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
	{
		m_topLeftMat = topLeftMat;
		m_topRightMat = topRightMat;
		m_bottomLeftMat = bottomLeftMat;
	}
	//
	void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
	{
		m_topLeftMat += topLeftMat;
		m_topRightMat += topRightMat;
		m_bottomLeftMat += bottomLeftMat;
	}
	//
	void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity();  }
	//
	btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
	{
		m_topLeftMat -= mat.m_topLeftMat;
		m_topRightMat -= mat.m_topRightMat;
		m_bottomLeftMat -= mat.m_bottomLeftMat;
		return *this;
	}
	//
	btSpatialForceVector operator * (const btSpatialMotionVector &vec)
	{
		return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
	}
};

struct btSpatialTransformationMatrix
{
	btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
	btVector3 m_trnVec;
	//
	enum eOutputOperation
	{
		None = 0,
		Add = 1,
		Subtract = 2
	};
	//
	template<typename SpatialVectorType>
	void transform(	const SpatialVectorType &inVec,
                      SpatialVectorType &outVec,
					eOutputOperation outOp = None)
	{
		if(outOp == None)
		{
			outVec.m_topVec = m_rotMat * inVec.m_topVec;
			outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
		}
		else if(outOp == Add)
		{
			outVec.m_topVec += m_rotMat * inVec.m_topVec;
			outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
		}
		else if(outOp == Subtract)
		{
			outVec.m_topVec -= m_rotMat * inVec.m_topVec;
			outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
		}

	}

	template<typename SpatialVectorType>
	void transformRotationOnly(	const SpatialVectorType &inVec,
								SpatialVectorType &outVec,
								eOutputOperation outOp = None)
	{
		if(outOp == None)
		{
			outVec.m_topVec = m_rotMat * inVec.m_topVec;
			outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
		}
		else if(outOp == Add)
		{
			outVec.m_topVec += m_rotMat * inVec.m_topVec;
			outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
		}
		else if(outOp == Subtract)
		{
			outVec.m_topVec -= m_rotMat * inVec.m_topVec;
			outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
		}

	}

	template<typename SpatialVectorType>
	void transformInverse(	const SpatialVectorType &inVec,
							SpatialVectorType &outVec,
							eOutputOperation outOp = None)
	{
		if(outOp == None)
		{
			outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
			outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
		}
		else if(outOp == Add)
		{
			outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
			outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
		}
		else if(outOp == Subtract)
		{
			outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
			outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
		}
	}

	template<typename SpatialVectorType>
	void transformInverseRotationOnly(	const SpatialVectorType &inVec,
										SpatialVectorType &outVec,
										eOutputOperation outOp = None)
	{
		if(outOp == None)
		{
			outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
			outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
		}
		else if(outOp == Add)
		{
			outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
			outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
		}
		else if(outOp == Subtract)
		{
			outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
			outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
		}

	}

	void transformInverse(	const btSymmetricSpatialDyad &inMat,
							btSymmetricSpatialDyad &outMat,
							eOutputOperation outOp = None)
	{
		const btMatrix3x3 r_cross(	0, -m_trnVec[2], m_trnVec[1],
								m_trnVec[2], 0, -m_trnVec[0],
								-m_trnVec[1], m_trnVec[0], 0);

		if(outOp == None)
		{
			outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
			outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
			outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
		}
		else if(outOp == Add)
		{
			outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
			outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
			outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
		}
		else if(outOp == Subtract)
		{
			outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
			outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
			outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
		}
	}

	template<typename SpatialVectorType>
	SpatialVectorType operator * (const SpatialVectorType &vec)
	{
		SpatialVectorType out;
		transform(vec, out);
		return out;
	}
};

template<typename SpatialVectorType>
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
{
	//output op maybe?

	out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
	out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
	out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
	//maybe simple a*spatTranspose(a) would be nicer?
}

template<typename SpatialVectorType>
btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
{
	btSymmetricSpatialDyad out;

	out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
	out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
	out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);

	return out;
	//maybe simple a*spatTranspose(a) would be nicer?
}

#endif //BT_SPATIAL_ALGEBRA_H


text:
#ifndef BT_CPU_UTILITY_H
#define BT_CPU_UTILITY_H

#include "LinearMath/btScalar.h"

#include <string.h>//memset
#ifdef  USE_SIMD
#include <emmintrin.h>
#ifdef BT_ALLOW_SSE4
#include <intrin.h>
#endif //BT_ALLOW_SSE4
#endif //USE_SIMD

#if defined BT_USE_NEON
#define ARM_NEON_GCC_COMPATIBILITY  1
#include <arm_neon.h>
#include <sys/types.h>
#include <sys/sysctl.h> //for sysctlbyname
#endif //BT_USE_NEON

///Rudimentary btCpuFeatureUtility for CPU features: only report the features that Bullet actually uses (SSE4/FMA3, NEON_HPFP)
///We assume SSE2 in case BT_USE_SSE2 is defined in LinearMath/btScalar.h
class btCpuFeatureUtility
{
public:
	enum btCpuFeature
	{
		CPU_FEATURE_FMA3=1,
		CPU_FEATURE_SSE4_1=2,
		CPU_FEATURE_NEON_HPFP=4
	};

	static int getCpuFeatures()
	{

		static int capabilities = 0;
		static bool testedCapabilities = false;
		if (0 != testedCapabilities)
		{
			return capabilities;
		}

#ifdef BT_USE_NEON
		{
			uint32_t hasFeature = 0;
			size_t featureSize = sizeof(hasFeature);
			int err = sysctlbyname("hw.optional.neon_hpfp", &hasFeature, &featureSize, NULL, 0);
			if (0 == err && hasFeature)
				capabilities |= CPU_FEATURE_NEON_HPFP;
		}
#endif //BT_USE_NEON

#ifdef  BT_ALLOW_SSE4
		{
			int					cpuInfo[4];
			memset(cpuInfo, 0, sizeof(cpuInfo));
			unsigned long long	sseExt = 0;
			__cpuid(cpuInfo, 1);

			bool osUsesXSAVE_XRSTORE = cpuInfo[2] & (1 << 27) || false;
			bool cpuAVXSuport = cpuInfo[2] & (1 << 28) || false;

			if (osUsesXSAVE_XRSTORE && cpuAVXSuport)
			{
				sseExt = _xgetbv(0);
			}
			const int OSXSAVEFlag = (1UL << 27);
			const int AVXFlag = ((1UL << 28) | OSXSAVEFlag);
			const int FMAFlag = ((1UL << 12) | AVXFlag | OSXSAVEFlag);
			if ((cpuInfo[2] & FMAFlag) == FMAFlag && (sseExt & 6) == 6)
			{
				capabilities |= btCpuFeatureUtility::CPU_FEATURE_FMA3;
			}

			const int SSE41Flag = (1 << 19);
			if (cpuInfo[2] & SSE41Flag)
			{
				capabilities |= btCpuFeatureUtility::CPU_FEATURE_SSE4_1;
			}
		}
#endif//BT_ALLOW_SSE4

		testedCapabilities = true;
		return capabilities;
	}

};

#endif //BT_CPU_UTILITY_H


text:
/*
Copyright (c) 2003-2006 Gino van den Bergen / 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_GEOMETRY_UTIL_H
#define BT_GEOMETRY_UTIL_H

#include "btVector3.h"
#include "btAlignedObjectArray.h"

///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices.
class btGeometryUtil
{
	public:

		static void	getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );

		static void	getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );

		static bool	isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar	margin);

		static bool	isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar	margin);

		static bool	areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar	margin);

};

#endif //BT_GEOMETRY_UTIL_H