make sure aligned structures/classes use the btAlignedAlloc/btAlignedFree, by overriding the operator new/delete for that struct/class.

integrated some contributions from IBM Germany for libspe2

src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

@@ -213,7 +213,8 @@ m_handleSentinel(handleSentinel)
 {
 	if (!m_pairCache)
 	{
-		m_pairCache = new btOverlappingPairCache();
+		void* ptr = btAlignedAlloc(sizeof(btOverlappingPairCache),16);
+		m_pairCache = new(ptr) btOverlappingPairCache();
 		m_ownsPairCache = true;
 	}
 
@@ -230,7 +231,8 @@ m_handleSentinel(handleSentinel)
 	m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
 
 	// allocate handles buffer and put all handles on free list
-	m_pHandles = new Handle[maxHandles];
+	void* ptr = btAlignedAlloc(sizeof(Handle)*maxHandles,16);
+	m_pHandles = new(ptr) Handle[maxHandles];
 	m_maxHandles = maxHandles;
 	m_numHandles = 0;
 
@@ -245,7 +247,10 @@ m_handleSentinel(handleSentinel)
 	{
 		// allocate edge buffers
 		for (int i = 0; i < 3; i++)
-		m_pEdges[i] = new Edge[maxHandles * 2];
+		{
+			void* ptr = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16);
+			m_pEdges[i] = new(ptr) Edge[maxHandles * 2];
+		}
 	}
 	//removed overlap management
 
@@ -275,12 +280,14 @@ btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
 {
 	
 	for (int i = 2; i >= 0; i--)
-		delete[] m_pEdges[i];
-	delete[] m_pHandles;
+	{
+		btAlignedFree(m_pEdges[i]);
+	}
+	btAlignedFree(m_pHandles);
 
 	if (m_ownsPairCache)
 	{
-		delete m_pairCache;
+		btAlignedFree(m_pairCache);
 	}
 }
 

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -118,63 +118,18 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
 {
 	
-#ifdef USE_DISPATCH_REGISTRY_ARRAY
-	
 	btCollisionAlgorithmConstructionInfo ci;
 
 	ci.m_dispatcher1 = this;
 	ci.m_manifold = sharedManifold;
-	btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]
-	->CreateCollisionAlgorithm(ci,body0,body1);
-#else
-	btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1);
-#endif //USE_DISPATCH_REGISTRY_ARRAY
+	btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0,body1);
+
 	return algo;
 }
 
 
 
 
-#ifndef USE_DISPATCH_REGISTRY_ARRAY
-
-btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
-{
-	m_count++;
-	
-	btCollisionAlgorithmConstructionInfo ci;
-	ci.m_dispatcher = this;
-	
-	if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConvex() )
-	{
-		return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1);
-	}
-
-	if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConcave())
-	{
-		return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
-	}
-
-	if (body1->getCollisionShape()->isConvex() && body0->getCollisionShape()->isConcave())
-	{
-		return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
-	}
-
-	if (body0->getCollisionShape()->isCompound())
-	{
-		return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
-	} else
-	{
-		if (body1->getCollisionShape()->isCompound())
-		{
-			return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
-		}
-	}
-
-	//failed to find an algorithm
-	return new btEmptyAlgorithm(ci);
-	
-}
-#endif //USE_DISPATCH_REGISTRY_ARRAY
 
 bool	btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
 {

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -28,6 +28,7 @@ subject to the following restrictions:
 struct	btBroadphaseProxy;
 class	btCollisionShape;
 #include "LinearMath/btMotionState.h"
+#include "LinearMath/btAlignedAllocator.h"
 
 
 
@@ -89,6 +90,8 @@ protected:
 
 public:
 
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	enum CollisionFlags
 	{
 		CF_STATIC_OBJECT= 1,

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h

@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "btTriangleMeshShape.h"
 #include "btOptimizedBvh.h"
+#include "LinearMath/btAlignedAllocator.h"
 
 ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
 ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
@@ -31,6 +32,8 @@ ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape
 
 public:
 
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btBvhTriangleMeshShape() :btTriangleMeshShape(0),m_bvh(0),m_ownsBvh(false) {};
 	btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true);
 

src/BulletCollision/CollisionShapes/btCompoundShape.h

@@ -28,6 +28,8 @@ class btOptimizedBvh;
 
 ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btTransform			m_transform;
 	btCollisionShape*	m_childShape;
 	int					m_childShapeType;
@@ -47,6 +49,8 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape	: public btCollisionShape
 	btOptimizedBvh*					m_aabbTree;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btCompoundShape();
 
 	virtual ~btCompoundShape();

src/BulletCollision/CollisionShapes/btConvexHullShape.h

@@ -29,6 +29,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexShape
 	btAlignedObjectArray<btPoint3>	m_points;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	
 	///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory.

src/BulletCollision/CollisionShapes/btConvexShape.h

@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btMatrix3x3.h"
 #include "btCollisionMargin.h"
+#include "LinearMath/btAlignedAllocator.h"
 
 //todo: get rid of this btConvexCastResult thing!
 struct btConvexCastResult;
@@ -36,6 +37,8 @@ ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape
 
 public:
 
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	virtual ~btConvexShape()
 	{
 

src/BulletCollision/CollisionShapes/btOptimizedBvh.h

@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 
 #include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedAllocator.h"
 
 
 //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
@@ -34,6 +35,7 @@ class btStridingMeshInterface;
 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
 ATTRIBUTE_ALIGNED16	(struct) btQuantizedBvhNode
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	//12 bytes
 	unsigned short int	m_quantizedAabbMin[3];
@@ -63,6 +65,8 @@ ATTRIBUTE_ALIGNED16	(struct) btQuantizedBvhNode
 /// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
 ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	//32 bytes
 	btVector3	m_aabbMinOrg;
 	btVector3	m_aabbMaxOrg;
@@ -84,6 +88,8 @@ ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
 ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
 {
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	//12 bytes
 	unsigned short int	m_quantizedAabbMin[3];
 	unsigned short int	m_quantizedAabbMax[3];
@@ -145,6 +151,8 @@ ATTRIBUTE_ALIGNED16(class) btOptimizedBvh
 	btVector3			m_bvhAabbMax;
 	btVector3			m_bvhQuantization;
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	enum btTraversalMode
 	{
 		TRAVERSAL_STACKLESS = 0,

src/BulletCollision/CollisionShapes/btSphereShape.h

@@ -25,6 +25,8 @@ ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape
 {
 	
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btSphereShape (btScalar radius);
 	
 	

src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h

@@ -24,6 +24,8 @@ subject to the following restrictions:
 ///todo: explain with pictures
 ATTRIBUTE_ALIGNED16( struct)	btIndexedMesh
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	int			m_numTriangles;
 	const unsigned char *		m_triangleIndexBase;
 	int			m_triangleIndexStride;
@@ -49,6 +51,8 @@ ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshIn
 		
 public:
 
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btTriangleIndexVertexArray()
 	{
 	}

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
 #include "btManifoldPoint.h"
+#include "LinearMath/btAlignedAllocator.h"
 
 struct btCollisionResult;
 
@@ -55,6 +56,8 @@ ATTRIBUTE_ALIGNED16( class) btPersistentManifold
 
 public:
 
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	int m_index1;
 
 	btPersistentManifold();

src/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -20,12 +20,15 @@ class	btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btAlignedAllocator.h"
 
 
 
 ///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
 ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btVector3		m_centerOfMassPosition;
 	btVector3		m_linearVelocity;
 	btVector3		m_angularVelocity;

src/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -27,6 +27,8 @@ class	btRigidBody;
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
 ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btVector3	m_relpos1CrossNormal;
 	btVector3	m_relpos2CrossNormal;
 	btVector3	m_contactNormal;

src/LinearMath/btPoolAllocator.h

@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define _BT_POOL_ALLOCATOR_H
 
 #include "btScalar.h"
+#include "btAlignedAllocator.h"
 
 class btPoolAllocator
 {
@@ -32,7 +33,7 @@ public:
 		:m_elemSize(elemSize),
 		m_maxElements(maxElements)
 	{
-		m_pool = new unsigned char[m_elemSize*m_maxElements];
+		m_pool = (unsigned char*) btAlignedAlloc(m_elemSize*m_maxElements,16);
 
 		unsigned char* p = m_pool;
         m_firstFree = p;
@@ -47,7 +48,7 @@ public:
 
 	~btPoolAllocator()
 	{
-		delete m_pool;
+		btAlignedFree( m_pool);
 	}
 
 	void*	allocate(int size)

src/LinearMath/btScalar.h

@@ -25,11 +25,13 @@ subject to the following restrictions:
 
 
 
+
 #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_ALIGNED128(a) a
 		#else
 			#define BT_HAS_ALIGNED_ALOCATOR
 			#pragma warning(disable:4530)
@@ -37,6 +39,7 @@ subject to the following restrictions:
 			#pragma warning(disable:4786)
 			#define SIMD_FORCE_INLINE __forceinline
 			#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+			#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
 		#ifdef _XBOX
 			#define BT_USE_VMX128
 
@@ -52,29 +55,61 @@ subject to the following restrictions:
 		#define btAssert assert
 		//btFullAssert is optional, slows down a lot
 		#define btFullAssert(x)
+
+		#define btLikely(_c)  _c
+		#define btUnlikely(_c) _c
+
 #else
 	
 #if defined	(__CELLOS_LV2__)
 		#define SIMD_FORCE_INLINE inline
 		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
 		#ifndef assert
 		#include <assert.h>
 		#endif
 		#define btAssert assert
 		//btFullAssert is optional, slows down a lot
 		#define btFullAssert(x)
+
+		#define btLikely(_c)  _c
+		#define btUnlikely(_c) _c
+
 #else
 
+#ifdef USE_LIBSPE2
+
+		#define SIMD_FORCE_INLINE __inline
+		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+		#ifndef assert
+		#include <assert.h>
+		#endif
+		#define btAssert assert
+		//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
 	//non-windows systems
 
 		#define SIMD_FORCE_INLINE inline
 		#define ATTRIBUTE_ALIGNED16(a) a
+		#define ATTRIBUTE_ALIGNED128(a) a
 		#ifndef assert
 		#include <assert.h>
 		#endif
 		#define btAssert assert
 		//btFullAssert is optional, slows down a lot
 		#define btFullAssert(x)
+
+
+#endif // LIBSPE2
+
 #endif	//__CELLOS_LV2__
 #endif
 
@@ -94,6 +129,14 @@ typedef float btScalar;
 #endif
 
 
+#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*)		{ }	\
+
+
+
 #if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
 		
 SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }

src/LinearMath/btStackAlloc.h

@@ -21,6 +21,7 @@ Nov.2006
 #define BT_STACK_ALLOC
 
 #include "btScalar.h" //for btAssert
+#include "btAlignedAllocator.h"
 
 struct btBlock
 {
@@ -39,7 +40,7 @@ public:
 	inline void		create(unsigned int size)
 	{
 		destroy();
-		data		=	new unsigned char[size];
+		data		=  (unsigned char*) btAlignedAlloc(size,16);
 		totalsize	=	size;
 	}
 	inline void		destroy()
@@ -49,7 +50,9 @@ public:
 
 		if(usedsize==0)
 		{
-			if(!ischild)		delete[] data;
+			if(!ischild)		
+				btAlignedFree(data);
+
 			data				=	0;
 			usedsize			=	0;
 		}