bt->b3

src/Bullet3Geometry/b3AabbUtil.h

@@ -0,0 +1,232 @@
+/*
+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_AABB_UTIL2
+#define BT_AABB_UTIL2
+
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3MinMax.h"
+
+
+
+SIMD_FORCE_INLINE void AabbExpand (b3Vector3& aabbMin,
+								   b3Vector3& aabbMax,
+								   const b3Vector3& expansionMin,
+								   const b3Vector3& expansionMax)
+{
+	aabbMin = aabbMin + expansionMin;
+	aabbMax = aabbMax + expansionMax;
+}
+
+/// conservative test for overlap between two aabbs
+SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
+								const b3Vector3 &point)
+{
+	bool overlap = true;
+	overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
+	overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap;
+	overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap;
+	return overlap;
+}
+
+
+/// conservative test for overlap between two aabbs
+SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
+								const b3Vector3 &aabbMin2, const b3Vector3 &aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
+	overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
+	overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
+	return overlap;
+}
+
+/// conservative test for overlap between triangle and aabb
+SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const b3Vector3 *vertices,
+									const b3Vector3 &aabbMin, const b3Vector3 &aabbMax)
+{
+	const b3Vector3 &p1 = vertices[0];
+	const b3Vector3 &p2 = vertices[1];
+	const b3Vector3 &p3 = vertices[2];
+
+	if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
+	if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
+
+	if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
+	if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
+  
+	if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
+	if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
+	return true;
+}
+
+
+SIMD_FORCE_INLINE int	btOutcode(const b3Vector3& p,const b3Vector3& halfExtent) 
+{
+	return (p.getX()  < -halfExtent.getX() ? 0x01 : 0x0) |    
+		   (p.getX() >  halfExtent.getX() ? 0x08 : 0x0) |
+		   (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |    
+		   (p.getY() >  halfExtent.getY() ? 0x10 : 0x0) |
+		   (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |    
+		   (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
+}
+
+
+
+SIMD_FORCE_INLINE bool btRayAabb2(const b3Vector3& rayFrom,
+								  const b3Vector3& rayInvDirection,
+								  const unsigned int raySign[3],
+								  const b3Vector3 bounds[2],
+								  b3Scalar& tmin,
+								  b3Scalar lambda_min,
+								  b3Scalar lambda_max)
+{
+	b3Scalar tmax, tymin, tymax, tzmin, tzmax;
+	tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+	tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
+	tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+	tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
+
+	if ( (tmin > tymax) || (tymin > tmax) )
+		return false;
+
+	if (tymin > tmin)
+		tmin = tymin;
+
+	if (tymax < tmax)
+		tmax = tymax;
+
+	tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+	tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+
+	if ( (tmin > tzmax) || (tzmin > tmax) )
+		return false;
+	if (tzmin > tmin)
+		tmin = tzmin;
+	if (tzmax < tmax)
+		tmax = tzmax;
+	return ( (tmin < lambda_max) && (tmax > lambda_min) );
+}
+
+SIMD_FORCE_INLINE bool btRayAabb(const b3Vector3& rayFrom, 
+								 const b3Vector3& rayTo, 
+								 const b3Vector3& aabbMin, 
+								 const b3Vector3& aabbMax,
+					  b3Scalar& param, b3Vector3& normal) 
+{
+	b3Vector3 aabbHalfExtent = (aabbMax-aabbMin)* b3Scalar(0.5);
+	b3Vector3 aabbCenter = (aabbMax+aabbMin)* b3Scalar(0.5);
+	b3Vector3	source = rayFrom - aabbCenter;
+	b3Vector3	target = rayTo - aabbCenter;
+	int	sourceOutcode = btOutcode(source,aabbHalfExtent);
+	int targetOutcode = btOutcode(target,aabbHalfExtent);
+	if ((sourceOutcode & targetOutcode) == 0x0)
+	{
+		b3Scalar lambda_enter = b3Scalar(0.0);
+		b3Scalar lambda_exit  = param;
+		b3Vector3 r = target - source;
+		int i;
+		b3Scalar	normSign = 1;
+		b3Vector3	hitNormal(0,0,0);
+		int bit=1;
+
+		for (int j=0;j<2;j++)
+		{
+			for (i = 0; i != 3; ++i)
+			{
+				if (sourceOutcode & bit)
+				{
+					b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					if (lambda_enter <= lambda)
+					{
+						lambda_enter = lambda;
+						hitNormal.setValue(0,0,0);
+						hitNormal[i] = normSign;
+					}
+				}
+				else if (targetOutcode & bit) 
+				{
+					b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					btSetMin(lambda_exit, lambda);
+				}
+				bit<<=1;
+			}
+			normSign = b3Scalar(-1.);
+		}
+		if (lambda_enter <= lambda_exit)
+		{
+			param = lambda_enter;
+			normal = hitNormal;
+			return true;
+		}
+	}
+	return false;
+}
+
+
+
+SIMD_FORCE_INLINE	void btTransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
+{
+	b3Vector3 halfExtentsWithMargin = halfExtents+b3Vector3(margin,margin,margin);
+	b3Matrix3x3 abs_b = t.getBasis().absolute();  
+	b3Vector3 center = t.getOrigin();
+    b3Vector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
+	aabbMinOut = center - extent;
+	aabbMaxOut = center + extent;
+}
+
+
+SIMD_FORCE_INLINE	void btTransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
+{
+		btAssert(localAabbMin.getX() <= localAabbMax.getX());
+		btAssert(localAabbMin.getY() <= localAabbMax.getY());
+		btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
+		b3Vector3 localHalfExtents = b3Scalar(0.5)*(localAabbMax-localAabbMin);
+		localHalfExtents+=b3Vector3(margin,margin,margin);
+
+		b3Vector3 localCenter = b3Scalar(0.5)*(localAabbMax+localAabbMin);
+		b3Matrix3x3 abs_b = trans.getBasis().absolute();  
+		b3Vector3 center = trans(localCenter);
+        b3Vector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
+		aabbMinOut = center-extent;
+		aabbMaxOut = center+extent;
+}
+
+#define USE_BANCHLESS 1
+#ifdef USE_BANCHLESS
+	//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
+	SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
+	{		
+		return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
+			& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
+			& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
+			1, 0));
+	}
+#else
+	SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
+	{
+		bool overlap = true;
+		overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+		overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+		overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+		return overlap;
+	}
+#endif //USE_BANCHLESS
+
+#endif //BT_AABB_UTIL2
+
+

src/Bullet3Geometry/b3ConvexHullComputer.h

@@ -0,0 +1,103 @@
+/*
+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 "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.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 b3ConvexHullComputer
+{
+	private:
+		b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
+
+	public:
+
+		class Edge
+		{
+			private:
+				int next;
+				int reverse;
+				int targetVertex;
+
+				friend class b3ConvexHullComputer;
+
+			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
+		b3AlignedObjectArray<b3Vector3> vertices;
+
+		// Edges of the output hull
+		b3AlignedObjectArray<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
+		b3AlignedObjectArray<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".
+		*/
+		b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
+		{
+			return compute(coords, false, stride, count, shrink, shrinkClamp);
+		}
+
+		// same as above, but double precision
+		b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp)
+		{
+			return compute(coords, true, stride, count, shrink, shrinkClamp);
+		}
+};
+
+
+#endif //BT_CONVEX_HULL_COMPUTER_H
+

src/Bullet3Geometry/b3GeometryUtil.cpp

@@ -0,0 +1,185 @@
+/*
+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.
+*/
+
+
+
+#include "b3GeometryUtil.h"
+
+
+/*
+  Make sure this dummy function never changes so that it
+  can be used by probes that are checking whether the
+  library is actually installed.
+*/
+extern "C"
+{	
+	void btBulletMathProbe ();
+
+	void btBulletMathProbe () {}
+}
+
+
+bool	b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar	margin)
+{
+	int numbrushes = planeEquations.size();
+	for (int i=0;i<numbrushes;i++)
+	{
+		const b3Vector3& N1 = planeEquations[i];
+		b3Scalar dist = b3Scalar(N1.dot(point))+b3Scalar(N1[3])-margin;
+		if (dist>b3Scalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+		
+}
+
+
+bool	b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar	margin)
+{
+	int numvertices = vertices.size();
+	for (int i=0;i<numvertices;i++)
+	{
+		const b3Vector3& N1 = vertices[i];
+		b3Scalar dist = b3Scalar(planeNormal.dot(N1))+b3Scalar(planeNormal[3])-margin;
+		if (dist>b3Scalar(0.))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations);
+
+bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations)
+{
+	int numbrushes = planeEquations.size();
+	for (int i=0;i<numbrushes;i++)
+	{
+		const b3Vector3& N1 = planeEquations[i];
+		if (planeEquation.dot(N1) > b3Scalar(0.999))
+		{
+			return false;
+		} 
+	}
+	return true;
+}
+
+void	b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut )
+{
+		const int numvertices = vertices.size();
+	// brute force:
+	for (int i=0;i<numvertices;i++)
+	{
+		const b3Vector3& N1 = vertices[i];
+		
+
+		for (int j=i+1;j<numvertices;j++)
+		{
+			const b3Vector3& N2 = vertices[j];
+				
+			for (int k=j+1;k<numvertices;k++)
+			{
+
+				const b3Vector3& N3 = vertices[k];
+
+				b3Vector3 planeEquation,edge0,edge1;
+				edge0 = N2-N1;
+				edge1 = N3-N1;
+				b3Scalar normalSign = b3Scalar(1.);
+				for (int ww=0;ww<2;ww++)
+				{
+					planeEquation = normalSign * edge0.cross(edge1);
+					if (planeEquation.length2() > b3Scalar(0.0001))
+					{
+						planeEquation.normalize();
+						if (notExist(planeEquation,planeEquationsOut))
+						{
+							planeEquation[3] = -planeEquation.dot(N1);
+							
+								//check if inside, and replace supportingVertexOut if needed
+								if (areVerticesBehindPlane(planeEquation,vertices,b3Scalar(0.01)))
+								{
+									planeEquationsOut.push_back(planeEquation);
+								}
+						}
+					}
+					normalSign = b3Scalar(-1.);
+				}
+			
+			}
+		}
+	}
+
+}
+
+void	b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut )
+{
+	const int numbrushes = planeEquations.size();
+	// brute force:
+	for (int i=0;i<numbrushes;i++)
+	{
+		const b3Vector3& N1 = planeEquations[i];
+		
+
+		for (int j=i+1;j<numbrushes;j++)
+		{
+			const b3Vector3& N2 = planeEquations[j];
+				
+			for (int k=j+1;k<numbrushes;k++)
+			{
+
+				const b3Vector3& N3 = planeEquations[k];
+
+				b3Vector3 n2n3; n2n3 = N2.cross(N3);
+				b3Vector3 n3n1; n3n1 = N3.cross(N1);
+				b3Vector3 n1n2; n1n2 = N1.cross(N2);
+				
+				if ( ( n2n3.length2() > b3Scalar(0.0001) ) &&
+					 ( n3n1.length2() > b3Scalar(0.0001) ) &&
+					 ( n1n2.length2() > b3Scalar(0.0001) ) )
+				{
+					//point P out of 3 plane equations:
+
+					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
+					//P =  -------------------------------------------------------------------------  
+					//   N1 . ( N2 * N3 )  
+
+
+					b3Scalar quotient = (N1.dot(n2n3));
+					if (btFabs(quotient) > b3Scalar(0.000001))
+					{
+						quotient = b3Scalar(-1.) / quotient;
+						n2n3 *= N1[3];
+						n3n1 *= N2[3];
+						n1n2 *= N3[3];
+						b3Vector3 potentialVertex = n2n3;
+						potentialVertex += n3n1;
+						potentialVertex += n1n2;
+						potentialVertex *= quotient;
+
+						//check if inside, and replace supportingVertexOut if needed
+						if (isPointInsidePlanes(planeEquations,potentialVertex,b3Scalar(0.01)))
+						{
+							verticesOut.push_back(potentialVertex);
+						}
+					}
+				}
+			}
+		}
+	}
+}
+

src/Bullet3Geometry/b3GeometryUtil.h

@@ -0,0 +1,42 @@
+/*
+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 "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+///The b3GeometryUtil helper class provides a few methods to convert between plane equations and vertices.
+class b3GeometryUtil
+{
+	public:
+	
+	
+		static void	getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut );
+
+		static void	getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations , b3AlignedObjectArray<b3Vector3>& verticesOut );
+	
+		static bool	isInside(const b3AlignedObjectArray<b3Vector3>& vertices, const b3Vector3& planeNormal, b3Scalar	margin);
+		
+		static bool	isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar	margin);
+
+		static bool	areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar	margin);
+
+};
+
+
+#endif //BT_GEOMETRY_UTIL_H
+

src/Bullet3Geometry/b3GrahamScan2dConvexHull.h

@@ -0,0 +1,117 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2011 Advanced Micro Devices, Inc.  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 GRAHAM_SCAN_2D_CONVEX_HULL_H
+#define GRAHAM_SCAN_2D_CONVEX_HULL_H
+
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+struct GrahamVector3 : public b3Vector3
+{
+	GrahamVector3(const b3Vector3& org, int orgIndex)
+		:b3Vector3(org),
+			m_orgIndex(orgIndex)
+	{
+	}
+	b3Scalar	m_angle;
+	int m_orgIndex;
+};
+
+
+struct btAngleCompareFunc {
+	b3Vector3 m_anchor;
+	btAngleCompareFunc(const b3Vector3& anchor)
+	: m_anchor(anchor) 
+	{
+	}
+	bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
+		if (a.m_angle != b.m_angle)
+			return a.m_angle < b.m_angle;
+		else
+		{
+			b3Scalar al = (a-m_anchor).length2();
+			b3Scalar bl = (b-m_anchor).length2();
+			if (al != bl)
+				return  al < bl;
+			else
+			{
+				return a.m_orgIndex < b.m_orgIndex;
+			}
+		}
+	}
+};
+
+inline void GrahamScanConvexHull2D(b3AlignedObjectArray<GrahamVector3>& originalPoints, b3AlignedObjectArray<GrahamVector3>& hull, const b3Vector3& normalAxis)
+{
+	b3Vector3 axis0,axis1;
+	btPlaneSpace1(normalAxis,axis0,axis1);
+	
+
+	if (originalPoints.size()<=1)
+	{
+		for (int i=0;i<originalPoints.size();i++)
+			hull.push_back(originalPoints[0]);
+		return;
+	}
+	//step1 : find anchor point with smallest projection on axis0 and move it to first location
+	for (int i=0;i<originalPoints.size();i++)
+	{
+//		const b3Vector3& left = originalPoints[i];
+//		const b3Vector3& right = originalPoints[0];
+		b3Scalar projL = originalPoints[i].dot(axis0);
+		b3Scalar projR = originalPoints[0].dot(axis0);
+		if (projL < projR)
+		{
+			originalPoints.swap(0,i);
+		}
+	}
+
+	//also precompute angles
+	originalPoints[0].m_angle = -1e30f;
+	for (int i=1;i<originalPoints.size();i++)
+	{
+		b3Vector3 xvec = axis0;
+		b3Vector3 ar = originalPoints[i]-originalPoints[0];
+		originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length();
+	}
+
+	//step 2: sort all points, based on 'angle' with this anchor
+	btAngleCompareFunc comp(originalPoints[0]);
+	originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
+
+	int i;
+	for (i = 0; i<2; i++) 
+		hull.push_back(originalPoints[i]);
+
+	//step 3: keep all 'convex' points and discard concave points (using back tracking)
+	for (; i != originalPoints.size(); i++) 
+	{
+		bool isConvex = false;
+		while (!isConvex&& hull.size()>1) {
+			b3Vector3& a = hull[hull.size()-2];
+			b3Vector3& b = hull[hull.size()-1];
+			isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
+			if (!isConvex)
+				hull.pop_back();
+			else 
+				hull.push_back(originalPoints[i]);
+		}
+	}
+}
+
+#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H