Remove co-planar faces from convex hull, using 2d Graham scan

Improve SAT performance, by skipping back-facing features
Add assert in array class (probably fires in places)

Demos/OpenGL/DemoApplication.cpp

@@ -786,7 +786,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 
 
 								btVector3 pickPos = rayCallback.m_hitPointWorld;
-								printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
+								//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
 
 
 								btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -1260,7 +1260,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
 
 					btVector3 normalColor(1,1,0);
 					btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]);
-					getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
+					//getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor);
 					
 					
 				}

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -47,6 +47,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl
 	{
 		btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
 		const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
+		if (numOverlappingPairs)
+		{
 		btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
 		
 		for (int i=0;i<numOverlappingPairs;i++)
@@ -65,6 +67,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl
 		}
 		}
 	}
+}
 
 #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
 void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)

src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp

@@ -17,7 +17,6 @@ subject to the following restrictions:
 ///This file was written by Erwin Coumans
 ///Separating axis rest based on work from Pierre Terdiman, see
 ///And contact clipping based on work from Simon Hobbs
-#define TEST_INTERNAL_OBJECTS 1
 
 #include "btConvexPolyhedron.h"
 #include "LinearMath/btHashMap.h"
@@ -151,6 +150,7 @@ void	btConvexPolyhedron::initialize()
 		}
 	}
 
+#ifdef USE_CONNECTED_FACES
 	for(int i=0;i<m_faces.size();i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
@@ -169,6 +169,7 @@ void	btConvexPolyhedron::initialize()
 			m_faces[i].m_connectedFaces[j] = connectedFace;
 		}
 	}
+#endif//USE_CONNECTED_FACES
 
 	for(int i=0;i<m_faces.size();i++)
 	{

src/BulletCollision/CollisionShapes/btConvexPolyhedron.h

@@ -23,11 +23,13 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+#define TEST_INTERNAL_OBJECTS 1
+
 
 struct btFace
 {
 	btAlignedObjectArray<int>	m_indices;
-	btAlignedObjectArray<int>	m_connectedFaces;
+//	btAlignedObjectArray<int>	m_connectedFaces;
 	btScalar	m_plane[4];
 };
 

src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp

@@ -17,6 +17,9 @@ subject to the following restrictions:
 #include "btConvexPolyhedron.h"
 #include "LinearMath/btConvexHullComputer.h"
 #include <new>
+#include "LinearMath/btGeometryUtil.h"
+#include "LinearMath/btGrahamScan2dConvexHull.h"
+
 
 btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
 m_polyhedron(0)
@@ -32,34 +35,53 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape()
 	}
 }
 
+
 bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 {
+
 	if (m_polyhedron)
 		btAlignedFree(m_polyhedron);
 	
 	void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
 	m_polyhedron = new (mem) btConvexPolyhedron;
 
-	btAlignedObjectArray<btVector3> tmpVertices;
+		btAlignedObjectArray<btVector3> orgVertices;
+
 	for (int i=0;i<getNumVertices();i++)
 	{
-		btVector3& newVertex = tmpVertices.expand();
+		btVector3& newVertex = orgVertices.expand();
 		getVertex(i,newVertex);
 	}
 
+
+	btAlignedObjectArray<btVector3> planeEquations;
+	btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
+
+	btAlignedObjectArray<btVector3> shiftedPlaneEquations;
+	for (int p=0;p<planeEquations.size();p++)
+	{
+		   btVector3 plane = planeEquations[p];
+		   plane[3] -= getMargin();
+		   shiftedPlaneEquations.push_back(plane);
+	}
+
+	btAlignedObjectArray<btVector3> tmpVertices;
+
+	btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
+
+
 	btConvexHullComputer conv;
 	conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
 
-	
-
 	btAlignedObjectArray<btVector3> faceNormals;
 	int numFaces = conv.faces.size();
 	faceNormals.resize(numFaces);
 	btConvexHullComputer* convexUtil = &conv;
 
 	
+	btAlignedObjectArray<btFace>	tmpFaces;
+	tmpFaces.resize(numFaces);
 
-	m_polyhedron->m_faces.resize(numFaces);
 	int numVertices = convexUtil->vertices.size();
 	m_polyhedron->m_vertices.resize(numVertices);
 	for (int p=0;p<numVertices;p++)
@@ -67,6 +89,7 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 		m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
 	}
 
+
 	for (int i=0;i<numFaces;i++)
 	{
 		int face = convexUtil->faces[i];
@@ -85,7 +108,7 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 		{
 			
 			int src = edge->getSourceVertex();
-			m_polyhedron->m_faces[i].m_indices.push_back(src);
+			tmpFaces[i].m_indices.push_back(src);
 			int targ = edge->getTargetVertex();
 			btVector3 wa = convexUtil->vertices[src];
 
@@ -105,10 +128,10 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 		{
 			faceNormals[i] = edges[0].cross(edges[1]);
 			faceNormals[i].normalize();
-			m_polyhedron->m_faces[i].m_plane[0] = faceNormals[i].getX();
+			tmpFaces[i].m_plane[0] = faceNormals[i].getX();
-			m_polyhedron->m_faces[i].m_plane[1] = faceNormals[i].getY();
+			tmpFaces[i].m_plane[1] = faceNormals[i].getY();
-			m_polyhedron->m_faces[i].m_plane[2] = faceNormals[i].getZ();
+			tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
-			m_polyhedron->m_faces[i].m_plane[3] = planeEq;
+			tmpFaces[i].m_plane[3] = planeEq;
 
 		}
 		else
@@ -117,21 +140,108 @@ bool	btPolyhedralConvexShape::initializePolyhedralFeatures()
 			faceNormals[i].setZero();
 		}
 
-		for (int v=0;v<m_polyhedron->m_faces[i].m_indices.size();v++)
+		for (int v=0;v<tmpFaces[i].m_indices.size();v++)
 		{
-			btScalar eq = m_polyhedron->m_vertices[m_polyhedron->m_faces[i].m_indices[v]].dot(faceNormals[i]);
+			btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
 			if (planeEq>eq)
 			{
 				planeEq=eq;
 			}
 		}
-		m_polyhedron->m_faces[i].m_plane[3] = -planeEq;
+		tmpFaces[i].m_plane[3] = -planeEq;
+	}
+
+	//merge coplanar faces and copy them to m_polyhedron
+
+	btScalar faceWeldThreshold= 0.999f;
+	btAlignedObjectArray<int> todoFaces;
+	for (int i=0;i<tmpFaces.size();i++)
+		todoFaces.push_back(i);
+
+	while (todoFaces.size())
+	{
+		btAlignedObjectArray<int> coplanarFaceGroup;
+		int refFace = todoFaces[todoFaces.size()-1];
+
+		coplanarFaceGroup.push_back(refFace);
+		btFace& faceA = tmpFaces[refFace];
+		todoFaces.pop_back();
+
+		btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
+		for (int j=todoFaces.size()-1;j>=0;j--)
+		{
+			int i = todoFaces[j];
+			btFace& faceB = tmpFaces[i];
+			btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
+			if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
+			{
+				coplanarFaceGroup.push_back(i);
+				todoFaces.remove(i);
+			}
+		}
+
+
+		if (coplanarFaceGroup.size()>1)
+		{
+			//do the merge: use Graham Scan 2d convex hull
+
+			btAlignedObjectArray<GrahamVector2> orgpoints;
+
+			for (int i=0;i<coplanarFaceGroup.size();i++)
+			{
+//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
+
+				btFace& face = tmpFaces[coplanarFaceGroup[i]];
+				btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
+				btVector3 xyPlaneNormal(0,0,1);
+
+				btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal);
+				
+				for (int f=0;f<face.m_indices.size();f++)
+				{
+					int orgIndex = face.m_indices[f];
+					btVector3 pt = m_polyhedron->m_vertices[orgIndex];
+					btVector3 rotatedPt =  quatRotate(rotationArc,pt);
+					rotatedPt.setZ(0);
+					bool found = false;
+
+					for (int i=0;i<orgpoints.size();i++)
+					{
+						if ((rotatedPt-orgpoints[i]).length2()<0.001)
+						{
+							found=true;
+							break;
+						}
+					}
+					if (!found)
+						orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex));
+				}
+			}
+
+			btFace combinedFace;
+			for (int i=0;i<4;i++)
+				combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
+
+			btAlignedObjectArray<GrahamVector2> hull;
+			GrahamScanConvexHull2D(orgpoints,hull);
+
+			for (int i=0;i<hull.size();i++)
+			{
+				combinedFace.m_indices.push_back(hull[i].m_orgIndex);
+			}
+			m_polyhedron->m_faces.push_back(combinedFace);
+		} else
+		{
+			for (int i=0;i<coplanarFaceGroup.size();i++)
+			{
+				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
+			}
+
 		}
 
 
 
-
+	}
-	
 	
 	m_polyhedron->initialize();
 

src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp

@@ -75,7 +75,6 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
 		ds = de;
 	}
 }
-#include <stdio.h>
 
 
 static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth)
@@ -106,7 +105,6 @@ inline bool IsAlmostZero(const btVector3& v)
 	return true;
 }
 
-#define TEST_INTERNAL_OBJECTS 1
 #ifdef TEST_INTERNAL_OBJECTS
 
 inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
@@ -170,11 +168,11 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 {
 	gActualSATPairTests++;
 
-#ifdef TEST_INTERNAL_OBJECTS
+//#ifdef TEST_INTERNAL_OBJECTS
 	const btVector3 c0 = transA * hullA.m_localCenter;
 	const btVector3 c1 = transB * hullB.m_localCenter;
 	const btVector3 DeltaC2 = c0 - c1;
-#endif
+//#endif
 
 	btScalar dmin = FLT_MAX;
 	int curPlaneTests=0;
@@ -185,6 +183,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	{
 		const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
 		const btVector3 faceANormalWS = transA.getBasis() * Normal;
+		if (DeltaC2.dot(faceANormalWS)<0)
+			continue;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
@@ -211,6 +211,8 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 	{
 		const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
 		const btVector3 WorldNormal = transB.getBasis() * Normal;
+		if (DeltaC2.dot(WorldNormal)<0)
+			continue;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
@@ -249,6 +251,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 			if(!IsAlmostZero(Cross))
 			{
 				Cross = Cross.normalize();
+				if (DeltaC2.dot(Cross)<0)
+					continue;
+
 
 #ifdef TEST_INTERNAL_OBJECTS
 				gExpectedNbTests++;
@@ -311,18 +316,29 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 	int numVerticesA = polyA.m_indices.size();
 	for(int e0=0;e0<numVerticesA;e0++)
 	{
-		/*const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
+		const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
 		const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
 		const btVector3 edge0 = a - b;
 		const btVector3 WorldEdge0 = transA.getBasis() * edge0;
-		*/
+		btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
 
+		btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
+		btVector3 worldA1 = transA*a;
+		btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
+		
+//int otherFace=0;
+#ifdef BLA1
 		int otherFace = polyA.m_connectedFaces[e0];
 		btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
 		btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
 
 		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
 		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
+#else 
+		btVector3 planeNormalWS = planeNormalWS1;
+		btScalar planeEqWS=planeEqWS1;
+		
+#endif
 		//clip face
 
 		clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
@@ -380,19 +396,24 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 
 }
 
-void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+
+void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
 
+	btVector3 separatingNormal = separatingNormal1.normalized();
+	const btVector3 c0 = transA * hullA.m_localCenter;
+	const btVector3 c1 = transB * hullB.m_localCenter;
+	const btVector3 DeltaC2 = c0 - c1;
+
+
 	btScalar curMaxDist=maxDist;
 	int closestFaceB=-1;
-
-	{
 	btScalar dmax = -FLT_MAX;
+	{
 		for(int face=0;face<hullB.m_faces.size();face++)
 		{
 			const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
 			const btVector3 WorldNormal = transB.getBasis() * Normal;
-
 			btScalar d = WorldNormal.dot(separatingNormal);
 			if (d > dmax)
 			{
@@ -401,17 +422,6 @@ void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
 			}
 		}
 	}
-
-
-
-	if (closestFaceB<0)
-	{
-		return;
-	}
-
-
-
-	// setup initial clip face (minimizing face from hull B)
 				btVertexArray worldVertsB1;
 				{
 					const btFace& polyB = hullB.m_faces[closestFaceB];
@@ -423,6 +433,8 @@ void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
 					}
 				}
 
+	
+	if (closestFaceB>=0)
 		clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
 
 }

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -675,7 +675,12 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		{
 			if (m_manifolds.size() + m_constraints.size()>0)
 			{
-				m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+
+				btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
+				btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
+				btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
+				
+				m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
 			}
 			m_bodies.resize(0);
 			m_manifolds.resize(0);

src/LinearMath/btAlignedObjectArray.h

@@ -140,21 +140,29 @@ class btAlignedObjectArray
 		
 		SIMD_FORCE_INLINE const T& at(int n) const
 		{
+			btAssert(n>=0);
+			btAssert(n<size());
 			return m_data[n];
 		}
 
 		SIMD_FORCE_INLINE T& at(int n)
 		{
+			btAssert(n>=0);
+			btAssert(n<size());
 			return m_data[n];
 		}
 
 		SIMD_FORCE_INLINE const T& operator[](int n) const
 		{
+			btAssert(n>=0);
+			btAssert(n<size());
 			return m_data[n];
 		}
 
 		SIMD_FORCE_INLINE T& operator[](int n)
 		{
+			btAssert(n>=0);
+			btAssert(n<size());
 			return m_data[n];
 		}
 		
@@ -171,6 +179,7 @@ class btAlignedObjectArray
 
 		SIMD_FORCE_INLINE	void	pop_back()
 		{
+			btAssert(m_size>0);
 			m_size--;
 			m_data[m_size].~T();
 		}

src/LinearMath/btGrahamScan2dConvexHull.h

@@ -0,0 +1,110 @@
+/*
+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 "btVector3.h"
+#include "btAlignedObjectArray.h"
+
+struct GrahamVector2 : public btVector3
+{
+	GrahamVector2(const btVector3& org, int orgIndex)
+		:btVector3(org),
+			m_orgIndex(orgIndex)
+	{
+	}
+	btScalar	m_angle;
+	int m_orgIndex;
+};
+
+
+struct btAngleCompareFunc {
+	btVector3 m_anchor;
+	btAngleCompareFunc(const btVector3& anchor)
+	: m_anchor(anchor) 
+	{
+	}
+	bool operator()(const GrahamVector2& a, const GrahamVector2& b) {
+		if (a.m_angle != b.m_angle)
+			return a.m_angle < b.m_angle;
+		else
+		{
+			btScalar al = (a-m_anchor).length2();
+			btScalar bl = (b-m_anchor).length2();
+			if (al != bl)
+				return  al < bl;
+			else
+			{
+				return a.uid < b.uid;
+			}
+		}
+	}
+};
+
+inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& originalPoints, btAlignedObjectArray<btVector3>& hull)
+{
+	if (originalPoints.size()<=1)
+	{
+		for (int i=0;i<originalPoints.size();i++)
+			hull.push_back(originalPoints[0]);
+		return;
+	}
+	//step1 : find anchor point with smallest x/y and move it to first location
+	//also precompute angles
+	for (int i=0;i<originalPoints.size();i++)
+	{
+		const btVector3& left = originalPoints[i];
+		const btVector3& right = originalPoints[0];
+		if (left.x() < right.x() || !(right.x() < left.x()) && left.y() < right.y())
+		{
+			originalPoints.swap(0,i);
+		}
+	}
+
+	for (int i=0;i<originalPoints.size();i++)
+	{
+		btVector3 xvec(1,0,0);
+		btVector3 ar = originalPoints[i]-originalPoints[0];
+		originalPoints[i].m_angle = btCross(xvec, ar).dot(btVector3(0,0,1)) / 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) {
+			btVector3& a = hull[hull.size()-2];
+			btVector3& b = hull[hull.size()-1];
+			isConvex = btCross(a-b,a-originalPoints[i]).dot(btVector3(0,0,1))> 0;
+			if (!isConvex)
+				hull.pop_back();
+			else 
+				hull.push_back(originalPoints[i]);
+		}
+	}
+}
+
+#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H