Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

thirdparty/README.md

@@ -19,7 +19,7 @@ comments.
 ## bullet
 
 - Upstream: https://github.com/bulletphysics/bullet3
-- Version: git (12409f1118a7c7a266f9071350c70789dfe73bb9, Commits on Sep 6, 2018 )
+- Version: git (126b676, 2018-12-31)
 - License: zlib
 
 Files extracted from upstream source:

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h

@@ -19,22 +19,20 @@ subject to the following restrictions:
 #include "Bullet3Common/b3Vector3.h"
 struct b3BroadphaseProxy;
 
-
-struct	b3BroadphaseAabbCallback
+struct b3BroadphaseAabbCallback
 {
 	virtual ~b3BroadphaseAabbCallback() {}
-	virtual bool	process(const b3BroadphaseProxy* proxy) = 0;
+	virtual bool process(const b3BroadphaseProxy* proxy) = 0;
 };
 
-
-struct	b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
+struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
 {
 	///added some cached data to accelerate ray-AABB tests
-	b3Vector3		m_rayDirectionInverse;
-	unsigned int	m_signs[3];
-	b3Scalar		m_lambda_max;
+	b3Vector3 m_rayDirectionInverse;
+	unsigned int m_signs[3];
+	b3Scalar m_lambda_max;
 
 	virtual ~b3BroadphaseRayCallback() {}
 };
 
-#endif //B3_BROADPHASE_CALLBACK_H
+#endif  //B3_BROADPHASE_CALLBACK_H

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h

@@ -27,46 +27,43 @@ subject to the following restrictions:
 // Compile time config
 //
 
-#define	B3_DBVT_BP_PROFILE					0
+#define B3_DBVT_BP_PROFILE 0
 //#define B3_DBVT_BP_SORTPAIRS				1
-#define B3_DBVT_BP_PREVENTFALSEUPDATE		0
-#define B3_DBVT_BP_ACCURATESLEEPING		0
-#define B3_DBVT_BP_ENABLE_BENCHMARK		0
-#define B3_DBVT_BP_MARGIN					(b3Scalar)0.05
+#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
+#define B3_DBVT_BP_ACCURATESLEEPING 0
+#define B3_DBVT_BP_ENABLE_BENCHMARK 0
+#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
 
 #if B3_DBVT_BP_PROFILE
-#define	B3_DBVT_BP_PROFILING_RATE	256
+#define B3_DBVT_BP_PROFILING_RATE 256
 
 #endif
 
-
-
-
-B3_ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3BroadphaseProxy
 {
+	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-B3_DECLARE_ALIGNED_ALLOCATOR();
-	
 	///optional filtering to cull potential collisions
 	enum CollisionFilterGroups
 	{
-	        DefaultFilter = 1,
-	        StaticFilter = 2,
-	        KinematicFilter = 4,
-	        DebrisFilter = 8,
-			SensorTrigger = 16,
-			CharacterFilter = 32,
-	        AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+		DefaultFilter = 1,
+		StaticFilter = 2,
+		KinematicFilter = 4,
+		DebrisFilter = 8,
+		SensorTrigger = 16,
+		CharacterFilter = 32,
+		AllFilter = -1  //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
 	};
 
 	//Usually the client b3CollisionObject or Rigidbody class
-	void*	m_clientObject;
+	void* m_clientObject;
 	int m_collisionFilterGroup;
 	int m_collisionFilterMask;
-	int			m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+	int m_uniqueId;  //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
 
-	b3Vector3	m_aabbMin;
-	b3Vector3	m_aabbMax;
+	b3Vector3 m_aabbMin;
+	b3Vector3 m_aabbMax;
 
 	B3_FORCE_INLINE int getUid() const
 	{
@@ -74,116 +71,112 @@ B3_DECLARE_ALIGNED_ALLOCATOR();
 	}
 
 	//used for memory pools
-	b3BroadphaseProxy() :m_clientObject(0)
+	b3BroadphaseProxy() : m_clientObject(0)
 	{
 	}
 
-	b3BroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup,  int collisionFilterMask)
-		:m_clientObject(userPtr),
-		m_collisionFilterGroup(collisionFilterGroup),
-		m_collisionFilterMask(collisionFilterMask),
-		m_aabbMin(aabbMin),
-		m_aabbMax(aabbMax)
+	b3BroadphaseProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
+		: m_clientObject(userPtr),
+		  m_collisionFilterGroup(collisionFilterGroup),
+		  m_collisionFilterMask(collisionFilterMask),
+		  m_aabbMin(aabbMin),
+		  m_aabbMax(aabbMax)
 	{
 	}
 };
 
-
-
-
-
 //
 // b3DbvtProxy
 //
 struct b3DbvtProxy : b3BroadphaseProxy
 {
-	/* Fields		*/ 
+	/* Fields		*/
 	//b3DbvtAabbMm	aabb;
-	b3DbvtNode*		leaf;
-	b3DbvtProxy*	links[2];
-	int				stage;
-	/* ctor			*/ 
+	b3DbvtNode* leaf;
+	b3DbvtProxy* links[2];
+	int stage;
+	/* ctor			*/
 
 	explicit b3DbvtProxy() {}
-	b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup,  int collisionFilterMask) :
-	b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
+	b3DbvtProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : b3BroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
 	{
-		links[0]=links[1]=0;
+		links[0] = links[1] = 0;
 	}
 };
 
-typedef b3AlignedObjectArray<b3DbvtProxy*>	b3DbvtProxyArray;
+typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
 
 ///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
 ///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
 ///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3.
-struct	b3DynamicBvhBroadphase 
+struct b3DynamicBvhBroadphase
 {
-	/* Config		*/ 
-	enum	{
-		DYNAMIC_SET			=	0,	/* Dynamic set index	*/ 
-		FIXED_SET			=	1,	/* Fixed set index		*/ 
-		STAGECOUNT			=	2	/* Number of stages		*/ 
+	/* Config		*/
+	enum
+	{
+		DYNAMIC_SET = 0, /* Dynamic set index	*/
+		FIXED_SET = 1,   /* Fixed set index		*/
+		STAGECOUNT = 2   /* Number of stages		*/
 	};
-	/* Fields		*/ 
-	b3DynamicBvh					m_sets[2];					// Dbvt sets
-	b3DbvtProxy*			m_stageRoots[STAGECOUNT+1];	// Stages list
-
-	b3AlignedObjectArray<b3DbvtProxy>	m_proxies;
-	b3OverlappingPairCache*	m_paircache;				// Pair cache
-	b3Scalar				m_prediction;				// Velocity prediction
-	int						m_stageCurrent;				// Current stage
-	int						m_fupdates;					// % of fixed updates per frame
-	int						m_dupdates;					// % of dynamic updates per frame
-	int						m_cupdates;					// % of cleanup updates per frame
-	int						m_newpairs;					// Number of pairs created
-	int						m_fixedleft;				// Fixed optimization left
-	unsigned				m_updates_call;				// Number of updates call
-	unsigned				m_updates_done;				// Number of updates done
-	b3Scalar				m_updates_ratio;			// m_updates_done/m_updates_call
-	int						m_pid;						// Parse id
-	int						m_cid;						// Cleanup index
-	bool					m_releasepaircache;			// Release pair cache on delete
-	bool					m_deferedcollide;			// Defere dynamic/static collision to collide call
-	bool					m_needcleanup;				// Need to run cleanup?
+	/* Fields		*/
+	b3DynamicBvh m_sets[2];                     // Dbvt sets
+	b3DbvtProxy* m_stageRoots[STAGECOUNT + 1];  // Stages list
+
+	b3AlignedObjectArray<b3DbvtProxy> m_proxies;
+	b3OverlappingPairCache* m_paircache;  // Pair cache
+	b3Scalar m_prediction;                // Velocity prediction
+	int m_stageCurrent;                   // Current stage
+	int m_fupdates;                       // % of fixed updates per frame
+	int m_dupdates;                       // % of dynamic updates per frame
+	int m_cupdates;                       // % of cleanup updates per frame
+	int m_newpairs;                       // Number of pairs created
+	int m_fixedleft;                      // Fixed optimization left
+	unsigned m_updates_call;              // Number of updates call
+	unsigned m_updates_done;              // Number of updates done
+	b3Scalar m_updates_ratio;             // m_updates_done/m_updates_call
+	int m_pid;                            // Parse id
+	int m_cid;                            // Cleanup index
+	bool m_releasepaircache;              // Release pair cache on delete
+	bool m_deferedcollide;                // Defere dynamic/static collision to collide call
+	bool m_needcleanup;                   // Need to run cleanup?
 #if B3_DBVT_BP_PROFILE
-	b3Clock					m_clock;
-	struct	{
-		unsigned long		m_total;
-		unsigned long		m_ddcollide;
-		unsigned long		m_fdcollide;
-		unsigned long		m_cleanup;
-		unsigned long		m_jobcount;
-	}				m_profiling;
+	b3Clock m_clock;
+	struct
+	{
+		unsigned long m_total;
+		unsigned long m_ddcollide;
+		unsigned long m_fdcollide;
+		unsigned long m_cleanup;
+		unsigned long m_jobcount;
+	} m_profiling;
 #endif
-	/* Methods		*/ 
-	b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0);
+	/* Methods		*/
+	b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache = 0);
 	virtual ~b3DynamicBvhBroadphase();
-	void							collide(b3Dispatcher* dispatcher);
-	void							optimize();
-	
+	void collide(b3Dispatcher* dispatcher);
+	void optimize();
+
 	/* b3BroadphaseInterface Implementation	*/
-	b3BroadphaseProxy*				createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr, int collisionFilterGroup, int collisionFilterMask);
-	virtual void					destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
-	virtual void					setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
-	virtual void					rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0));
-	virtual void					aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
+	b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int objectIndex, void* userPtr, int collisionFilterGroup, int collisionFilterMask);
+	virtual void destroyProxy(b3BroadphaseProxy* proxy, b3Dispatcher* dispatcher);
+	virtual void setAabb(int objectId, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* dispatcher);
+	virtual void rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin = b3MakeVector3(0, 0, 0), const b3Vector3& aabbMax = b3MakeVector3(0, 0, 0));
+	virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
 
 	//virtual void					getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
-	virtual void					getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
-	virtual	void					calculateOverlappingPairs(b3Dispatcher* dispatcher=0);
-	virtual	b3OverlappingPairCache*	getOverlappingPairCache();
-	virtual	const b3OverlappingPairCache*	getOverlappingPairCache() const;
-	virtual	void					getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const;
-	virtual	void					printStats();
-
+	virtual void getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const;
+	virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher = 0);
+	virtual b3OverlappingPairCache* getOverlappingPairCache();
+	virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
+	virtual void getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const;
+	virtual void printStats();
 
 	///reset broadphase internal structures, to ensure determinism/reproducability
 	virtual void resetPool(b3Dispatcher* dispatcher);
 
-	void	performDeferredRemoval(b3Dispatcher* dispatcher);
-	
-	void	setVelocityPrediction(b3Scalar prediction)
+	void performDeferredRemoval(b3Dispatcher* dispatcher);
+
+	void setVelocityPrediction(b3Scalar prediction)
 	{
 		m_prediction = prediction;
 	}
@@ -192,15 +185,13 @@ struct	b3DynamicBvhBroadphase
 		return m_prediction;
 	}
 
-	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. 
+	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
 	///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
 	///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
 	///http://code.google.com/p/bullet/issues/detail?id=223
-	void							setAabbForceUpdate(		b3BroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* /*dispatcher*/);
+	void setAabbForceUpdate(b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/);
 
 	//static void						benchmark(b3BroadphaseInterface*);
-
-
 };
 
 #endif

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h

@@ -23,20 +23,20 @@ subject to the following restrictions:
 
 typedef b3Int4 b3BroadphasePair;
 
-inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
+inline b3Int4 b3MakeBroadphasePair(int xx, int yy)
 {
 	b3Int4 pair;
 
 	if (xx < yy)
-    { 
-        pair.x = xx; 
-        pair.y = yy;
-    }
-    else 
-    { 
+	{
+		pair.x = xx;
+		pair.y = yy;
+	}
+	else
+	{
 		pair.x = yy;
-        pair.y = xx;
-    }
+		pair.y = xx;
+	}
 	pair.z = B3_NEW_PAIR_MARKER;
 	pair.w = B3_NEW_PAIR_MARKER;
 	return pair;
@@ -51,22 +51,20 @@ inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
 
 class b3BroadphasePairSortPredicate
 {
-	public:
-
-		bool operator() ( const b3BroadphasePair& a, const b3BroadphasePair& b ) const
-		{
-			const int uidA0 = a.x;
-			const int uidB0 = b.x;
-			const int uidA1 = a.y;
-			const int uidB1 = b.y;
-			return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); 
-		}
+public:
+	bool operator()(const b3BroadphasePair& a, const b3BroadphasePair& b) const
+	{
+		const int uidA0 = a.x;
+		const int uidB0 = b.x;
+		const int uidA1 = a.y;
+		const int uidB1 = b.y;
+		return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
+	}
 };
 
-B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b) 
+B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
 {
-	 return (a.x == b.x ) && (a.y == b.y );
+	return (a.x == b.x) && (a.y == b.y);
 }
 
-#endif //B3_OVERLAPPING_PAIR_H
-
+#endif  //B3_OVERLAPPING_PAIR_H

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp

@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "b3OverlappingPairCache.h"
 
 //#include "b3Dispatcher.h"
@@ -23,35 +21,26 @@ subject to the following restrictions:
 
 #include <stdio.h>
 
-int	b3g_overlappingPairs = 0;
-int b3g_removePairs =0;
-int b3g_addedPairs =0;
-int b3g_findPairs =0;
-
-
-
+int b3g_overlappingPairs = 0;
+int b3g_removePairs = 0;
+int b3g_addedPairs = 0;
+int b3g_findPairs = 0;
 
-b3HashedOverlappingPairCache::b3HashedOverlappingPairCache():
-	m_overlapFilterCallback(0)
+b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
 //,	m_blockedForChanges(false)
 {
-	int initialAllocatedSize= 2;
+	int initialAllocatedSize = 2;
 	m_overlappingPairArray.reserve(initialAllocatedSize);
 	growTables();
 }
 
-
-
-
 b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
 {
 }
 
-
-
-void	b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
+void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
 {
-/*	if (pair.m_algorithm)
+	/*	if (pair.m_algorithm)
 	{
 		{
 			pair.m_algorithm->~b3CollisionAlgorithm();
@@ -60,91 +49,74 @@ void	b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b
 		}
 	}
 	*/
-
 }
 
-
-
-
-void	b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
+void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
 {
-
-	class	CleanPairCallback : public b3OverlapCallback
+	class CleanPairCallback : public b3OverlapCallback
 	{
 		int m_cleanProxy;
-		b3OverlappingPairCache*	m_pairCache;
+		b3OverlappingPairCache* m_pairCache;
 		b3Dispatcher* m_dispatcher;
 
 	public:
-		CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
-			:m_cleanProxy(cleanProxy),
-			m_pairCache(pairCache),
-			m_dispatcher(dispatcher)
+		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
 		{
 		}
-		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		virtual bool processOverlap(b3BroadphasePair& pair)
 		{
 			if ((pair.x == m_cleanProxy) ||
 				(pair.y == m_cleanProxy))
 			{
-				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
 			}
 			return false;
 		}
-		
 	};
 
-	CleanPairCallback cleanPairs(proxy,this,dispatcher);
-
-	processAllOverlappingPairs(&cleanPairs,dispatcher);
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
 
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
 }
 
-
-
-
-void	b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
+void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
 {
-
-	class	RemovePairCallback : public b3OverlapCallback
+	class RemovePairCallback : public b3OverlapCallback
 	{
 		int m_obsoleteProxy;
 
 	public:
 		RemovePairCallback(int obsoleteProxy)
-			:m_obsoleteProxy(obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
 		{
 		}
-		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		virtual bool processOverlap(b3BroadphasePair& pair)
 		{
 			return ((pair.x == m_obsoleteProxy) ||
-				(pair.y == m_obsoleteProxy));
+					(pair.y == m_obsoleteProxy));
 		}
-		
 	};
 
-
 	RemovePairCallback removeCallback(proxy);
 
-	processAllOverlappingPairs(&removeCallback,dispatcher);
+	processAllOverlappingPairs(&removeCallback, dispatcher);
 }
 
-
-
-
-
 b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
 {
 	b3g_findPairs++;
-	if(proxy0 >proxy1) 
-		b3Swap(proxy0,proxy1);
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
 	int proxyId1 = proxy0;
 	int proxyId2 = proxy1;
 
 	/*if (proxyId1 > proxyId2) 
 		b3Swap(proxyId1, proxyId2);*/
 
-	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
 
 	if (hash >= m_hashTable.size())
 	{
@@ -169,9 +141,8 @@ b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
 
 //#include <stdio.h>
 
-void	b3HashedOverlappingPairCache::growTables()
+void b3HashedOverlappingPairCache::growTables()
 {
-
 	int newCapacity = m_overlappingPairArray.capacity();
 
 	if (m_hashTable.size() < newCapacity)
@@ -182,10 +153,9 @@ void	b3HashedOverlappingPairCache::growTables()
 		m_hashTable.resize(newCapacity);
 		m_next.resize(newCapacity);
 
-
 		int i;
 
-		for (i= 0; i < newCapacity; ++i)
+		for (i = 0; i < newCapacity; ++i)
 		{
 			m_hashTable[i] = B3_NULL_PAIR;
 		}
@@ -194,35 +164,31 @@ void	b3HashedOverlappingPairCache::growTables()
 			m_next[i] = B3_NULL_PAIR;
 		}
 
-		for(i=0;i<curHashtableSize;i++)
+		for (i = 0; i < curHashtableSize; i++)
 		{
-	
 			const b3BroadphasePair& pair = m_overlappingPairArray[i];
 			int proxyId1 = pair.x;
 			int proxyId2 = pair.y;
 			/*if (proxyId1 > proxyId2) 
 				b3Swap(proxyId1, proxyId2);*/
-			int	hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
+			int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 			m_next[i] = m_hashTable[hashValue];
 			m_hashTable[hashValue] = i;
 		}
-
-
 	}
 }
 
 b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
 {
-	if(proxy0>proxy1) 
-		b3Swap(proxy0,proxy1);
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
 	int proxyId1 = proxy0;
 	int proxyId2 = proxy1;
 
 	/*if (proxyId1 > proxyId2) 
 		b3Swap(proxyId1, proxyId2);*/
 
-	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));	// New hash value with new mask
-
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
 
 	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair != NULL)
@@ -243,8 +209,8 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
 	pair = &m_overlappingPairArray.expandNonInitializing();
 
 	//this is where we add an actual pair, so also call the 'ghost'
-//	if (m_ghostPairCallback)
-//		m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
+	//	if (m_ghostPairCallback)
+	//		m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
 
 	int newCapacity = m_overlappingPairArray.capacity();
 
@@ -252,16 +218,15 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
 	{
 		growTables();
 		//hash with new capacity
-		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
 	}
-	
-	*pair = b3MakeBroadphasePair(proxy0,proxy1);
-	
-//	pair->m_pProxy0 = proxy0;
-//	pair->m_pProxy1 = proxy1;
+
+	*pair = b3MakeBroadphasePair(proxy0, proxy1);
+
+	//	pair->m_pProxy0 = proxy0;
+	//	pair->m_pProxy1 = proxy1;
 	//pair->m_algorithm = 0;
 	//pair->m_internalTmpValue = 0;
-	
 
 	m_next[count] = m_hashTable[hash];
 	m_hashTable[hash] = count;
@@ -269,20 +234,18 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
 	return pair;
 }
 
-
-
-void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,b3Dispatcher* dispatcher)
+void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
 {
 	b3g_removePairs++;
-	if(proxy0>proxy1) 
-		b3Swap(proxy0,proxy1);
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
 	int proxyId1 = proxy0;
 	int proxyId2 = proxy1;
 
 	/*if (proxyId1 > proxyId2) 
 		b3Swap(proxyId1, proxyId2);*/
 
-	int	hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
 
 	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
 	if (pair == NULL)
@@ -290,9 +253,7 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
 		return 0;
 	}
 
-	cleanOverlappingPair(*pair,dispatcher);
-
-	
+	cleanOverlappingPair(*pair, dispatcher);
 
 	int pairIndex = int(pair - &m_overlappingPairArray[0]);
 	b3Assert(pairIndex < m_overlappingPairArray.size());
@@ -336,8 +297,8 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
 
 	// Remove the last pair from the hash table.
 	const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
-		/* missing swap here too, Nat. */ 
-	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity()-1));
+	/* missing swap here too, Nat. */
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity() - 1));
 
 	index = m_hashTable[lastHash];
 	b3Assert(index != B3_NULL_PAIR);
@@ -372,47 +333,42 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
 }
 //#include <stdio.h>
 
-void	b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
+void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
 {
-
 	int i;
 
-//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
-	for (i=0;i<m_overlappingPairArray.size();)
+	//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i = 0; i < m_overlappingPairArray.size();)
 	{
-	
 		b3BroadphasePair* pair = &m_overlappingPairArray[i];
 		if (callback->processOverlap(*pair))
 		{
-			removeOverlappingPair(pair->x,pair->y,dispatcher);
+			removeOverlappingPair(pair->x, pair->y, dispatcher);
 
 			b3g_overlappingPairs--;
-		} else
+		}
+		else
 		{
 			i++;
 		}
 	}
 }
 
-
-
-
-
-void	b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
 {
 	///need to keep hashmap in sync with pair address, so rebuild all
 	b3BroadphasePairArray tmpPairs;
 	int i;
-	for (i=0;i<m_overlappingPairArray.size();i++)
+	for (i = 0; i < m_overlappingPairArray.size(); i++)
 	{
 		tmpPairs.push_back(m_overlappingPairArray[i]);
 	}
 
-	for (i=0;i<tmpPairs.size();i++)
+	for (i = 0; i < tmpPairs.size(); i++)
 	{
-		removeOverlappingPair(tmpPairs[i].x,tmpPairs[i].y,dispatcher);
+		removeOverlappingPair(tmpPairs[i].x, tmpPairs[i].y, dispatcher);
 	}
-	
+
 	for (i = 0; i < m_next.size(); i++)
 	{
 		m_next[i] = B3_NULL_PAIR;
@@ -420,33 +376,29 @@ void	b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher
 
 	tmpPairs.quickSort(b3BroadphasePairSortPredicate());
 
-	for (i=0;i<tmpPairs.size();i++)
+	for (i = 0; i < tmpPairs.size(); i++)
 	{
-		addOverlappingPair(tmpPairs[i].x ,tmpPairs[i].y);
+		addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y);
 	}
-
-	
 }
 
-
-void*	b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, b3Dispatcher* dispatcher )
+void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
 {
 	if (!hasDeferredRemoval())
 	{
-		b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0,proxy1);
-		
+		b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1);
 
 		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
 		if (findIndex < m_overlappingPairArray.size())
 		{
 			b3g_overlappingPairs--;
 			b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
-			
-			cleanOverlappingPair(pair,dispatcher);
+
+			cleanOverlappingPair(pair, dispatcher);
 			//if (m_ghostPairCallback)
 			//	m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
-			
-			m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1);
+
+			m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
 			m_overlappingPairArray.pop_back();
 			return 0;
 		}
@@ -455,100 +407,77 @@ void*	b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1,
 	return 0;
 }
 
-
-
-
-
-
-
-
-b3BroadphasePair*	b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1)
+b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0, int proxy1)
 {
 	//don't add overlap with own
 	b3Assert(proxy0 != proxy1);
 
-	if (!needsBroadphaseCollision(proxy0,proxy1))
+	if (!needsBroadphaseCollision(proxy0, proxy1))
 		return 0;
-	
+
 	b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
-	*pair = b3MakeBroadphasePair(proxy0,proxy1);
-	
-	
+	*pair = b3MakeBroadphasePair(proxy0, proxy1);
+
 	b3g_overlappingPairs++;
 	b3g_addedPairs++;
-	
-//	if (m_ghostPairCallback)
-//		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
+
+	//	if (m_ghostPairCallback)
+	//		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
 	return pair;
-	
 }
 
 ///this findPair becomes really slow. Either sort the list to speedup the query, or
 ///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
 ///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
 ///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
- b3BroadphasePair*	b3SortedOverlappingPairCache::findPair(int proxy0,int proxy1)
+b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0, int proxy1)
 {
-	if (!needsBroadphaseCollision(proxy0,proxy1))
+	if (!needsBroadphaseCollision(proxy0, proxy1))
 		return 0;
 
-	b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1);
+	b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0, proxy1);
 	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
 
 	if (findIndex < m_overlappingPairArray.size())
 	{
 		//b3Assert(it != m_overlappingPairSet.end());
-		 b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
 		return pair;
 	}
 	return 0;
 }
 
-
-
-
-
-
-
-
-
-
 //#include <stdio.h>
 
-void	b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
+void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
 {
-
 	int i;
 
-	for (i=0;i<m_overlappingPairArray.size();)
+	for (i = 0; i < m_overlappingPairArray.size();)
 	{
-	
 		b3BroadphasePair* pair = &m_overlappingPairArray[i];
 		if (callback->processOverlap(*pair))
 		{
-			cleanOverlappingPair(*pair,dispatcher);
+			cleanOverlappingPair(*pair, dispatcher);
 			pair->x = -1;
 			pair->y = -1;
-			m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+			m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
 			m_overlappingPairArray.pop_back();
 			b3g_overlappingPairs--;
-		} else
+		}
+		else
 		{
 			i++;
 		}
 	}
 }
 
-
-
-
-b3SortedOverlappingPairCache::b3SortedOverlappingPairCache():
-	m_blockedForChanges(false),
-	m_hasDeferredRemoval(true),
-	m_overlapFilterCallback(0)
+b3SortedOverlappingPairCache::b3SortedOverlappingPairCache() : m_blockedForChanges(false),
+															   m_hasDeferredRemoval(true),
+															   m_overlapFilterCallback(0)
 
 {
-	int initialAllocatedSize= 2;
+	int initialAllocatedSize = 2;
 	m_overlappingPairArray.reserve(initialAllocatedSize);
 }
 
@@ -556,9 +485,9 @@ b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
 {
 }
 
-void	b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
+void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
 {
-/*	if (pair.m_algorithm)
+	/*	if (pair.m_algorithm)
 	{
 		{
 			pair.m_algorithm->~b3CollisionAlgorithm();
@@ -570,69 +499,61 @@ void	b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b
 	*/
 }
 
-
-void	b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
+void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
 {
-
-	class	CleanPairCallback : public b3OverlapCallback
+	class CleanPairCallback : public b3OverlapCallback
 	{
 		int m_cleanProxy;
-		b3OverlappingPairCache*	m_pairCache;
+		b3OverlappingPairCache* m_pairCache;
 		b3Dispatcher* m_dispatcher;
 
 	public:
-		CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
-			:m_cleanProxy(cleanProxy),
-			m_pairCache(pairCache),
-			m_dispatcher(dispatcher)
+		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
 		{
 		}
-		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		virtual bool processOverlap(b3BroadphasePair& pair)
 		{
 			if ((pair.x == m_cleanProxy) ||
 				(pair.y == m_cleanProxy))
 			{
-				m_pairCache->cleanOverlappingPair(pair,m_dispatcher);
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
 			}
 			return false;
 		}
-		
 	};
 
-	CleanPairCallback cleanPairs(proxy,this,dispatcher);
-
-	processAllOverlappingPairs(&cleanPairs,dispatcher);
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
 
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
 }
 
-
-void	b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
+void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
 {
-
-	class	RemovePairCallback : public b3OverlapCallback
+	class RemovePairCallback : public b3OverlapCallback
 	{
 		int m_obsoleteProxy;
 
 	public:
 		RemovePairCallback(int obsoleteProxy)
-			:m_obsoleteProxy(obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
 		{
 		}
-		virtual	bool	processOverlap(b3BroadphasePair& pair)
+		virtual bool processOverlap(b3BroadphasePair& pair)
 		{
 			return ((pair.x == m_obsoleteProxy) ||
-				(pair.y == m_obsoleteProxy));
+					(pair.y == m_obsoleteProxy));
 		}
-		
 	};
 
 	RemovePairCallback removeCallback(proxy);
 
-	processAllOverlappingPairs(&removeCallback,dispatcher);
+	processAllOverlappingPairs(&removeCallback, dispatcher);
 }
 
-void	b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
 {
 	//should already be sorted
 }
-

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h

@@ -22,152 +22,136 @@ subject to the following restrictions:
 class b3Dispatcher;
 #include "b3OverlappingPair.h"
 
+typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
 
-
-typedef b3AlignedObjectArray<b3BroadphasePair>	b3BroadphasePairArray;
-
-struct	b3OverlapCallback
+struct b3OverlapCallback
 {
 	virtual ~b3OverlapCallback()
-	{}
+	{
+	}
 	//return true for deletion of the pair
-	virtual bool	processOverlap(b3BroadphasePair& pair) = 0;
-
+	virtual bool processOverlap(b3BroadphasePair& pair) = 0;
 };
 
 struct b3OverlapFilterCallback
 {
 	virtual ~b3OverlapFilterCallback()
-	{}
+	{
+	}
 	// return true when pairs need collision
-	virtual bool	needBroadphaseCollision(int proxy0,int proxy1) const = 0;
+	virtual bool needBroadphaseCollision(int proxy0, int proxy1) const = 0;
 };
 
-
-
-
-
-
-
 extern int b3g_removePairs;
 extern int b3g_addedPairs;
 extern int b3g_findPairs;
 
-const int B3_NULL_PAIR=0xffffffff;
+const int B3_NULL_PAIR = 0xffffffff;
 
 ///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
 ///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
-class b3OverlappingPairCache 
+class b3OverlappingPairCache
 {
 public:
-	virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor
+	virtual ~b3OverlappingPairCache() {}  // this is needed so we can get to the derived class destructor
 
-	virtual b3BroadphasePair*	getOverlappingPairArrayPtr() = 0;
-	
-	virtual const b3BroadphasePair*	getOverlappingPairArrayPtr() const = 0;
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
 
-	virtual b3BroadphasePairArray&	getOverlappingPairArray() = 0;
+	virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
 
-	virtual	void	cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) = 0;
+	virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
+
+	virtual void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher) = 0;
 
 	virtual int getNumOverlappingPairs() const = 0;
 
-	virtual void	cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) = 0;
+	virtual void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher) = 0;
 
-	virtual	void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
+	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
 
-	virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher) = 0;
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher) = 0;
 
 	virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
 
-	virtual bool	hasDeferredRemoval() = 0;
+	virtual bool hasDeferredRemoval() = 0;
 
 	//virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
 
-	virtual b3BroadphasePair* 	addOverlappingPair(int proxy0,int proxy1)=0;
-	virtual void*	removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher)=0;
-	virtual void	removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)=0;
-
-	virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
-
+	virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1) = 0;
+	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher) = 0;
+	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/) = 0;
 
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
 };
 
 /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
 class b3HashedOverlappingPairCache : public b3OverlappingPairCache
 {
-	b3BroadphasePairArray	m_overlappingPairArray;
+	b3BroadphasePairArray m_overlappingPairArray;
 	b3OverlapFilterCallback* m_overlapFilterCallback;
-//	bool		m_blockedForChanges;
-
+	//	bool		m_blockedForChanges;
 
 public:
 	b3HashedOverlappingPairCache();
 	virtual ~b3HashedOverlappingPairCache();
 
-	
-	virtual void	removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
+	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
 
-	virtual void*	removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
-	
-	B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const
+	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
+
+	B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0, int proxy1) const
 	{
 		if (m_overlapFilterCallback)
-			return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
 
-		bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		bool collides = true;  //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
 		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-		
+
 		return collides;
 	}
 
 	// Add a pair and return the new pair. If the pair already exists,
 	// no new pair is created and the old one is returned.
-	virtual b3BroadphasePair* 	addOverlappingPair(int proxy0,int proxy1)
+	virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1)
 	{
 		b3g_addedPairs++;
 
-		if (!needsBroadphaseCollision(proxy0,proxy1))
+		if (!needsBroadphaseCollision(proxy0, proxy1))
 			return 0;
 
-		return internalAddPair(proxy0,proxy1);
+		return internalAddPair(proxy0, proxy1);
 	}
 
-	
-
-	void	cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
+	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
 
-	
-	virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
 
-	virtual b3BroadphasePair*	getOverlappingPairArrayPtr()
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
 	{
 		return &m_overlappingPairArray[0];
 	}
 
-	const b3BroadphasePair*	getOverlappingPairArrayPtr() const
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
 	{
 		return &m_overlappingPairArray[0];
 	}
 
-	b3BroadphasePairArray&	getOverlappingPairArray()
+	b3BroadphasePairArray& getOverlappingPairArray()
 	{
 		return m_overlappingPairArray;
 	}
 
-	const b3BroadphasePairArray&	getOverlappingPairArray() const
+	const b3BroadphasePairArray& getOverlappingPairArray() const
 	{
 		return m_overlappingPairArray;
 	}
 
-	void	cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
-
-
+	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
 
 	b3BroadphasePair* findPair(int proxy0, int proxy1);
 
 	int GetCount() const { return m_overlappingPairArray.size(); }
-//	b3BroadphasePair* GetPairs() { return m_pairs; }
+	//	b3BroadphasePair* GetPairs() { return m_pairs; }
 
 	b3OverlapFilterCallback* getOverlapFilterCallback()
 	{
@@ -179,19 +163,19 @@ public:
 		m_overlapFilterCallback = callback;
 	}
 
-	int	getNumOverlappingPairs() const
+	int getNumOverlappingPairs() const
 	{
 		return m_overlappingPairArray.size();
 	}
+
 private:
-	
-	b3BroadphasePair* 	internalAddPair(int proxy0,int proxy1);
+	b3BroadphasePair* internalAddPair(int proxy0, int proxy1);
 
-	void	growTables();
+	void growTables();
 
 	B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
-	{	
-		return pair.x == proxyId1 && pair.y  == proxyId2;
+	{
+		return pair.x == proxyId1 && pair.y == proxyId2;
 	}
 
 	/*
@@ -210,43 +194,37 @@ private:
 	}
 	*/
 
-
-	
-	B3_FORCE_INLINE	unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
 	{
-		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
+		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) << 16));
 		// Thomas Wang's hash
 
 		key += ~(key << 15);
-		key ^=  (key >> 10);
-		key +=  (key << 3);
-		key ^=  (key >> 6);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
 		key += ~(key << 11);
-		key ^=  (key >> 16);
+		key ^= (key >> 16);
 		return static_cast<unsigned int>(key);
 	}
-	
-
-
-
 
 	B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
 	{
 		int proxyId1 = proxy0;
 		int proxyId2 = proxy1;
-		#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
+#if 0  // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
 		if (proxyId1 > proxyId2) 
 			b3Swap(proxyId1, proxyId2);
-		#endif
+#endif
 
 		int index = m_hashTable[hash];
-		
-		while( index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+
+		while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
 		{
 			index = m_next[index];
 		}
 
-		if ( index == B3_NULL_PAIR )
+		if (index == B3_NULL_PAIR)
 		{
 			return NULL;
 		}
@@ -256,161 +234,142 @@ private:
 		return &m_overlappingPairArray[index];
 	}
 
-	virtual bool	hasDeferredRemoval()
+	virtual bool hasDeferredRemoval()
 	{
 		return false;
 	}
 
-/*	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+	/*	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
 	{
 		m_ghostPairCallback = ghostPairCallback;
 	}
 	*/
 
-	virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher);
-	
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
 
 protected:
-	
-	b3AlignedObjectArray<int>	m_hashTable;
-	b3AlignedObjectArray<int>	m_next;
-//	b3OverlappingPairCallback*	m_ghostPairCallback;
-	
+	b3AlignedObjectArray<int> m_hashTable;
+	b3AlignedObjectArray<int> m_next;
+	//	b3OverlappingPairCallback*	m_ghostPairCallback;
 };
 
-
-
-
 ///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
 ///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
-class	b3SortedOverlappingPairCache : public b3OverlappingPairCache
+class b3SortedOverlappingPairCache : public b3OverlappingPairCache
 {
-	protected:
-		//avoid brute-force finding all the time
-		b3BroadphasePairArray	m_overlappingPairArray;
+protected:
+	//avoid brute-force finding all the time
+	b3BroadphasePairArray m_overlappingPairArray;
 
-		//during the dispatch, check that user doesn't destroy/create proxy
-		bool		m_blockedForChanges;
+	//during the dispatch, check that user doesn't destroy/create proxy
+	bool m_blockedForChanges;
 
-		///by default, do the removal during the pair traversal
-		bool		m_hasDeferredRemoval;
-		
-		//if set, use the callback instead of the built in filter in needBroadphaseCollision
-		b3OverlapFilterCallback* m_overlapFilterCallback;
+	///by default, do the removal during the pair traversal
+	bool m_hasDeferredRemoval;
 
-//		b3OverlappingPairCallback*	m_ghostPairCallback;
+	//if set, use the callback instead of the built in filter in needBroadphaseCollision
+	b3OverlapFilterCallback* m_overlapFilterCallback;
 
-	public:
-			
-		b3SortedOverlappingPairCache();	
-		virtual ~b3SortedOverlappingPairCache();
+	//		b3OverlappingPairCallback*	m_ghostPairCallback;
 
-		virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
+public:
+	b3SortedOverlappingPairCache();
+	virtual ~b3SortedOverlappingPairCache();
 
-		void*	removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
 
-		void	cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
-		
-		b3BroadphasePair*	addOverlappingPair(int proxy0,int proxy1);
+	void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
 
-		b3BroadphasePair*	findPair(int proxy0,int proxy1);
-			
-		
-		void	cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
+	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
 
-		virtual void	removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
+	b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1);
 
+	b3BroadphasePair* findPair(int proxy0, int proxy1);
 
-		inline bool needsBroadphaseCollision(int proxy0,int proxy1) const
-		{
-			if (m_overlapFilterCallback)
-				return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
+	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
 
-			bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
-			//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
-			
-			return collides;
-		}
-		
-		b3BroadphasePairArray&	getOverlappingPairArray()
-		{
-			return m_overlappingPairArray;
-		}
+	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
 
-		const b3BroadphasePairArray&	getOverlappingPairArray() const
-		{
-			return m_overlappingPairArray;
-		}
+	inline bool needsBroadphaseCollision(int proxy0, int proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
 
-		
+		bool collides = true;  //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
 
+		return collides;
+	}
 
-		b3BroadphasePair*	getOverlappingPairArrayPtr()
-		{
-			return &m_overlappingPairArray[0];
-		}
+	b3BroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
 
-		const b3BroadphasePair*	getOverlappingPairArrayPtr() const
-		{
-			return &m_overlappingPairArray[0];
-		}
+	const b3BroadphasePairArray& getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
 
-		int	getNumOverlappingPairs() const
-		{
-			return m_overlappingPairArray.size();
-		}
-		
-		b3OverlapFilterCallback* getOverlapFilterCallback()
-		{
-			return m_overlapFilterCallback;
-		}
+	b3BroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
 
-		void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
-		{
-			m_overlapFilterCallback = callback;
-		}
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
 
-		virtual bool	hasDeferredRemoval()
-		{
-			return m_hasDeferredRemoval;
-		}
+	int getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+
+	b3OverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
 
-/*		virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	virtual bool hasDeferredRemoval()
+	{
+		return m_hasDeferredRemoval;
+	}
+
+	/*		virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
 		{
 			m_ghostPairCallback = ghostPairCallback;
 		}
 		*/
-		virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher);
-		
-
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
 };
 
-
-
 ///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
 class b3NullPairCache : public b3OverlappingPairCache
 {
-
-	b3BroadphasePairArray	m_overlappingPairArray;
+	b3BroadphasePairArray m_overlappingPairArray;
 
 public:
-
-	virtual b3BroadphasePair*	getOverlappingPairArrayPtr()
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
 	{
 		return &m_overlappingPairArray[0];
 	}
-	const b3BroadphasePair*	getOverlappingPairArrayPtr() const
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
 	{
 		return &m_overlappingPairArray[0];
 	}
-	b3BroadphasePairArray&	getOverlappingPairArray()
+	b3BroadphasePairArray& getOverlappingPairArray()
 	{
 		return m_overlappingPairArray;
 	}
-	
-	virtual	void	cleanOverlappingPair(b3BroadphasePair& /*pair*/,b3Dispatcher* /*dispatcher*/)
-	{
 
+	virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/, b3Dispatcher* /*dispatcher*/)
+	{
 	}
 
 	virtual int getNumOverlappingPairs() const
@@ -418,16 +377,15 @@ public:
 		return 0;
 	}
 
-	virtual void	cleanProxyFromPairs(int /*proxy*/,b3Dispatcher* /*dispatcher*/)
+	virtual void cleanProxyFromPairs(int /*proxy*/, b3Dispatcher* /*dispatcher*/)
 	{
-
 	}
 
-	virtual	void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
+	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
 	{
 	}
 
-	virtual void	processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* /*dispatcher*/)
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* /*dispatcher*/)
 	{
 	}
 
@@ -436,39 +394,34 @@ public:
 		return 0;
 	}
 
-	virtual bool	hasDeferredRemoval()
+	virtual bool hasDeferredRemoval()
 	{
 		return true;
 	}
 
-//	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
-//	{
-//
-//	}
+	//	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
+	//	{
+	//
+	//	}
 
-	virtual b3BroadphasePair*	addOverlappingPair(int /*proxy0*/,int /*proxy1*/)
+	virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/, int /*proxy1*/)
 	{
 		return 0;
 	}
 
-	virtual void*	removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,b3Dispatcher* /*dispatcher*/)
+	virtual void* removeOverlappingPair(int /*proxy0*/, int /*proxy1*/, b3Dispatcher* /*dispatcher*/)
 	{
 		return 0;
 	}
 
-	virtual void	removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)
+	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/)
 	{
 	}
-	
-	virtual void	sortOverlappingPairs(b3Dispatcher* dispatcher)
+
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
 	{
-        (void) dispatcher;
+		(void)dispatcher;
 	}
-
-
 };
 
-
-#endif //B3_OVERLAPPING_PAIR_CACHE_H
-
-
+#endif  //B3_OVERLAPPING_PAIR_CACHE_H

thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h

@@ -2,7 +2,6 @@
 #ifndef B3_AABB_H
 #define B3_AABB_H
 
-
 #include "Bullet3Common/shared/b3Float4.h"
 #include "Bullet3Common/shared/b3Mat3x3.h"
 
@@ -10,44 +9,42 @@ typedef struct b3Aabb b3Aabb_t;
 
 struct b3Aabb
 {
-	union
-	{
+	union {
 		float m_min[4];
 		b3Float4 m_minVec;
 		int m_minIndices[4];
 	};
-	union
-	{
-		float	m_max[4];
+	union {
+		float m_max[4];
 		b3Float4 m_maxVec;
 		int m_signedMaxIndices[4];
 	};
 };
 
-inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,
-						b3Float4ConstArg pos,
-						b3QuatConstArg orn,
-						b3Float4* aabbMinOut,b3Float4* aabbMaxOut)
+inline void b3TransformAabb2(b3Float4ConstArg localAabbMin, b3Float4ConstArg localAabbMax, float margin,
+							 b3Float4ConstArg pos,
+							 b3QuatConstArg orn,
+							 b3Float4* aabbMinOut, b3Float4* aabbMaxOut)
 {
-		b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);
-		localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);
-		b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);
-		b3Mat3x3 m;
-		m = b3QuatGetRotationMatrix(orn);
-		b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
-		b3Float4 center = b3TransformPoint(localCenter,pos,orn);
-		
-		b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),
-										 b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),
-										 b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),
-										 0.f);
-		*aabbMinOut = center-extent;
-		*aabbMaxOut = center+extent;
+	b3Float4 localHalfExtents = 0.5f * (localAabbMax - localAabbMin);
+	localHalfExtents += b3MakeFloat4(margin, margin, margin, 0.f);
+	b3Float4 localCenter = 0.5f * (localAabbMax + localAabbMin);
+	b3Mat3x3 m;
+	m = b3QuatGetRotationMatrix(orn);
+	b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
+	b3Float4 center = b3TransformPoint(localCenter, pos, orn);
+
+	b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 0)),
+								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 1)),
+								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 2)),
+								   0.f);
+	*aabbMinOut = center - extent;
+	*aabbMaxOut = center + extent;
 }
 
 /// conservative test for overlap between two aabbs
-inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,
-								b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
+inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1, b3Float4ConstArg aabbMax1,
+								  b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
 {
 	bool overlap = true;
 	overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;
@@ -56,4 +53,4 @@ inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aab
 	return overlap;
 }
 
-#endif //B3_AABB_H
+#endif  //B3_AABB_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h

@@ -1,41 +1,39 @@
 #ifndef B3_CONFIG_H
 #define B3_CONFIG_H
 
-struct	b3Config
+struct b3Config
 {
-	int	m_maxConvexBodies;
-	int	m_maxConvexShapes;
-	int	m_maxBroadphasePairs;
+	int m_maxConvexBodies;
+	int m_maxConvexShapes;
+	int m_maxBroadphasePairs;
 	int m_maxContactCapacity;
 	int m_compoundPairCapacity;
 
 	int m_maxVerticesPerFace;
 	int m_maxFacesPerShape;
-	int	m_maxConvexVertices;
+	int m_maxConvexVertices;
 	int m_maxConvexIndices;
 	int m_maxConvexUniqueEdges;
-	
-	int	m_maxCompoundChildShapes;
-	
+
+	int m_maxCompoundChildShapes;
+
 	int m_maxTriConvexPairCapacity;
 
 	b3Config()
-		:m_maxConvexBodies(128*1024),
-		m_maxVerticesPerFace(64),
-		m_maxFacesPerShape(12),
-		m_maxConvexVertices(8192),
-		m_maxConvexIndices(81920),
-		m_maxConvexUniqueEdges(8192),
-		m_maxCompoundChildShapes(8192),
-		m_maxTriConvexPairCapacity(256*1024)
+		: m_maxConvexBodies(128 * 1024),
+		  m_maxVerticesPerFace(64),
+		  m_maxFacesPerShape(12),
+		  m_maxConvexVertices(8192),
+		  m_maxConvexIndices(81920),
+		  m_maxConvexUniqueEdges(8192),
+		  m_maxCompoundChildShapes(8192),
+		  m_maxTriConvexPairCapacity(256 * 1024)
 	{
 		m_maxConvexShapes = m_maxConvexBodies;
-		m_maxBroadphasePairs = 16*m_maxConvexBodies;
+		m_maxBroadphasePairs = 16 * m_maxConvexBodies;
 		m_maxContactCapacity = m_maxBroadphasePairs;
-		m_compoundPairCapacity = 1024*1024;
+		m_compoundPairCapacity = 1024 * 1024;
 	}
 };
 
-
-#endif//B3_CONFIG_H
-
+#endif  //B3_CONFIG_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h

@@ -19,28 +19,37 @@ subject to the following restrictions:
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
 
-B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3Contact4 : public b3Contact4Data
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	int getBodyA()const {return abs(m_bodyAPtrAndSignBit);}
-	int getBodyB()const {return abs(m_bodyBPtrAndSignBit);}
-	bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;}
-	bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;}
+	int getBodyA() const { return abs(m_bodyAPtrAndSignBit); }
+	int getBodyB() const { return abs(m_bodyBPtrAndSignBit); }
+	bool isBodyAFixed() const { return m_bodyAPtrAndSignBit < 0; }
+	bool isBodyBFixed() const { return m_bodyBPtrAndSignBit < 0; }
 	//	todo. make it safer
 	int& getBatchIdx() { return m_batchIdx; }
 	const int& getBatchIdx() const { return m_batchIdx; }
-	float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); }
-	void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); }
-	float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); }
-	void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); }
+	float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
+	void setRestituitionCoeff(float c)
+	{
+		b3Assert(c >= 0.f && c <= 1.f);
+		m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
+	}
+	float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
+	void setFrictionCoeff(float c)
+	{
+		b3Assert(c >= 0.f && c <= 1.f);
+		m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
+	}
 
 	//float& getNPoints() { return m_worldNormal[3]; }
-	int getNPoints() const { return (int) m_worldNormalOnB.w; }
+	int getNPoints() const { return (int)m_worldNormalOnB.w; }
 
 	float getPenetration(int idx) const { return m_worldPosB[idx].w; }
 
-	bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); }
+	bool isInvalid() const { return (getBodyA() == 0 || getBodyB() == 0); }
 };
 
-#endif //B3_CONTACT4_H
+#endif  //B3_CONTACT4_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp

@@ -13,52 +13,42 @@ subject to the following restrictions:
 */
 //Originally written by Erwin Coumans
 
-
 #include "b3ConvexUtility.h"
 #include "Bullet3Geometry/b3ConvexHullComputer.h"
 #include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
 #include "Bullet3Common/b3Quaternion.h"
 #include "Bullet3Common/b3HashMap.h"
 
-
-
-
-
 b3ConvexUtility::~b3ConvexUtility()
 {
 }
 
-bool	b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
+bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
 {
-	
-	
-
 	b3ConvexHullComputer conv;
-	conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f);
+	conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
 
 	b3AlignedObjectArray<b3Vector3> faceNormals;
 	int numFaces = conv.faces.size();
 	faceNormals.resize(numFaces);
 	b3ConvexHullComputer* convexUtil = &conv;
 
-	
-	b3AlignedObjectArray<b3MyFace>	tmpFaces;
+	b3AlignedObjectArray<b3MyFace> tmpFaces;
 	tmpFaces.resize(numFaces);
 
 	int numVertices = convexUtil->vertices.size();
 	m_vertices.resize(numVertices);
-	for (int p=0;p<numVertices;p++)
+	for (int p = 0; p < numVertices; p++)
 	{
 		m_vertices[p] = convexUtil->vertices[p];
 	}
 
-
-	for (int i=0;i<numFaces;i++)
+	for (int i = 0; i < numFaces; i++)
 	{
 		int face = convexUtil->faces[i];
 		//printf("face=%d\n",face);
-		const b3ConvexHullComputer::Edge*  firstEdge = &convexUtil->edges[face];
-		const b3ConvexHullComputer::Edge*  edge = firstEdge;
+		const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
+		const b3ConvexHullComputer::Edge* edge = firstEdge;
 
 		b3Vector3 edges[3];
 		int numEdges = 0;
@@ -66,25 +56,23 @@ bool	b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
 
 		do
 		{
-			
 			int src = edge->getSourceVertex();
 			tmpFaces[i].m_indices.push_back(src);
 			int targ = edge->getTargetVertex();
 			b3Vector3 wa = convexUtil->vertices[src];
 
 			b3Vector3 wb = convexUtil->vertices[targ];
-			b3Vector3 newEdge = wb-wa;
+			b3Vector3 newEdge = wb - wa;
 			newEdge.normalize();
-			if (numEdges<2)
+			if (numEdges < 2)
 				edges[numEdges++] = newEdge;
 
 			edge = edge->getNextEdgeOfFace();
-		} while (edge!=firstEdge);
+		} while (edge != firstEdge);
 
 		b3Scalar planeEq = 1e30f;
 
-		
-		if (numEdges==2)
+		if (numEdges == 2)
 		{
 			faceNormals[i] = edges[0].cross(edges[1]);
 			faceNormals[i].normalize();
@@ -92,20 +80,19 @@ bool	b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
 			tmpFaces[i].m_plane[1] = faceNormals[i].getY();
 			tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
 			tmpFaces[i].m_plane[3] = planeEq;
-
 		}
 		else
 		{
-			b3Assert(0);//degenerate?
+			b3Assert(0);  //degenerate?
 			faceNormals[i].setZero();
 		}
 
-		for (int v=0;v<tmpFaces[i].m_indices.size();v++)
+		for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
 		{
 			b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
-			if (planeEq>eq)
+			if (planeEq > eq)
 			{
-				planeEq=eq;
+				planeEq = eq;
 			}
 		}
 		tmpFaces[i].m_plane[3] = -planeEq;
@@ -113,89 +100,86 @@ bool	b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
 
 	//merge coplanar faces and copy them to m_polyhedron
 
-	b3Scalar faceWeldThreshold= 0.999f;
+	b3Scalar faceWeldThreshold = 0.999f;
 	b3AlignedObjectArray<int> todoFaces;
-	for (int i=0;i<tmpFaces.size();i++)
+	for (int i = 0; i < tmpFaces.size(); i++)
 		todoFaces.push_back(i);
 
 	while (todoFaces.size())
 	{
 		b3AlignedObjectArray<int> coplanarFaceGroup;
-		int refFace = todoFaces[todoFaces.size()-1];
+		int refFace = todoFaces[todoFaces.size() - 1];
 
 		coplanarFaceGroup.push_back(refFace);
 		b3MyFace& faceA = tmpFaces[refFace];
 		todoFaces.pop_back();
 
-		b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
-		for (int j=todoFaces.size()-1;j>=0;j--)
+		b3Vector3 faceNormalA = b3MakeVector3(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];
 			b3MyFace& faceB = tmpFaces[i];
-			b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
-			if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
+			b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
+			if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
 			{
 				coplanarFaceGroup.push_back(i);
 				todoFaces.remove(i);
 			}
 		}
 
-
 		bool did_merge = false;
-		if (coplanarFaceGroup.size()>1)
+		if (coplanarFaceGroup.size() > 1)
 		{
 			//do the merge: use Graham Scan 2d convex hull
 
 			b3AlignedObjectArray<b3GrahamVector3> orgpoints;
-			b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0);
+			b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
 
-			for (int i=0;i<coplanarFaceGroup.size();i++)
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
 			{
-//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
+				//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
 
 				b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
-				b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
-				averageFaceNormal+=faceNormal;
-				for (int f=0;f<face.m_indices.size();f++)
+				b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
+				averageFaceNormal += faceNormal;
+				for (int f = 0; f < face.m_indices.size(); f++)
 				{
 					int orgIndex = face.m_indices[f];
 					b3Vector3 pt = m_vertices[orgIndex];
-					
+
 					bool found = false;
 
-					for (int i=0;i<orgpoints.size();i++)
+					for (int i = 0; i < orgpoints.size(); i++)
 					{
 						//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
 						if (orgpoints[i].m_orgIndex == orgIndex)
 						{
-							found=true;
+							found = true;
 							break;
 						}
 					}
 					if (!found)
-						orgpoints.push_back(b3GrahamVector3(pt,orgIndex));
+						orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
 				}
 			}
 
-			
-
 			b3MyFace combinedFace;
-			for (int i=0;i<4;i++)
+			for (int i = 0; i < 4; i++)
 				combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
 
 			b3AlignedObjectArray<b3GrahamVector3> hull;
 
 			averageFaceNormal.normalize();
-			b3GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
+			b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
 
-			for (int i=0;i<hull.size();i++)
+			for (int i = 0; i < hull.size(); i++)
 			{
 				combinedFace.m_indices.push_back(hull[i].m_orgIndex);
-				for(int k = 0; k < orgpoints.size(); k++) 
+				for (int k = 0; k < orgpoints.size(); k++)
 				{
-					if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) 
+					if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
 					{
-						orgpoints[k].m_orgIndex = -1; // invalidate...
+						orgpoints[k].m_orgIndex = -1;  // invalidate...
 						break;
 					}
 				}
@@ -203,38 +187,41 @@ bool	b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
 
 			// are there rejected vertices?
 			bool reject_merge = false;
-			
-
 
-			for(int i = 0; i < orgpoints.size(); i++) {
-				if(orgpoints[i].m_orgIndex == -1)
-					continue; // this is in the hull...
+			for (int i = 0; i < orgpoints.size(); i++)
+			{
+				if (orgpoints[i].m_orgIndex == -1)
+					continue;  // this is in the hull...
 				// this vertex is rejected -- is anybody else using this vertex?
-				for(int j = 0; j < tmpFaces.size(); j++) {
-					
+				for (int j = 0; j < tmpFaces.size(); j++)
+				{
 					b3MyFace& face = tmpFaces[j];
 					// is this a face of the current coplanar group?
 					bool is_in_current_group = false;
-					for(int k = 0; k < coplanarFaceGroup.size(); k++) {
-						if(coplanarFaceGroup[k] == j) {
+					for (int k = 0; k < coplanarFaceGroup.size(); k++)
+					{
+						if (coplanarFaceGroup[k] == j)
+						{
 							is_in_current_group = true;
 							break;
 						}
 					}
-					if(is_in_current_group) // ignore this face...
+					if (is_in_current_group)  // ignore this face...
 						continue;
 					// does this face use this rejected vertex?
-					for(int v = 0; v < face.m_indices.size(); v++) {
-						if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
+					for (int v = 0; v < face.m_indices.size(); v++)
+					{
+						if (face.m_indices[v] == orgpoints[i].m_orgIndex)
+						{
 							// this rejected vertex is used in another face -- reject merge
 							reject_merge = true;
 							break;
 						}
 					}
-					if(reject_merge)
+					if (reject_merge)
 						break;
 				}
-				if(reject_merge)
+				if (reject_merge)
 					break;
 			}
 
@@ -245,18 +232,14 @@ bool	b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
 				m_faces.push_back(combinedFace);
 			}
 		}
-		if(!did_merge)
+		if (!did_merge)
 		{
-			for (int i=0;i<coplanarFaceGroup.size();i++)
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
 			{
 				b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
 				m_faces.push_back(face);
 			}
-
-		} 
-
-
-
+		}
 	}
 
 	initialize();
@@ -264,43 +247,38 @@ bool	b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
 	return true;
 }
 
-
-
-
-
-
 inline bool IsAlmostZero(const b3Vector3& v)
 {
-	if(fabsf(v.getX())>1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6)	return false;
+	if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
 	return true;
 }
 
 struct b3InternalVertexPair
 {
-	b3InternalVertexPair(short int v0,short int v1)
-		:m_v0(v0),
-		m_v1(v1)
+	b3InternalVertexPair(short int v0, short int v1)
+		: m_v0(v0),
+		  m_v1(v1)
 	{
-		if (m_v1>m_v0)
-			b3Swap(m_v0,m_v1);
+		if (m_v1 > m_v0)
+			b3Swap(m_v0, m_v1);
 	}
 	short int m_v0;
 	short int m_v1;
 	int getHash() const
 	{
-		return m_v0+(m_v1<<16);
+		return m_v0 + (m_v1 << 16);
 	}
 	bool equals(const b3InternalVertexPair& other) const
 	{
-		return m_v0==other.m_v0 && m_v1==other.m_v1;
+		return m_v0 == other.m_v0 && m_v1 == other.m_v1;
 	}
 };
 
 struct b3InternalEdge
 {
 	b3InternalEdge()
-		:m_face0(-1),
-		m_face1(-1)
+		: m_face0(-1),
+		  m_face1(-1)
 	{
 	}
 	short int m_face0;
@@ -312,23 +290,31 @@ struct b3InternalEdge
 #ifdef TEST_INTERNAL_OBJECTS
 bool b3ConvexUtility::testContainment() const
 {
-	for(int p=0;p<8;p++)
+	for (int p = 0; p < 8; p++)
 	{
 		b3Vector3 LocalPt;
-		if(p==0)		LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
-		else if(p==1)	LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
-		else if(p==2)	LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
-		else if(p==3)	LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
-		else if(p==4)	LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
-		else if(p==5)	LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
-		else if(p==6)	LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
-		else if(p==7)	LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
-
-		for(int i=0;i<m_faces.size();i++)
+		if (p == 0)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 1)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 2)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 3)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
+		else if (p == 4)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 5)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 6)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 7)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
+
+		for (int i = 0; i < m_faces.size(); i++)
 		{
 			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 			const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
-			if(d>0.0f)
+			if (d > 0.0f)
 				return false;
 		}
 	}
@@ -336,39 +322,38 @@ bool b3ConvexUtility::testContainment() const
 }
 #endif
 
-void	b3ConvexUtility::initialize()
+void b3ConvexUtility::initialize()
 {
-
-	b3HashMap<b3InternalVertexPair,b3InternalEdge> edges;
+	b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
 
 	b3Scalar TotalArea = 0.0f;
-	
+
 	m_localCenter.setValue(0, 0, 0);
-	for(int i=0;i<m_faces.size();i++)
+	for (int i = 0; i < m_faces.size(); i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
 		int NbTris = numVertices;
-		for(int j=0;j<NbTris;j++)
+		for (int j = 0; j < NbTris; j++)
 		{
-			int k = (j+1)%numVertices;
-			b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
+			int k = (j + 1) % numVertices;
+			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 			b3InternalEdge* edptr = edges.find(vp);
-			b3Vector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
+			b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
 			edge.normalize();
 
 			bool found = false;
-			b3Vector3 diff,diff2;
+			b3Vector3 diff, diff2;
 
-			for (int p=0;p<m_uniqueEdges.size();p++)
+			for (int p = 0; p < m_uniqueEdges.size(); p++)
 			{
-				diff = m_uniqueEdges[p]-edge;
-				diff2 = m_uniqueEdges[p]+edge;
+				diff = m_uniqueEdges[p] - edge;
+				diff2 = m_uniqueEdges[p] + edge;
 
-			//	if ((diff.length2()==0.f) || 
+				//	if ((diff.length2()==0.f) ||
 				//	(diff2.length2()==0.f))
 
-				if (IsAlmostZero(diff) || 
-				IsAlmostZero(diff2))
+				if (IsAlmostZero(diff) ||
+					IsAlmostZero(diff2))
 				{
 					found = true;
 					break;
@@ -382,106 +367,101 @@ void	b3ConvexUtility::initialize()
 
 			if (edptr)
 			{
-					//TBD: figure out why I added this assert
-//				b3Assert(edptr->m_face0>=0);
-	//			b3Assert(edptr->m_face1<0);
+				//TBD: figure out why I added this assert
+				//				b3Assert(edptr->m_face0>=0);
+				//			b3Assert(edptr->m_face1<0);
 				edptr->m_face1 = i;
-			} else
+			}
+			else
 			{
 				b3InternalEdge ed;
 				ed.m_face0 = i;
-				edges.insert(vp,ed);
+				edges.insert(vp, ed);
 			}
 		}
 	}
 
 #ifdef USE_CONNECTED_FACES
-	for(int i=0;i<m_faces.size();i++)
+	for (int i = 0; i < m_faces.size(); i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
 		m_faces[i].m_connectedFaces.resize(numVertices);
 
-		for(int j=0;j<numVertices;j++)
+		for (int j = 0; j < numVertices; j++)
 		{
-			int k = (j+1)%numVertices;
-			b3InternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
+			int k = (j + 1) % numVertices;
+			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
 			b3InternalEdge* edptr = edges.find(vp);
 			b3Assert(edptr);
-			b3Assert(edptr->m_face0>=0);
-			b3Assert(edptr->m_face1>=0);
+			b3Assert(edptr->m_face0 >= 0);
+			b3Assert(edptr->m_face1 >= 0);
 
-			int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
+			int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
 			m_faces[i].m_connectedFaces[j] = connectedFace;
 		}
 	}
-#endif//USE_CONNECTED_FACES
+#endif  //USE_CONNECTED_FACES
 
-	for(int i=0;i<m_faces.size();i++)
+	for (int i = 0; i < m_faces.size(); i++)
 	{
 		int numVertices = m_faces[i].m_indices.size();
-		int NbTris = numVertices-2;
-		
+		int NbTris = numVertices - 2;
+
 		const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
-		for(int j=1;j<=NbTris;j++)
+		for (int j = 1; j <= NbTris; j++)
 		{
-			int k = (j+1)%numVertices;
+			int k = (j + 1) % numVertices;
 			const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
 			const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
 			b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
-			b3Vector3 Center = (p0+p1+p2)/3.0f;
+			b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
 			m_localCenter += Area * Center;
 			TotalArea += Area;
 		}
 	}
 	m_localCenter /= TotalArea;
 
-
-
-
 #ifdef TEST_INTERNAL_OBJECTS
-	if(1)
+	if (1)
 	{
 		m_radius = FLT_MAX;
-		for(int i=0;i<m_faces.size();i++)
+		for (int i = 0; i < m_faces.size(); i++)
 		{
 			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
 			const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
-			if(dist<m_radius)
+			if (dist < m_radius)
 				m_radius = dist;
 		}
 
-	
 		b3Scalar MinX = FLT_MAX;
 		b3Scalar MinY = FLT_MAX;
 		b3Scalar MinZ = FLT_MAX;
 		b3Scalar MaxX = -FLT_MAX;
 		b3Scalar MaxY = -FLT_MAX;
 		b3Scalar MaxZ = -FLT_MAX;
-		for(int i=0; i<m_vertices.size(); i++)
+		for (int i = 0; i < m_vertices.size(); i++)
 		{
 			const b3Vector3& pt = m_vertices[i];
-			if(pt.getX()<MinX)	MinX = pt.getX();
-			if(pt.getX()>MaxX)	MaxX = pt.getX();
-			if(pt.getY()<MinY)	MinY = pt.getY();
-			if(pt.getY()>MaxY)	MaxY = pt.getY();
-			if(pt.getZ()<MinZ)	MinZ = pt.getZ();
-			if(pt.getZ()>MaxZ)	MaxZ = pt.getZ();
+			if (pt.getX() < MinX) MinX = pt.getX();
+			if (pt.getX() > MaxX) MaxX = pt.getX();
+			if (pt.getY() < MinY) MinY = pt.getY();
+			if (pt.getY() > MaxY) MaxY = pt.getY();
+			if (pt.getZ() < MinZ) MinZ = pt.getZ();
+			if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
 		}
-		mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
-		mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
-
-
+		mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
+		mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
 
-//		const b3Scalar r = m_radius / sqrtf(2.0f);
+		//		const b3Scalar r = m_radius / sqrtf(2.0f);
 		const b3Scalar r = m_radius / sqrtf(3.0f);
 		const int LargestExtent = mE.maxAxis();
-		const b3Scalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
+		const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
 		m_extents[0] = m_extents[1] = m_extents[2] = r;
-		m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
+		m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
 		bool FoundBox = false;
-		for(int j=0;j<1024;j++)
+		for (int j = 0; j < 1024; j++)
 		{
-			if(testContainment())
+			if (testContainment())
 			{
 				FoundBox = true;
 				break;
@@ -489,25 +469,25 @@ void	b3ConvexUtility::initialize()
 
 			m_extents[LargestExtent] -= Step;
 		}
-		if(!FoundBox)
+		if (!FoundBox)
 		{
 			m_extents[0] = m_extents[1] = m_extents[2] = r;
 		}
 		else
 		{
 			// Refine the box
-			const b3Scalar Step = (m_radius - r)/1024.0f;
-			const int e0 = (1<<LargestExtent) & 3;
-			const int e1 = (1<<e0) & 3;
+			const b3Scalar Step = (m_radius - r) / 1024.0f;
+			const int e0 = (1 << LargestExtent) & 3;
+			const int e1 = (1 << e0) & 3;
 
-			for(int j=0;j<1024;j++)
+			for (int j = 0; j < 1024; j++)
 			{
 				const b3Scalar Saved0 = m_extents[e0];
 				const b3Scalar Saved1 = m_extents[e1];
 				m_extents[e0] += Step;
 				m_extents[e1] += Step;
 
-				if(!testContainment())
+				if (!testContainment())
 				{
 					m_extents[e0] = Saved0;
 					m_extents[e1] = Saved1;

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h

@@ -20,43 +20,36 @@ subject to the following restrictions:
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "Bullet3Common/b3Transform.h"
 
-
-
-
 struct b3MyFace
 {
-	b3AlignedObjectArray<int>	m_indices;
-	b3Scalar	m_plane[4];
+	b3AlignedObjectArray<int> m_indices;
+	b3Scalar m_plane[4];
 };
 
-B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
+B3_ATTRIBUTE_ALIGNED16(class)
+b3ConvexUtility
 {
-	public:
+public:
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	b3Vector3		m_localCenter;
-	b3Vector3		m_extents;
-	b3Vector3		mC;
-	b3Vector3		mE;
-	b3Scalar		m_radius;
-	
-	b3AlignedObjectArray<b3Vector3>	m_vertices;
-	b3AlignedObjectArray<b3MyFace>	m_faces;
+	b3Vector3 m_localCenter;
+	b3Vector3 m_extents;
+	b3Vector3 mC;
+	b3Vector3 mE;
+	b3Scalar m_radius;
+
+	b3AlignedObjectArray<b3Vector3> m_vertices;
+	b3AlignedObjectArray<b3MyFace> m_faces;
 	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
 
-		
 	b3ConvexUtility()
 	{
 	}
 	virtual ~b3ConvexUtility();
 
-	bool	initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles=true);
-		
-	void	initialize();
-	bool testContainment() const;
-
-
+	bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles = true);
 
+	void initialize();
+	bool testContainment() const;
 };
 #endif
-	

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp

@@ -5,15 +5,13 @@
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
 
-
 struct b3CpuNarrowPhaseInternalData
 {
 	b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
-	b3AlignedObjectArray<b3Collidable>	m_collidablesCPU;
+	b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
 	b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
 	b3Config m_config;
 
-
 	b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
 	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
 	b3AlignedObjectArray<b3Vector3> m_convexVertices;
@@ -22,10 +20,9 @@ struct b3CpuNarrowPhaseInternalData
 
 	b3AlignedObjectArray<b3Contact4Data> m_contacts;
 
-	int	m_numAcceleratedShapes;
+	int m_numAcceleratedShapes;
 };
 
-
 const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
 {
 	return m_data->m_contacts;
@@ -41,7 +38,6 @@ const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) cons
 	return m_data->m_collidablesCPU[collidableIndex];
 }
 
-
 b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
 {
 	m_data = new b3CpuNarrowPhaseInternalData;
@@ -61,7 +57,7 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
 	int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
 	m_data->m_contacts.resize(maxContactCapacity);
 
-	for (int i=0;i<nPairs;i++)
+	for (int i = 0; i < nPairs; i++)
 	{
 		int bodyIndexA = pairs[i].x;
 		int bodyIndexB = pairs[i].y;
@@ -71,64 +67,57 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
 		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
 			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 		{
-//			computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
 		}
 
 		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
 			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
 		{
-//			computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
 			//printf("convex-sphere\n");
-			
 		}
 
 		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
 			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
 		{
-//			computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-//			printf("convex-plane\n");
-			
+			//			computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("convex-plane\n");
 		}
 
 		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
 			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 		{
-//			computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-//			printf("plane-convex\n");
-			
+			//			computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("plane-convex\n");
 		}
 
-			if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
 			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
 		{
-//			computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
-//			nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);	
-//			printf("convex-plane\n");
-			
+			//			computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
+			//			nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
+			//			printf("convex-plane\n");
 		}
 
-
-				if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
 			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
 		{
-//			computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
-//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-//			printf("convex-plane\n");
-			
+			//			computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("convex-plane\n");
 		}
 
 		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
 			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
 		{
-//			computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
-//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
-//			printf("plane-convex\n");
-			
+			//			computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("plane-convex\n");
 		}
 
 		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
@@ -137,54 +126,48 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
 			//printf("pairs[i].z=%d\n",pairs[i].z);
 			//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
 			//		m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
-			int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
-				m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity);
-
+			int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
+														m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
 
-			if (contactIndex>=0)
+			if (contactIndex >= 0)
 			{
 				pairs[i].z = contactIndex;
 			}
-//			printf("plane-convex\n");
-			
+			//			printf("plane-convex\n");
 		}
-
-
 	}
 
 	m_data->m_contacts.resize(numContacts);
 }
 
-int	b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
+int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
 {
 	int collidableIndex = allocateCollidable();
-	if (collidableIndex<0)
+	if (collidableIndex < 0)
 		return collidableIndex;
 
-	
 	b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
 	col.m_shapeType = SHAPE_CONVEX_HULL;
 	col.m_shapeIndex = -1;
-	
-	
+
 	{
-		b3Vector3 localCenter=b3MakeVector3(0,0,0);
-		for (int i=0;i<utilPtr->m_vertices.size();i++)
-			localCenter+=utilPtr->m_vertices[i];
-		localCenter*= (1.f/utilPtr->m_vertices.size());
+		b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
+		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
+			localCenter += utilPtr->m_vertices[i];
+		localCenter *= (1.f / utilPtr->m_vertices.size());
 		utilPtr->m_localCenter = localCenter;
 
-		col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col);
+		col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
 	}
 
-	if (col.m_shapeIndex>=0)
+	if (col.m_shapeIndex >= 0)
 	{
 		b3Aabb aabb;
-		
-		b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f);
-		b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f);
 
-		for (int i=0;i<utilPtr->m_vertices.size();i++)
+		b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
+		b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
+
+		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
 		{
 			myAabbMin.setMin(utilPtr->m_vertices[i]);
 			myAabbMax.setMax(utilPtr->m_vertices[i]);
@@ -200,44 +183,42 @@ int	b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
 		aabb.m_signedMaxIndices[3] = 0;
 
 		m_data->m_localShapeAABBCPU.push_back(aabb);
-
 	}
-	
+
 	return collidableIndex;
 }
 
-int	b3CpuNarrowPhase::allocateCollidable()
+int b3CpuNarrowPhase::allocateCollidable()
 {
 	int curSize = m_data->m_collidablesCPU.size();
-	if (curSize<m_data->m_config.m_maxConvexShapes)
+	if (curSize < m_data->m_config.m_maxConvexShapes)
 	{
 		m_data->m_collidablesCPU.expand();
 		return curSize;
 	}
 	else
 	{
-		b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes);
+		b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
 	}
 	return -1;
-
 }
 
-int	b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
+int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
 {
 	b3AlignedObjectArray<b3Vector3> verts;
 
-	unsigned char* vts = (unsigned char*) vertices;
-	for (int i=0;i<numVertices;i++)
+	unsigned char* vts = (unsigned char*)vertices;
+	for (int i = 0; i < numVertices; i++)
 	{
-		float* vertex = (float*) &vts[i*strideInBytes];
-		verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
+		float* vertex = (float*)&vts[i * strideInBytes];
+		verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
 	}
 
 	b3ConvexUtility* utilPtr = new b3ConvexUtility();
 	bool merge = true;
 	if (numVertices)
 	{
-		utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
+		utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
 	}
 
 	int collidableIndex = registerConvexHullShape(utilPtr);
@@ -246,74 +227,67 @@ int	b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideI
 	return collidableIndex;
 }
 
-
-int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
+int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
 {
+	m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
+	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
 
-	m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1);
-	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
-	
-    
-	b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
+	b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
 	convex.mC = convexPtr->mC;
 	convex.mE = convexPtr->mE;
-	convex.m_extents= convexPtr->m_extents;
+	convex.m_extents = convexPtr->m_extents;
 	convex.m_localCenter = convexPtr->m_localCenter;
 	convex.m_radius = convexPtr->m_radius;
-	
+
 	convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
 	int edgeOffset = m_data->m_uniqueEdges.size();
 	convex.m_uniqueEdgesOffset = edgeOffset;
-	
-	m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges);
-    
+
+	m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
+
 	//convex data here
 	int i;
-	for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
+	for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
 	{
-		m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i];
+		m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
 	}
-    
+
 	int faceOffset = m_data->m_convexFaces.size();
 	convex.m_faceOffset = faceOffset;
 	convex.m_numFaces = convexPtr->m_faces.size();
 
-	m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
-	
+	m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
 
-	for (i=0;i<convexPtr->m_faces.size();i++)
+	for (i = 0; i < convexPtr->m_faces.size(); i++)
 	{
-		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
-																			convexPtr->m_faces[i].m_plane[1],
-																			convexPtr->m_faces[i].m_plane[2],
-																			convexPtr->m_faces[i].m_plane[3]);
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
+																			   convexPtr->m_faces[i].m_plane[1],
+																			   convexPtr->m_faces[i].m_plane[2],
+																			   convexPtr->m_faces[i].m_plane[3]);
 
-		
 		int indexOffset = m_data->m_convexIndices.size();
 		int numIndices = convexPtr->m_faces[i].m_indices.size();
-		m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
-		m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
-		m_data->m_convexIndices.resize(indexOffset+numIndices);
-		for (int p=0;p<numIndices;p++)
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
+		m_data->m_convexIndices.resize(indexOffset + numIndices);
+		for (int p = 0; p < numIndices; p++)
 		{
-			m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p];
+			m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
 		}
 	}
-    
+
 	convex.m_numVertices = convexPtr->m_vertices.size();
 	int vertexOffset = m_data->m_convexVertices.size();
-	convex.m_vertexOffset =vertexOffset;
-	
-	m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
-	for (int i=0;i<convexPtr->m_vertices.size();i++)
+	convex.m_vertexOffset = vertexOffset;
+
+	m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
+	for (int i = 0; i < convexPtr->m_vertices.size(); i++)
 	{
-		m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i];
+		m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
 	}
 
 	(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
-	
-    
-    
+
 	return m_data->m_numAcceleratedShapes++;
 }
 

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h

@@ -12,66 +12,55 @@
 class b3CpuNarrowPhase
 {
 protected:
-
-	struct b3CpuNarrowPhaseInternalData*	m_data;
+	struct b3CpuNarrowPhaseInternalData* m_data;
 	int m_acceleratedCompanionShapeIndex;
 	int m_planeBodyIndex;
-	int	m_static0Index;
+	int m_static0Index;
 
-	int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col);
+	int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col);
 	int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
 
 public:
-
-	
-
-
 	b3CpuNarrowPhase(const struct b3Config& config);
 
 	virtual ~b3CpuNarrowPhase(void);
 
-	int		registerSphereShape(float radius);
-	int		registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
+	int registerSphereShape(float radius);
+	int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
 
 	int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
 	int registerFace(const b3Vector3& faceNormal, float faceConstant);
-	
-	int	registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling);
-	
+
+	int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling);
+
 	//do they need to be merged?
-	
-	int	registerConvexHullShape(b3ConvexUtility* utilPtr);
-	int	registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
+
+	int registerConvexHullShape(b3ConvexUtility* utilPtr);
+	int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
 
 	//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
-	void setObjectTransform(const float* position, const float* orientation , int bodyIndex);
+	void setObjectTransform(const float* position, const float* orientation, int bodyIndex);
 
-	void	writeAllBodiesToGpu();
-	void  reset();
-	void	readbackAllBodiesToCpu();
-	bool	getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const;
+	void writeAllBodiesToGpu();
+	void reset();
+	void readbackAllBodiesToCpu();
+	bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const;
 
-	void setObjectTransformCpu(float* position, float* orientation , int bodyIndex);
+	void setObjectTransformCpu(float* position, float* orientation, int bodyIndex);
 	void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
 
-	
 	//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
 	virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
 
-
-	
 	const struct b3RigidBodyData* getBodiesCpu() const;
 	//struct b3RigidBodyData* getBodiesCpu();
 
-	int	getNumBodiesGpu() const;
+	int getNumBodiesGpu() const;
 
-	
-	int	getNumBodyInertiasGpu() const;
+	int getNumBodyInertiasGpu() const;
 
-	
 	const struct b3Collidable* getCollidablesCpu() const;
-	int		getNumCollidablesGpu() const;
-
+	int getNumCollidablesGpu() const;
 
 	/*const struct b3Contact4* getContactsCPU() const;
 
@@ -80,8 +69,7 @@ public:
 	*/
 
 	const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
-	
-	
+
 	int getNumRigidBodies() const;
 
 	int allocateCollidable();
@@ -93,13 +81,12 @@ public:
 	b3Collidable& getCollidableCpu(int collidableIndex);
 	const b3Collidable& getCollidableCpu(int collidableIndex) const;
 
-	const b3CpuNarrowPhaseInternalData*	getInternalData() const
+	const b3CpuNarrowPhaseInternalData* getInternalData() const
 	{
-			return m_data;
+		return m_data;
 	}
 
 	const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
 };
 
-#endif //B3_CPU_NARROWPHASE_H
-
+#endif  //B3_CPU_NARROWPHASE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h

@@ -4,21 +4,22 @@
 
 #include "Bullet3Common/b3Vector3.h"
 
-B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3RayInfo
 {
 	b3Vector3 m_from;
 	b3Vector3 m_to;
 };
 
-B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3RayHit
 {
-		b3Scalar	m_hitFraction;
-		int	m_hitBody;
-		int	m_hitResult1;
-		int	m_hitResult2;
-		b3Vector3 m_hitPoint;
-		b3Vector3 m_hitNormal;
+	b3Scalar m_hitFraction;
+	int m_hitBody;
+	int m_hitResult1;
+	int m_hitResult2;
+	b3Vector3 m_hitPoint;
+	b3Vector3 m_hitNormal;
 };
 
-#endif //B3_RAYCAST_INFO_H
-
+#endif  //B3_RAYCAST_INFO_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h

@@ -20,11 +20,9 @@ subject to the following restrictions:
 #include "Bullet3Common/b3Matrix3x3.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
-
-inline float	b3GetInvMass(const b3RigidBodyData& body)
+inline float b3GetInvMass(const b3RigidBodyData& body)
 {
-		return body.m_invMass;
+	return body.m_invMass;
 }
 
-
-#endif//B3_RIGID_BODY_CL
+#endif  //B3_RIGID_BODY_CL

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h

@@ -7,14 +7,13 @@ typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
 struct b3BvhSubtreeInfoData
 {
 	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
 	//4 bytes, points to the root of the subtree
-	int			m_rootNodeIndex;
+	int m_rootNodeIndex;
 	//4 bytes
-	int			m_subtreeSize;
-	int			m_padding[3];
+	int m_subtreeSize;
+	int m_padding[3];
 };
 
-#endif //B3_BVH_SUBTREE_INFO_DATA_H
-
+#endif  //B3_BVH_SUBTREE_INFO_DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h

@@ -7,69 +7,64 @@
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
 
-
-
 // work-in-progress
-void   b3BvhTraversal( __global const b3Int4* pairs, 
-									__global const b3RigidBodyData* rigidBodies, 
-									__global const b3Collidable* collidables,
-									__global b3Aabb* aabbs,
-									__global b3Int4* concavePairsOut,
-									__global volatile int* numConcavePairsOut,
-									__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
-									__global const b3QuantizedBvhNode* quantizedNodesRoot,
-									__global const b3BvhInfo* bvhInfos,
-									int numPairs,
-									int maxNumConcavePairsCapacity,
-									int id)
+void b3BvhTraversal(__global const b3Int4* pairs,
+					__global const b3RigidBodyData* rigidBodies,
+					__global const b3Collidable* collidables,
+					__global b3Aabb* aabbs,
+					__global b3Int4* concavePairsOut,
+					__global volatile int* numConcavePairsOut,
+					__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
+					__global const b3QuantizedBvhNode* quantizedNodesRoot,
+					__global const b3BvhInfo* bvhInfos,
+					int numPairs,
+					int maxNumConcavePairsCapacity,
+					int id)
 {
-	
 	int bodyIndexA = pairs[id].x;
 	int bodyIndexB = pairs[id].y;
 	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
 	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
-	
+
 	//once the broadphase avoids static-static pairs, we can remove this test
-	if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
+	if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
 	{
 		return;
 	}
-		
-	if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
+
+	if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
 		return;
 
 	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
-		
-	if (shapeTypeB!=SHAPE_CONVEX_HULL &&
-		shapeTypeB!=SHAPE_SPHERE	&&
-		shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
-		)
+
+	if (shapeTypeB != SHAPE_CONVEX_HULL &&
+		shapeTypeB != SHAPE_SPHERE &&
+		shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
 		return;
 
 	b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
 
-	b3Float4	bvhAabbMin = bvhInfo.m_aabbMin;
-	b3Float4	bvhAabbMax = bvhInfo.m_aabbMax;
-	b3Float4	bvhQuantization = bvhInfo.m_quantization;
+	b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
+	b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
+	b3Float4 bvhQuantization = bvhInfo.m_quantization;
 	int numSubtreeHeaders = bvhInfo.m_numSubTrees;
 	__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
 	__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
-	
 
 	unsigned short int quantizedQueryAabbMin[3];
 	unsigned short int quantizedQueryAabbMax[3];
-	b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
-	b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
-	
-	for (int i=0;i<numSubtreeHeaders;i++)
+	b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
+	b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
+
+	for (int i = 0; i < numSubtreeHeaders; i++)
 	{
 		b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
-				
-		int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+
+		int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
 		if (overlap != 0)
 		{
 			int startNodeIndex = subtree.m_rootNodeIndex;
-			int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
+			int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
 			int curIndex = startNodeIndex;
 			int escapeIndex;
 			int isLeafNode;
@@ -77,43 +72,46 @@ void   b3BvhTraversal( __global const b3Int4* pairs,
 			while (curIndex < endNodeIndex)
 			{
 				b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
-				aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
+				aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
 				isLeafNode = b3IsLeaf(&rootNode);
 				if (aabbOverlap)
 				{
 					if (isLeafNode)
 					{
 						int triangleIndex = b3GetTriangleIndex(&rootNode);
-						if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
+						if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
 						{
-								int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
-								int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
-								for (int b=0;b<numChildrenB;b++)
+							int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
+							int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
+							for (int b = 0; b < numChildrenB; b++)
+							{
+								if ((pairIdx + b) < maxNumConcavePairsCapacity)
 								{
-									if ((pairIdx+b)<maxNumConcavePairsCapacity)
-									{
-										int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
-										b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
-										concavePairsOut[pairIdx+b] = newPair;
-									}
+									int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
+									b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
+									concavePairsOut[pairIdx + b] = newPair;
 								}
-						} else
+							}
+						}
+						else
 						{
 							int pairIdx = b3AtomicInc(numConcavePairsOut);
-							if (pairIdx<maxNumConcavePairsCapacity)
+							if (pairIdx < maxNumConcavePairsCapacity)
 							{
-								b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
+								b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
 								concavePairsOut[pairIdx] = newPair;
 							}
 						}
-					} 
+					}
 					curIndex++;
-				} else
+				}
+				else
 				{
 					if (isLeafNode)
 					{
 						curIndex++;
-					} else
+					}
+					else
 					{
 						escapeIndex = b3GetEscapeIndex(&rootNode);
 						curIndex += escapeIndex;
@@ -122,5 +120,4 @@ void   b3BvhTraversal( __global const b3Int4* pairs,
 			}
 		}
 	}
-
 }

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h

@@ -1,7 +1,6 @@
 #ifndef B3_CLIP_FACES_H
 #define B3_CLIP_FACES_H
 
-
 #include "Bullet3Common/shared/b3Int4.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
@@ -10,38 +9,36 @@
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
 
-
-inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float  t)
+inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
 {
-	return b3MakeFloat4(	a.x + (b.x - a.x) * t,
+	return b3MakeFloat4(a.x + (b.x - a.x) * t,
 						a.y + (b.y - a.y) * t,
 						a.z + (b.z - a.z) * t,
 						0.f);
 }
 
 // Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
-int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut)
+int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
 {
-	
 	int ve;
 	float ds, de;
 	int numVertsOut = 0;
-    //double-check next test
-    //	if (numVertsIn < 2)
-    //		return 0;
-    
-	b3Float4 firstVertex=pVtxIn[numVertsIn-1];
+	//double-check next test
+	//	if (numVertsIn < 2)
+	//		return 0;
+
+	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
 	b3Float4 endVertex = pVtxIn[0];
-	
-	ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS;
-    
+
+	ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
+
 	for (ve = 0; ve < numVertsIn; ve++)
 	{
-		endVertex=pVtxIn[ve];
-		de = b3Dot(planeNormalWS,endVertex)+planeEqWS;
-		if (ds<0)
+		endVertex = pVtxIn[ve];
+		de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
+		if (ds < 0)
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start < 0, end < 0, so output endVertex
 				ppVtxOut[numVertsOut++] = endVertex;
@@ -49,15 +46,15 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
 			else
 			{
 				// Start < 0, end >= 0, so output intersection
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
 			}
 		}
 		else
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start >= 0, end < 0 so output intersection and end
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
 				ppVtxOut[numVertsOut++] = endVertex;
 			}
 		}
@@ -67,90 +64,81 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
 	return numVertsOut;
 }
 
-
-__kernel void   clipFacesAndFindContactsKernel(    __global const b3Float4* separatingNormals,
-                                                   __global const int* hasSeparatingAxis,
-                                                   __global b3Int4* clippingFacesOut,
-                                                   __global b3Float4* worldVertsA1,
-                                                   __global b3Float4* worldNormalsA1,
-                                                   __global b3Float4* worldVertsB1,
-                                                   __global b3Float4* worldVertsB2,
-                                                    int vertexFaceCapacity,
-															int pairIndex
-                                                   )
+__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
+											 __global const int* hasSeparatingAxis,
+											 __global b3Int4* clippingFacesOut,
+											 __global b3Float4* worldVertsA1,
+											 __global b3Float4* worldNormalsA1,
+											 __global b3Float4* worldVertsB1,
+											 __global b3Float4* worldVertsB2,
+											 int vertexFaceCapacity,
+											 int pairIndex)
 {
-//    int i = get_global_id(0);
+	//    int i = get_global_id(0);
 	//int pairIndex = i;
 	int i = pairIndex;
-    
+
 	float minDist = -1e30f;
 	float maxDist = 0.02f;
-    
-//	if (i<numPairs)
+
+	//	if (i<numPairs)
 	{
-        
 		if (hasSeparatingAxis[i])
 		{
-            
-//			int bodyIndexA = pairs[i].x;
-	//		int bodyIndexB = pairs[i].y;
-		    
-            int numLocalContactsOut = 0;
-
-            int capacityWorldVertsB2 = vertexFaceCapacity;
-            
-            __global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2];
-            __global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2];
-            
-
-            {
-                __global b3Int4* clippingFaces = clippingFacesOut;
-            
-                
-                int closestFaceA = clippingFaces[pairIndex].x;
-               // int closestFaceB = clippingFaces[pairIndex].y;
-                int numVertsInA = clippingFaces[pairIndex].z;
-                int numVertsInB = clippingFaces[pairIndex].w;
-                
-                int numVertsOut = 0;
-                
-                if (closestFaceA>=0)
-                {
-                    
-                    
-                    
-                    // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
-                    
-                    for(int e0=0;e0<numVertsInA;e0++)
-                    {
-                        const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0];
-                        const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)];
-                        const b3Float4 WorldEdge0 = aw - bw;
-                        b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
-                        b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1);
-                        b3Float4 worldA1 = aw;
-                        float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1);
-                        b3Float4 planeNormalWS = planeNormalWS1;
-                        float planeEqWS=planeEqWS1;
-                        numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut);
-                        __global b3Float4* tmp = pVtxOut;
-                        pVtxOut = pVtxIn;
-                        pVtxIn = tmp;
-                        numVertsInB = numVertsOut;
-                        numVertsOut = 0;
-                    }
-                    
-                    b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
-                    float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]);
-                    
-                    for (int i=0;i<numVertsInB;i++)
-                    {
-                        float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS;
-                        if (depth <=minDist)
-                        {
-                            depth = minDist;
-                        }
-/*
+			//			int bodyIndexA = pairs[i].x;
+			//		int bodyIndexB = pairs[i].y;
+
+			int numLocalContactsOut = 0;
+
+			int capacityWorldVertsB2 = vertexFaceCapacity;
+
+			__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
+			__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
+
+			{
+				__global b3Int4* clippingFaces = clippingFacesOut;
+
+				int closestFaceA = clippingFaces[pairIndex].x;
+				// int closestFaceB = clippingFaces[pairIndex].y;
+				int numVertsInA = clippingFaces[pairIndex].z;
+				int numVertsInB = clippingFaces[pairIndex].w;
+
+				int numVertsOut = 0;
+
+				if (closestFaceA >= 0)
+				{
+					// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+
+					for (int e0 = 0; e0 < numVertsInA; e0++)
+					{
+						const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
+						const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
+						const b3Float4 WorldEdge0 = aw - bw;
+						b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
+						b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
+						b3Float4 worldA1 = aw;
+						float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
+						b3Float4 planeNormalWS = planeNormalWS1;
+						float planeEqWS = planeEqWS1;
+						numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
+						__global b3Float4* tmp = pVtxOut;
+						pVtxOut = pVtxIn;
+						pVtxIn = tmp;
+						numVertsInB = numVertsOut;
+						numVertsOut = 0;
+					}
+
+					b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
+					float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
+
+					for (int i = 0; i < numVertsInB; i++)
+					{
+						float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
+						if (depth <= minDist)
+						{
+							depth = minDist;
+						}
+						/*
 						static float maxDepth = 0.f;
 						if (depth < maxDepth)
 						{
@@ -163,26 +151,21 @@ __kernel void   clipFacesAndFindContactsKernel(    __global const b3Float4* sepa
 
 						}
 */
-                        if (depth <=maxDist)
-                        {
-                            b3Float4 pointInWorld = pVtxIn[i];
-                            pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
-                        }
-                    }
-                    
-                }
-                clippingFaces[pairIndex].w =numLocalContactsOut;
-                
-
-            }
-            
-            for (int i=0;i<numLocalContactsOut;i++)
-                pVtxIn[i] = pVtxOut[i];
-                
-		}//		if (hasSeparatingAxis[i])
-	}//	if (i<numPairs)
-    
-}
+						if (depth <= maxDist)
+						{
+							b3Float4 pointInWorld = pVtxIn[i];
+							pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
+						}
+					}
+				}
+				clippingFaces[pairIndex].w = numLocalContactsOut;
+			}
+
+			for (int i = 0; i < numLocalContactsOut; i++)
+				pVtxIn[i] = pVtxOut[i];
 
-#endif //B3_CLIP_FACES_H
+		}  //		if (hasSeparatingAxis[i])
+	}      //	if (i<numPairs)
+}
 
+#endif  //B3_CLIP_FACES_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h

@@ -2,40 +2,36 @@
 #ifndef B3_COLLIDABLE_H
 #define B3_COLLIDABLE_H
 
-
 #include "Bullet3Common/shared/b3Float4.h"
 #include "Bullet3Common/shared/b3Quat.h"
 
 enum b3ShapeTypes
 {
-	SHAPE_HEIGHT_FIELD=1,
+	SHAPE_HEIGHT_FIELD = 1,
 
-	SHAPE_CONVEX_HULL=3,
-	SHAPE_PLANE=4,
-	SHAPE_CONCAVE_TRIMESH=5,
-	SHAPE_COMPOUND_OF_CONVEX_HULLS=6,
-	SHAPE_SPHERE=7,
+	SHAPE_CONVEX_HULL = 3,
+	SHAPE_PLANE = 4,
+	SHAPE_CONCAVE_TRIMESH = 5,
+	SHAPE_COMPOUND_OF_CONVEX_HULLS = 6,
+	SHAPE_SPHERE = 7,
 	MAX_NUM_SHAPE_TYPES,
 };
 
 typedef struct b3Collidable b3Collidable_t;
 
-
 struct b3Collidable
 {
 	union {
 		int m_numChildShapes;
 		int m_bvhIndex;
 	};
-	union
-	{
+	union {
 		float m_radius;
-		int	m_compoundBvhIndex;
+		int m_compoundBvhIndex;
 	};
 
 	int m_shapeType;
-	union
-	{
+	union {
 		int m_shapeIndex;
 		float m_height;
 	};
@@ -44,33 +40,30 @@ struct b3Collidable
 typedef struct b3GpuChildShape b3GpuChildShape_t;
 struct b3GpuChildShape
 {
-	b3Float4	m_childPosition;
-	b3Quat		m_childOrientation;
-	union
-	{
-		int			m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS
-		int			m_capsuleAxis;
+	b3Float4 m_childPosition;
+	b3Quat m_childOrientation;
+	union {
+		int m_shapeIndex;  //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
+		int m_capsuleAxis;
 	};
-	union 
-	{
-		float		m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
-		int			m_numChildShapes;//used for compound shape
+	union {
+		float m_radius;        //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
+		int m_numChildShapes;  //used for compound shape
 	};
-	union 
-	{
-		float		m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES
-		int	m_collidableShapeIndex;
+	union {
+		float m_height;  //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
+		int m_collidableShapeIndex;
 	};
-	int			m_shapeType;
+	int m_shapeType;
 };
 
 struct b3CompoundOverlappingPair
 {
 	int m_bodyIndexA;
 	int m_bodyIndexB;
-//	int	m_pairType;
+	//	int	m_pairType;
 	int m_childShapeIndexA;
 	int m_childShapeIndexB;
 };
 
-#endif //B3_COLLIDABLE_H
+#endif  //B3_COLLIDABLE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h

@@ -3,26 +3,24 @@
 
 #include "Bullet3Common/shared/b3Float4.h"
 
-typedef  struct b3Contact4Data b3Contact4Data_t;
+typedef struct b3Contact4Data b3Contact4Data_t;
 
 struct b3Contact4Data
 {
-	b3Float4	m_worldPosB[4];
-//	b3Float4	m_localPosA[4];
-//	b3Float4	m_localPosB[4];
-	b3Float4	m_worldNormalOnB;	//	w: m_nPoints
-	unsigned short  m_restituitionCoeffCmp;
-	unsigned short  m_frictionCoeffCmp;
+	b3Float4 m_worldPosB[4];
+	//	b3Float4	m_localPosA[4];
+	//	b3Float4	m_localPosB[4];
+	b3Float4 m_worldNormalOnB;  //	w: m_nPoints
+	unsigned short m_restituitionCoeffCmp;
+	unsigned short m_frictionCoeffCmp;
 	int m_batchIdx;
-	int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr
+	int m_bodyAPtrAndSignBit;  //x:m_bodyAPtr, y:m_bodyBPtr
 	int m_bodyBPtrAndSignBit;
 
-	int	m_childIndexA;
-	int	m_childIndexB;
+	int m_childIndexA;
+	int m_childIndexB;
 	int m_unused1;
 	int m_unused2;
-
-
 };
 
 inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
@@ -35,6 +33,4 @@ inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numP
 	contact->m_worldNormalOnB.w = (float)numPoints;
 };
 
-
-
-#endif //B3_CONTACT4DATA_H
+#endif  //B3_CONTACT4DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h

@@ -2,48 +2,43 @@
 #ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
 #define B3_CONTACT_CONVEX_CONVEX_SAT_H
 
-
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
 
 #define B3_MAX_VERTS 1024
 
-
-
-inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float  t)
+inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
 {
-	return b3MakeVector3(	a.x + (b.x - a.x) * t,
-						a.y + (b.y - a.y) * t,
-						a.z + (b.z - a.z) * t,
-						0.f);
+	return b3MakeVector3(a.x + (b.x - a.x) * t,
+						 a.y + (b.y - a.y) * t,
+						 a.z + (b.z - a.z) * t,
+						 0.f);
 }
 
-
 // Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
-inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut)
+inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
 {
-	
 	int ve;
 	float ds, de;
 	int numVertsOut = 0;
 	if (numVertsIn < 2)
 		return 0;
 
-	b3Float4 firstVertex=pVtxIn[numVertsIn-1];
+	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
 	b3Float4 endVertex = pVtxIn[0];
-	
-	ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS;
+
+	ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
 
 	for (ve = 0; ve < numVertsIn; ve++)
 	{
-		endVertex=pVtxIn[ve];
+		endVertex = pVtxIn[ve];
 
-		de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS;
+		de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
 
-		if (ds<0)
+		if (ds < 0)
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start < 0, end < 0, so output endVertex
 				ppVtxOut[numVertsOut++] = endVertex;
@@ -51,15 +46,15 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
 			else
 			{
 				// Start < 0, end >= 0, so output intersection
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
 			}
 		}
 		else
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start >= 0, end < 0 so output intersection and end
-				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
 				ppVtxOut[numVertsOut++] = endVertex;
 			}
 		}
@@ -69,36 +64,35 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
 	return numVertsOut;
 }
 
-
-inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,  
-	const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
-	b3Float4* worldVertsB2, int capacityWorldVertsB2,
-	const float minDist, float maxDist,
-	const b3AlignedObjectArray<b3Float4>& verticesA,	const b3AlignedObjectArray<b3GpuFace>& facesA,	const b3AlignedObjectArray<int>& indicesA,
-	//const b3Float4* verticesB,	const b3GpuFace* facesB,	const int* indicesB,
-	b3Float4* contactsOut,
-	int contactCapacity)
+inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
+								 const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
+								 b3Float4* worldVertsB2, int capacityWorldVertsB2,
+								 const float minDist, float maxDist,
+								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+								 //const b3Float4* verticesB,	const b3GpuFace* facesB,	const int* indicesB,
+								 b3Float4* contactsOut,
+								 int contactCapacity)
 {
 	int numContactsOut = 0;
 
 	b3Float4* pVtxIn = worldVertsB1;
 	b3Float4* pVtxOut = worldVertsB2;
-	
+
 	int numVertsIn = numWorldVertsB1;
 	int numVertsOut = 0;
 
-	int closestFaceA=-1;
+	int closestFaceA = -1;
 	{
 		float dmin = FLT_MAX;
-		for(int face=0;face<hullA->m_numFaces;face++)
+		for (int face = 0; face < hullA->m_numFaces; face++)
 		{
 			const b3Float4 Normal = b3MakeVector3(
-				facesA[hullA->m_faceOffset+face].m_plane.x, 
-				facesA[hullA->m_faceOffset+face].m_plane.y, 
-				facesA[hullA->m_faceOffset+face].m_plane.z,0.f);
-			const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
-		
-			float d = b3Dot3F4(faceANormalWS,separatingNormal);
+				facesA[hullA->m_faceOffset + face].m_plane.x,
+				facesA[hullA->m_faceOffset + face].m_plane.y,
+				facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
+
+			float d = b3Dot3F4(faceANormalWS, separatingNormal);
 			if (d < dmin)
 			{
 				dmin = d;
@@ -106,33 +100,33 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
 			}
 		}
 	}
-	if (closestFaceA<0)
+	if (closestFaceA < 0)
 		return numContactsOut;
 
-	b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA];
+	b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
 
 	// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
 	//int numContacts = numWorldVertsB1;
 	int numVerticesA = polyA.m_numIndices;
-	for(int e0=0;e0<numVerticesA;e0++)
+	for (int e0 = 0; e0 < numVerticesA; e0++)
 	{
-		const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]];
-		const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]];
+		const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
+		const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
 		const b3Float4 edge0 = a - b;
-		const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0);
-		b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
-		b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA);
-
-		b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1);
-		b3Float4 worldA1 = b3TransformPoint(a,posA,ornA);
-		float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1);
-		
+		const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
+		b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
+		b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
+
+		b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
+		b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
+		float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
+
 		b3Float4 planeNormalWS = planeNormalWS1;
-		float planeEqWS=planeEqWS1;
-		
+		float planeEqWS = planeEqWS1;
+
 		//clip face
 		//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
-		numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);
+		numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
 
 		//btSwap(pVtxIn,pVtxOut);
 		b3Float4* tmp = pVtxOut;
@@ -142,32 +136,32 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
 		numVertsOut = 0;
 	}
 
-	
 	// only keep points that are behind the witness face
 	{
-		b3Float4 localPlaneNormal  = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);
+		b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
 		float localPlaneEq = polyA.m_plane.w;
-		b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal);
-		float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA);
-		for (int i=0;i<numVertsIn;i++)
+		b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
+		float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
+		for (int i = 0; i < numVertsIn; i++)
 		{
-			float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS;
-			if (depth <=minDist)
+			float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
+			if (depth <= minDist)
 			{
 				depth = minDist;
 			}
-			if (numContactsOut<contactCapacity)
+			if (numContactsOut < contactCapacity)
 			{
-				if (depth <=maxDist)
+				if (depth <= maxDist)
 				{
 					b3Float4 pointInWorld = pVtxIn[i];
 					//resultOut.addContactPoint(separatingNormal,point,depth);
-					contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth);
+					contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
 					//printf("depth=%f\n",depth);
 				}
-			} else
+			}
+			else
 			{
-				b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity);
+				b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
 			}
 		}
 	}
@@ -175,62 +169,60 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
 	return numContactsOut;
 }
 
+inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
+								 const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+								 const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
+								 b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
+								 const float minDist, float maxDist,
+								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+								 const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
 
-
-inline int	b3ClipHullAgainstHull(const b3Float4& separatingNormal, 
-	const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, 
-	const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB, 
-	b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
-	const float minDist, float maxDist,
-	const b3AlignedObjectArray<b3Float4>& verticesA,	const b3AlignedObjectArray<b3GpuFace>& facesA,	const b3AlignedObjectArray<int>& indicesA,
-	const b3AlignedObjectArray<b3Float4>& verticesB,	const b3AlignedObjectArray<b3GpuFace>& facesB,	const b3AlignedObjectArray<int>& indicesB,
-
-	b3Float4*	contactsOut,
-	int contactCapacity)
+								 b3Float4* contactsOut,
+								 int contactCapacity)
 {
 	int numContactsOut = 0;
-	int numWorldVertsB1= 0;
-	
+	int numWorldVertsB1 = 0;
+
 	B3_PROFILE("clipHullAgainstHull");
 
 	//float curMaxDist=maxDist;
-	int closestFaceB=-1;
+	int closestFaceB = -1;
 	float dmax = -FLT_MAX;
 
 	{
 		//B3_PROFILE("closestFaceB");
-		if (hullB.m_numFaces!=1)
+		if (hullB.m_numFaces != 1)
 		{
 			//printf("wtf\n");
 		}
 		static bool once = true;
 		//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
-		
-		for(int face=0;face<hullB.m_numFaces;face++)
+
+		for (int face = 0; face < hullB.m_numFaces; face++)
 		{
 #ifdef BT_DEBUG_SAT_FACE
 			if (once)
-				printf("face %d\n",face);
-			const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face];
+				printf("face %d\n", face);
+			const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
 			if (once)
 			{
-				for (int i=0;i<faceB->m_numIndices;i++)
+				for (int i = 0; i < faceB->m_numIndices; i++)
 				{
-					b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]];
-					printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z);
+					b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
+					printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
 				}
 			}
-#endif //BT_DEBUG_SAT_FACE
-			//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
+#endif  //BT_DEBUG_SAT_FACE \
+	//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
 			{
-				const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x, 
-					facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f);
+				const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
+													  facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
 				const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
 #ifdef BT_DEBUG_SAT_FACE
 				if (once)
-					printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z);
+					printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
 #endif
-				float d = b3Dot3F4(WorldNormal,separatingNormal);
+				float d = b3Dot3F4(WorldNormal, separatingNormal);
 				if (d > dmax)
 				{
 					dmax = d;
@@ -241,79 +233,73 @@ inline int	b3ClipHullAgainstHull(const b3Float4& separatingNormal,
 		once = false;
 	}
 
-	
-	b3Assert(closestFaceB>=0);
+	b3Assert(closestFaceB >= 0);
 	{
 		//B3_PROFILE("worldVertsB1");
-		const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB];
+		const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
 		const int numVertices = polyB.m_numIndices;
-		for(int e0=0;e0<numVertices;e0++)
+		for (int e0 = 0; e0 < numVertices; e0++)
 		{
-			const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
-			worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
+			const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
+			worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
 		}
 	}
 
-	if (closestFaceB>=0)
+	if (closestFaceB >= 0)
 	{
 		//B3_PROFILE("clipFaceAgainstHull");
-		numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA, 
-				posA,ornA,
-				worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,
-				verticesA,				facesA,				indicesA,
-				contactsOut,contactCapacity);
+		numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
+											   posA, ornA,
+											   worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
+											   verticesA, facesA, indicesA,
+											   contactsOut, contactCapacity);
 	}
 
 	return numContactsOut;
 }
 
-
-
-
 inline int b3ClipHullHullSingle(
-			int bodyIndexA, int bodyIndexB,
-										 const b3Float4& posA,
-										 const b3Quaternion& ornA,
-										 const b3Float4& posB,
-										 const b3Quaternion& ornB,
-
-			int collidableIndexA, int collidableIndexB,
-
-			const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf, 
-			b3AlignedObjectArray<b3Contact4Data>* globalContactOut, 
-			int& nContacts,
-			
-			const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
-			const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
-	
-			const b3AlignedObjectArray<b3Vector3>& verticesA, 
-			const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, 
-			const b3AlignedObjectArray<b3GpuFace>& facesA,
-			const b3AlignedObjectArray<int>& indicesA,
-	
-			const b3AlignedObjectArray<b3Vector3>& verticesB,
-			const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
-			const b3AlignedObjectArray<b3GpuFace>& facesB,
-			const b3AlignedObjectArray<int>& indicesB,
-
-			const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
-			const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
-			const b3Vector3& sepNormalWorldSpace,
-			int maxContactCapacity			)
+	int bodyIndexA, int bodyIndexB,
+	const b3Float4& posA,
+	const b3Quaternion& ornA,
+	const b3Float4& posB,
+	const b3Quaternion& ornB,
+
+	int collidableIndexA, int collidableIndexB,
+
+	const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
+	b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
+	int& nContacts,
+
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
+
+	const b3AlignedObjectArray<b3Vector3>& verticesA,
+	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
+	const b3AlignedObjectArray<b3GpuFace>& facesA,
+	const b3AlignedObjectArray<int>& indicesA,
+
+	const b3AlignedObjectArray<b3Vector3>& verticesB,
+	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
+	const b3AlignedObjectArray<b3GpuFace>& facesB,
+	const b3AlignedObjectArray<int>& indicesB,
+
+	const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
+	const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
+	const b3Vector3& sepNormalWorldSpace,
+	int maxContactCapacity)
 {
 	int contactIndex = -1;
 	b3ConvexPolyhedronData hullA, hullB;
-    
-    b3Collidable colA = hostCollidablesA[collidableIndexA];
-    hullA = hostConvexDataA[colA.m_shapeIndex];
-    //printf("numvertsA = %d\n",hullA.m_numVertices);
-    
-    
-    b3Collidable colB = hostCollidablesB[collidableIndexB];
-    hullB = hostConvexDataB[colB.m_shapeIndex];
-    //printf("numvertsB = %d\n",hullB.m_numVertices);
-    
-	
+
+	b3Collidable colA = hostCollidablesA[collidableIndexA];
+	hullA = hostConvexDataA[colA.m_shapeIndex];
+	//printf("numvertsA = %d\n",hullA.m_numVertices);
+
+	b3Collidable colB = hostCollidablesB[collidableIndexB];
+	hullB = hostConvexDataB[colB.m_shapeIndex];
+	//printf("numvertsB = %d\n",hullB.m_numVertices);
+
 	b3Float4 contactsOut[B3_MAX_VERTS];
 	int localContactCapacity = B3_MAX_VERTS;
 
@@ -321,187 +307,168 @@ inline int b3ClipHullHullSingle(
 	b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
 	b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
 #endif
-	
-	
+
 	{
-		
 		b3Float4 worldVertsB1[B3_MAX_VERTS];
 		b3Float4 worldVertsB2[B3_MAX_VERTS];
 		int capacityWorldVerts = B3_MAX_VERTS;
 
-		b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f);
+		b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
 		int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
 		int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
 
 		b3Scalar minDist = -1;
 		b3Scalar maxDist = 0.;
 
-		        
-
-		b3Transform trA,trB;
+		b3Transform trA, trB;
 		{
-		//B3_PROFILE("b3TransformPoint computation");
-		//trA.setIdentity();
-		trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z));
-		trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w));
-				
-		//trB.setIdentity();
-		trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z));
-		trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w));
+			//B3_PROFILE("b3TransformPoint computation");
+			//trA.setIdentity();
+			trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
+			trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
+
+			//trB.setIdentity();
+			trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
+			trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
 		}
 
 		b3Quaternion trAorn = trA.getRotation();
-        b3Quaternion trBorn = trB.getRotation();
-        
-		int numContactsOut = b3ClipHullAgainstHull(hostNormal, 
-						hostConvexDataA.at(shapeA), 
-						hostConvexDataB.at(shapeB),
-								(b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
-								(b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
-								worldVertsB1,worldVertsB2,capacityWorldVerts,
-								minDist, maxDist,
-								verticesA,	facesA,indicesA,
-								verticesB,	facesB,indicesB,
-								
-								contactsOut,localContactCapacity);
-
-		if (numContactsOut>0)
+		b3Quaternion trBorn = trB.getRotation();
+
+		int numContactsOut = b3ClipHullAgainstHull(hostNormal,
+												   hostConvexDataA.at(shapeA),
+												   hostConvexDataB.at(shapeB),
+												   (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
+												   (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
+												   worldVertsB1, worldVertsB2, capacityWorldVerts,
+												   minDist, maxDist,
+												   verticesA, facesA, indicesA,
+												   verticesB, facesB, indicesB,
+
+												   contactsOut, localContactCapacity);
+
+		if (numContactsOut > 0)
 		{
 			B3_PROFILE("overlap");
 
 			b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
-//			b3Float4 centerOut;
-			
+			//			b3Float4 centerOut;
+
 			b3Int4 contactIdx;
 			contactIdx.x = 0;
 			contactIdx.y = 1;
 			contactIdx.z = 2;
 			contactIdx.w = 3;
-			
+
 			int numPoints = 0;
-					
+
 			{
 				B3_PROFILE("extractManifold");
-				numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB,  &contactIdx);
+				numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
 			}
-					
+
 			b3Assert(numPoints);
-					
-			if (nContacts<maxContactCapacity)
+
+			if (nContacts < maxContactCapacity)
 			{
 				contactIndex = nContacts;
 				globalContactOut->expand();
 				b3Contact4Data& contact = globalContactOut->at(nContacts);
-				contact.m_batchIdx = 0;//i;
-				contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA;
-				contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB;
+				contact.m_batchIdx = 0;  //i;
+				contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
+				contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
 
 				contact.m_frictionCoeffCmp = 45874;
 				contact.m_restituitionCoeffCmp = 0;
-					
-			//	float distance = 0.f;
-				for (int p=0;p<numPoints;p++)
+
+				//	float distance = 0.f;
+				for (int p = 0; p < numPoints; p++)
 				{
-					contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B
-					contact.m_worldNormalOnB = normalOnSurfaceB; 
+					contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];  //check if it is actually on B
+					contact.m_worldNormalOnB = normalOnSurfaceB;
 				}
 				//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
 				contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
 				nContacts++;
-			} else
+			}
+			else
 			{
-				b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity);
+				b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
 			}
 		}
 	}
 	return contactIndex;
 }
 
-	
-
-
-
 inline int b3ContactConvexConvexSAT(
-																int pairIndex,
-																int bodyIndexA, int bodyIndexB, 
-																int collidableIndexA, int collidableIndexB, 
-																const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies, 
-																const b3AlignedObjectArray<b3Collidable>& collidables,
-																const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
-																const b3AlignedObjectArray<b3Float4>& convexVertices,
-																const b3AlignedObjectArray<b3Float4>& uniqueEdges,
-																const b3AlignedObjectArray<int>& convexIndices,
-																const b3AlignedObjectArray<b3GpuFace>& faces,
-																b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
-																int& nGlobalContactsOut,
-																int maxContactCapacity)
+	int pairIndex,
+	int bodyIndexA, int bodyIndexB,
+	int collidableIndexA, int collidableIndexB,
+	const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
+	const b3AlignedObjectArray<b3Collidable>& collidables,
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
+	const b3AlignedObjectArray<b3Float4>& convexVertices,
+	const b3AlignedObjectArray<b3Float4>& uniqueEdges,
+	const b3AlignedObjectArray<int>& convexIndices,
+	const b3AlignedObjectArray<b3GpuFace>& faces,
+	b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
+	int& nGlobalContactsOut,
+	int maxContactCapacity)
 {
 	int contactIndex = -1;
 
-
 	b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
 	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
 	b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
 	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
-	
 
 	b3ConvexPolyhedronData hullA, hullB;
-    
-	b3Float4 sepNormalWorldSpace;
 
-	
+	b3Float4 sepNormalWorldSpace;
 
-    b3Collidable colA = collidables[collidableIndexA];
-    hullA = convexShapes[colA.m_shapeIndex];
-    //printf("numvertsA = %d\n",hullA.m_numVertices);
-    
-    
-    b3Collidable colB = collidables[collidableIndexB];
-    hullB = convexShapes[colB.m_shapeIndex];
-    //printf("numvertsB = %d\n",hullB.m_numVertices);
-    
-	
+	b3Collidable colA = collidables[collidableIndexA];
+	hullA = convexShapes[colA.m_shapeIndex];
+	//printf("numvertsA = %d\n",hullA.m_numVertices);
 
+	b3Collidable colB = collidables[collidableIndexB];
+	hullB = convexShapes[colB.m_shapeIndex];
+	//printf("numvertsB = %d\n",hullB.m_numVertices);
 
 #ifdef _WIN32
 	b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
 	b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
 #endif
-	
-		bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB,
-							posA,
-							ornA,
-							posB,
-							ornB,
-
-							convexVertices,uniqueEdges,faces,convexIndices,
-							convexVertices,uniqueEdges,faces,convexIndices,
-							
-							sepNormalWorldSpace
-							);
-
-	
+
+	bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
+											 posA,
+											 ornA,
+											 posB,
+											 ornB,
+
+											 convexVertices, uniqueEdges, faces, convexIndices,
+											 convexVertices, uniqueEdges, faces, convexIndices,
+
+											 sepNormalWorldSpace);
+
 	if (foundSepAxis)
 	{
-		
-		
 		contactIndex = b3ClipHullHullSingle(
 			bodyIndexA, bodyIndexB,
-						   posA,ornA,
-						   posB,ornB,
+			posA, ornA,
+			posB, ornB,
 			collidableIndexA, collidableIndexB,
-			&rigidBodies, 
+			&rigidBodies,
 			&globalContactsOut,
 			nGlobalContactsOut,
-			
+
 			convexShapes,
 			convexShapes,
-	
-			convexVertices, 
-			uniqueEdges, 
+
+			convexVertices,
+			uniqueEdges,
 			faces,
 			convexIndices,
-	
+
 			convexVertices,
 			uniqueEdges,
 			faces,
@@ -511,10 +478,9 @@ inline int b3ContactConvexConvexSAT(
 			collidables,
 			sepNormalWorldSpace,
 			maxContactCapacity);
-			
 	}
 
 	return contactIndex;
 }
 
-#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H
+#endif  //B3_CONTACT_CONVEX_CONVEX_SAT_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h

@@ -2,32 +2,24 @@
 #ifndef B3_CONTACT_SPHERE_SPHERE_H
 #define B3_CONTACT_SPHERE_SPHERE_H
 
-
-
-
-
-void	computeContactSphereConvex(int pairIndex,
-																int bodyIndexA, int bodyIndexB, 
-																int collidableIndexA, int collidableIndexB, 
-																const b3RigidBodyData* rigidBodies, 
-																const b3Collidable* collidables,
-																const b3ConvexPolyhedronData* convexShapes,
-																const b3Vector3* convexVertices,
-																const int* convexIndices,
-																const b3GpuFace* faces,
-																b3Contact4* globalContactsOut,
-																int& nGlobalContactsOut,
-																int maxContactCapacity)
+void computeContactSphereConvex(int pairIndex,
+								int bodyIndexA, int bodyIndexB,
+								int collidableIndexA, int collidableIndexB,
+								const b3RigidBodyData* rigidBodies,
+								const b3Collidable* collidables,
+								const b3ConvexPolyhedronData* convexShapes,
+								const b3Vector3* convexVertices,
+								const int* convexIndices,
+								const b3GpuFace* faces,
+								b3Contact4* globalContactsOut,
+								int& nGlobalContactsOut,
+								int maxContactCapacity)
 {
-
 	float radius = collidables[collidableIndexA].m_radius;
 	float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
 	b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
 
-
-
 	float4 pos = rigidBodies[bodyIndexB].m_pos;
-	
 
 	b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
 
@@ -44,63 +36,64 @@ void	computeContactSphereConvex(int pairIndex,
 	int numFaces = convexShapes[shapeIndex].m_numFaces;
 	float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
 	float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
-	float minDist = -1000000.f; // TODO: What is the largest/smallest float?
+	float minDist = -1000000.f;  // TODO: What is the largest/smallest float?
 	bool bCollide = true;
 	int region = -1;
 	float4 localHitNormal;
-	for ( int f = 0; f < numFaces; f++ )
+	for (int f = 0; f < numFaces; f++)
 	{
-		b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
+		b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
 		float4 planeEqn;
-		float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
-		float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
+		float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
+		float4 n1 = localPlaneNormal;  //quatRotate(quat,localPlaneNormal);
 		planeEqn = n1;
 		planeEqn[3] = face.m_plane.w;
 
 		float4 pntReturn;
 		float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
 
-		if ( dist > radius)
+		if (dist > radius)
 		{
 			bCollide = false;
 			break;
 		}
 
-		if ( dist > 0 )
+		if (dist > 0)
 		{
 			//might hit an edge or vertex
 			b3Vector3 out;
 
 			bool isInPoly = IsPointInPolygon(spherePos,
-					&face,
-					&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
-					convexIndices,
-                    &out);
+											 &face,
+											 &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
+											 convexIndices,
+											 &out);
 			if (isInPoly)
 			{
-				if (dist>minDist)
+				if (dist > minDist)
 				{
 					minDist = dist;
 					closestPnt = pntReturn;
 					localHitNormal = planeEqn;
-					region=1;
+					region = 1;
 				}
-			} else
+			}
+			else
 			{
-				b3Vector3 tmp = spherePos-out;
+				b3Vector3 tmp = spherePos - out;
 				b3Scalar l2 = tmp.length2();
-				if (l2<radius*radius)
+				if (l2 < radius * radius)
 				{
-					dist  = b3Sqrt(l2);
-					if (dist>minDist)
+					dist = b3Sqrt(l2);
+					if (dist > minDist)
 					{
 						minDist = dist;
 						closestPnt = out;
-						localHitNormal = tmp/dist;
-						region=2;
+						localHitNormal = tmp / dist;
+						region = 2;
 					}
-					
-				} else
+				}
+				else
 				{
 					bCollide = false;
 					break;
@@ -109,12 +102,12 @@ void	computeContactSphereConvex(int pairIndex,
 		}
 		else
 		{
-			if ( dist > minDist )
+			if (dist > minDist)
 			{
 				minDist = dist;
 				closestPnt = pntReturn;
 				localHitNormal = planeEqn;
-				region=3;
+				region = 3;
 			}
 		}
 	}
@@ -123,40 +116,38 @@ void	computeContactSphereConvex(int pairIndex,
 
 	if (bCollide && minDist > -10000)
 	{
-		
-		float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
+		float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal;  //-hitNormalWorld;
 		float4 pOnB1 = tr(closestPnt);
 		//printf("dist ,%f,",minDist);
-		float actualDepth = minDist-radius;
-		if (actualDepth<0)
-		{
-		//printf("actualDepth = ,%f,", actualDepth);
-		//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
-		//printf("region=,%d,\n", region);
-		pOnB1[3] = actualDepth;
-
-		int dstIdx;
-//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
-		
-		if (nGlobalContactsOut < maxContactCapacity)
+		float actualDepth = minDist - radius;
+		if (actualDepth < 0)
 		{
-			dstIdx=nGlobalContactsOut;
-			nGlobalContactsOut++;
-
-			b3Contact4* c = &globalContactsOut[dstIdx];
-			c->m_worldNormalOnB = normalOnSurfaceB1;
-			c->setFrictionCoeff(0.7);
-			c->setRestituitionCoeff(0.f);
-
-			c->m_batchIdx = pairIndex;
-			c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
-			c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
-			c->m_worldPosB[0] = pOnB1;
-			int numPoints = 1;
-			c->m_worldNormalOnB.w = (b3Scalar)numPoints;
-		}//if (dstIdx < numPairs)
+			//printf("actualDepth = ,%f,", actualDepth);
+			//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
+			//printf("region=,%d,\n", region);
+			pOnB1[3] = actualDepth;
+
+			int dstIdx;
+			//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
+
+			if (nGlobalContactsOut < maxContactCapacity)
+			{
+				dstIdx = nGlobalContactsOut;
+				nGlobalContactsOut++;
+
+				b3Contact4* c = &globalContactsOut[dstIdx];
+				c->m_worldNormalOnB = normalOnSurfaceB1;
+				c->setFrictionCoeff(0.7);
+				c->setRestituitionCoeff(0.f);
+
+				c->m_batchIdx = pairIndex;
+				c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
+				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
+				c->m_worldPosB[0] = pOnB1;
+				int numPoints = 1;
+				c->m_worldNormalOnB.w = (b3Scalar)numPoints;
+			}  //if (dstIdx < numPairs)
 		}
-	}//if (hasCollision)
-	
+	}  //if (hasCollision)
 }
-#endif //B3_CONTACT_SPHERE_SPHERE_H
+#endif  //B3_CONTACT_SPHERE_SPHERE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h

@@ -2,8 +2,6 @@
 #ifndef B3_CONVEX_POLYHEDRON_DATA_H
 #define B3_CONVEX_POLYHEDRON_DATA_H
 
-
-
 #include "Bullet3Common/shared/b3Float4.h"
 #include "Bullet3Common/shared/b3Quat.h"
 
@@ -21,20 +19,20 @@ typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
 
 struct b3ConvexPolyhedronData
 {
-	b3Float4		m_localCenter;
-	b3Float4		m_extents;
-	b3Float4		mC;
-	b3Float4		mE;
+	b3Float4 m_localCenter;
+	b3Float4 m_extents;
+	b3Float4 mC;
+	b3Float4 mE;
 
-	float			m_radius;
-	int	m_faceOffset;
+	float m_radius;
+	int m_faceOffset;
 	int m_numFaces;
-	int	m_numVertices;
+	int m_numVertices;
 
 	int m_vertexOffset;
-	int	m_uniqueEdgesOffset;
-	int	m_numUniqueEdges;
+	int m_uniqueEdgesOffset;
+	int m_numUniqueEdges;
 	int m_unused;
 };
 
-#endif //B3_CONVEX_POLYHEDRON_DATA_H
+#endif  //B3_CONVEX_POLYHEDRON_DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h

@@ -1,28 +1,27 @@
 #ifndef B3_FIND_SEPARATING_AXIS_H
 #define B3_FIND_SEPARATING_AXIS_H
 
-
-inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull,  const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
+inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
 {
 	min = FLT_MAX;
 	max = -FLT_MAX;
 	int numVerts = hull.m_numVertices;
 
-	const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir);
+	const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
 
-	b3Scalar offset = b3Dot3F4(pos,dir);
+	b3Scalar offset = b3Dot3F4(pos, dir);
 
-	for(int i=0;i<numVerts;i++)
+	for (int i = 0; i < numVerts; i++)
 	{
 		//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
 		//b3Scalar dp = pt.dot(dir);
 		//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
-		b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset+i],localDir);
+		b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
 		//b3Assert(dp==dpL);
-		if(dp < min)	min = dp;
-		if(dp > max)	max = dp;
+		if (dp < min) min = dp;
+		if (dp > max) max = dp;
 	}
-	if(min>max)
+	if (min > max)
 	{
 		b3Scalar tmp = min;
 		min = max;
@@ -32,44 +31,42 @@ inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull,  const b3Float4& p
 	max += offset;
 }
 
-
-inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, 
-	const b3Float4& posA,const b3Quaternion& ornA,
-	const b3Float4& posB,const b3Quaternion& ornB,
-	const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth)
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+						  const b3Float4& posA, const b3Quaternion& ornA,
+						  const b3Float4& posB, const b3Quaternion& ornB,
+						  const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
 {
-	b3Scalar Min0,Max0;
-	b3Scalar Min1,Max1;
-	b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0);
-	b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1);
+	b3Scalar Min0, Max0;
+	b3Scalar Min1, Max1;
+	b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
+	b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
 
-	if(Max0<Min1 || Max1<Min0)
+	if (Max0 < Min1 || Max1 < Min0)
 		return false;
 
 	b3Scalar d0 = Max0 - Min1;
-	b3Assert(d0>=0.0f);
+	b3Assert(d0 >= 0.0f);
 	b3Scalar d1 = Max1 - Min0;
-	b3Assert(d1>=0.0f);
-	depth = d0<d1 ? d0:d1;
+	b3Assert(d1 >= 0.0f);
+	depth = d0 < d1 ? d0 : d1;
 	return true;
 }
 
-
-inline bool b3FindSeparatingAxis(	const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, 
-	const b3Float4& posA1,
-	const b3Quaternion& ornA,
-	const b3Float4& posB1,
-	const b3Quaternion& ornB,
-	const b3AlignedObjectArray<b3Vector3>& verticesA,
-	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, 
-	const b3AlignedObjectArray<b3GpuFace>& facesA,
-	const b3AlignedObjectArray<int>& indicesA,
-	const b3AlignedObjectArray<b3Vector3>& verticesB, 
-	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB, 
-	const b3AlignedObjectArray<b3GpuFace>& facesB,
-	const b3AlignedObjectArray<int>& indicesB,
-
-	b3Vector3& sep)
+inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+								 const b3Float4& posA1,
+								 const b3Quaternion& ornA,
+								 const b3Float4& posB1,
+								 const b3Quaternion& ornB,
+								 const b3AlignedObjectArray<b3Vector3>& verticesA,
+								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
+								 const b3AlignedObjectArray<b3GpuFace>& facesA,
+								 const b3AlignedObjectArray<int>& indicesA,
+								 const b3AlignedObjectArray<b3Vector3>& verticesB,
+								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
+								 const b3AlignedObjectArray<b3GpuFace>& facesB,
+								 const b3AlignedObjectArray<int>& indicesB,
+
+								 b3Vector3& sep)
 {
 	B3_PROFILE("findSeparatingAxis");
 
@@ -77,42 +74,41 @@ inline bool b3FindSeparatingAxis(	const b3ConvexPolyhedronData& hullA, const b3C
 	posA.w = 0.f;
 	b3Float4 posB = posB1;
 	posB.w = 0.f;
-//#ifdef TEST_INTERNAL_OBJECTS
+	//#ifdef TEST_INTERNAL_OBJECTS
 	b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
 
 	b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
 	b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
-	b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
+	b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
 	const b3Float4 deltaC2 = c0 - c1;
-//#endif
+	//#endif
 
 	b3Scalar dmin = FLT_MAX;
-	int curPlaneTests=0;
+	int curPlaneTests = 0;
 
 	int numFacesA = hullA.m_numFaces;
 	// Test normals from hullA
-	for(int i=0;i<numFacesA;i++)
+	for (int i = 0; i < numFacesA; i++)
 	{
-		const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane;
-		b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
+		const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
+		b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
 
-		if (b3Dot3F4(deltaC2,faceANormalWS)<0)
-			faceANormalWS*=-1.f;
+		if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
+			faceANormalWS *= -1.f;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
 		gExpectedNbTests++;
-		if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
 			continue;
 		gActualNbTests++;
 #endif
 
-		
 		b3Scalar d;
-		if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d))
+		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
 			return false;
 
-		if(d<dmin)
+		if (d < dmin)
 		{
 			dmin = d;
 			sep = (b3Vector3&)faceANormalWS;
@@ -121,86 +117,81 @@ inline bool b3FindSeparatingAxis(	const b3ConvexPolyhedronData& hullA, const b3C
 
 	int numFacesB = hullB.m_numFaces;
 	// Test normals from hullB
-	for(int i=0;i<numFacesB;i++)
+	for (int i = 0; i < numFacesB; i++)
 	{
-		b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane;
+		b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
 		b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
 
-		if (b3Dot3F4(deltaC2,WorldNormal)<0)
+		if (b3Dot3F4(deltaC2, WorldNormal) < 0)
 		{
-			WorldNormal*=-1.f;
+			WorldNormal *= -1.f;
 		}
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
 		gExpectedNbTests++;
-		if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
 			continue;
 		gActualNbTests++;
 #endif
 
 		b3Scalar d;
-		if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d))
+		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
 			return false;
 
-		if(d<dmin)
+		if (d < dmin)
 		{
 			dmin = d;
 			sep = (b3Vector3&)WorldNormal;
 		}
 	}
 
-//	b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
+	//	b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
 
 	int curEdgeEdge = 0;
 	// Test edges
-	for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
+	for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
 	{
-		const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0];
-		b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0);
+		const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
+		b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
 
-		for(int e1=0;e1<hullB.m_numUniqueEdges;e1++)
+		for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
 		{
-			const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1];
-			b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1);
-
+			const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
+			b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
 
-			b3Float4 crossje = b3Cross3(edge0World,edge1World);
+			b3Float4 crossje = b3Cross3(edge0World, edge1World);
 
 			curEdgeEdge++;
-			if(!b3IsAlmostZero((b3Vector3&)crossje))
+			if (!b3IsAlmostZero((b3Vector3&)crossje))
 			{
 				crossje = b3FastNormalized3(crossje);
-				if (b3Dot3F4(deltaC2,crossje)<0)
-					crossje*=-1.f;
-
+				if (b3Dot3F4(deltaC2, crossje) < 0)
+					crossje *= -1.f;
 
 #ifdef TEST_INTERNAL_OBJECTS
 				gExpectedNbTests++;
-				if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
+				if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
 					continue;
 				gActualNbTests++;
 #endif
 
 				b3Scalar dist;
-				if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist))
+				if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
 					return false;
 
-				if(dist<dmin)
+				if (dist < dmin)
 				{
 					dmin = dist;
 					sep = (b3Vector3&)crossje;
 				}
 			}
 		}
-
 	}
 
-	
-	if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f)
+	if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
 		sep = -sep;
 
 	return true;
 }
 
-#endif //B3_FIND_SEPARATING_AXIS_H
-
+#endif  //B3_FIND_SEPARATING_AXIS_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h

@@ -15,9 +15,6 @@
  *  See the License for more information.
  */
 
-
-
-
 #ifndef B3_MPR_PENETRATION_H
 #define B3_MPR_PENETRATION_H
 
@@ -27,9 +24,6 @@
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
 
-
-
-
 #ifdef __cplusplus
 #define B3_MPR_SQRT sqrtf
 #else
@@ -41,196 +35,171 @@
 #define B3_MPR_TOLERANCE 1E-6f
 #define B3_MPR_MAX_ITERATIONS 1000
 
-struct _b3MprSupport_t 
+struct _b3MprSupport_t
 {
-    b3Float4 v;  //!< Support point in minkowski sum
-    b3Float4 v1; //!< Support point in obj1
-    b3Float4 v2; //!< Support point in obj2
+	b3Float4 v;   //!< Support point in minkowski sum
+	b3Float4 v1;  //!< Support point in obj1
+	b3Float4 v2;  //!< Support point in obj2
 };
 typedef struct _b3MprSupport_t b3MprSupport_t;
 
-struct _b3MprSimplex_t 
+struct _b3MprSimplex_t
 {
-    b3MprSupport_t ps[4];
-    int last; //!< index of last added point
+	b3MprSupport_t ps[4];
+	int last;  //!< index of last added point
 };
 typedef struct _b3MprSimplex_t b3MprSimplex_t;
 
-inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
+inline b3MprSupport_t *b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
 {
-    return &s->ps[idx];
+	return &s->ps[idx];
 }
 
 inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
 {
-    s->last = size - 1;
+	s->last = size - 1;
 }
 
-
 inline int b3MprSimplexSize(const b3MprSimplex_t *s)
 {
-    return s->last + 1;
+	return s->last + 1;
 }
 
-
-inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)
+inline const b3MprSupport_t *b3MprSimplexPoint(const b3MprSimplex_t *s, int idx)
 {
-    // here is no check on boundaries
-    return &s->ps[idx];
+	// here is no check on boundaries
+	return &s->ps[idx];
 }
 
 inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)
 {
-    *d = *s;
+	*d = *s;
 }
 
 inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)
 {
-    b3MprSupportCopy(s->ps + pos, a);
+	b3MprSupportCopy(s->ps + pos, a);
 }
 
-
 inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
 {
-    b3MprSupport_t supp;
+	b3MprSupport_t supp;
 
-    b3MprSupportCopy(&supp, &s->ps[pos1]);
-    b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
-    b3MprSupportCopy(&s->ps[pos2], &supp);
+	b3MprSupportCopy(&supp, &s->ps[pos1]);
+	b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
+	b3MprSupportCopy(&s->ps[pos2], &supp);
 }
 
-
 inline int b3MprIsZero(float val)
 {
-    return B3_MPR_FABS(val) < FLT_EPSILON;
+	return B3_MPR_FABS(val) < FLT_EPSILON;
 }
 
-
-
 inline int b3MprEq(float _a, float _b)
 {
-    float ab;
-    float a, b;
+	float ab;
+	float a, b;
 
-    ab = B3_MPR_FABS(_a - _b);
-    if (B3_MPR_FABS(ab) < FLT_EPSILON)
-        return 1;
+	ab = B3_MPR_FABS(_a - _b);
+	if (B3_MPR_FABS(ab) < FLT_EPSILON)
+		return 1;
 
-    a = B3_MPR_FABS(_a);
-    b = B3_MPR_FABS(_b);
-    if (b > a){
-        return ab < FLT_EPSILON * b;
-    }else{
-        return ab < FLT_EPSILON * a;
-    }
+	a = B3_MPR_FABS(_a);
+	b = B3_MPR_FABS(_b);
+	if (b > a)
+	{
+		return ab < FLT_EPSILON * b;
+	}
+	else
+	{
+		return ab < FLT_EPSILON * a;
+	}
 }
 
-
-inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)
+inline int b3MprVec3Eq(const b3Float4 *a, const b3Float4 *b)
 {
-    return b3MprEq((*a).x, (*b).x)
-            && b3MprEq((*a).y, (*b).y)
-            && b3MprEq((*a).z, (*b).z);
+	return b3MprEq((*a).x, (*b).x) && b3MprEq((*a).y, (*b).y) && b3MprEq((*a).z, (*b).z);
 }
 
-
-
-inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, 	b3ConstArray(b3Float4) verticesA)
+inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec, __global const b3ConvexPolyhedronData_t *hull, b3ConstArray(b3Float4) verticesA)
 {
-	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
+	b3Float4 supVec = b3MakeFloat4(0, 0, 0, 0);
 	float maxDot = -B3_LARGE_FLOAT;
 
-    if( 0 < hull->m_numVertices )
-    {
-        const b3Float4 scaled = supportVec;
+	if (0 < hull->m_numVertices)
+	{
+		const b3Float4 scaled = supportVec;
 		int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
-        return verticesA[hull->m_vertexOffset+index];
-    }
-
-    return supVec;
+		return verticesA[hull->m_vertexOffset + index];
+	}
 
+	return supVec;
 }
 
-
-B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
-													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
-													b3ConstArray(b3Collidable_t)				cpuCollidables,
-													b3ConstArray(b3Float4)					cpuVertices,
-													__global b3Float4* sepAxis,
-														const b3Float4* _dir, b3Float4* outp, int logme)
+B3_STATIC void b3MprConvexSupport(int pairIndex, int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+								  b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+								  b3ConstArray(b3Collidable_t) cpuCollidables,
+								  b3ConstArray(b3Float4) cpuVertices,
+								  __global b3Float4 *sepAxis,
+								  const b3Float4 *_dir, b3Float4 *outp, int logme)
 {
 	//dir is in worldspace, move to local space
-	
+
 	b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;
 	b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;
-	
-	b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);
-	
-	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);
-	
+
+	b3Float4 dir = b3MakeFloat4((*_dir).x, (*_dir).y, (*_dir).z, 0.f);
+
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), dir);
 
 	//find local support vertex
 	int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
-	
-	b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);
-	__global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
-	
+
+	b3Assert(cpuCollidables[colIndex].m_shapeType == SHAPE_CONVEX_HULL);
+	__global const b3ConvexPolyhedronData_t *hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
+
 	b3Float4 pInA;
 	if (logme)
 	{
-
-
-	//	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
+		//	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
 		float maxDot = -B3_LARGE_FLOAT;
 
-		if( 0 < hull->m_numVertices )
+		if (0 < hull->m_numVertices)
 		{
 			const b3Float4 scaled = localDir;
 			int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
-			pInA = cpuVertices[hull->m_vertexOffset+index];
-			
+			pInA = cpuVertices[hull->m_vertexOffset + index];
 		}
-
-
-	} else
+	}
+	else
 	{
-		pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);
+		pInA = b3LocalGetSupportVertex(localDir, hull, cpuVertices);
 	}
 
 	//move vertex to world space
-	*outp = b3TransformPoint(pInA,pos,orn);
-	
+	*outp = b3TransformPoint(pInA, pos, orn);
 }
 
-inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB,   b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
-													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
-													b3ConstArray(b3Collidable_t)				cpuCollidables,
-													b3ConstArray(b3Float4)					cpuVertices,
-													__global b3Float4* sepAxis,
-													const b3Float4* _dir, b3MprSupport_t *supp)
+inline void b3MprSupport(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+						 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+						 b3ConstArray(b3Collidable_t) cpuCollidables,
+						 b3ConstArray(b3Float4) cpuVertices,
+						 __global b3Float4 *sepAxis,
+						 const b3Float4 *_dir, b3MprSupport_t *supp)
 {
-    b3Float4 dir;
+	b3Float4 dir;
 	dir = *_dir;
-	b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);
-    dir = *_dir*-1.f;
-	b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);
-    supp->v = supp->v1 - supp->v2;
+	b3MprConvexSupport(pairIndex, bodyIndexA, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v1, 0);
+	dir = *_dir * -1.f;
+	b3MprConvexSupport(pairIndex, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v2, 0);
+	supp->v = supp->v1 - supp->v2;
 }
 
-
-
-
-
-
-
-
-
 inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
 {
-
-    center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
+	center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
 	center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
-    center->v = center->v1 - center->v2;
+	center->v = center->v1 - center->v2;
 }
 
 inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
@@ -243,48 +212,45 @@ inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
 
 inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)
 {
-    (*v).x += (*w).x;
-    (*v).y += (*w).y;
-    (*v).z += (*w).z;
+	(*v).x += (*w).x;
+	(*v).y += (*w).y;
+	(*v).z += (*w).z;
 }
 
 inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)
 {
-    *v = *w;
+	*v = *w;
 }
 
 inline void b3MprVec3Scale(b3Float4 *d, float k)
 {
-    *d *= k;
+	*d *= k;
 }
 
 inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
 {
-    float dot;
+	float dot;
 
-	dot = b3Dot3F4(*a,*b);
-    return dot;
+	dot = b3Dot3F4(*a, *b);
+	return dot;
 }
 
-
 inline float b3MprVec3Len2(const b3Float4 *v)
 {
-    return b3MprVec3Dot(v, v);
+	return b3MprVec3Dot(v, v);
 }
 
 inline void b3MprVec3Normalize(b3Float4 *d)
 {
-    float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
-    b3MprVec3Scale(d, k);
+	float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
+	b3MprVec3Scale(d, k);
 }
 
 inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
 {
-	*d = b3Cross3(*a,*b);
-	
+	*d = b3Cross3(*a, *b);
 }
 
-
 inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
 {
 	*d = *v - *w;
@@ -292,629 +258,631 @@ inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
 
 inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
 {
-    b3Float4 v2v1, v3v1;
+	b3Float4 v2v1, v3v1;
 
-    b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
-                       &b3MprSimplexPoint(portal, 1)->v);
-    b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
-                       &b3MprSimplexPoint(portal, 1)->v);
-    b3MprVec3Cross(dir, &v2v1, &v3v1);
-    b3MprVec3Normalize(dir);
+	b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
+				  &b3MprSimplexPoint(portal, 1)->v);
+	b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
+				  &b3MprSimplexPoint(portal, 1)->v);
+	b3MprVec3Cross(dir, &v2v1, &v3v1);
+	b3MprVec3Normalize(dir);
 }
 
-
 inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
-                                       const b3Float4 *dir)
+								  const b3Float4 *dir)
 {
-    float dot;
-    dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
-    return b3MprIsZero(dot) || dot > 0.f;
+	float dot;
+	dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
+	return b3MprIsZero(dot) || dot > 0.f;
 }
 
 inline int portalReachTolerance(const b3MprSimplex_t *portal,
-                                     const b3MprSupport_t *v4,
-                                     const b3Float4 *dir)
+								const b3MprSupport_t *v4,
+								const b3Float4 *dir)
 {
-    float dv1, dv2, dv3, dv4;
-    float dot1, dot2, dot3;
+	float dv1, dv2, dv3, dv4;
+	float dot1, dot2, dot3;
 
-    // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
+	// find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
 
-    dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
-    dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
-    dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
-    dv4 = b3MprVec3Dot(&v4->v, dir);
+	dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
+	dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
+	dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
+	dv4 = b3MprVec3Dot(&v4->v, dir);
 
-    dot1 = dv4 - dv1;
-    dot2 = dv4 - dv2;
-    dot3 = dv4 - dv3;
+	dot1 = dv4 - dv1;
+	dot2 = dv4 - dv2;
+	dot3 = dv4 - dv3;
 
-    dot1 = B3_MPR_FMIN(dot1, dot2);
-    dot1 = B3_MPR_FMIN(dot1, dot3);
+	dot1 = B3_MPR_FMIN(dot1, dot2);
+	dot1 = B3_MPR_FMIN(dot1, dot3);
 
-    return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
+	return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
 }
 
-inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,   
-                                         const b3MprSupport_t *v4,
-                                         const b3Float4 *dir)
+inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,
+									const b3MprSupport_t *v4,
+									const b3Float4 *dir)
 {
-    float dot;
-    dot = b3MprVec3Dot(&v4->v, dir);
-    return b3MprIsZero(dot) || dot > 0.f;
+	float dot;
+	dot = b3MprVec3Dot(&v4->v, dir);
+	return b3MprIsZero(dot) || dot > 0.f;
 }
 
 inline void b3ExpandPortal(b3MprSimplex_t *portal,
-                              const b3MprSupport_t *v4)
-{
-    float dot;
-    b3Float4 v4v0;
-
-    b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
-    dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
-    if (dot > 0.f){
-        dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
-        if (dot > 0.f){
-            b3MprSimplexSet(portal, 1, v4);
-        }else{
-            b3MprSimplexSet(portal, 3, v4);
-        }
-    }else{
-        dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
-        if (dot > 0.f){
-            b3MprSimplexSet(portal, 2, v4);
-        }else{
-            b3MprSimplexSet(portal, 1, v4);
-        }
-    }
-}
-
-
-
-B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
-													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
-													b3ConstArray(b3Collidable_t)				cpuCollidables,
-													b3ConstArray(b3Float4)					cpuVertices,
-													__global b3Float4* sepAxis,
-													__global int*	hasSepAxis,
-													b3MprSimplex_t *portal)
-{
-    b3Float4 dir, va, vb;
-    float dot;
-    int cont;
-	
-	
-
-    // vertex 0 is center of portal
-    b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));
-    // vertex 0 is center of portal
-    b3MprSimplexSetSize(portal, 1);
-	
-
-
-	b3Float4 zero = b3MakeFloat4(0,0,0,0);
-	b3Float4* b3mpr_vec3_origin = &zero;
-
-    if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){
-        // Portal's center lies on origin (0,0,0) => we know that objects
-        // intersect but we would need to know penetration info.
-        // So move center little bit...
-        b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
-        b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
-    }
-
-
-    // vertex 1 = support in direction of origin
-    b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
-    b3MprVec3Scale(&dir, -1.f);
-    b3MprVec3Normalize(&dir);
-
-
-    b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));
-
-    b3MprSimplexSetSize(portal, 2);
-
-    // test if origin isn't outside of v1
-    dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
-	
-
-    if (b3MprIsZero(dot) || dot < 0.f)
-        return -1;
-
-
-    // vertex 2
-    b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
-                       &b3MprSimplexPoint(portal, 1)->v);
-    if (b3MprIsZero(b3MprVec3Len2(&dir))){
-        if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){
-            // origin lies on v1
-            return 1;
-        }else{
-            // origin lies on v0-v1 segment
-            return 2;
-        }
-    }
-
-    b3MprVec3Normalize(&dir);
-	 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));
-    
-    dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
-    if (b3MprIsZero(dot) || dot < 0.f)
-        return -1;
-
-    b3MprSimplexSetSize(portal, 3);
-
-    // vertex 3 direction
-    b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
-                     &b3MprSimplexPoint(portal, 0)->v);
-    b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
-                     &b3MprSimplexPoint(portal, 0)->v);
-    b3MprVec3Cross(&dir, &va, &vb);
-    b3MprVec3Normalize(&dir);
-
-    // it is better to form portal faces to be oriented "outside" origin
-    dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
-    if (dot > 0.f){
-        b3MprSimplexSwap(portal, 1, 2);
-        b3MprVec3Scale(&dir, -1.f);
-    }
-
-    while (b3MprSimplexSize(portal) < 4){
-		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));
-        
-        dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
-        if (b3MprIsZero(dot) || dot < 0.f)
-            return -1;
-
-        cont = 0;
-
-        // test if origin is outside (v1, v0, v3) - set v2 as v3 and
-        // continue
-        b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
-                          &b3MprSimplexPoint(portal, 3)->v);
-        dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
-        if (dot < 0.f && !b3MprIsZero(dot)){
-            b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
-            cont = 1;
-        }
-
-        if (!cont){
-            // test if origin is outside (v3, v0, v2) - set v1 as v3 and
-            // continue
-            b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
-                              &b3MprSimplexPoint(portal, 2)->v);
-            dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
-            if (dot < 0.f && !b3MprIsZero(dot)){
-                b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
-                cont = 1;
-            }
-        }
-
-        if (cont){
-            b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
-                             &b3MprSimplexPoint(portal, 0)->v);
-            b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
-                             &b3MprSimplexPoint(portal, 0)->v);
-            b3MprVec3Cross(&dir, &va, &vb);
-            b3MprVec3Normalize(&dir);
-        }else{
-            b3MprSimplexSetSize(portal, 4);
-        }
-    }
-
-    return 0;
-}
-
-
-B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
-													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
-													b3ConstArray(b3Collidable_t)				cpuCollidables,
-													b3ConstArray(b3Float4)					cpuVertices,
-													__global b3Float4* sepAxis,
-													b3MprSimplex_t *portal)
-{
-    b3Float4 dir;
-    b3MprSupport_t v4;
-
-	for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)
-    //while (1)
+						   const b3MprSupport_t *v4)
+{
+	float dot;
+	b3Float4 v4v0;
+
+	b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
+	if (dot > 0.f)
 	{
-        // compute direction outside the portal (from v0 throught v1,v2,v3
-        // face)
-        b3PortalDir(portal, &dir);
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
+		if (dot > 0.f)
+		{
+			b3MprSimplexSet(portal, 1, v4);
+		}
+		else
+		{
+			b3MprSimplexSet(portal, 3, v4);
+		}
+	}
+	else
+	{
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
+		if (dot > 0.f)
+		{
+			b3MprSimplexSet(portal, 2, v4);
+		}
+		else
+		{
+			b3MprSimplexSet(portal, 1, v4);
+		}
+	}
+}
 
-        // test if origin is inside the portal
-        if (portalEncapsulesOrigin(portal, &dir))
-            return 0;
+B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							   b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							   b3ConstArray(b3Collidable_t) cpuCollidables,
+							   b3ConstArray(b3Float4) cpuVertices,
+							   __global b3Float4 *sepAxis,
+							   __global int *hasSepAxis,
+							   b3MprSimplex_t *portal)
+{
+	b3Float4 dir, va, vb;
+	float dot;
+	int cont;
 
-        // get next support point
-        
-		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
+	// vertex 0 is center of portal
+	b3FindOrigin(bodyIndexA, bodyIndexB, cpuBodyBuf, b3MprSimplexPointW(portal, 0));
+	// vertex 0 is center of portal
+	b3MprSimplexSetSize(portal, 1);
 
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
 
-        // test if v4 can expand portal to contain origin and if portal
-        // expanding doesn't reach given tolerance
-        if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
-                || portalReachTolerance(portal, &v4, &dir))
+	if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin))
+	{
+		// Portal's center lies on origin (0,0,0) => we know that objects
+		// intersect but we would need to know penetration info.
+		// So move center little bit...
+		b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
+		b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
+	}
+
+	// vertex 1 = support in direction of origin
+	b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Scale(&dir, -1.f);
+	b3MprVec3Normalize(&dir);
+
+	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 1));
+
+	b3MprSimplexSetSize(portal, 2);
+
+	// test if origin isn't outside of v1
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
+
+	if (b3MprIsZero(dot) || dot < 0.f)
+		return -1;
+
+	// vertex 2
+	b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	if (b3MprIsZero(b3MprVec3Len2(&dir)))
+	{
+		if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin))
 		{
-            return -1;
-        }
+			// origin lies on v1
+			return 1;
+		}
+		else
+		{
+			// origin lies on v0-v1 segment
+			return 2;
+		}
+	}
+
+	b3MprVec3Normalize(&dir);
+	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 2));
+
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
+	if (b3MprIsZero(dot) || dot < 0.f)
+		return -1;
+
+	b3MprSimplexSetSize(portal, 3);
+
+	// vertex 3 direction
+	b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+				  &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+				  &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Cross(&dir, &va, &vb);
+	b3MprVec3Normalize(&dir);
+
+	// it is better to form portal faces to be oriented "outside" origin
+	dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
+	if (dot > 0.f)
+	{
+		b3MprSimplexSwap(portal, 1, 2);
+		b3MprVec3Scale(&dir, -1.f);
+	}
+
+	while (b3MprSimplexSize(portal) < 4)
+	{
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 3));
 
-        // v1-v2-v3 triangle must be rearranged to face outside Minkowski
-        // difference (direction from v0).
-        b3ExpandPortal(portal, &v4);
-    }
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
+		if (b3MprIsZero(dot) || dot < 0.f)
+			return -1;
 
-    return -1;
+		cont = 0;
+
+		// test if origin is outside (v1, v0, v3) - set v2 as v3 and
+		// continue
+		b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
+					   &b3MprSimplexPoint(portal, 3)->v);
+		dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+		if (dot < 0.f && !b3MprIsZero(dot))
+		{
+			b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
+			cont = 1;
+		}
+
+		if (!cont)
+		{
+			// test if origin is outside (v3, v0, v2) - set v1 as v3 and
+			// continue
+			b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
+						   &b3MprSimplexPoint(portal, 2)->v);
+			dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+			if (dot < 0.f && !b3MprIsZero(dot))
+			{
+				b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
+				cont = 1;
+			}
+		}
+
+		if (cont)
+		{
+			b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+						  &b3MprSimplexPoint(portal, 0)->v);
+			b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+						  &b3MprSimplexPoint(portal, 0)->v);
+			b3MprVec3Cross(&dir, &va, &vb);
+			b3MprVec3Normalize(&dir);
+		}
+		else
+		{
+			b3MprSimplexSetSize(portal, 4);
+		}
+	}
+
+	return 0;
 }
 
-B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
+B3_STATIC int b3RefinePortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							 b3ConstArray(b3Collidable_t) cpuCollidables,
+							 b3ConstArray(b3Float4) cpuVertices,
+							 __global b3Float4 *sepAxis,
+							 b3MprSimplex_t *portal)
 {
+	b3Float4 dir;
+	b3MprSupport_t v4;
 
-	b3Float4 zero = b3MakeFloat4(0,0,0,0);
-	b3Float4* b3mpr_vec3_origin = &zero;
+	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
+	//while (1)
+	{
+		// compute direction outside the portal (from v0 throught v1,v2,v3
+		// face)
+		b3PortalDir(portal, &dir);
 
-    b3Float4 dir;
-    size_t i;
-    float b[4], sum, inv;
-    b3Float4 vec, p1, p2;
+		// test if origin is inside the portal
+		if (portalEncapsulesOrigin(portal, &dir))
+			return 0;
 
-    b3PortalDir(portal, &dir);
+		// get next support point
 
-    // use barycentric coordinates of tetrahedron to find origin
-    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
-                       &b3MprSimplexPoint(portal, 2)->v);
-    b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
 
-    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
-                       &b3MprSimplexPoint(portal, 2)->v);
-    b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+		// test if v4 can expand portal to contain origin and if portal
+		// expanding doesn't reach given tolerance
+		if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
+		{
+			return -1;
+		}
 
-    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
-                       &b3MprSimplexPoint(portal, 1)->v);
-    b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+		// v1-v2-v3 triangle must be rearranged to face outside Minkowski
+		// difference (direction from v0).
+		b3ExpandPortal(portal, &v4);
+	}
 
-    b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
-                       &b3MprSimplexPoint(portal, 1)->v);
-    b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+	return -1;
+}
+
+B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
+{
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	b3Float4 dir;
+	size_t i;
+	float b[4], sum, inv;
+	b3Float4 vec, p1, p2;
+
+	b3PortalDir(portal, &dir);
+
+	// use barycentric coordinates of tetrahedron to find origin
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+				   &b3MprSimplexPoint(portal, 2)->v);
+	b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+				   &b3MprSimplexPoint(portal, 2)->v);
+	b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
 
 	sum = b[0] + b[1] + b[2] + b[3];
 
-    if (b3MprIsZero(sum) || sum < 0.f){
+	if (b3MprIsZero(sum) || sum < 0.f)
+	{
 		b[0] = 0.f;
 
-        b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
-                           &b3MprSimplexPoint(portal, 3)->v);
-        b[1] = b3MprVec3Dot(&vec, &dir);
-        b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
-                           &b3MprSimplexPoint(portal, 1)->v);
-        b[2] = b3MprVec3Dot(&vec, &dir);
-        b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
-                           &b3MprSimplexPoint(portal, 2)->v);
-        b[3] = b3MprVec3Dot(&vec, &dir);
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+					   &b3MprSimplexPoint(portal, 3)->v);
+		b[1] = b3MprVec3Dot(&vec, &dir);
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+					   &b3MprSimplexPoint(portal, 1)->v);
+		b[2] = b3MprVec3Dot(&vec, &dir);
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+					   &b3MprSimplexPoint(portal, 2)->v);
+		b[3] = b3MprVec3Dot(&vec, &dir);
 
 		sum = b[1] + b[2] + b[3];
 	}
 
 	inv = 1.f / sum;
 
-    b3MprVec3Copy(&p1, b3mpr_vec3_origin);
-    b3MprVec3Copy(&p2, b3mpr_vec3_origin);
-    for (i = 0; i < 4; i++){
-        b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
-        b3MprVec3Scale(&vec, b[i]);
-        b3MprVec3Add(&p1, &vec);
+	b3MprVec3Copy(&p1, b3mpr_vec3_origin);
+	b3MprVec3Copy(&p2, b3mpr_vec3_origin);
+	for (i = 0; i < 4; i++)
+	{
+		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
+		b3MprVec3Scale(&vec, b[i]);
+		b3MprVec3Add(&p1, &vec);
 
-        b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
-        b3MprVec3Scale(&vec, b[i]);
-        b3MprVec3Add(&p2, &vec);
-    }
-    b3MprVec3Scale(&p1, inv);
-    b3MprVec3Scale(&p2, inv);
+		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
+		b3MprVec3Scale(&vec, b[i]);
+		b3MprVec3Add(&p2, &vec);
+	}
+	b3MprVec3Scale(&p1, inv);
+	b3MprVec3Scale(&p2, inv);
 
-    b3MprVec3Copy(pos, &p1);
-    b3MprVec3Add(pos, &p2);
-    b3MprVec3Scale(pos, 0.5);
+	b3MprVec3Copy(pos, &p1);
+	b3MprVec3Add(pos, &p2);
+	b3MprVec3Scale(pos, 0.5);
 }
 
 inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)
 {
-    b3Float4 ab;
-    b3MprVec3Sub2(&ab, a, b);
-    return b3MprVec3Len2(&ab);
+	b3Float4 ab;
+	b3MprVec3Sub2(&ab, a, b);
+	return b3MprVec3Len2(&ab);
 }
 
 inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
-                                                  const b3Float4 *x0,
-                                                  const b3Float4 *b,
-                                                  b3Float4 *witness)
-{
-    // The computation comes from solving equation of segment:
-    //      S(t) = x0 + t.d
-    //          where - x0 is initial point of segment
-    //                - d is direction of segment from x0 (|d| > 0)
-    //                - t belongs to <0, 1> interval
-    // 
-    // Than, distance from a segment to some point P can be expressed:
-    //      D(t) = |x0 + t.d - P|^2
-    //          which is distance from any point on segment. Minimization
-    //          of this function brings distance from P to segment.
-    // Minimization of D(t) leads to simple quadratic equation that's
-    // solving is straightforward.
-    //
-    // Bonus of this method is witness point for free.
-
-    float dist, t;
-    b3Float4 d, a;
-
-    // direction of segment
-    b3MprVec3Sub2(&d, b, x0);
-
-    // precompute vector from P to x0
-    b3MprVec3Sub2(&a, x0, P);
-
-    t  = -1.f * b3MprVec3Dot(&a, &d);
-    t /= b3MprVec3Len2(&d);
-
-    if (t < 0.f || b3MprIsZero(t)){
-        dist = b3MprVec3Dist2(x0, P);
-        if (witness)
-            b3MprVec3Copy(witness, x0);
-    }else if (t > 1.f || b3MprEq(t, 1.f)){
-        dist = b3MprVec3Dist2(b, P);
-        if (witness)
-            b3MprVec3Copy(witness, b);
-    }else{
-        if (witness){
-            b3MprVec3Copy(witness, &d);
-            b3MprVec3Scale(witness, t);
-            b3MprVec3Add(witness, x0);
-            dist = b3MprVec3Dist2(witness, P);
-        }else{
-            // recycling variables
-            b3MprVec3Scale(&d, t);
-            b3MprVec3Add(&d, &a);
-            dist = b3MprVec3Len2(&d);
-        }
-    }
-
-    return dist;
-}
+										 const b3Float4 *x0,
+										 const b3Float4 *b,
+										 b3Float4 *witness)
+{
+	// The computation comes from solving equation of segment:
+	//      S(t) = x0 + t.d
+	//          where - x0 is initial point of segment
+	//                - d is direction of segment from x0 (|d| > 0)
+	//                - t belongs to <0, 1> interval
+	//
+	// Than, distance from a segment to some point P can be expressed:
+	//      D(t) = |x0 + t.d - P|^2
+	//          which is distance from any point on segment. Minimization
+	//          of this function brings distance from P to segment.
+	// Minimization of D(t) leads to simple quadratic equation that's
+	// solving is straightforward.
+	//
+	// Bonus of this method is witness point for free.
+
+	float dist, t;
+	b3Float4 d, a;
+
+	// direction of segment
+	b3MprVec3Sub2(&d, b, x0);
+
+	// precompute vector from P to x0
+	b3MprVec3Sub2(&a, x0, P);
+
+	t = -1.f * b3MprVec3Dot(&a, &d);
+	t /= b3MprVec3Len2(&d);
+
+	if (t < 0.f || b3MprIsZero(t))
+	{
+		dist = b3MprVec3Dist2(x0, P);
+		if (witness)
+			b3MprVec3Copy(witness, x0);
+	}
+	else if (t > 1.f || b3MprEq(t, 1.f))
+	{
+		dist = b3MprVec3Dist2(b, P);
+		if (witness)
+			b3MprVec3Copy(witness, b);
+	}
+	else
+	{
+		if (witness)
+		{
+			b3MprVec3Copy(witness, &d);
+			b3MprVec3Scale(witness, t);
+			b3MprVec3Add(witness, x0);
+			dist = b3MprVec3Dist2(witness, P);
+		}
+		else
+		{
+			// recycling variables
+			b3MprVec3Scale(&d, t);
+			b3MprVec3Add(&d, &a);
+			dist = b3MprVec3Len2(&d);
+		}
+	}
 
+	return dist;
+}
 
 inline float b3MprVec3PointTriDist2(const b3Float4 *P,
-                                const b3Float4 *x0, const b3Float4 *B,
-                                const b3Float4 *C,
-                                b3Float4 *witness)
-{
-    // Computation comes from analytic expression for triangle (x0, B, C)
-    //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
-    // Then equation for distance is:
-    //      D(s, t) = | T(s, t) - P |^2
-    // This leads to minimization of quadratic function of two variables.
-    // The solution from is taken only if s is between 0 and 1, t is
-    // between 0 and 1 and t + s < 1, otherwise distance from segment is
-    // computed.
-
-    b3Float4 d1, d2, a;
-    float u, v, w, p, q, r;
-    float s, t, dist, dist2;
-    b3Float4 witness2;
-
-    b3MprVec3Sub2(&d1, B, x0);
-    b3MprVec3Sub2(&d2, C, x0);
-    b3MprVec3Sub2(&a, x0, P);
-
-    u = b3MprVec3Dot(&a, &a);
-    v = b3MprVec3Dot(&d1, &d1);
-    w = b3MprVec3Dot(&d2, &d2);
-    p = b3MprVec3Dot(&a, &d1);
-    q = b3MprVec3Dot(&a, &d2);
-    r = b3MprVec3Dot(&d1, &d2);
-
-    s = (q * r - w * p) / (w * v - r * r);
-    t = (-s * r - q) / w;
-
-    if ((b3MprIsZero(s) || s > 0.f)
-            && (b3MprEq(s, 1.f) || s < 1.f)
-            && (b3MprIsZero(t) || t > 0.f)
-            && (b3MprEq(t, 1.f) || t < 1.f)
-            && (b3MprEq(t + s, 1.f) || t + s < 1.f)){
-
-        if (witness){
-            b3MprVec3Scale(&d1, s);
-            b3MprVec3Scale(&d2, t);
-            b3MprVec3Copy(witness, x0);
-            b3MprVec3Add(witness, &d1);
-            b3MprVec3Add(witness, &d2);
-
-            dist = b3MprVec3Dist2(witness, P);
-        }else{
-            dist  = s * s * v;
-            dist += t * t * w;
-            dist += 2.f * s * t * r;
-            dist += 2.f * s * p;
-            dist += 2.f * t * q;
-            dist += u;
-        }
-    }else{
-        dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
-
-        dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
-        if (dist2 < dist){
-            dist = dist2;
-            if (witness)
-                b3MprVec3Copy(witness, &witness2);
-        }
-
-        dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
-        if (dist2 < dist){
-            dist = dist2;
-            if (witness)
-                b3MprVec3Copy(witness, &witness2);
-        }
-    }
-
-    return dist;
-}
-
-
-B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB,  b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, 
-													b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
-													b3ConstArray(b3Collidable_t)				cpuCollidables,
-													b3ConstArray(b3Float4)					cpuVertices,
-													__global b3Float4* sepAxis,
-                       b3MprSimplex_t *portal,
-                       float *depth, b3Float4 *pdir, b3Float4 *pos)
-{
-    b3Float4 dir;
-    b3MprSupport_t v4;
-    unsigned long iterations;
-
-	b3Float4 zero = b3MakeFloat4(0,0,0,0);
-	b3Float4* b3mpr_vec3_origin = &zero;
-
-
-    iterations = 1UL;
-	for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++)
-    //while (1)
+									const b3Float4 *x0, const b3Float4 *B,
+									const b3Float4 *C,
+									b3Float4 *witness)
+{
+	// Computation comes from analytic expression for triangle (x0, B, C)
+	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
+	// Then equation for distance is:
+	//      D(s, t) = | T(s, t) - P |^2
+	// This leads to minimization of quadratic function of two variables.
+	// The solution from is taken only if s is between 0 and 1, t is
+	// between 0 and 1 and t + s < 1, otherwise distance from segment is
+	// computed.
+
+	b3Float4 d1, d2, a;
+	float u, v, w, p, q, r;
+	float s, t, dist, dist2;
+	b3Float4 witness2;
+
+	b3MprVec3Sub2(&d1, B, x0);
+	b3MprVec3Sub2(&d2, C, x0);
+	b3MprVec3Sub2(&a, x0, P);
+
+	u = b3MprVec3Dot(&a, &a);
+	v = b3MprVec3Dot(&d1, &d1);
+	w = b3MprVec3Dot(&d2, &d2);
+	p = b3MprVec3Dot(&a, &d1);
+	q = b3MprVec3Dot(&a, &d2);
+	r = b3MprVec3Dot(&d1, &d2);
+
+	s = (q * r - w * p) / (w * v - r * r);
+	t = (-s * r - q) / w;
+
+	if ((b3MprIsZero(s) || s > 0.f) && (b3MprEq(s, 1.f) || s < 1.f) && (b3MprIsZero(t) || t > 0.f) && (b3MprEq(t, 1.f) || t < 1.f) && (b3MprEq(t + s, 1.f) || t + s < 1.f))
+	{
+		if (witness)
+		{
+			b3MprVec3Scale(&d1, s);
+			b3MprVec3Scale(&d2, t);
+			b3MprVec3Copy(witness, x0);
+			b3MprVec3Add(witness, &d1);
+			b3MprVec3Add(witness, &d2);
+
+			dist = b3MprVec3Dist2(witness, P);
+		}
+		else
+		{
+			dist = s * s * v;
+			dist += t * t * w;
+			dist += 2.f * s * t * r;
+			dist += 2.f * s * p;
+			dist += 2.f * t * q;
+			dist += u;
+		}
+	}
+	else
+	{
+		dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
+
+		dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				b3MprVec3Copy(witness, &witness2);
+		}
+
+		dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				b3MprVec3Copy(witness, &witness2);
+		}
+	}
+
+	return dist;
+}
+
+B3_STATIC void b3FindPenetr(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							b3ConstArray(b3Collidable_t) cpuCollidables,
+							b3ConstArray(b3Float4) cpuVertices,
+							__global b3Float4 *sepAxis,
+							b3MprSimplex_t *portal,
+							float *depth, b3Float4 *pdir, b3Float4 *pos)
+{
+	b3Float4 dir;
+	b3MprSupport_t v4;
+	unsigned long iterations;
+
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	iterations = 1UL;
+	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
+	//while (1)
 	{
-        // compute portal direction and obtain next support point
-        b3PortalDir(portal, &dir);
-        
-		 b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4);
+		// compute portal direction and obtain next support point
+		b3PortalDir(portal, &dir);
 
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
 
-        // reached tolerance -> find penetration info
-        if (portalReachTolerance(portal, &v4, &dir)
-                || iterations ==B3_MPR_MAX_ITERATIONS)
+		// reached tolerance -> find penetration info
+		if (portalReachTolerance(portal, &v4, &dir) || iterations == B3_MPR_MAX_ITERATIONS)
 		{
-            *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);
-            *depth = B3_MPR_SQRT(*depth);
-			
+			*depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin, &b3MprSimplexPoint(portal, 1)->v, &b3MprSimplexPoint(portal, 2)->v, &b3MprSimplexPoint(portal, 3)->v, pdir);
+			*depth = B3_MPR_SQRT(*depth);
+
 			if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
 			{
-				
 				*pdir = dir;
-			} 
+			}
 			b3MprVec3Normalize(pdir);
-			
-            // barycentric coordinates:
-            b3FindPos(portal, pos);
 
+			// barycentric coordinates:
+			b3FindPos(portal, pos);
 
-            return;
-        }
+			return;
+		}
 
-        b3ExpandPortal(portal, &v4);
+		b3ExpandPortal(portal, &v4);
 
-        iterations++;
-    }
+		iterations++;
+	}
 }
 
-B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)
+B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal, float *depth, b3Float4 *dir, b3Float4 *pos)
 {
-    // Touching contact on portal's v1 - so depth is zero and direction
-    // is unimportant and pos can be guessed
-    *depth = 0.f;
-    b3Float4 zero = b3MakeFloat4(0,0,0,0);
-	b3Float4* b3mpr_vec3_origin = &zero;
-
+	// Touching contact on portal's v1 - so depth is zero and direction
+	// is unimportant and pos can be guessed
+	*depth = 0.f;
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
 
 	b3MprVec3Copy(dir, b3mpr_vec3_origin);
 
-    b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
-    b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
-    b3MprVec3Scale(pos, 0.5);
+	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+	b3MprVec3Scale(pos, 0.5);
 }
 
 B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
-                              float *depth, b3Float4 *dir, b3Float4 *pos)
+								   float *depth, b3Float4 *dir, b3Float4 *pos)
 {
-    
-    // Origin lies on v0-v1 segment.
-    // Depth is distance to v1, direction also and position must be
-    // computed
+	// Origin lies on v0-v1 segment.
+	// Depth is distance to v1, direction also and position must be
+	// computed
 
-    b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
-    b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
-    b3MprVec3Scale(pos, 0.5f);
+	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+	b3MprVec3Scale(pos, 0.5f);
 
-    
-    b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
-    *depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
-    b3MprVec3Normalize(dir);
+	b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
+	*depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
+	b3MprVec3Normalize(dir);
 }
 
-
-
 inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
-					b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
-					b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, 
-					b3ConstArray(b3Collidable_t)	cpuCollidables,
-					b3ConstArray(b3Float4)	cpuVertices,
-					__global b3Float4* sepAxis,
-					__global int*	hasSepAxis,
-					float *depthOut, b3Float4* dirOut, b3Float4* posOut)
-{
-	
-	 b3MprSimplex_t portal;
-
-	 
-//	if (!hasSepAxis[pairIndex])
+							b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							b3ConstArray(b3Collidable_t) cpuCollidables,
+							b3ConstArray(b3Float4) cpuVertices,
+							__global b3Float4 *sepAxis,
+							__global int *hasSepAxis,
+							float *depthOut, b3Float4 *dirOut, b3Float4 *posOut)
+{
+	b3MprSimplex_t portal;
+
+	//	if (!hasSepAxis[pairIndex])
 	//	return -1;
-	
+
 	hasSepAxis[pairIndex] = 0;
-	 int res;
+	int res;
+
+	// Phase 1: Portal discovery
+	res = b3DiscoverPortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, hasSepAxis, &portal);
 
-    // Phase 1: Portal discovery
-    res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);
-	
-	  
 	//sepAxis[pairIndex] = *pdir;//or -dir?
 
 	switch (res)
 	{
-	case 0:
+		case 0:
 		{
 			// Phase 2: Portal refinement
-		
-			res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);
+
+			res = b3RefinePortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal);
 			if (res < 0)
 				return -1;
 
 			// Phase 3. Penetration info
-			b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);
+			b3FindPenetr(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal, depthOut, dirOut, posOut);
 			hasSepAxis[pairIndex] = 1;
 			sepAxis[pairIndex] = -*dirOut;
 			break;
 		}
-	case 1:
+		case 1:
 		{
-			 // Touching contact on portal's v1.
+			// Touching contact on portal's v1.
 			b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);
 			break;
 		}
-	case 2:
+		case 2:
 		{
-			
-			b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);
+			b3FindPenetrSegment(&portal, depthOut, dirOut, posOut);
 			break;
 		}
-	default:
+		default:
 		{
-			hasSepAxis[pairIndex]=0;
+			hasSepAxis[pairIndex] = 0;
 			//if (res < 0)
 			//{
-				// Origin isn't inside portal - no collision.
-				return -1;
+			// Origin isn't inside portal - no collision.
+			return -1;
 			//}
 		}
 	};
-	
+
 	return 0;
 };
 
-
-
-#endif //B3_MPR_PENETRATION_H
+#endif  //B3_MPR_PENETRATION_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h

@@ -8,189 +8,168 @@
 
 #define GET_NPOINTS(x) (x).m_worldNormalOnB.w
 
-
 int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
 {
-	if( nPoints == 0 )
-        return 0;
-    
-    if (nPoints <=4)
-        return nPoints;
-    
-    
-    if (nPoints >64)
-        nPoints = 64;
-    
-	b3Float4 center = b3MakeFloat4(0,0,0,0);
+	if (nPoints == 0)
+		return 0;
+
+	if (nPoints <= 4)
+		return nPoints;
+
+	if (nPoints > 64)
+		nPoints = 64;
+
+	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
 	{
-		
-		for (int i=0;i<nPoints;i++)
+		for (int i = 0; i < nPoints; i++)
 			center += p[i];
 		center /= (float)nPoints;
 	}
-    
-	
-    
+
 	//	sample 4 directions
-    
-    b3Float4 aVector = p[0] - center;
-    b3Float4 u = b3Cross( nearNormal, aVector );
-    b3Float4 v = b3Cross( nearNormal, u );
-    u = b3Normalized( u );
-    v = b3Normalized( v );
-    
-    
-    //keep point with deepest penetration
-    float minW= FLT_MAX;
-    
-    int minIndex=-1;
-    
-    b3Float4 maxDots;
-    maxDots.x = FLT_MIN;
-    maxDots.y = FLT_MIN;
-    maxDots.z = FLT_MIN;
-    maxDots.w = FLT_MIN;
-    
-    //	idx, distance
-    for(int ie = 0; ie<nPoints; ie++ )
-    {
-        if (p[ie].w<minW)
-        {
-            minW = p[ie].w;
-            minIndex=ie;
-        }
-        float f;
-        b3Float4 r = p[ie]-center;
-        f = b3Dot( u, r );
-        if (f<maxDots.x)
-        {
-            maxDots.x = f;
-            contactIdx[0].x = ie;
-        }
-        
-        f = b3Dot( -u, r );
-        if (f<maxDots.y)
-        {
-            maxDots.y = f;
-            contactIdx[0].y = ie;
-        }
-        
-        
-        f = b3Dot( v, r );
-        if (f<maxDots.z)
-        {
-            maxDots.z = f;
-            contactIdx[0].z = ie;
-        }
-        
-        f = b3Dot( -v, r );
-        if (f<maxDots.w)
-        {
-            maxDots.w = f;
-            contactIdx[0].w = ie;
-        }
-        
-    }
-    
-    if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
-    {
-        //replace the first contact with minimum (todo: replace contact with least penetration)
-        contactIdx[0].x = minIndex;
-    }
-    
-    return 4;
-    
+
+	b3Float4 aVector = p[0] - center;
+	b3Float4 u = b3Cross(nearNormal, aVector);
+	b3Float4 v = b3Cross(nearNormal, u);
+	u = b3Normalized(u);
+	v = b3Normalized(v);
+
+	//keep point with deepest penetration
+	float minW = FLT_MAX;
+
+	int minIndex = -1;
+
+	b3Float4 maxDots;
+	maxDots.x = FLT_MIN;
+	maxDots.y = FLT_MIN;
+	maxDots.z = FLT_MIN;
+	maxDots.w = FLT_MIN;
+
+	//	idx, distance
+	for (int ie = 0; ie < nPoints; ie++)
+	{
+		if (p[ie].w < minW)
+		{
+			minW = p[ie].w;
+			minIndex = ie;
+		}
+		float f;
+		b3Float4 r = p[ie] - center;
+		f = b3Dot(u, r);
+		if (f < maxDots.x)
+		{
+			maxDots.x = f;
+			contactIdx[0].x = ie;
+		}
+
+		f = b3Dot(-u, r);
+		if (f < maxDots.y)
+		{
+			maxDots.y = f;
+			contactIdx[0].y = ie;
+		}
+
+		f = b3Dot(v, r);
+		if (f < maxDots.z)
+		{
+			maxDots.z = f;
+			contactIdx[0].z = ie;
+		}
+
+		f = b3Dot(-v, r);
+		if (f < maxDots.w)
+		{
+			maxDots.w = f;
+			contactIdx[0].w = ie;
+		}
+	}
+
+	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+	{
+		//replace the first contact with minimum (todo: replace contact with least penetration)
+		contactIdx[0].x = minIndex;
+	}
+
+	return 4;
 }
 
-__kernel void   b3NewContactReductionKernel( __global b3Int4* pairs,
-                                                   __global const b3RigidBodyData_t* rigidBodies,
-                                                   __global const b3Float4* separatingNormals,
-                                                   __global const int* hasSeparatingAxis,
-                                                   __global struct b3Contact4Data* globalContactsOut,
-                                                   __global b3Int4* clippingFaces,
-                                                   __global b3Float4* worldVertsB2,
-                                                   volatile __global int* nGlobalContactsOut,
-                                                   int vertexFaceCapacity,
-												   int contactCapacity,
-                                                   int numPairs,
-												   int pairIndex
-                                                   )
+__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
+										  __global const b3RigidBodyData_t* rigidBodies,
+										  __global const b3Float4* separatingNormals,
+										  __global const int* hasSeparatingAxis,
+										  __global struct b3Contact4Data* globalContactsOut,
+										  __global b3Int4* clippingFaces,
+										  __global b3Float4* worldVertsB2,
+										  volatile __global int* nGlobalContactsOut,
+										  int vertexFaceCapacity,
+										  int contactCapacity,
+										  int numPairs,
+										  int pairIndex)
 {
-//    int i = get_global_id(0);
+	//    int i = get_global_id(0);
 	//int pairIndex = i;
 	int i = pairIndex;
 
-    b3Int4 contactIdx;
-    contactIdx=b3MakeInt4(0,1,2,3);
-    
-	if (i<numPairs)
+	b3Int4 contactIdx;
+	contactIdx = b3MakeInt4(0, 1, 2, 3);
+
+	if (i < numPairs)
 	{
-        
 		if (hasSeparatingAxis[i])
 		{
-            
-			
-            
-            
 			int nPoints = clippingFaces[pairIndex].w;
-           
-            if (nPoints>0)
-            {
-
-                 __global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity];
-                b3Float4 normal = -separatingNormals[i];
-                
-                int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
-            
-                int dstIdx;
-                dstIdx = b3AtomicInc( nGlobalContactsOut);
-				
-//#if 0
-                b3Assert(dstIdx < contactCapacity);
+
+			if (nPoints > 0)
+			{
+				__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
+				b3Float4 normal = -separatingNormals[i];
+
+				int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
+
+				int dstIdx;
+				dstIdx = b3AtomicInc(nGlobalContactsOut);
+
+				//#if 0
+				b3Assert(dstIdx < contactCapacity);
 				if (dstIdx < contactCapacity)
 				{
-
 					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
 					c->m_worldNormalOnB = -normal;
-					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
+					c->m_restituitionCoeffCmp = (0.f * 0xffff);
+					c->m_frictionCoeffCmp = (0.7f * 0xffff);
 					c->m_batchIdx = pairIndex;
 					int bodyA = pairs[pairIndex].x;
 					int bodyB = pairs[pairIndex].y;
 
 					pairs[pairIndex].w = dstIdx;
 
-					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
-					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
-                    c->m_childIndexA =-1;
-					c->m_childIndexB =-1;
-
-                    switch (nReducedContacts)
-                    {
-                        case 4:
-                            c->m_worldPosB[3] = pointsIn[contactIdx.w];
-                        case 3:
-                            c->m_worldPosB[2] = pointsIn[contactIdx.z];
-                        case 2:
-                            c->m_worldPosB[1] = pointsIn[contactIdx.y];
-                        case 1:
-                            c->m_worldPosB[0] = pointsIn[contactIdx.x];
-                        default:
-                        {
-                        }
-                    };
-                    
+					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
+					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
+					c->m_childIndexA = -1;
+					c->m_childIndexB = -1;
+
+					switch (nReducedContacts)
+					{
+						case 4:
+							c->m_worldPosB[3] = pointsIn[contactIdx.w];
+						case 3:
+							c->m_worldPosB[2] = pointsIn[contactIdx.z];
+						case 2:
+							c->m_worldPosB[1] = pointsIn[contactIdx.y];
+						case 1:
+							c->m_worldPosB[0] = pointsIn[contactIdx.x];
+						default:
+						{
+						}
+					};
+
 					GET_NPOINTS(*c) = nReducedContacts;
-                    
-                 }
-                 
-                
-//#endif
-				
-			}//		if (numContactsOut>0)
-		}//		if (hasSeparatingAxis[i])
-	}//	if (i<numPairs)
-
-    
-    
+				}
+
+				//#endif
+
+			}  //		if (numContactsOut>0)
+		}      //		if (hasSeparatingAxis[i])
+	}          //	if (i<numPairs)
 }
 #endif

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h

@@ -14,57 +14,56 @@ typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
 struct b3QuantizedBvhNodeData
 {
 	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
 	//4 bytes
-	int	m_escapeIndexOrTriangleIndex;
+	int m_escapeIndexOrTriangleIndex;
 };
 
-inline int	b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
+inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
 {
-	unsigned int x=0;
-	unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
+	unsigned int x = 0;
+	unsigned int y = (~(x & 0)) << (31 - B3_MAX_NUM_PARTS_IN_BITS);
 	// Get only the lower bits where the triangle index is stored
-	return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
+	return (rootNode->m_escapeIndexOrTriangleIndex & ~(y));
 }
 
 inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
 {
 	//skipindex is negative (internal node), triangleindex >=0 (leafnode)
-	return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
+	return (rootNode->m_escapeIndexOrTriangleIndex >= 0) ? 1 : 0;
 }
-	
+
 inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
 {
 	return -rootNode->m_escapeIndexOrTriangleIndex;
 }
 
-inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
+inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2, int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
 {
-	b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
-	clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
+	b3Float4 clampedPoint = b3MaxFloat4(point2, bvhAabbMin);
+	clampedPoint = b3MinFloat4(clampedPoint, bvhAabbMax);
 
 	b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
 	if (isMax)
 	{
-		out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
-		out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
-		out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
-	} else
+		out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
+		out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
+		out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
+	}
+	else
 	{
-		out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
-		out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
-		out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
+		out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
+		out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
+		out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
 	}
-
 }
 
-
 inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
-								const unsigned short int* aabbMin1,
-								const unsigned short int* aabbMax1,
-								const unsigned short int* aabbMin2,
-								const unsigned short int* aabbMax2)
+	const unsigned short int* aabbMin1,
+	const unsigned short int* aabbMax1,
+	const unsigned short int* aabbMin2,
+	const unsigned short int* aabbMax2)
 {
 	//int overlap = 1;
 	if (aabbMin1[0] > aabbMax2[0])
@@ -86,5 +85,4 @@ inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
 	//return overlap;
 }
 
-
-#endif //B3_QUANTIZED_BVH_NODE_H
+#endif  //B3_QUANTIZED_BVH_NODE_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h

@@ -3,95 +3,87 @@
 
 inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
 {
-	if( nPoints == 0 )
-        return 0;
-    
-    if (nPoints <=4)
-        return nPoints;
-    
-    
-    if (nPoints >64)
-        nPoints = 64;
-    
-	b3Float4 center = b3MakeFloat4(0,0,0,0);
+	if (nPoints == 0)
+		return 0;
+
+	if (nPoints <= 4)
+		return nPoints;
+
+	if (nPoints > 64)
+		nPoints = 64;
+
+	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
 	{
-		
-		for (int i=0;i<nPoints;i++)
+		for (int i = 0; i < nPoints; i++)
 			center += p[i];
 		center /= (float)nPoints;
 	}
-    
-	
-    
+
 	//	sample 4 directions
-    
-    b3Float4 aVector = p[0] - center;
-    b3Float4 u = b3Cross3( nearNormal, aVector );
-    b3Float4 v = b3Cross3( nearNormal, u );
-    u = b3FastNormalized3( u );
-    v = b3FastNormalized3( v );
-    
-    
-    //keep point with deepest penetration
-    float minW= FLT_MAX;
-    
-    int minIndex=-1;
-    
-    b3Float4 maxDots;
-    maxDots.x = FLT_MIN;
-    maxDots.y = FLT_MIN;
-    maxDots.z = FLT_MIN;
-    maxDots.w = FLT_MIN;
-    
-    //	idx, distance
-    for(int ie = 0; ie<nPoints; ie++ )
-    {
-        if (p[ie].w<minW)
-        {
-            minW = p[ie].w;
-            minIndex=ie;
-        }
-        float f;
-        b3Float4 r = p[ie]-center;
-        f = b3Dot3F4( u, r );
-        if (f<maxDots.x)
-        {
-            maxDots.x = f;
-            contactIdx[0].x = ie;
-        }
-        
-        f = b3Dot3F4( -u, r );
-        if (f<maxDots.y)
-        {
-            maxDots.y = f;
-            contactIdx[0].y = ie;
-        }
-        
-        
-        f = b3Dot3F4( v, r );
-        if (f<maxDots.z)
-        {
-            maxDots.z = f;
-            contactIdx[0].z = ie;
-        }
-        
-        f = b3Dot3F4( -v, r );
-        if (f<maxDots.w)
-        {
-            maxDots.w = f;
-            contactIdx[0].w = ie;
-        }
-        
-    }
-    
-    if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
-    {
-        //replace the first contact with minimum (todo: replace contact with least penetration)
-        contactIdx[0].x = minIndex;
-    }
-    
-    return 4;
-    
+
+	b3Float4 aVector = p[0] - center;
+	b3Float4 u = b3Cross3(nearNormal, aVector);
+	b3Float4 v = b3Cross3(nearNormal, u);
+	u = b3FastNormalized3(u);
+	v = b3FastNormalized3(v);
+
+	//keep point with deepest penetration
+	float minW = FLT_MAX;
+
+	int minIndex = -1;
+
+	b3Float4 maxDots;
+	maxDots.x = FLT_MIN;
+	maxDots.y = FLT_MIN;
+	maxDots.z = FLT_MIN;
+	maxDots.w = FLT_MIN;
+
+	//	idx, distance
+	for (int ie = 0; ie < nPoints; ie++)
+	{
+		if (p[ie].w < minW)
+		{
+			minW = p[ie].w;
+			minIndex = ie;
+		}
+		float f;
+		b3Float4 r = p[ie] - center;
+		f = b3Dot3F4(u, r);
+		if (f < maxDots.x)
+		{
+			maxDots.x = f;
+			contactIdx[0].x = ie;
+		}
+
+		f = b3Dot3F4(-u, r);
+		if (f < maxDots.y)
+		{
+			maxDots.y = f;
+			contactIdx[0].y = ie;
+		}
+
+		f = b3Dot3F4(v, r);
+		if (f < maxDots.z)
+		{
+			maxDots.z = f;
+			contactIdx[0].z = ie;
+		}
+
+		f = b3Dot3F4(-v, r);
+		if (f < maxDots.w)
+		{
+			maxDots.w = f;
+			contactIdx[0].w = ie;
+		}
+	}
+
+	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+	{
+		//replace the first contact with minimum (todo: replace contact with least penetration)
+		contactIdx[0].x = minIndex;
+	}
+
+	return 4;
 }
 
-#endif //B3_REDUCE_CONTACTS_H
+#endif  //B3_REDUCE_CONTACTS_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h

@@ -7,18 +7,17 @@
 
 typedef struct b3RigidBodyData b3RigidBodyData_t;
 
-
 struct b3RigidBodyData
 {
-	b3Float4				m_pos;
-	b3Quat					m_quat;
-	b3Float4				m_linVel;
-	b3Float4				m_angVel;
-
-	int 					m_collidableIdx;
-	float 				m_invMass;
-	float 				m_restituitionCoeff;
-	float 				m_frictionCoeff;
+	b3Float4 m_pos;
+	b3Quat m_quat;
+	b3Float4 m_linVel;
+	b3Float4 m_angVel;
+
+	int m_collidableIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
 };
 
 typedef struct b3InertiaData b3InertiaData_t;
@@ -29,6 +28,4 @@ struct b3InertiaData
 	b3Mat3x3 m_initInvInertia;
 };
 
-
-#endif //B3_RIGIDBODY_DATA_H
-	
+#endif  //B3_RIGIDBODY_DATA_H

thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h

@@ -1,40 +1,35 @@
 #ifndef B3_UPDATE_AABBS_H
 #define B3_UPDATE_AABBS_H
 
-
-
 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
-
-
-void b3ComputeWorldAabb(  int bodyId, __global const b3RigidBodyData_t* bodies, __global const  b3Collidable_t* collidables, __global const  b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
+void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
 {
 	__global const b3RigidBodyData_t* body = &bodies[bodyId];
 
 	b3Float4 position = body->m_pos;
-	b3Quat	orientation = body->m_quat;
-	
+	b3Quat orientation = body->m_quat;
+
 	int collidableIndex = body->m_collidableIdx;
 	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
-		
-	if (shapeIndex>=0)
+
+	if (shapeIndex >= 0)
 	{
-				
 		b3Aabb_t localAabb = localShapeAABB[collidableIndex];
 		b3Aabb_t worldAabb;
-		
-		b3Float4 aabbAMinOut,aabbAMaxOut;	
+
+		b3Float4 aabbAMinOut, aabbAMaxOut;
 		float margin = 0.f;
-		b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);
-		
-		worldAabb.m_minVec =aabbAMinOut;
+		b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &aabbAMinOut, &aabbAMaxOut);
+
+		worldAabb.m_minVec = aabbAMinOut;
 		worldAabb.m_minIndices[3] = bodyId;
 		worldAabb.m_maxVec = aabbAMaxOut;
-		worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;
+		worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass == 0.f ? 0 : 1;
 		worldAabbs[bodyId] = worldAabb;
 	}
 }
 
-#endif //B3_UPDATE_AABBS_H
+#endif  //B3_UPDATE_AABBS_H

thirdparty/bullet/Bullet3Common/b3AlignedAllocator.cpp

@@ -15,9 +15,11 @@ subject to the following restrictions:
 
 #include "b3AlignedAllocator.h"
 
+#ifdef B3_ALLOCATOR_STATISTICS
 int b3g_numAlignedAllocs = 0;
 int b3g_numAlignedFree = 0;
-int b3g_totalBytesAlignedAllocs = 0;//detect memory leaks
+int b3g_totalBytesAlignedAllocs = 0;  //detect memory leaks
+#endif
 
 static void *b3AllocDefault(size_t size)
 {
@@ -29,12 +31,10 @@ static void b3FreeDefault(void *ptr)
 	free(ptr);
 }
 
-static b3AllocFunc* b3s_allocFunc = b3AllocDefault;
-static b3FreeFunc* b3s_freeFunc = b3FreeDefault;
-
-
+static b3AllocFunc *b3s_allocFunc = b3AllocDefault;
+static b3FreeFunc *b3s_freeFunc = b3FreeDefault;
 
-#if defined (B3_HAS_ALIGNED_ALLOCATOR)
+#if defined(B3_HAS_ALIGNED_ALLOCATOR)
 #include <malloc.h>
 static void *b3AlignedAllocDefault(size_t size, int alignment)
 {
@@ -59,123 +59,128 @@ static inline void b3AlignedFreeDefault(void *ptr)
 }
 #else
 
-
-
-
-
 static inline void *b3AlignedAllocDefault(size_t size, int alignment)
 {
-  void *ret;
-  char *real;
-  real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment-1));
-  if (real) {
-	ret = b3AlignPointer(real + sizeof(void *),alignment);
-    *((void **)(ret)-1) = (void *)(real);
-  } else {
-    ret = (void *)(real);
-  }
-  return (ret);
+	void *ret;
+	char *real;
+	real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment - 1));
+	if (real)
+	{
+		ret = b3AlignPointer(real + sizeof(void *), alignment);
+		*((void **)(ret)-1) = (void *)(real);
+	}
+	else
+	{
+		ret = (void *)(real);
+	}
+	return (ret);
 }
 
 static inline void b3AlignedFreeDefault(void *ptr)
 {
-  void* real;
+	void *real;
 
-  if (ptr) {
-    real = *((void **)(ptr)-1);
-    b3s_freeFunc(real);
-  }
+	if (ptr)
+	{
+		real = *((void **)(ptr)-1);
+		b3s_freeFunc(real);
+	}
 }
 #endif
 
-
-static b3AlignedAllocFunc* b3s_alignedAllocFunc = b3AlignedAllocDefault;
-static b3AlignedFreeFunc* b3s_alignedFreeFunc = b3AlignedFreeDefault;
+static b3AlignedAllocFunc *b3s_alignedAllocFunc = b3AlignedAllocDefault;
+static b3AlignedFreeFunc *b3s_alignedFreeFunc = b3AlignedFreeDefault;
 
 void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
 {
-  b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
-  b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
+	b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
+	b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
 }
 
 void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
 {
-  b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
-  b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
+	b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
+	b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
 }
 
 #ifdef B3_DEBUG_MEMORY_ALLOCATIONS
 //this generic allocator provides the total allocated number of bytes
 #include <stdio.h>
 
-void*   b3AlignedAllocInternal  (size_t size, int alignment,int line,char* filename)
+void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filename)
 {
- void *ret;
- char *real;
-
- b3g_totalBytesAlignedAllocs += size;
- b3g_numAlignedAllocs++;
-
- 
- real = (char *)b3s_allocFunc(size + 2*sizeof(void *) + (alignment-1));
- if (real) {
-   ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment);
-   *((void **)(ret)-1) = (void *)(real);
-       *((int*)(ret)-2) = size;
-
- } else {
-   ret = (void *)(real);//??
- }
+	void *ret;
+	char *real;
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_totalBytesAlignedAllocs += size;
+	b3g_numAlignedAllocs++;
+#endif
+	real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
+	if (real)
+	{
+		ret = (void *)b3AlignPointer(real + 2 * sizeof(void *), alignment);
+		*((void **)(ret)-1) = (void *)(real);
+		*((int *)(ret)-2) = size;
+	}
+	else
+	{
+		ret = (void *)(real);  //??
+	}
 
- b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedAllocs,real, filename,line,size);
+	b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedAllocs, real, filename, line, size);
 
- int* ptr = (int*)ret;
- *ptr = 12;
- return (ret);
+	int *ptr = (int *)ret;
+	*ptr = 12;
+	return (ret);
 }
 
-void    b3AlignedFreeInternal   (void* ptr,int line,char* filename)
+void b3AlignedFreeInternal(void *ptr, int line, char *filename)
 {
+	void *real;
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_numAlignedFree++;
+#endif
+	if (ptr)
+	{
+		real = *((void **)(ptr)-1);
+		int size = *((int *)(ptr)-2);
+#ifdef B3_ALLOCATOR_STATISTICS
+		b3g_totalBytesAlignedAllocs -= size;
+#endif
+		b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
 
- void* real;
- b3g_numAlignedFree++;
-
- if (ptr) {
-   real = *((void **)(ptr)-1);
-       int size = *((int*)(ptr)-2);
-       b3g_totalBytesAlignedAllocs -= size;
-
-	   b3Printf("free #%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedFree,real, filename,line,size);
-
-   b3s_freeFunc(real);
- } else
- {
-	 b3Printf("NULL ptr\n");
- }
+		b3s_freeFunc(real);
+	}
+	else
+	{
+		b3Printf("NULL ptr\n");
+	}
 }
 
-#else //B3_DEBUG_MEMORY_ALLOCATIONS
+#else  //B3_DEBUG_MEMORY_ALLOCATIONS
 
-void*	b3AlignedAllocInternal	(size_t size, int alignment)
+void *b3AlignedAllocInternal(size_t size, int alignment)
 {
+#ifdef B3_ALLOCATOR_STATISTICS
 	b3g_numAlignedAllocs++;
-	void* ptr;
+#endif
+	void *ptr;
 	ptr = b3s_alignedAllocFunc(size, alignment);
-//	b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
+	//	b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
 	return ptr;
 }
 
-void	b3AlignedFreeInternal	(void* ptr)
+void b3AlignedFreeInternal(void *ptr)
 {
 	if (!ptr)
 	{
 		return;
 	}
-
+#ifdef B3_ALLOCATOR_STATISTICS
 	b3g_numAlignedFree++;
-//	b3Printf("b3AlignedFreeInternal %x\n",ptr);
+#endif
+	//	b3Printf("b3AlignedFreeInternal %x\n",ptr);
 	b3s_alignedFreeFunc(ptr);
 }
 
-#endif //B3_DEBUG_MEMORY_ALLOCATIONS
-
+#endif  //B3_DEBUG_MEMORY_ALLOCATIONS

thirdparty/bullet/Bullet3Common/b3AlignedAllocator.h

@@ -24,84 +24,87 @@ subject to the following restrictions:
 //#define B3_DEBUG_MEMORY_ALLOCATIONS 1
 #ifdef B3_DEBUG_MEMORY_ALLOCATIONS
 
-#define b3AlignedAlloc(a,b) \
-		b3AlignedAllocInternal(a,b,__LINE__,__FILE__)
+#define b3AlignedAlloc(a, b) \
+	b3AlignedAllocInternal(a, b, __LINE__, __FILE__)
 
 #define b3AlignedFree(ptr) \
-		b3AlignedFreeInternal(ptr,__LINE__,__FILE__)
+	b3AlignedFreeInternal(ptr, __LINE__, __FILE__)
 
-void*	b3AlignedAllocInternal	(size_t size, int alignment,int line,char* filename);
+void* b3AlignedAllocInternal(size_t size, int alignment, int line, char* filename);
 
-void	b3AlignedFreeInternal	(void* ptr,int line,char* filename);
+void b3AlignedFreeInternal(void* ptr, int line, char* filename);
 
 #else
-	void*	b3AlignedAllocInternal	(size_t size, int alignment);
-	void	b3AlignedFreeInternal	(void* ptr);
+void* b3AlignedAllocInternal(size_t size, int alignment);
+void b3AlignedFreeInternal(void* ptr);
 
-	#define b3AlignedAlloc(size,alignment) b3AlignedAllocInternal(size,alignment)
-	#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
+#define b3AlignedAlloc(size, alignment) b3AlignedAllocInternal(size, alignment)
+#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
 
 #endif
-typedef int	btSizeType;
+typedef int btSizeType;
 
-typedef void *(b3AlignedAllocFunc)(size_t size, int alignment);
-typedef void (b3AlignedFreeFunc)(void *memblock);
-typedef void *(b3AllocFunc)(size_t size);
-typedef void (b3FreeFunc)(void *memblock);
+typedef void*(b3AlignedAllocFunc)(size_t size, int alignment);
+typedef void(b3AlignedFreeFunc)(void* memblock);
+typedef void*(b3AllocFunc)(size_t size);
+typedef void(b3FreeFunc)(void* memblock);
 
 ///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
-void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc);
+void b3AlignedAllocSetCustom(b3AllocFunc* allocFunc, b3FreeFunc* freeFunc);
 ///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
-void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc);
-
+void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc* allocFunc, b3AlignedFreeFunc* freeFunc);
 
 ///The b3AlignedAllocator is a portable class for aligned memory allocations.
 ///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
-template < typename T , unsigned Alignment >
-class b3AlignedAllocator {
-	
-	typedef b3AlignedAllocator< T , Alignment > self_type;
-	
-public:
+template <typename T, unsigned Alignment>
+class b3AlignedAllocator
+{
+	typedef b3AlignedAllocator<T, Alignment> self_type;
 
+public:
 	//just going down a list:
 	b3AlignedAllocator() {}
 	/*
 	b3AlignedAllocator( const self_type & ) {}
 	*/
 
-	template < typename Other >
-	b3AlignedAllocator( const b3AlignedAllocator< Other , Alignment > & ) {}
+	template <typename Other>
+	b3AlignedAllocator(const b3AlignedAllocator<Other, Alignment>&)
+	{
+	}
 
-	typedef const T*         const_pointer;
-	typedef const T&         const_reference;
-	typedef T*               pointer;
-	typedef T&               reference;
-	typedef T                value_type;
+	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  ( btSizeType        n   , const_pointer *      hint = 0 ) {
+	pointer address(reference ref) const { return &ref; }
+	const_pointer address(const_reference ref) const { return &ref; }
+	pointer allocate(btSizeType n, const_pointer* hint = 0)
+	{
 		(void)hint;
-		return reinterpret_cast< pointer >(b3AlignedAlloc( sizeof(value_type) * n , Alignment ));
+		return reinterpret_cast<pointer>(b3AlignedAlloc(sizeof(value_type) * n, Alignment));
 	}
-	void          construct ( pointer          ptr , const value_type &   value    ) { new (ptr) value_type( value ); }
-	void          deallocate( pointer          ptr ) {
-		b3AlignedFree( reinterpret_cast< void * >( ptr ) );
+	void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
+	void deallocate(pointer ptr)
+	{
+		b3AlignedFree(reinterpret_cast<void*>(ptr));
 	}
-	void          destroy   ( pointer          ptr )                                 { ptr->~value_type(); }
-	
+	void destroy(pointer ptr) { ptr->~value_type(); }
 
-	template < typename O > struct rebind {
-		typedef b3AlignedAllocator< O , Alignment > other;
+	template <typename O>
+	struct rebind
+	{
+		typedef b3AlignedAllocator<O, Alignment> other;
 	};
-	template < typename O >
-	self_type & operator=( const b3AlignedAllocator< O , Alignment > & ) { return *this; }
+	template <typename O>
+	self_type& operator=(const b3AlignedAllocator<O, Alignment>&)
+	{
+		return *this;
+	}
 
-	friend bool operator==( const self_type & , const self_type & ) { return true; }
+	friend bool operator==(const self_type&, const self_type&) { return true; }
 };
 
-
-
-#endif //B3_ALIGNED_ALLOCATOR
-
+#endif  //B3_ALIGNED_ALLOCATOR

thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h

@@ -13,11 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef B3_OBJECT_ARRAY__
 #define B3_OBJECT_ARRAY__
 
-#include "b3Scalar.h" // has definitions like B3_FORCE_INLINE
+#include "b3Scalar.h"  // has definitions like B3_FORCE_INLINE
 #include "b3AlignedAllocator.h"
 
 ///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
@@ -28,402 +27,386 @@ subject to the following restrictions:
 
 #define B3_USE_PLACEMENT_NEW 1
 //#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
-#define B3_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
+#define B3_ALLOW_ARRAY_COPY_OPERATOR  // enabling this can accidently perform deep copies of data if you are not careful
 
 #ifdef B3_USE_MEMCPY
 #include <memory.h>
 #include <string.h>
-#endif //B3_USE_MEMCPY
+#endif  //B3_USE_MEMCPY
 
 #ifdef B3_USE_PLACEMENT_NEW
-#include <new> //for placement new
-#endif //B3_USE_PLACEMENT_NEW
-
+#include <new>  //for placement new
+#endif          //B3_USE_PLACEMENT_NEW
 
 ///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
 ///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
-template <typename T> 
-//template <class T> 
+template <typename T>
+//template <class T>
 class b3AlignedObjectArray
 {
-	b3AlignedAllocator<T , 16>	m_allocator;
+	b3AlignedAllocator<T, 16> m_allocator;
 
-	int					m_size;
-	int					m_capacity;
-	T*					m_data;
+	int m_size;
+	int m_capacity;
+	T* m_data;
 	//PCK: added this line
-	bool				m_ownsMemory;
+	bool m_ownsMemory;
 
 #ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
 public:
-	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other)
+	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other)
 	{
 		copyFromArray(other);
 		return *this;
 	}
-#else//B3_ALLOW_ARRAY_COPY_OPERATOR
+#else   //B3_ALLOW_ARRAY_COPY_OPERATOR
 private:
-		B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other);
-#endif//B3_ALLOW_ARRAY_COPY_OPERATOR
+	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other);
+#endif  //B3_ALLOW_ARRAY_COPY_OPERATOR
 
 protected:
-		B3_FORCE_INLINE	int	allocSize(int size)
-		{
-			return (size ? size*2 : 1);
-		}
-		B3_FORCE_INLINE	void	copy(int start,int end, T* dest) const
-		{
-			int i;
-			for (i=start;i<end;++i)
+	B3_FORCE_INLINE int allocSize(int size)
+	{
+		return (size ? size * 2 : 1);
+	}
+	B3_FORCE_INLINE void copy(int start, int end, T* dest) const
+	{
+		int i;
+		for (i = start; i < end; ++i)
 #ifdef B3_USE_PLACEMENT_NEW
-				new (&dest[i]) T(m_data[i]);
+			new (&dest[i]) T(m_data[i]);
 #else
-				dest[i] = m_data[i];
-#endif //B3_USE_PLACEMENT_NEW
-		}
+			dest[i] = m_data[i];
+#endif  //B3_USE_PLACEMENT_NEW
+	}
 
-		B3_FORCE_INLINE	void	init()
+	B3_FORCE_INLINE void init()
+	{
+		//PCK: added this line
+		m_ownsMemory = true;
+		m_data = 0;
+		m_size = 0;
+		m_capacity = 0;
+	}
+	B3_FORCE_INLINE void destroy(int first, int last)
+	{
+		int i;
+		for (i = first; i < last; i++)
 		{
-			//PCK: added this line
-			m_ownsMemory = true;
-			m_data = 0;
-			m_size = 0;
-			m_capacity = 0;
+			m_data[i].~T();
 		}
-		B3_FORCE_INLINE	void	destroy(int first,int last)
+	}
+
+	B3_FORCE_INLINE void* allocate(int size)
+	{
+		if (size)
+			return m_allocator.allocate(size);
+		return 0;
+	}
+
+	B3_FORCE_INLINE void deallocate()
+	{
+		if (m_data)
 		{
-			int i;
-			for (i=first; i<last;i++)
+			//PCK: enclosed the deallocation in this block
+			if (m_ownsMemory)
 			{
-				m_data[i].~T();
+				m_allocator.deallocate(m_data);
 			}
+			m_data = 0;
 		}
+	}
 
-		B3_FORCE_INLINE	void* allocate(int size)
-		{
-			if (size)
-				return m_allocator.allocate(size);
-			return 0;
-		}
+public:
+	b3AlignedObjectArray()
+	{
+		init();
+	}
 
-		B3_FORCE_INLINE	void	deallocate()
-		{
-			if(m_data)	{
-				//PCK: enclosed the deallocation in this block
-				if (m_ownsMemory)
-				{
-					m_allocator.deallocate(m_data);
-				}
-				m_data = 0;
-			}
-		}
+	~b3AlignedObjectArray()
+	{
+		clear();
+	}
 
-	
+	///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
+	b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
+	{
+		init();
 
+		int otherSize = otherArray.size();
+		resize(otherSize);
+		otherArray.copy(0, otherSize, m_data);
+	}
 
-	public:
-		
-		b3AlignedObjectArray()
-		{
-			init();
-		}
+	/// return the number of elements in the array
+	B3_FORCE_INLINE int size() const
+	{
+		return m_size;
+	}
 
-		~b3AlignedObjectArray()
-		{
-			clear();
-		}
+	B3_FORCE_INLINE const T& at(int n) const
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
 
-		///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
-		b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
-		{
-			init();
+	B3_FORCE_INLINE T& at(int n)
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
 
-			int otherSize = otherArray.size();
-			resize (otherSize);
-			otherArray.copy(0, otherSize, m_data);
-		}
+	B3_FORCE_INLINE const T& operator[](int n) const
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
 
-		
-		
-		/// return the number of elements in the array
-		B3_FORCE_INLINE	int size() const
-		{	
-			return m_size;
-		}
-		
-		B3_FORCE_INLINE const T& at(int n) const
-		{
-			b3Assert(n>=0);
-			b3Assert(n<size());
-			return m_data[n];
-		}
+	B3_FORCE_INLINE T& operator[](int n)
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
 
-		B3_FORCE_INLINE T& at(int n)
-		{
-			b3Assert(n>=0);
-			b3Assert(n<size());
-			return m_data[n];
-		}
+	///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
+	B3_FORCE_INLINE void clear()
+	{
+		destroy(0, size());
 
-		B3_FORCE_INLINE const T& operator[](int n) const
-		{
-			b3Assert(n>=0);
-			b3Assert(n<size());
-			return m_data[n];
-		}
+		deallocate();
 
-		B3_FORCE_INLINE T& operator[](int n)
-		{
-			b3Assert(n>=0);
-			b3Assert(n<size());
-			return m_data[n];
-		}
-		
+		init();
+	}
 
-		///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
-		B3_FORCE_INLINE	void	clear()
+	B3_FORCE_INLINE void pop_back()
+	{
+		b3Assert(m_size > 0);
+		m_size--;
+		m_data[m_size].~T();
+	}
+
+	///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+	///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
+	B3_FORCE_INLINE void resizeNoInitialize(int newsize)
+	{
+		int curSize = size();
+
+		if (newsize < curSize)
 		{
-			destroy(0,size());
-			
-			deallocate();
-			
-			init();
 		}
-
-		B3_FORCE_INLINE	void	pop_back()
+		else
 		{
-			b3Assert(m_size>0);
-			m_size--;
-			m_data[m_size].~T();
+			if (newsize > size())
+			{
+				reserve(newsize);
+			}
+			//leave this uninitialized
 		}
+		m_size = newsize;
+	}
 
+	B3_FORCE_INLINE void resize(int newsize, const T& fillData = T())
+	{
+		int curSize = size();
 
-		///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
-		///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
-		B3_FORCE_INLINE	void	resizeNoInitialize(int newsize)
+		if (newsize < curSize)
 		{
-			int curSize = size();
-
-			if (newsize < curSize)
-			{
-			} else
+			for (int i = newsize; i < curSize; i++)
 			{
-				if (newsize > size())
-				{
-					reserve(newsize);
-				}
-				//leave this uninitialized
+				m_data[i].~T();
 			}
-			m_size = newsize;
 		}
-	
-		B3_FORCE_INLINE	void	resize(int newsize, const T& fillData=T())
+		else
 		{
-			int curSize = size();
-
-			if (newsize < curSize)
-			{
-				for(int i = newsize; i < curSize; i++)
-				{
-					m_data[i].~T();
-				}
-			} else
+			if (newsize > size())
 			{
-				if (newsize > size())
-				{
-					reserve(newsize);
-				}
-#ifdef B3_USE_PLACEMENT_NEW
-				for (int i=curSize;i<newsize;i++)
-				{
-					new ( &m_data[i]) T(fillData);
-				}
-#endif //B3_USE_PLACEMENT_NEW
-
+				reserve(newsize);
 			}
-
-			m_size = newsize;
-		}
-		B3_FORCE_INLINE	T&  expandNonInitializing( )
-		{	
-			int sz = size();
-			if( sz == capacity() )
+#ifdef B3_USE_PLACEMENT_NEW
+			for (int i = curSize; i < newsize; i++)
 			{
-				reserve( allocSize(size()) );
+				new (&m_data[i]) T(fillData);
 			}
-			m_size++;
+#endif  //B3_USE_PLACEMENT_NEW
+		}
 
-			return m_data[sz];		
+		m_size = newsize;
+	}
+	B3_FORCE_INLINE T& expandNonInitializing()
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
 		}
+		m_size++;
 
+		return m_data[sz];
+	}
 
-		B3_FORCE_INLINE	T&  expand( const T& fillValue=T())
-		{	
-			int sz = size();
-			if( sz == capacity() )
-			{
-				reserve( allocSize(size()) );
-			}
-			m_size++;
+	B3_FORCE_INLINE T& expand(const T& fillValue = T())
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+		m_size++;
 #ifdef B3_USE_PLACEMENT_NEW
-			new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
+		new (&m_data[sz]) T(fillValue);  //use the in-place new (not really allocating heap memory)
 #endif
 
-			return m_data[sz];		
-		}
+		return m_data[sz];
+	}
 
+	B3_FORCE_INLINE void push_back(const T& _Val)
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
 
-		B3_FORCE_INLINE	void push_back(const T& _Val)
-		{	
-			int sz = size();
-			if( sz == capacity() )
-			{
-				reserve( allocSize(size()) );
-			}
-			
 #ifdef B3_USE_PLACEMENT_NEW
-			new ( &m_data[m_size] ) T(_Val);
+		new (&m_data[m_size]) T(_Val);
 #else
-			m_data[size()] = _Val;			
-#endif //B3_USE_PLACEMENT_NEW
+		m_data[size()] = _Val;
+#endif  //B3_USE_PLACEMENT_NEW
 
-			m_size++;
-		}
+		m_size++;
+	}
 
-	
-		/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
-		B3_FORCE_INLINE	int capacity() const
-		{	
-			return m_capacity;
-		}
-		
-		B3_FORCE_INLINE	void reserve(int _Count)
-		{	// determine new minimum length of allocated storage
-			if (capacity() < _Count)
-			{	// not enough room, reallocate
-				T*	s = (T*)allocate(_Count);
-				b3Assert(s);
-				if (s==0)
-				{
-					b3Error("b3AlignedObjectArray reserve out-of-memory\n");
-					_Count=0;
-					m_size=0;
-				}
-				copy(0, size(), s);
-
-				destroy(0,size());
-
-				deallocate();
-				
-				//PCK: added this line
-				m_ownsMemory = true;
-
-				m_data = s;
-				
-				m_capacity = _Count;
+	/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+	B3_FORCE_INLINE int capacity() const
+	{
+		return m_capacity;
+	}
 
+	B3_FORCE_INLINE void reserve(int _Count)
+	{  // determine new minimum length of allocated storage
+		if (capacity() < _Count)
+		{  // not enough room, reallocate
+			T* s = (T*)allocate(_Count);
+			b3Assert(s);
+			if (s == 0)
+			{
+				b3Error("b3AlignedObjectArray reserve out-of-memory\n");
+				_Count = 0;
+				m_size = 0;
 			}
-		}
+			copy(0, size(), s);
 
+			destroy(0, size());
 
-		class less
-		{
-			public:
+			deallocate();
+
+			//PCK: added this line
+			m_ownsMemory = true;
+
+			m_data = s;
 
-				bool operator() ( const T& a, const T& b )
-				{
-					return ( a < b );
-				}
-		};
-	
+			m_capacity = _Count;
+		}
+	}
 
-		template <typename L>
-		void quickSortInternal(const L& CompareFunc,int lo, int hi)
+	class less
+	{
+	public:
+		bool operator()(const T& a, const T& b)
 		{
-		//  lo is the lower index, hi is the upper index
-		//  of the region of array a that is to be sorted
-			int i=lo, j=hi;
-			T x=m_data[(lo+hi)/2];
-
-			//  partition
-			do
-			{    
-				while (CompareFunc(m_data[i],x)) 
-					i++; 
-				while (CompareFunc(x,m_data[j])) 
-					j--;
-				if (i<=j)
-				{
-					swap(i,j);
-					i++; j--;
-				}
-			} while (i<=j);
-
-			//  recursion
-			if (lo<j) 
-				quickSortInternal( CompareFunc, lo, j);
-			if (i<hi) 
-				quickSortInternal( CompareFunc, i, hi);
+			return (a < b);
 		}
+	};
 
+	template <typename L>
+	void quickSortInternal(const L& CompareFunc, int lo, int hi)
+	{
+		//  lo is the lower index, hi is the upper index
+		//  of the region of array a that is to be sorted
+		int i = lo, j = hi;
+		T x = m_data[(lo + hi) / 2];
 
-		template <typename L>
-		void quickSort(const L& CompareFunc)
+		//  partition
+		do
 		{
-			//don't sort 0 or 1 elements
-			if (size()>1)
+			while (CompareFunc(m_data[i], x))
+				i++;
+			while (CompareFunc(x, m_data[j]))
+				j--;
+			if (i <= j)
 			{
-				quickSortInternal(CompareFunc,0,size()-1);
+				swap(i, j);
+				i++;
+				j--;
 			}
+		} while (i <= j);
+
+		//  recursion
+		if (lo < j)
+			quickSortInternal(CompareFunc, lo, j);
+		if (i < hi)
+			quickSortInternal(CompareFunc, i, hi);
+	}
+
+	template <typename L>
+	void quickSort(const L& CompareFunc)
+	{
+		//don't sort 0 or 1 elements
+		if (size() > 1)
+		{
+			quickSortInternal(CompareFunc, 0, size() - 1);
 		}
+	}
 
+	///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+	template <typename L>
+	void downHeap(T* pArr, int k, int n, const L& CompareFunc)
+	{
+		/*  PRE: a[k+1..N] is a heap */
+		/* POST:  a[k..N]  is a heap */
 
-		///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
-		template <typename L>
-		void downHeap(T *pArr, int k, int n, const L& CompareFunc)
+		T temp = pArr[k - 1];
+		/* k has child(s) */
+		while (k <= n / 2)
 		{
-			/*  PRE: a[k+1..N] is a heap */
-			/* POST:  a[k..N]  is a heap */
-			
-			T temp = pArr[k - 1];
-			/* k has child(s) */
-			while (k <= n/2) 
+			int child = 2 * k;
+
+			if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
+			{
+				child++;
+			}
+			/* pick larger child */
+			if (CompareFunc(temp, pArr[child - 1]))
 			{
-				int child = 2*k;
-				
-				if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
-				{
-					child++;
-				}
-				/* pick larger child */
-				if (CompareFunc(temp , pArr[child - 1]))
-				{
-					/* move child up */
-					pArr[k - 1] = pArr[child - 1];
-					k = child;
-				}
-				else
-				{
-					break;
-				}
+				/* move child up */
+				pArr[k - 1] = pArr[child - 1];
+				k = child;
 			}
-			pArr[k - 1] = temp;
-		} /*downHeap*/
+			else
+			{
+				break;
+			}
+		}
+		pArr[k - 1] = temp;
+	} /*downHeap*/
 
-		void	swap(int index0,int index1)
-		{
+	void swap(int index0, int index1)
+	{
 #ifdef B3_USE_MEMCPY
-			char	temp[sizeof(T)];
-			memcpy(temp,&m_data[index0],sizeof(T));
-			memcpy(&m_data[index0],&m_data[index1],sizeof(T));
-			memcpy(&m_data[index1],temp,sizeof(T));
+		char temp[sizeof(T)];
+		memcpy(temp, &m_data[index0], sizeof(T));
+		memcpy(&m_data[index0], &m_data[index1], sizeof(T));
+		memcpy(&m_data[index1], temp, sizeof(T));
 #else
-			T temp = m_data[index0];
-			m_data[index0] = m_data[index1];
-			m_data[index1] = temp;
-#endif //B3_USE_PLACEMENT_NEW
-
-		}
+		T temp = m_data[index0];
+		m_data[index0] = m_data[index1];
+		m_data[index1] = temp;
+#endif  //B3_USE_PLACEMENT_NEW
+	}
 
 	template <typename L>
 	void heapSort(const L& CompareFunc)
@@ -431,49 +414,48 @@ protected:
 		/* sort a[0..N-1],  N.B. 0 to N-1 */
 		int k;
 		int n = m_size;
-		for (k = n/2; k > 0; k--) 
+		for (k = n / 2; k > 0; k--)
 		{
 			downHeap(m_data, k, n, CompareFunc);
 		}
 
 		/* a[1..N] is now a heap */
-		while ( n>=1 ) 
+		while (n >= 1)
 		{
-			swap(0,n-1); /* largest of a[0..n-1] */
-
+			swap(0, n - 1); /* largest of a[0..n-1] */
 
 			n = n - 1;
 			/* restore a[1..i-1] heap */
 			downHeap(m_data, 1, n, CompareFunc);
-		} 
+		}
 	}
 
 	///non-recursive binary search, assumes sorted array
-	int	findBinarySearch(const T& key) const
+	int findBinarySearch(const T& key) const
 	{
 		int first = 0;
-		int last = size()-1;
+		int last = size() - 1;
 
 		//assume sorted array
-		while (first <= last) {
+		while (first <= last)
+		{
 			int mid = (first + last) / 2;  // compute mid point.
-			if (key > m_data[mid]) 
+			if (key > m_data[mid])
 				first = mid + 1;  // repeat search in top half.
-			else if (key < m_data[mid]) 
-				last = mid - 1; // repeat search in bottom half.
+			else if (key < m_data[mid])
+				last = mid - 1;  // repeat search in bottom half.
 			else
-				return mid;     // found it. return position /////
+				return mid;  // found it. return position /////
 		}
-		return size();    // failed to find key
+		return size();  // failed to find key
 	}
 
-
-	int	findLinearSearch(const T& key) const
+	int findLinearSearch(const T& key) const
 	{
-		int index=size();
+		int index = size();
 		int i;
 
-		for (i=0;i<size();i++)
+		for (i = 0; i < size(); i++)
 		{
 			if (m_data[i] == key)
 			{
@@ -483,36 +465,35 @@ protected:
 		}
 		return index;
 	}
-    
-    int	findLinearSearch2(const T& key) const
-    {
-        int index=-1;
-        int i;
-        
-        for (i=0;i<size();i++)
-        {
-            if (m_data[i] == key)
-            {
-                index = i;
-                break;
-            }
-        }
-        return index;
-    }
-
-	void	remove(const T& key)
+
+	int findLinearSearch2(const T& key) const
 	{
+		int index = -1;
+		int i;
 
+		for (i = 0; i < size(); i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	void remove(const T& key)
+	{
 		int findIndex = findLinearSearch(key);
-		if (findIndex<size())
+		if (findIndex < size())
 		{
-			swap( findIndex,size()-1);
+			swap(findIndex, size() - 1);
 			pop_back();
 		}
 	}
 
 	//PCK: whole function
-	void initializeFromBuffer(void *buffer, int size, int capacity)
+	void initializeFromBuffer(void* buffer, int size, int capacity)
 	{
 		clear();
 		m_ownsMemory = false;
@@ -524,18 +505,18 @@ protected:
 	void copyFromArray(const b3AlignedObjectArray& otherArray)
 	{
 		int otherSize = otherArray.size();
-		resize (otherSize);
+		resize(otherSize);
 		otherArray.copy(0, otherSize, m_data);
 	}
 
 	void removeAtIndex(int index)
-    {
-        if (index<size())
-        {
-            swap( index,size()-1);
-            pop_back();
-        }
-    }
+	{
+		if (index < size())
+		{
+			swap(index, size() - 1);
+			pop_back();
+		}
+	}
 };
 
-#endif //B3_OBJECT_ARRAY__
+#endif  //B3_OBJECT_ARRAY__

thirdparty/bullet/Bullet3Common/b3CommandLineArgs.h

@@ -12,51 +12,54 @@
 class b3CommandLineArgs
 {
 protected:
-
 	std::map<std::string, std::string> pairs;
 
 public:
-
 	// Constructor
 	b3CommandLineArgs(int argc, char **argv)
 	{
-		addArgs(argc,argv);
+		addArgs(argc, argv);
 	}
 
-	void addArgs(int argc, char**argv)
+	void addArgs(int argc, char **argv)
 	{
-	    for (int i = 1; i < argc; i++)
-	    {
-	        std::string arg = argv[i];
+		for (int i = 1; i < argc; i++)
+		{
+			std::string arg = argv[i];
+
+			if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-'))
+			{
+				continue;
+			}
 
-			if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-')) {
-	        	continue;
-	        }
+			std::string::size_type pos;
+			std::string key, val;
+			if ((pos = arg.find('=')) == std::string::npos)
+			{
+				key = std::string(arg, 2, arg.length() - 2);
+				val = "";
+			}
+			else
+			{
+				key = std::string(arg, 2, pos - 2);
+				val = std::string(arg, pos + 1, arg.length() - 1);
+			}
 
-        	std::string::size_type pos;
-		    std::string key, val;
-	        if ((pos = arg.find( '=')) == std::string::npos) {
-	        	key = std::string(arg, 2, arg.length() - 2);
-	        	val = "";
-	        } else {
-	        	key = std::string(arg, 2, pos - 2);
-	        	val = std::string(arg, pos + 1, arg.length() - 1);
-	        }
-			
 			//only add new keys, don't replace existing
-			if(pairs.find(key) == pairs.end())
+			if (pairs.find(key) == pairs.end())
 			{
-        		pairs[key] = val;
+				pairs[key] = val;
 			}
-	    }
+		}
 	}
 
-	bool CheckCmdLineFlag(const char* arg_name)
+	bool CheckCmdLineFlag(const char *arg_name)
 	{
 		std::map<std::string, std::string>::iterator itr;
-		if ((itr = pairs.find(arg_name)) != pairs.end()) {
+		if ((itr = pairs.find(arg_name)) != pairs.end())
+		{
 			return true;
-	    }
+		}
 		return false;
 	}
 
@@ -73,29 +76,31 @@ template <typename T>
 inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
 {
 	std::map<std::string, std::string>::iterator itr;
-	if ((itr = pairs.find(arg_name)) != pairs.end()) {
+	if ((itr = pairs.find(arg_name)) != pairs.end())
+	{
 		std::istringstream strstream(itr->second);
 		strstream >> val;
 		return true;
-    }
+	}
 	return false;
 }
 
 template <>
-inline bool b3CommandLineArgs::GetCmdLineArgument<char*>(const char* arg_name, char* &val)
+inline bool b3CommandLineArgs::GetCmdLineArgument<char *>(const char *arg_name, char *&val)
 {
 	std::map<std::string, std::string>::iterator itr;
-	if ((itr = pairs.find(arg_name)) != pairs.end()) {
-
+	if ((itr = pairs.find(arg_name)) != pairs.end())
+	{
 		std::string s = itr->second;
-		val = (char*) malloc(sizeof(char) * (s.length() + 1));
+		val = (char *)malloc(sizeof(char) * (s.length() + 1));
 		std::strcpy(val, s.c_str());
 		return true;
-	} else {
-    	val = NULL;
+	}
+	else
+	{
+		val = NULL;
 	}
 	return false;
 }
 
-
-#endif //COMMAND_LINE_ARGS_H
+#endif  //COMMAND_LINE_ARGS_H

thirdparty/bullet/Bullet3Common/b3FileUtils.h

@@ -3,7 +3,7 @@
 
 #include <stdio.h>
 #include "b3Scalar.h"
-#include <stddef.h>//ptrdiff_h
+#include <stddef.h>  //ptrdiff_h
 #include <string.h>
 
 struct b3FileUtils
@@ -17,42 +17,42 @@ struct b3FileUtils
 
 	static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
 	{
-		FILE* f=0;
-		f = fopen(orgFileName,"rb");
-                if (f)
-                {
+		FILE* f = 0;
+		f = fopen(orgFileName, "rb");
+		if (f)
+		{
 			//printf("original file found: [%s]\n", orgFileName);
-			sprintf(relativeFileName,"%s", orgFileName);
+			sprintf(relativeFileName, "%s", orgFileName);
 			fclose(f);
 			return true;
 		}
 
-		//printf("Trying various directories, relative to current working directory\n");	
-			const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
-			int numPrefixes = sizeof(prefix)/sizeof(const char*);
-	
-			f=0;
-			bool fileFound = false;
+		//printf("Trying various directories, relative to current working directory\n");
+		const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+		int numPrefixes = sizeof(prefix) / sizeof(const char*);
 
-			for (int i=0;!f && i<numPrefixes;i++)
-			{
+		f = 0;
+		bool fileFound = false;
+
+		for (int i = 0; !f && i < numPrefixes; i++)
+		{
 #ifdef _MSC_VER
-				sprintf_s(relativeFileName,maxRelativeFileNameMaxLen,"%s%s",prefix[i],orgFileName);
+			sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
 #else
-				sprintf(relativeFileName,"%s%s",prefix[i],orgFileName);
+			sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
 #endif
-				f = fopen(relativeFileName,"rb");
-				if (f)
-				{
-					fileFound = true;
-					break;
-				}
-			}
+			f = fopen(relativeFileName, "rb");
 			if (f)
 			{
-				fclose(f);
+				fileFound = true;
+				break;
 			}
-	
+		}
+		if (f)
+		{
+			fclose(f);
+		}
+
 		return fileFound;
 	}
 
@@ -60,8 +60,8 @@ struct b3FileUtils
 	{
 		size_t const patlen = strlen(pattern);
 		size_t patcnt = 0;
-		const char * oriptr;
-		const char * patloc;
+		const char* oriptr;
+		const char* patloc;
 		// find how many times the pattern occurs in the original string
 		for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
 		{
@@ -70,29 +70,27 @@ struct b3FileUtils
 		return oriptr;
 	}
 
-	
-
 	static int extractPath(const char* fileName, char* path, int maxPathLength)
 	{
 		const char* stripped = strip2(fileName, "/");
 		stripped = strip2(stripped, "\\");
 
-		ptrdiff_t len = stripped-fileName;
-		b3Assert((len+1)<maxPathLength);
+		ptrdiff_t len = stripped - fileName;
+		b3Assert((len + 1) < maxPathLength);
 
-		if (len && ((len+1)<maxPathLength))
+		if (len && ((len + 1) < maxPathLength))
 		{
-
-			for (int i=0;i<len;i++)
+			for (int i = 0; i < len; i++)
 			{
 				path[i] = fileName[i];
 			}
-			path[len]=0;
-		} else
+			path[len] = 0;
+		}
+		else
 		{
 			len = 0;
-			b3Assert(maxPathLength>0);
-			if (maxPathLength>0)
+			b3Assert(maxPathLength > 0);
+			if (maxPathLength > 0)
 			{
 				path[len] = 0;
 			}
@@ -102,23 +100,21 @@ struct b3FileUtils
 
 	static char toLowerChar(const char t)
 	{
-		if (t>=(char)'A' && t<=(char)'Z')
+		if (t >= (char)'A' && t <= (char)'Z')
 			return t + ((char)'a' - (char)'A');
 		else
 			return t;
 	}
 
-
 	static void toLower(char* str)
 	{
-		int len=strlen(str);
-		for (int i=0;i<len;i++)
+		int len = strlen(str);
+		for (int i = 0; i < len; i++)
 		{
 			str[i] = toLowerChar(str[i]);
 		}
 	}
 
-
 	/*static const char* strip2(const char* name, const char* pattern)
 	{
 		size_t const patlen = strlen(pattern);
@@ -133,6 +129,5 @@ struct b3FileUtils
 		return oriptr;
 	}
 	*/
-
 };
-#endif //B3_FILE_UTILS_H
+#endif  //B3_FILE_UTILS_H

thirdparty/bullet/Bullet3Common/b3HashMap.h

@@ -13,86 +13,80 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef B3_HASH_MAP_H
 #define B3_HASH_MAP_H
 
 #include "b3AlignedObjectArray.h"
 
-
 #include <string>
 
 ///very basic hashable string implementation, compatible with b3HashMap
 struct b3HashString
 {
 	std::string m_string;
-	unsigned int	m_hash;
+	unsigned int m_hash;
 
-	B3_FORCE_INLINE	unsigned int getHash()const
+	B3_FORCE_INLINE unsigned int getHash() const
 	{
 		return m_hash;
 	}
 
-
 	b3HashString(const char* name)
-		:m_string(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 InitialFNV = 2166136261u;
 		static const unsigned int FNVMultiple = 16777619u;
 
 		/* Fowler / Noll / Vo (FNV) Hash */
 		unsigned int hash = InitialFNV;
 		int len = m_string.length();
-		for(int i = 0; i<len; i++)
+		for (int i = 0; i < len; i++)
 		{
-			hash = hash ^ (m_string[i]);       /* xor  the low 8 bits */
-			hash = hash * FNVMultiple;  /* multiply by the magic number */
+			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 portableStringCompare(const char* src, const char* dst) const
 	{
-			int ret = 0 ;
+		int ret = 0;
 
-			while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst)
-					++src, ++dst;
+		while (!(ret = *(unsigned char*)src - *(unsigned char*)dst) && *dst)
+			++src, ++dst;
 
-			if ( ret < 0 )
-					ret = -1 ;
-			else if ( ret > 0 )
-					ret = 1 ;
+		if (ret < 0)
+			ret = -1;
+		else if (ret > 0)
+			ret = 1;
 
-			return( ret );
+		return (ret);
 	}
 
 	bool equals(const b3HashString& other) const
 	{
 		return (m_string == other.m_string);
 	}
-
 };
 
-
-const int B3_HASH_NULL=0xffffffff;
-
+const int B3_HASH_NULL = 0xffffffff;
 
 class b3HashInt
 {
-	int	m_uid;
+	int m_uid;
+
 public:
-	b3HashInt(int uid)	:m_uid(uid)
+	b3HashInt(int uid) : m_uid(uid)
 	{
 	}
 
-	int	getUid1() const
+	int getUid1() const
 	{
 		return m_uid;
 	}
 
-	void	setUid1(int uid)
+	void setUid1(int uid)
 	{
 		m_uid = uid;
 	}
@@ -102,34 +96,34 @@ public:
 		return getUid1() == other.getUid1();
 	}
 	//to our success
-	B3_FORCE_INLINE	unsigned int getHash()const
+	B3_FORCE_INLINE unsigned int getHash() const
 	{
 		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);
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
 		return key;
 	}
 };
 
-
-
 class b3HashPtr
 {
-
-	union
-	{
-		const void*	m_pointer;
-		int	m_hashValues[2];
+	union {
+		const void* m_pointer;
+		int m_hashValues[2];
 	};
 
 public:
-
 	b3HashPtr(const void* ptr)
-		:m_pointer(ptr)
+		: m_pointer(ptr)
 	{
 	}
 
-	const void*	getPointer() const
+	const void* getPointer() const
 	{
 		return m_pointer;
 	}
@@ -140,65 +134,69 @@ public:
 	}
 
 	//to our success
-	B3_FORCE_INLINE	unsigned int getHash()const
+	B3_FORCE_INLINE unsigned int getHash() const
 	{
-		const bool VOID_IS_8 = ((sizeof(void*)==8));
-		
-		int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
-	
+		const bool VOID_IS_8 = ((sizeof(void*) == 8));
+
+		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);
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
 		return key;
 	}
-
-	
 };
 
-
 template <class Value>
 class b3HashKeyPtr
 {
-        int     m_uid;
+	int m_uid;
+
 public:
+	b3HashKeyPtr(int uid) : m_uid(uid)
+	{
+	}
 
-        b3HashKeyPtr(int uid)    :m_uid(uid)
-        {
-        }
-
-        int     getUid1() const
-        {
-                return m_uid;
-        }
-
-        bool equals(const b3HashKeyPtr<Value>& other) const
-        {
-                return getUid1() == other.getUid1();
-        }
-
-        //to our success
-        B3_FORCE_INLINE       unsigned int getHash()const
-        {
-                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;
-        }
-
-        
-};
+	int getUid1() const
+	{
+		return m_uid;
+	}
 
+	bool equals(const b3HashKeyPtr<Value>& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		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 b3HashKey
 {
-	int	m_uid;
-public:
+	int m_uid;
 
-	b3HashKey(int uid)	:m_uid(uid)
+public:
+	b3HashKey(int uid) : m_uid(uid)
 	{
 	}
 
-	int	getUid1() const
+	int getUid1() const
 	{
 		return m_uid;
 	}
@@ -208,30 +206,33 @@ public:
 		return getUid1() == other.getUid1();
 	}
 	//to our success
-	B3_FORCE_INLINE	unsigned int getHash()const
+	B3_FORCE_INLINE unsigned int getHash() const
 	{
 		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);
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
 		return key;
 	}
 };
 
-
 ///The b3HashMap template class implements a generic and lightweight hashmap.
 ///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
 template <class Key, class Value>
 class b3HashMap
 {
-
 protected:
-	b3AlignedObjectArray<int>		m_hashTable;
-	b3AlignedObjectArray<int>		m_next;
-	
-	b3AlignedObjectArray<Value>		m_valueArray;
-	b3AlignedObjectArray<Key>		m_keyArray;
+	b3AlignedObjectArray<int> m_hashTable;
+	b3AlignedObjectArray<int> m_next;
+
+	b3AlignedObjectArray<Value> m_valueArray;
+	b3AlignedObjectArray<Key> m_keyArray;
 
-	void	growTables(const Key& /*key*/)
+	void growTables(const Key& /*key*/)
 	{
 		int newCapacity = m_valueArray.capacity();
 
@@ -245,7 +246,7 @@ protected:
 
 			int i;
 
-			for (i= 0; i < newCapacity; ++i)
+			for (i = 0; i < newCapacity; ++i)
 			{
 				m_hashTable[i] = B3_HASH_NULL;
 			}
@@ -254,30 +255,28 @@ protected:
 				m_next[i] = B3_HASH_NULL;
 			}
 
-			for(i=0;i<curHashtableSize;i++)
+			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
+				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);
+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 != B3_HASH_NULL)
 		{
-			m_valueArray[index]=value;
+			m_valueArray[index] = value;
 			return;
 		}
 
@@ -291,19 +290,19 @@ protected:
 		{
 			growTables(key);
 			//hash with new capacity
-			hash = key.getHash() & (m_valueArray.capacity()-1);
+			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);
+	void remove(const Key& key)
+	{
+		int hash = key.getHash() & (m_valueArray.capacity() - 1);
 
 		int pairIndex = findIndex(key);
-		
-		if (pairIndex ==B3_HASH_NULL)
+
+		if (pairIndex == B3_HASH_NULL)
 		{
 			return;
 		}
@@ -344,7 +343,7 @@ protected:
 		}
 
 		// Remove the last pair from the hash table.
-		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
+		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
 
 		index = m_hashTable[lastHash];
 		b3Assert(index != B3_HASH_NULL);
@@ -376,10 +375,8 @@ protected:
 
 		m_valueArray.pop_back();
 		m_keyArray.pop_back();
-
 	}
 
-
 	int size() const
 	{
 		return m_valueArray.size();
@@ -399,23 +396,24 @@ protected:
 		return &m_valueArray[index];
 	}
 
-	 Key getKeyAtIndex(int index)
-    {
-        b3Assert(index < m_keyArray.size());
-        return m_keyArray[index];
-    }
-    
-    const Key getKeyAtIndex(int index) const
-    {
-        b3Assert(index < m_keyArray.size());
-        return m_keyArray[index];
-    }
+	Key getKeyAtIndex(int index)
+	{
+		b3Assert(index < m_keyArray.size());
+		return m_keyArray[index];
+	}
+
+	const Key getKeyAtIndex(int index) const
+	{
+		b3Assert(index < m_keyArray.size());
+		return m_keyArray[index];
+	}
 
-	Value* operator[](const Key& key) {
+	Value* operator[](const Key& key)
+	{
 		return find(key);
 	}
 
-	const Value*	find(const Key& key) const
+	const Value* find(const Key& key) const
 	{
 		int index = findIndex(key);
 		if (index == B3_HASH_NULL)
@@ -425,7 +423,7 @@ protected:
 		return &m_valueArray[index];
 	}
 
-	Value*	find(const Key& key)
+	Value* find(const Key& key)
 	{
 		int index = findIndex(key);
 		if (index == B3_HASH_NULL)
@@ -435,10 +433,9 @@ protected:
 		return &m_valueArray[index];
 	}
 
-
-	int	findIndex(const Key& key) const
+	int findIndex(const Key& key) const
 	{
-		unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
+		unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
 
 		if (hash >= (unsigned int)m_hashTable.size())
 		{
@@ -453,14 +450,13 @@ protected:
 		return index;
 	}
 
-	void	clear()
+	void clear()
 	{
 		m_hashTable.clear();
 		m_next.clear();
 		m_valueArray.clear();
 		m_keyArray.clear();
 	}
-
 };
 
-#endif //B3_HASH_MAP_H
+#endif  //B3_HASH_MAP_H

thirdparty/bullet/Bullet3Common/b3Logging.cpp

@@ -20,17 +20,16 @@ subject to the following restrictions:
 
 #ifdef _WIN32
 #include <windows.h>
-#endif //_WIN32
-
+#endif  //_WIN32
 
 void b3PrintfFuncDefault(const char* msg)
 {
 #ifdef _WIN32
 	OutputDebugStringA(msg);
 #endif
-	printf("%s",msg);
-    //is this portable?
-    fflush(stdout);
+	printf("%s", msg);
+	//is this portable?
+	fflush(stdout);
 }
 
 void b3WarningMessageFuncDefault(const char* msg)
@@ -38,32 +37,26 @@ void b3WarningMessageFuncDefault(const char* msg)
 #ifdef _WIN32
 	OutputDebugStringA(msg);
 #endif
-	printf("%s",msg);
-    //is this portable?
-    fflush(stdout);
-
+	printf("%s", msg);
+	//is this portable?
+	fflush(stdout);
 }
 
-
 void b3ErrorMessageFuncDefault(const char* msg)
 {
 #ifdef _WIN32
 	OutputDebugStringA(msg);
 #endif
-	printf("%s",msg);
+	printf("%s", msg);
 
-    //is this portable?
-    fflush(stdout);
-    
+	//is this portable?
+	fflush(stdout);
 }
 
-
-
 static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
 static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
 static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
 
-
 ///The developer can route b3Printf output using their own implementation
 void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
 {
@@ -81,54 +74,50 @@ void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
 //#define B3_MAX_DEBUG_STRING_LENGTH 2048
 #define B3_MAX_DEBUG_STRING_LENGTH 32768
 
-
-void b3OutputPrintfVarArgsInternal(const char *str, ...)
+void b3OutputPrintfVarArgsInternal(const char* str, ...)
 {
-    char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
-    va_list argList;
-    va_start(argList, str);
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
 #ifdef _MSC_VER
-    vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
 #else
-    vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
 #endif
-        (b3s_printfFunc)(strDebug);
-    va_end(argList);    
+	(b3s_printfFunc)(strDebug);
+	va_end(argList);
 }
-void b3OutputWarningMessageVarArgsInternal(const char *str, ...)
+void b3OutputWarningMessageVarArgsInternal(const char* str, ...)
 {
-    char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
-    va_list argList;
-    va_start(argList, str);
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
 #ifdef _MSC_VER
-    vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
 #else
-    vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
 #endif
-        (b3s_warningMessageFunc)(strDebug);
-    va_end(argList);    
+	(b3s_warningMessageFunc)(strDebug);
+	va_end(argList);
 }
-void b3OutputErrorMessageVarArgsInternal(const char *str, ...)
+void b3OutputErrorMessageVarArgsInternal(const char* str, ...)
 {
-	
-    char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0};
-    va_list argList;
-    va_start(argList, str);
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
 #ifdef _MSC_VER
-    vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
 #else
-    vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList);
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
 #endif
-        (b3s_errorMessageFunc)(strDebug);
-    va_end(argList);    
-
+	(b3s_errorMessageFunc)(strDebug);
+	va_end(argList);
 }
 
-
-void	b3EnterProfileZoneDefault(const char* name)
+void b3EnterProfileZoneDefault(const char* name)
 {
 }
-void	b3LeaveProfileZoneDefault()
+void b3LeaveProfileZoneDefault()
 {
 }
 static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault;
@@ -151,10 +140,6 @@ void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
 	b3s_leaveFunc = leaveFunc;
 }
 
-
-
-
 #ifndef _MSC_VER
 #undef vsprintf_s
 #endif
-

thirdparty/bullet/Bullet3Common/b3Logging.h

@@ -3,75 +3,84 @@
 #define B3_LOGGING_H
 
 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif
-    
+
 ///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
 ///You can also customize the message by uncommenting out a different line below
 #define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
-//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
-//#define b3Printf b3OutputPrintfVarArgsInternal
-//#define b3Printf(...) printf(__VA_ARGS__)
-//#define b3Printf(...)
-
-#define b3Warning(...) do {b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n",__FILE__,__LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); }while(0)
-#define b3Error(...) do {b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n",__FILE__,__LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); } while(0)
-
+	//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
+	//#define b3Printf b3OutputPrintfVarArgsInternal
+	//#define b3Printf(...) printf(__VA_ARGS__)
+	//#define b3Printf(...)
+
+#define b3Warning(...)                                                                    \
+	do                                                                                    \
+	{                                                                                     \
+		b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__); \
+		b3OutputWarningMessageVarArgsInternal(__VA_ARGS__);                               \
+	} while (0)
+#define b3Error(...)                                                                  \
+	do                                                                                \
+	{                                                                                 \
+		b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__); \
+		b3OutputErrorMessageVarArgsInternal(__VA_ARGS__);                             \
+	} while (0)
 
 #ifndef B3_NO_PROFILE
 
-void b3EnterProfileZone(const char* name);
-void b3LeaveProfileZone();
+	void b3EnterProfileZone(const char* name);
+	void b3LeaveProfileZone();
 #ifdef __cplusplus
 
-class	b3ProfileZone
-{
-public:
-	b3ProfileZone(const char* name)
-	{ 
-		b3EnterProfileZone( name ); 
-	}
-
-	~b3ProfileZone()
-	{ 
-		b3LeaveProfileZone(); 
-	}
-};
-
-#define	B3_PROFILE( name )			b3ProfileZone __profile( name )
+	class b3ProfileZone
+	{
+	public:
+		b3ProfileZone(const char* name)
+		{
+			b3EnterProfileZone(name);
+		}
+
+		~b3ProfileZone()
+		{
+			b3LeaveProfileZone();
+		}
+	};
+
+#define B3_PROFILE(name) b3ProfileZone __profile(name)
 #endif
 
-#else //B3_NO_PROFILE
+#else  //B3_NO_PROFILE
 
-#define	B3_PROFILE( name )
+#define B3_PROFILE(name)
 #define b3StartProfile(a)
 #define b3StopProfile
 
-#endif //#ifndef B3_NO_PROFILE
-
+#endif  //#ifndef B3_NO_PROFILE
 
-typedef void (b3PrintfFunc)(const char* msg);
-typedef void (b3WarningMessageFunc)(const char* msg);
-typedef void (b3ErrorMessageFunc)(const char* msg);
-typedef void (b3EnterProfileZoneFunc)(const char* msg);
-typedef void (b3LeaveProfileZoneFunc)();
+	typedef void(b3PrintfFunc)(const char* msg);
+	typedef void(b3WarningMessageFunc)(const char* msg);
+	typedef void(b3ErrorMessageFunc)(const char* msg);
+	typedef void(b3EnterProfileZoneFunc)(const char* msg);
+	typedef void(b3LeaveProfileZoneFunc)();
 
-///The developer can route b3Printf output using their own implementation
-void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
-void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
-void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
+	///The developer can route b3Printf output using their own implementation
+	void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
+	void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
+	void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
 
-///Set custom profile zone functions (zones can be nested)
-void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
-void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
+	///Set custom profile zone functions (zones can be nested)
+	void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
+	void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
 
-///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
-void b3OutputPrintfVarArgsInternal(const char *str, ...);
-void b3OutputWarningMessageVarArgsInternal(const char *str, ...);
-void b3OutputErrorMessageVarArgsInternal(const char *str, ...);
+	///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
+	void b3OutputPrintfVarArgsInternal(const char* str, ...);
+	void b3OutputWarningMessageVarArgsInternal(const char* str, ...);
+	void b3OutputErrorMessageVarArgsInternal(const char* str, ...);
 
 #ifdef __cplusplus
-    }
+}
 #endif
 
-#endif//B3_LOGGING_H
+#endif  //B3_LOGGING_H

thirdparty/bullet/Bullet3Common/b3MinMax.h

@@ -12,60 +12,58 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef B3_GEN_MINMAX_H
 #define B3_GEN_MINMAX_H
 
 #include "b3Scalar.h"
 
 template <class T>
-B3_FORCE_INLINE const T& b3Min(const T& a, const T& b) 
+B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
 {
-  return a < b ? a : b ;
+	return a < b ? a : b;
 }
 
 template <class T>
-B3_FORCE_INLINE const T& b3Max(const T& a, const T& b) 
+B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
 {
-  return  a > b ? a : b;
+	return a > b ? a : b;
 }
 
 template <class T>
-B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub) 
+B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
 {
-	return a < lb ? lb : (ub < a ? ub : a); 
+	return a < lb ? lb : (ub < a ? ub : a);
 }
 
 template <class T>
-B3_FORCE_INLINE void b3SetMin(T& a, const T& b) 
+B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
 {
-    if (b < a) 
+	if (b < a)
 	{
 		a = b;
 	}
 }
 
 template <class T>
-B3_FORCE_INLINE void b3SetMax(T& a, const T& b) 
+B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
 {
-    if (a < b) 
+	if (a < b)
 	{
 		a = b;
 	}
 }
 
 template <class T>
-B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub) 
+B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
 {
-	if (a < lb) 
+	if (a < lb)
 	{
-		a = lb; 
+		a = lb;
 	}
-	else if (ub < a) 
+	else if (ub < a)
 	{
 		a = ub;
 	}
 }
 
-#endif //B3_GEN_MINMAX_H
+#endif  //B3_GEN_MINMAX_H

thirdparty/bullet/Bullet3Common/b3PoolAllocator.h

@@ -12,7 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef _BT_POOL_ALLOCATOR_H
 #define _BT_POOL_ALLOCATOR_H
 
@@ -22,37 +21,37 @@ subject to the following restrictions:
 ///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
 class b3PoolAllocator
 {
-	int				m_elemSize;
-	int				m_maxElements;
-	int				m_freeCount;
-	void*			m_firstFree;
-	unsigned char*	m_pool;
+	int m_elemSize;
+	int m_maxElements;
+	int m_freeCount;
+	void* m_firstFree;
+	unsigned char* m_pool;
 
 public:
-
 	b3PoolAllocator(int elemSize, int maxElements)
-		:m_elemSize(elemSize),
-		m_maxElements(maxElements)
+		: m_elemSize(elemSize),
+		  m_maxElements(maxElements)
 	{
-		m_pool = (unsigned char*) b3AlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
+		m_pool = (unsigned char*)b3AlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
 
 		unsigned char* p = m_pool;
-        m_firstFree = p;
-        m_freeCount = m_maxElements;
-        int count = m_maxElements;
-        while (--count) {
-            *(void**)p = (p + m_elemSize);
-            p += m_elemSize;
-        }
-        *(void**)p = 0;
-    }
+		m_firstFree = p;
+		m_freeCount = m_maxElements;
+		int count = m_maxElements;
+		while (--count)
+		{
+			*(void**)p = (p + m_elemSize);
+			p += m_elemSize;
+		}
+		*(void**)p = 0;
+	}
 
 	~b3PoolAllocator()
 	{
-		b3AlignedFree( m_pool);
+		b3AlignedFree(m_pool);
 	}
 
-	int	getFreeCount() const
+	int getFreeCount() const
 	{
 		return m_freeCount;
 	}
@@ -67,21 +66,22 @@ public:
 		return m_maxElements;
 	}
 
-	void*	allocate(int size)
+	void* allocate(int size)
 	{
 		// release mode fix
 		(void)size;
-		b3Assert(!size || size<=m_elemSize);
-		b3Assert(m_freeCount>0);
-        void* result = m_firstFree;
-        m_firstFree = *(void**)m_firstFree;
-        --m_freeCount;
-        return result;
+		b3Assert(!size || size <= m_elemSize);
+		b3Assert(m_freeCount > 0);
+		void* result = m_firstFree;
+		m_firstFree = *(void**)m_firstFree;
+		--m_freeCount;
+		return result;
 	}
 
 	bool validPtr(void* ptr)
 	{
-		if (ptr) {
+		if (ptr)
+		{
 			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
 			{
 				return true;
@@ -90,32 +90,32 @@ public:
 		return false;
 	}
 
-	void	freeMemory(void* ptr)
+	void freeMemory(void* ptr)
 	{
-		 if (ptr) {
-            b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
+		if (ptr)
+		{
+			b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
 
-            *(void**)ptr = m_firstFree;
-            m_firstFree = ptr;
-            ++m_freeCount;
-        }
+			*(void**)ptr = m_firstFree;
+			m_firstFree = ptr;
+			++m_freeCount;
+		}
 	}
 
-	int	getElementSize() const
+	int getElementSize() const
 	{
 		return m_elemSize;
 	}
 
-	unsigned char*	getPoolAddress()
+	unsigned char* getPoolAddress()
 	{
 		return m_pool;
 	}
 
-	const unsigned char*	getPoolAddress() const
+	const unsigned char* getPoolAddress() const
 	{
 		return m_pool;
 	}
-
 };
 
-#endif //_BT_POOL_ALLOCATOR_H
+#endif  //_BT_POOL_ALLOCATOR_H

thirdparty/bullet/Bullet3Common/b3QuadWord.h

@@ -12,18 +12,13 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef B3_SIMD_QUADWORD_H
 #define B3_SIMD_QUADWORD_H
 
 #include "b3Scalar.h"
 #include "b3MinMax.h"
 
-
-
-
-
-#if defined (__CELLOS_LV2) && defined (__SPU__)
+#if defined(__CELLOS_LV2) && defined(__SPU__)
 #include <altivec.h>
 #endif
 
@@ -31,58 +26,64 @@ subject to the following restrictions:
  * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
  */
 #ifndef USE_LIBSPE2
-B3_ATTRIBUTE_ALIGNED16(class) b3QuadWord
+B3_ATTRIBUTE_ALIGNED16(class)
+b3QuadWord
 #else
 class b3QuadWord
 #endif
 {
 protected:
-
-#if defined (__SPU__) && defined (__CELLOS_LV2__)
+#if defined(__SPU__) && defined(__CELLOS_LV2__)
 	union {
 		vec_float4 mVec128;
-		b3Scalar	m_floats[4];
+		b3Scalar m_floats[4];
 	};
+
 public:
-	vec_float4	get128() const
+	vec_float4 get128() const
 	{
 		return mVec128;
 	}
 
-#else //__CELLOS_LV2__ __SPU__
+#else  //__CELLOS_LV2__ __SPU__
 
-#if defined(B3_USE_SSE) || defined(B3_USE_NEON) 
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
 public:
 	union {
 		b3SimdFloat4 mVec128;
-		b3Scalar	m_floats[4];
-		struct {b3Scalar x,y,z,w;};
+		b3Scalar m_floats[4];
+		struct
+		{
+			b3Scalar x, y, z, w;
+		};
 	};
+
 public:
-	B3_FORCE_INLINE	b3SimdFloat4	get128() const
+	B3_FORCE_INLINE b3SimdFloat4 get128() const
 	{
 		return mVec128;
 	}
-	B3_FORCE_INLINE	void	set128(b3SimdFloat4 v128)
+	B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
 	{
 		mVec128 = v128;
 	}
 #else
 public:
-	union
-	{
-		b3Scalar	m_floats[4];
-		struct {b3Scalar x,y,z,w;};
+	union {
+		b3Scalar m_floats[4];
+		struct
+		{
+			b3Scalar x, y, z, w;
+		};
 	};
-#endif // B3_USE_SSE
+#endif  // B3_USE_SSE
 
-#endif //__CELLOS_LV2__ __SPU__
+#endif  //__CELLOS_LV2__ __SPU__
 
-	public:
-  
+public:
 #if defined(B3_USE_SSE) || defined(B3_USE_NEON)
 
-	// Set Vector 
+	// Set Vector
 	B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
 	{
 		mVec128 = vec;
@@ -95,151 +96,147 @@ public:
 	}
 
 	// Assignment Operator
-	B3_FORCE_INLINE b3QuadWord& 
-	operator=(const b3QuadWord& v) 
+	B3_FORCE_INLINE b3QuadWord&
+	operator=(const b3QuadWord& v)
 	{
 		mVec128 = v.mVec128;
-		
+
 		return *this;
 	}
-	
+
 #endif
 
-  /**@brief Return the x value */
-		B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
-  /**@brief Return the y value */
-		B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
-  /**@brief Return the z value */
-		B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
-  /**@brief Set the x value */
-		B3_FORCE_INLINE void	setX(b3Scalar _x) { m_floats[0] = _x;};
-  /**@brief Set the y value */
-		B3_FORCE_INLINE void	setY(b3Scalar _y) { m_floats[1] = _y;};
-  /**@brief Set the z value */
-		B3_FORCE_INLINE void	setZ(b3Scalar _z) { m_floats[2] = _z;};
-  /**@brief Set the w value */
-		B3_FORCE_INLINE void	setW(b3Scalar _w) { m_floats[3] = _w;};
-  /**@brief Return the x value */
-
-
-	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	/**@brief Return the x value */
+	B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
+	/**@brief Set the x value */
+	B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x; };
+	/**@brief Set the y value */
+	B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y; };
+	/**@brief Set the z value */
+	B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z; };
+	/**@brief Set the w value */
+	B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
+	/**@brief Return the x value */
+
+	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}
 	//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
 	///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
-	B3_FORCE_INLINE	operator       b3Scalar *()       { return &m_floats[0]; }
-	B3_FORCE_INLINE	operator const b3Scalar *() const { return &m_floats[0]; }
+	B3_FORCE_INLINE operator b3Scalar*() { return &m_floats[0]; }
+	B3_FORCE_INLINE operator const b3Scalar*() const { return &m_floats[0]; }
 
-	B3_FORCE_INLINE	bool	operator==(const b3QuadWord& other) const
+	B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
 	{
 #ifdef B3_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]));
+		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
 	}
 
-	B3_FORCE_INLINE	bool	operator!=(const b3QuadWord& other) const
+	B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
 	{
 		return !(*this == other);
 	}
 
-  /**@brief Set x,y,z and zero w 
+	/**@brief Set x,y,z and zero w 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    */
-		B3_FORCE_INLINE void 	setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
-		{
-			m_floats[0]=_x;
-			m_floats[1]=_y;
-			m_floats[2]=_z;
-			m_floats[3] = 0.f;
-		}
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = 0.f;
+	}
 
-/*		void getValue(b3Scalar *m) const 
+	/*		void getValue(b3Scalar *m) const 
 		{
 			m[0] = m_floats[0];
 			m[1] = m_floats[1];
 			m[2] = m_floats[2];
 		}
 */
-/**@brief Set the values 
+	/**@brief Set the values 
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    * @param w Value of w
    */
-		B3_FORCE_INLINE void	setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
-		{
-			m_floats[0]=_x;
-			m_floats[1]=_y;
-			m_floats[2]=_z;
-			m_floats[3]=_w;
-		}
-  /**@brief No initialization constructor */
-		B3_FORCE_INLINE b3QuadWord()
-		//	:m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
-		{
-		}
- 
-  /**@brief Three argument constructor (zeros w)
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = _w;
+	}
+	/**@brief No initialization constructor */
+	B3_FORCE_INLINE b3QuadWord()
+	//	:m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
+	{
+	}
+
+	/**@brief Three argument constructor (zeros w)
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    */
-		B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)		
-		{
-			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
-		}
+	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
+	}
 
-/**@brief Initializing constructor
+	/**@brief Initializing constructor
    * @param x Value of x
    * @param y Value of y
    * @param z Value of z
    * @param w Value of w
    */
-		B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w) 
-		{
-			m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
-		}
+	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _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 b3QuadWord
+	/**@brief Set each element to the max of the current values and the values of another b3QuadWord
    * @param other The other b3QuadWord to compare with 
    */
-		B3_FORCE_INLINE void	setMax(const b3QuadWord& other)
-		{
-        #ifdef B3_USE_SSE
-            mVec128 = _mm_max_ps(mVec128, other.mVec128);
-        #elif defined(B3_USE_NEON)
-            mVec128 = vmaxq_f32(mVec128, other.mVec128);
-        #else
-        	b3SetMax(m_floats[0], other.m_floats[0]);
-			b3SetMax(m_floats[1], other.m_floats[1]);
-			b3SetMax(m_floats[2], other.m_floats[2]);
-			b3SetMax(m_floats[3], other.m_floats[3]);
-		#endif
-        }
-  /**@brief Set each element to the min of the current values and the values of another b3QuadWord
+	B3_FORCE_INLINE void setMax(const b3QuadWord& other)
+	{
+#ifdef B3_USE_SSE
+		mVec128 = _mm_max_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmaxq_f32(mVec128, other.mVec128);
+#else
+		b3SetMax(m_floats[0], other.m_floats[0]);
+		b3SetMax(m_floats[1], other.m_floats[1]);
+		b3SetMax(m_floats[2], other.m_floats[2]);
+		b3SetMax(m_floats[3], other.m_floats[3]);
+#endif
+	}
+	/**@brief Set each element to the min of the current values and the values of another b3QuadWord
    * @param other The other b3QuadWord to compare with 
    */
-		B3_FORCE_INLINE void	setMin(const b3QuadWord& other)
-		{
-        #ifdef B3_USE_SSE
-            mVec128 = _mm_min_ps(mVec128, other.mVec128);
-        #elif defined(B3_USE_NEON)
-            mVec128 = vminq_f32(mVec128, other.mVec128);
-        #else
-        	b3SetMin(m_floats[0], other.m_floats[0]);
-			b3SetMin(m_floats[1], other.m_floats[1]);
-			b3SetMin(m_floats[2], other.m_floats[2]);
-			b3SetMin(m_floats[3], other.m_floats[3]);
-		#endif
-        }
-
-
-
+	B3_FORCE_INLINE void setMin(const b3QuadWord& other)
+	{
+#ifdef B3_USE_SSE
+		mVec128 = _mm_min_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vminq_f32(mVec128, other.mVec128);
+#else
+		b3SetMin(m_floats[0], other.m_floats[0]);
+		b3SetMin(m_floats[1], other.m_floats[1]);
+		b3SetMin(m_floats[2], other.m_floats[2]);
+		b3SetMin(m_floats[3], other.m_floats[3]);
+#endif
+	}
 };
 
-#endif //B3_SIMD_QUADWORD_H
+#endif  //B3_SIMD_QUADWORD_H

thirdparty/bullet/Bullet3Common/b3Random.h

@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef B3_GEN_RANDOM_H
 #define B3_GEN_RANDOM_H
 
@@ -26,8 +24,8 @@ subject to the following restrictions:
 
 #define B3_RAND_MAX UINT_MAX
 
-B3_FORCE_INLINE void         b3Srand(unsigned int seed) { init_genrand(seed); }
-B3_FORCE_INLINE unsigned int b3rand()                   { return genrand_int32(); }
+B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); }
+B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); }
 
 #else
 
@@ -35,8 +33,8 @@ B3_FORCE_INLINE unsigned int b3rand()                   { return genrand_int32()
 
 #define B3_RAND_MAX RAND_MAX
 
-B3_FORCE_INLINE void         b3Srand(unsigned int seed) { srand(seed); } 
-B3_FORCE_INLINE unsigned int b3rand()                   { return rand(); }
+B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); }
+B3_FORCE_INLINE unsigned int b3rand() { return rand(); }
 
 #endif
 
@@ -45,6 +43,4 @@ inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange)
 	return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange;
 }
 
-
-#endif //B3_GEN_RANDOM_H
-
+#endif  //B3_GEN_RANDOM_H

thirdparty/bullet/Bullet3Common/b3ResizablePool.h

@@ -4,10 +4,10 @@
 
 #include "Bullet3Common/b3AlignedObjectArray.h"
 
-enum 
+enum
 {
-	B3_POOL_HANDLE_TERMINAL_FREE=-1,
-	B3_POOL_HANDLE_TERMINAL_USED =-2
+	B3_POOL_HANDLE_TERMINAL_FREE = -1,
+	B3_POOL_HANDLE_TERMINAL_USED = -2
 };
 
 template <typename U>
@@ -20,25 +20,23 @@ struct b3PoolBodyHandle : public U
 	{
 		m_nextFreeHandle = next;
 	}
-	int	getNextFree() const
+	int getNextFree() const
 	{
 		return m_nextFreeHandle;
 	}
 };
 
-template <typename T> 
+template <typename T>
 class b3ResizablePool
 {
-
 protected:
-	b3AlignedObjectArray<T>	m_bodyHandles;
-	int m_numUsedHandles;						// number of active handles
-	int	m_firstFreeHandle;		// free handles list
+	b3AlignedObjectArray<T> m_bodyHandles;
+	int m_numUsedHandles;   // number of active handles
+	int m_firstFreeHandle;  // free handles list
 
 	T* getHandleInternal(int handle)
 	{
 		return &m_bodyHandles[handle];
-
 	}
 	const T* getHandleInternal(int handle) const
 	{
@@ -46,17 +44,16 @@ protected:
 	}
 
 public:
-	
 	b3ResizablePool()
 	{
 		initHandles();
 	}
-	
+
 	virtual ~b3ResizablePool()
 	{
 		exitHandles();
 	}
-///handle management
+	///handle management
 
 	int getNumHandles() const
 	{
@@ -65,44 +62,40 @@ public:
 
 	void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
 	{
-
-		for (int i=0;i<m_bodyHandles.size();i++)
+		for (int i = 0; i < m_bodyHandles.size(); i++)
 		{
-			if (m_bodyHandles[i].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
+			if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
 			{
 				usedHandles.push_back(i);
 			}
 		}
 	}
 
-	
-
 	T* getHandle(int handle)
 	{
-		b3Assert(handle>=0);
-		b3Assert(handle<m_bodyHandles.size());
-		if ((handle<0) || (handle>=m_bodyHandles.size()))
+		b3Assert(handle >= 0);
+		b3Assert(handle < m_bodyHandles.size());
+		if ((handle < 0) || (handle >= m_bodyHandles.size()))
 		{
 			return 0;
 		}
 
-		if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
 		{
 			return &m_bodyHandles[handle];
 		}
 		return 0;
-
 	}
 	const T* getHandle(int handle) const
 	{
-		b3Assert(handle>=0);
-		b3Assert(handle<m_bodyHandles.size());
-		if ((handle<0) || (handle>=m_bodyHandles.size()))
+		b3Assert(handle >= 0);
+		b3Assert(handle < m_bodyHandles.size());
+		if ((handle < 0) || (handle >= m_bodyHandles.size()))
 		{
 			return 0;
 		}
 
-		if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
 		{
 			return &m_bodyHandles[handle];
 		}
@@ -120,7 +113,6 @@ public:
 			for (int i = curCapacity; i < newCapacity; i++)
 				m_bodyHandles[i].setNextFree(i + 1);
 
-
 			m_bodyHandles[newCapacity - 1].setNextFree(-1);
 		}
 		m_firstFreeHandle = curCapacity;
@@ -142,19 +134,18 @@ public:
 
 	int allocHandle()
 	{
-		b3Assert(m_firstFreeHandle>=0);
+		b3Assert(m_firstFreeHandle >= 0);
 
 		int handle = m_firstFreeHandle;
 		m_firstFreeHandle = getHandleInternal(handle)->getNextFree();
 		m_numUsedHandles++;
 
-		if (m_firstFreeHandle<0)
+		if (m_firstFreeHandle < 0)
 		{
 			//int curCapacity = m_bodyHandles.size();
-			int additionalCapacity= m_bodyHandles.size();
+			int additionalCapacity = m_bodyHandles.size();
 			increaseHandleCapacity(additionalCapacity);
 
-
 			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
 		}
 		getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
@@ -162,12 +153,11 @@ public:
 		return handle;
 	}
 
-
 	void freeHandle(int handle)
 	{
 		b3Assert(handle >= 0);
 
-		if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED)
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
 		{
 			getHandleInternal(handle)->clear();
 			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
@@ -176,7 +166,6 @@ public:
 		}
 	}
 };
-	///end handle management
-	
-	#endif //B3_RESIZABLE_POOL_H
-	
+///end handle management
+
+#endif  //B3_RESIZABLE_POOL_H

thirdparty/bullet/Bullet3Common/b3Scalar.h

@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef B3_SCALAR_H
 #define B3_SCALAR_H
 
@@ -22,238 +20,252 @@ subject to the following restrictions:
 #pragma unmanaged
 #endif
 
-
-
 #include <math.h>
-#include <stdlib.h>//size_t for MSVC 6.0
+#include <stdlib.h>  //size_t for MSVC 6.0
 #include <float.h>
 
 //Original repository is at http://github.com/erwincoumans/bullet3
 #define B3_BULLET_VERSION 300
 
-inline int	b3GetVersion()
+inline int b3GetVersion()
 {
 	return B3_BULLET_VERSION;
 }
 
-#if defined(DEBUG) || defined (_DEBUG)
+#if defined(DEBUG) || defined(_DEBUG)
 #define B3_DEBUG
 #endif
 
-#include "b3Logging.h"//for b3Error
-
+#include "b3Logging.h"  //for b3Error
 
 #ifdef _WIN32
 
-		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
+#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined(_MSC_VER) && _MSC_VER < 1300)
 
-			#define B3_FORCE_INLINE inline
-			#define B3_ATTRIBUTE_ALIGNED16(a) a
-			#define B3_ATTRIBUTE_ALIGNED64(a) a
-			#define B3_ATTRIBUTE_ALIGNED128(a) a
-		#else
-			//#define B3_HAS_ALIGNED_ALLOCATOR
-			#pragma warning(disable : 4324) // disable padding warning
+#define B3_FORCE_INLINE inline
+#define B3_ATTRIBUTE_ALIGNED16(a) a
+#define B3_ATTRIBUTE_ALIGNED64(a) a
+#define B3_ATTRIBUTE_ALIGNED128(a) a
+#else
+//#define B3_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 : 4996)  //Turn off warnings about deprecated C routines
 //			#pragma warning(disable:4786) // Disable the "debug name too long" warning
 
-			#define B3_FORCE_INLINE __forceinline
-			#define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
-			#define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
-			#define B3_ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
-		#ifdef _XBOX
-			#define B3_USE_VMX128
-
-			#include <ppcintrinsics.h>
- 			#define B3_HAVE_NATIVE_FSEL
- 			#define b3Fsel(a,b,c) __fsel((a),(b),(c))
-		#else
-
-#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (B3_USE_DOUBLE_PRECISION))
-	#if (defined (_M_IX86) || defined (_M_X64))
-			#define B3_USE_SSE
-			#ifdef B3_USE_SSE
-			//B3_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 B3_USE_SSE_IN_API
-			#endif //B3_USE_SSE
-			#include <emmintrin.h>
-	#endif
+#define B3_FORCE_INLINE __forceinline
+#define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+#define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
+#define B3_ATTRIBUTE_ALIGNED128(a) __declspec(align(128)) a
+#ifdef _XBOX
+#define B3_USE_VMX128
+
+#include <ppcintrinsics.h>
+#define B3_HAVE_NATIVE_FSEL
+#define b3Fsel(a, b, c) __fsel((a), (b), (c))
+#else
+
+#if (defined(_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined(B3_USE_DOUBLE_PRECISION))
+#if (defined(_M_IX86) || defined(_M_X64))
+#define B3_USE_SSE
+#ifdef B3_USE_SSE
+//B3_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 B3_USE_SSE_IN_API
+#endif  //B3_USE_SSE
+#include <emmintrin.h>
+#endif
 #endif
 
-		#endif//_XBOX
+#endif  //_XBOX
 
-		#endif //__MINGW32__
+#endif  //__MINGW32__
 
 #ifdef B3_DEBUG
-	#ifdef _MSC_VER
-		#include <stdio.h>
-		#define b3Assert(x) { if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak();	}}
-	#else//_MSC_VER
-		#include <assert.h>
-		#define b3Assert assert
-	#endif//_MSC_VER
+#ifdef _MSC_VER
+#include <stdio.h>
+#define b3Assert(x)               \
+	{                             \
+		if (!(x))                 \
+		{                         \
+			b3Error(              \
+				"Assert "__FILE__ \
+				":%u (" #x ")\n", \
+				__LINE__);        \
+			__debugbreak();       \
+		}                         \
+	}
+#else  //_MSC_VER
+#include <assert.h>
+#define b3Assert assert
+#endif  //_MSC_VER
 #else
-		#define b3Assert(x)
+#define b3Assert(x)
 #endif
-		//b3FullAssert is optional, slows down a lot
-		#define b3FullAssert(x)
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
 
-		#define b3Likely(_c)  _c
-		#define b3Unlikely(_c) _c
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
 
 #else
-	
-#if defined	(__CELLOS_LV2__)
-		#define B3_FORCE_INLINE inline __attribute__((always_inline))
-		#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-		#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
-		#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
-		#ifndef assert
-		#include <assert.h>
-		#endif
+
+#if defined(__CELLOS_LV2__)
+#define B3_FORCE_INLINE inline __attribute__((always_inline))
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
 #ifdef B3_DEBUG
 #ifdef __SPU__
 #include <spu_printf.h>
 #define printf spu_printf
-	#define b3Assert(x) {if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
+#define b3Assert(x)               \
+	{                             \
+		if (!(x))                 \
+		{                         \
+			b3Error(              \
+				"Assert "__FILE__ \
+				":%u (" #x ")\n", \
+				__LINE__);        \
+			spu_hcmpeq(0, 0);     \
+		}                         \
+	}
 #else
-	#define b3Assert assert
+#define b3Assert assert
 #endif
-	
+
 #else
-		#define b3Assert(x)
+#define b3Assert(x)
 #endif
-		//b3FullAssert is optional, slows down a lot
-		#define b3FullAssert(x)
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
 
-		#define b3Likely(_c)  _c
-		#define b3Unlikely(_c) _c
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
 
 #else
 
 #ifdef USE_LIBSPE2
 
-		#define B3_FORCE_INLINE __inline
-		#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-		#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
-		#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
-		#ifndef assert
-		#include <assert.h>
-		#endif
+#define B3_FORCE_INLINE __inline
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+#ifndef assert
+#include <assert.h>
+#endif
 #ifdef B3_DEBUG
-		#define b3Assert assert
+#define b3Assert assert
 #else
-		#define b3Assert(x)
+#define b3Assert(x)
 #endif
-		//b3FullAssert is optional, slows down a lot
-		#define b3FullAssert(x)
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
 
+#define b3Likely(_c) __builtin_expect((_c), 1)
+#define b3Unlikely(_c) __builtin_expect((_c), 0)
 
-		#define b3Likely(_c)   __builtin_expect((_c), 1)
-		#define b3Unlikely(_c) __builtin_expect((_c), 0)
-		
-
 #else
-	//non-windows systems
-
-#if (defined (__APPLE__) && (!defined (B3_USE_DOUBLE_PRECISION)))
-    #if defined (__i386__) || defined (__x86_64__)
-        #define B3_USE_SSE
-		//B3_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 B3_USE_SSE_IN_API
-        #ifdef B3_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 //B3_USE_SSE
-    #elif defined( __armv7__ )
-        #ifdef __clang__
-            #define B3_USE_NEON 1
-
-            #if defined B3_USE_NEON && defined (__clang__)
-                #include <arm_neon.h>
-            #endif//B3_USE_NEON
-       #endif //__clang__
-    #endif//__arm__
-
-	#define B3_FORCE_INLINE inline __attribute__ ((always_inline))
+//non-windows systems
+
+#if (defined(__APPLE__) && (!defined(B3_USE_DOUBLE_PRECISION)))
+#if defined(__i386__) || defined(__x86_64__)
+#define B3_USE_SSE
+//B3_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 B3_USE_SSE_IN_API
+#ifdef B3_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  //B3_USE_SSE
+#elif defined(__armv7__)
+#ifdef __clang__
+#define B3_USE_NEON 1
+
+#if defined B3_USE_NEON && defined(__clang__)
+#include <arm_neon.h>
+#endif  //B3_USE_NEON
+#endif  //__clang__
+#endif  //__arm__
+
+#define B3_FORCE_INLINE inline __attribute__((always_inline))
 ///@todo: check out alignment methods for other platforms/compilers
-	#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-	#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
-	#define B3_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 b3Assert(x)\
-	{\
-	if(!(x))\
-	{\
-		b3Error("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\
-		asm volatile ("int3");\
-	}\
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_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 b3Assert(x)                                                             \
+	{                                                                           \
+		if (!(x))                                                               \
+		{                                                                       \
+			b3Error("Assert %s in line %d, file %s\n", #x, __LINE__, __FILE__); \
+			asm volatile("int3");                                               \
+		}                                                                       \
 	}
-	#else//defined (__i386__) || defined (__x86_64__)
-		#define b3Assert assert
-	#endif//defined (__i386__) || defined (__x86_64__)
-	#else//defined(DEBUG) || defined (_DEBUG)
-		#define b3Assert(x)
-	#endif//defined(DEBUG) || defined (_DEBUG)
-
-	//b3FullAssert is optional, slows down a lot
-	#define b3FullAssert(x)
-	#define b3Likely(_c)  _c
-	#define b3Unlikely(_c) _c
+#else  //defined (__i386__) || defined (__x86_64__)
+#define b3Assert assert
+#endif  //defined (__i386__) || defined (__x86_64__)
+#else   //defined(DEBUG) || defined (_DEBUG)
+#define b3Assert(x)
+#endif  //defined(DEBUG) || defined (_DEBUG)
+
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
 
 #else
 
-		#define B3_FORCE_INLINE inline
-		///@todo: check out alignment methods for other platforms/compilers
-		#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
-		#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
-		#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
-		///#define B3_ATTRIBUTE_ALIGNED16(a) a
-		///#define B3_ATTRIBUTE_ALIGNED64(a) a
-		///#define B3_ATTRIBUTE_ALIGNED128(a) a
-		#ifndef assert
-		#include <assert.h>
-		#endif
-
-#if defined(DEBUG) || defined (_DEBUG)
-		#define b3Assert assert
+#define B3_FORCE_INLINE inline
+///@todo: check out alignment methods for other platforms/compilers
+#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
+#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
+#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
+///#define B3_ATTRIBUTE_ALIGNED16(a) a
+///#define B3_ATTRIBUTE_ALIGNED64(a) a
+///#define B3_ATTRIBUTE_ALIGNED128(a) a
+#ifndef assert
+#include <assert.h>
+#endif
+
+#if defined(DEBUG) || defined(_DEBUG)
+#define b3Assert assert
 #else
-		#define b3Assert(x)
+#define b3Assert(x)
 #endif
 
-		//b3FullAssert is optional, slows down a lot
-		#define b3FullAssert(x)
-		#define b3Likely(_c)  _c
-		#define b3Unlikely(_c) _c
-#endif //__APPLE__ 
+//b3FullAssert is optional, slows down a lot
+#define b3FullAssert(x)
+#define b3Likely(_c) _c
+#define b3Unlikely(_c) _c
+#endif  //__APPLE__
 
-#endif // LIBSPE2
+#endif  // LIBSPE2
 
-#endif	//__CELLOS_LV2__
+#endif  //__CELLOS_LV2__
 #endif
 
-
 ///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
 #if defined(B3_USE_DOUBLE_PRECISION)
 typedef double b3Scalar;
@@ -267,34 +279,34 @@ typedef float b3Scalar;
 
 #ifdef B3_USE_SSE
 typedef __m128 b3SimdFloat4;
-#endif//B3_USE_SSE
+#endif  //B3_USE_SSE
 
-#if defined B3_USE_SSE_IN_API && defined (B3_USE_SSE)
+#if defined B3_USE_SSE_IN_API && defined(B3_USE_SSE)
 #ifdef _WIN32
 
 #ifndef B3_NAN
 static int b3NanMask = 0x7F800001;
-#define B3_NAN (*(float*)&b3NanMask)
+#define B3_NAN (*(float *)&b3NanMask)
 #endif
 
 #ifndef B3_INFINITY_MASK
-static  int b3InfinityMask = 0x7F800000;
-#define B3_INFINITY_MASK (*(float*)&b3InfinityMask)
+static int b3InfinityMask = 0x7F800000;
+#define B3_INFINITY_MASK (*(float *)&b3InfinityMask)
 #endif
 
-inline __m128 operator + (const __m128 A, const __m128 B)
+inline __m128 operator+(const __m128 A, const __m128 B)
 {
-    return _mm_add_ps(A, B);
+	return _mm_add_ps(A, B);
 }
 
-inline __m128 operator - (const __m128 A, const __m128 B)
+inline __m128 operator-(const __m128 A, const __m128 B)
 {
-    return _mm_sub_ps(A, B);
+	return _mm_sub_ps(A, B);
 }
 
-inline __m128 operator * (const __m128 A, const __m128 B)
+inline __m128 operator*(const __m128 A, const __m128 B)
 {
-    return _mm_mul_ps(A, B);
+	return _mm_mul_ps(A, B);
 }
 
 #define b3CastfTo128i(a) (_mm_castps_si128(a))
@@ -302,18 +314,19 @@ inline __m128 operator * (const __m128 A, const __m128 B)
 #define b3CastiTo128f(a) (_mm_castsi128_ps(a))
 #define b3CastdTo128f(a) (_mm_castpd_ps(a))
 #define b3CastdTo128i(a) (_mm_castpd_si128(a))
-#define b3Assign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
+#define b3Assign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3)
 
-#else//_WIN32
+#else  //_WIN32
 
 #define b3CastfTo128i(a) ((__m128i)(a))
 #define b3CastfTo128d(a) ((__m128d)(a))
-#define b3CastiTo128f(a)  ((__m128) (a))
-#define b3CastdTo128f(a) ((__m128) (a))
+#define b3CastiTo128f(a) ((__m128)(a))
+#define b3CastdTo128f(a) ((__m128)(a))
 #define b3CastdTo128i(a) ((__m128i)(a))
-#define b3Assign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
-#endif//_WIN32
-#endif //B3_USE_SSE_IN_API
+#define b3Assign128(r0, r1, r2, r3) \
+	(__m128) { r0, r1, r2, r3 }
+#endif  //_WIN32
+#endif  //B3_USE_SSE_IN_API
 
 #ifdef B3_USE_NEON
 #include <arm_neon.h>
@@ -321,142 +334,160 @@ inline __m128 operator * (const __m128 A, const __m128 B)
 typedef float32x4_t b3SimdFloat4;
 #define B3_INFINITY INFINITY
 #define B3_NAN NAN
-#define b3Assign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
+#define b3Assign128(r0, r1, r2, r3) \
+	(float32x4_t) { r0, r1, r2, r3 }
 #endif
 
-
-
-
-
-#define B3_DECLARE_ALIGNED_ALLOCATOR() \
-   B3_FORCE_INLINE void* operator new(size_t sizeInBytes)   { return b3AlignedAlloc(sizeInBytes,16); }   \
-   B3_FORCE_INLINE void  operator delete(void* ptr)         { b3AlignedFree(ptr); }   \
-   B3_FORCE_INLINE void* operator new(size_t, void* ptr)   { return ptr; }   \
-   B3_FORCE_INLINE void  operator delete(void*, void*)      { }   \
-   B3_FORCE_INLINE void* operator new[](size_t sizeInBytes)   { return b3AlignedAlloc(sizeInBytes,16); }   \
-   B3_FORCE_INLINE void  operator delete[](void* ptr)         { b3AlignedFree(ptr); }   \
-   B3_FORCE_INLINE void* operator new[](size_t, void* ptr)   { return ptr; }   \
-   B3_FORCE_INLINE void  operator delete[](void*, void*)      { }   \
-
-
+#define B3_DECLARE_ALIGNED_ALLOCATOR()                                                                   \
+	B3_FORCE_INLINE void *operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); }   \
+	B3_FORCE_INLINE void operator delete(void *ptr) { b3AlignedFree(ptr); }                              \
+	B3_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; }                                \
+	B3_FORCE_INLINE void operator delete(void *, void *) {}                                              \
+	B3_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
+	B3_FORCE_INLINE void operator delete[](void *ptr) { b3AlignedFree(ptr); }                            \
+	B3_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; }                              \
+	B3_FORCE_INLINE void operator delete[](void *, void *) {}
 
 #if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS)
-		
-B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x) { return sqrt(x); }
+
+B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x)
+{
+	return sqrt(x);
+}
 B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
 B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
 B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
 B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
-B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) { if (x<b3Scalar(-1))	x=b3Scalar(-1); if (x>b3Scalar(1))	x=b3Scalar(1); return acos(x); }
-B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) { if (x<b3Scalar(-1))	x=b3Scalar(-1); if (x>b3Scalar(1))	x=b3Scalar(1); return asin(x); }
+B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
+{
+	if (x < b3Scalar(-1)) x = b3Scalar(-1);
+	if (x > b3Scalar(1)) x = b3Scalar(1);
+	return acos(x);
+}
+B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
+{
+	if (x < b3Scalar(-1)) x = b3Scalar(-1);
+	if (x > b3Scalar(1)) x = b3Scalar(1);
+	return asin(x);
+}
 B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
 B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
 B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
 B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); }
-B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return pow(x,y); }
-B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmod(x,y); }
+B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return pow(x, y); }
+B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmod(x, y); }
 
 #else
-		
-B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y) 
-{ 
+
+B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y)
+{
 #ifdef USE_APPROXIMATION
-    double x, z, tempf;
-    unsigned long *tfptr = ((unsigned long *)&tempf) + 1;
+	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*b3Scalar(0.5);
-	x = (b3Scalar(1.5)*x)-(x*x)*(x*z);         /* iteration formula     */
-	x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
-	x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
-	x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
-	x = (b3Scalar(1.5)*x)-(x*x)*(x*z);
-	return x*y;
+	*tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */
+	x = tempf;
+	z = y * b3Scalar(0.5);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z); /* iteration formula     */
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	x = (b3Scalar(1.5) * x) - (x * x) * (x * z);
+	return x * y;
 #else
-	return sqrtf(y); 
+	return sqrtf(y);
 #endif
 }
 B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
 B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
 B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
 B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
-B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) { 
-	if (x<b3Scalar(-1))	
-		x=b3Scalar(-1); 
-	if (x>b3Scalar(1))	
-		x=b3Scalar(1);
-	return acosf(x); 
+B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
+{
+	if (x < b3Scalar(-1))
+		x = b3Scalar(-1);
+	if (x > b3Scalar(1))
+		x = b3Scalar(1);
+	return acosf(x);
 }
-B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) { 
-	if (x<b3Scalar(-1))	
-		x=b3Scalar(-1); 
-	if (x>b3Scalar(1))	
-		x=b3Scalar(1);
-	return asinf(x); 
+B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
+{
+	if (x < b3Scalar(-1))
+		x = b3Scalar(-1);
+	if (x > b3Scalar(1))
+		x = b3Scalar(1);
+	return asinf(x);
 }
 B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); }
 B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
 B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); }
 B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); }
-B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return powf(x,y); }
-B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
-	
+B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x, b3Scalar y) { return powf(x, y); }
+B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x, b3Scalar y) { return fmodf(x, y); }
+
 #endif
 
-#define B3_2_PI         b3Scalar(6.283185307179586232)
-#define B3_PI           (B3_2_PI * b3Scalar(0.5))
-#define B3_HALF_PI      (B3_2_PI * b3Scalar(0.25))
+#define B3_2_PI b3Scalar(6.283185307179586232)
+#define B3_PI (B3_2_PI * b3Scalar(0.5))
+#define B3_HALF_PI (B3_2_PI * b3Scalar(0.25))
 #define B3_RADS_PER_DEG (B3_2_PI / b3Scalar(360.0))
-#define B3_DEGS_PER_RAD  (b3Scalar(360.0) / B3_2_PI)
+#define B3_DEGS_PER_RAD (b3Scalar(360.0) / B3_2_PI)
 #define B3_SQRT12 b3Scalar(0.7071067811865475244008443621048490)
 
-#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0)/b3Sqrt(b3Scalar(x))))		/* reciprocal square root */
-
+#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0) / b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
 
 #ifdef B3_USE_DOUBLE_PRECISION
-#define B3_EPSILON      DBL_EPSILON
-#define B3_INFINITY     DBL_MAX
+#define B3_EPSILON DBL_EPSILON
+#define B3_INFINITY DBL_MAX
 #else
-#define B3_EPSILON      FLT_EPSILON
-#define B3_INFINITY     FLT_MAX
+#define B3_EPSILON FLT_EPSILON
+#define B3_INFINITY FLT_MAX
 #endif
 
-B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x) 
+B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
 {
 	b3Scalar coeff_1 = B3_PI / 4.0f;
 	b3Scalar coeff_2 = 3.0f * coeff_1;
 	b3Scalar abs_y = b3Fabs(y);
 	b3Scalar angle;
-	if (x >= 0.0f) {
+	if (x >= 0.0f)
+	{
 		b3Scalar r = (x - abs_y) / (x + abs_y);
 		angle = coeff_1 - coeff_1 * r;
-	} else {
+	}
+	else
+	{
 		b3Scalar r = (x + abs_y) / (abs_y - x);
 		angle = coeff_2 - coeff_1 * r;
 	}
 	return (y < 0.0f) ? -angle : angle;
 }
 
-B3_FORCE_INLINE bool      b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
+B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
 
-B3_FORCE_INLINE bool	b3Equal(b3Scalar a, b3Scalar eps) {
+B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps)
+{
 	return (((a) <= eps) && !((a) < -eps));
 }
-B3_FORCE_INLINE bool	b3GreaterEqual (b3Scalar a, b3Scalar eps) {
+B3_FORCE_INLINE bool b3GreaterEqual(b3Scalar a, b3Scalar eps)
+{
 	return (!((a) <= eps));
 }
 
-
-B3_FORCE_INLINE int       b3IsNegative(b3Scalar x) {
-    return x < b3Scalar(0.0) ? 1 : 0;
+B3_FORCE_INLINE int b3IsNegative(b3Scalar x)
+{
+	return x < b3Scalar(0.0) ? 1 : 0;
 }
 
 B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; }
 B3_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * B3_DEGS_PER_RAD; }
 
-#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+#define B3_DECLARE_HANDLE(name) \
+	typedef struct name##__     \
+	{                           \
+		int unused;             \
+	} * name
 
 #ifndef b3Fsel
 B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
@@ -464,60 +495,57 @@ B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
 	return a >= 0 ? b : c;
 }
 #endif
-#define b3Fsels(a,b,c) (b3Scalar)b3Fsel(a,b,c)
-
+#define b3Fsels(a, b, c) (b3Scalar) b3Fsel(a, b, c)
 
 B3_FORCE_INLINE bool b3MachineIsLittleEndian()
 {
-   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;
+	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;
 }
 
-
-
 ///b3Select 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
-B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) 
+B3_FORCE_INLINE unsigned b3Select(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)); 
+	// 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));
 }
 B3_FORCE_INLINE int b3Select(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));
+	unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+	unsigned testEqz = ~testNz;
+	return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
 }
 B3_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
 {
 #ifdef B3_HAVE_NATIVE_FSEL
-    return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+	return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
 #else
-    return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; 
+	return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
 #endif
 }
 
-template<typename T> B3_FORCE_INLINE void b3Swap(T& a, T& b)
+template <typename T>
+B3_FORCE_INLINE void b3Swap(T &a, T &b)
 {
 	T tmp = a;
 	a = b;
 	b = tmp;
 }
 
-
 //PCK: endian swapping functions
 B3_FORCE_INLINE unsigned b3SwapEndian(unsigned val)
 {
-	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8)  | ((val & 0x000000ff) << 24));
+	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
 }
 
 B3_FORCE_INLINE unsigned short b3SwapEndian(unsigned short val)
@@ -532,87 +560,85 @@ B3_FORCE_INLINE unsigned b3SwapEndian(int val)
 
 B3_FORCE_INLINE unsigned short b3SwapEndian(short val)
 {
-	return b3SwapEndian((unsigned short) val);
+	return b3SwapEndian((unsigned short)val);
 }
 
 ///b3SwapFloat uses using char pointers to swap the endianness
 ////b3SwapFloat/b3SwapDouble 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. 
+///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
-B3_FORCE_INLINE unsigned int  b3SwapEndianFloat(float d)
+B3_FORCE_INLINE unsigned int b3SwapEndianFloat(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;
+	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
-B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a) 
+B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
 {
-    float d = 0.0f;
-    unsigned char *src = (unsigned char *)&a;
-    unsigned char *dst = (unsigned char *)&d;
+	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];
+	dst[0] = src[3];
+	dst[1] = src[2];
+	dst[2] = src[1];
+	dst[3] = src[0];
 
-    return d;
+	return d;
 }
 
-
 // swap using char pointers
-B3_FORCE_INLINE void  b3SwapEndianDouble(double d, unsigned char* dst)
+B3_FORCE_INLINE void b3SwapEndianDouble(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];
-
+	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
-B3_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src) 
+B3_FORCE_INLINE double b3UnswapEndianDouble(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];
+	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;
 }
 
 // returns normalized value in range [-B3_PI, B3_PI]
-B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians) 
+B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
 {
 	angleInRadians = b3Fmod(angleInRadians, B3_2_PI);
-	if(angleInRadians < -B3_PI)
+	if (angleInRadians < -B3_PI)
 	{
 		return angleInRadians + B3_2_PI;
 	}
-	else if(angleInRadians > B3_PI)
+	else if (angleInRadians > B3_PI)
 	{
 		return angleInRadians - B3_2_PI;
 	}
@@ -626,38 +652,34 @@ B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
 struct b3TypedObject
 {
 	b3TypedObject(int objectType)
-		:m_objectType(objectType)
+		: m_objectType(objectType)
 	{
 	}
-	int	m_objectType;
+	int m_objectType;
 	inline int getObjectType() const
 	{
 		return m_objectType;
 	}
 };
 
-
-  
 ///align a pointer to the provided alignment, upwards
-template <typename T>T* b3AlignPointer(T* unalignedPtr, size_t alignment)
+template <typename T>
+T *b3AlignPointer(T *unalignedPtr, size_t alignment)
 {
-		
 	struct b3ConvertPointerSizeT
 	{
-		union 
-		{
-				T* ptr;
-				size_t integer;
+		union {
+			T *ptr;
+			size_t integer;
 		};
 	};
-    b3ConvertPointerSizeT converter;
-    
-    
+	b3ConvertPointerSizeT converter;
+
 	const size_t bit_mask = ~(alignment - 1);
-    converter.ptr = unalignedPtr;
-	converter.integer += alignment-1;
+	converter.ptr = unalignedPtr;
+	converter.integer += alignment - 1;
 	converter.integer &= bit_mask;
 	return converter.ptr;
 }
 
-#endif //B3_SCALAR_H
+#endif  //B3_SCALAR_H

thirdparty/bullet/Bullet3Common/b3StackAlloc.h

@@ -20,97 +20,99 @@ Nov.2006
 #ifndef B3_STACK_ALLOC
 #define B3_STACK_ALLOC
 
-#include "b3Scalar.h" //for b3Assert
+#include "b3Scalar.h"  //for b3Assert
 #include "b3AlignedAllocator.h"
 
 ///The b3Block class is an internal structure for the b3StackAlloc memory allocator.
 struct b3Block
 {
-	b3Block*			previous;
-	unsigned char*		address;
+	b3Block* previous;
+	unsigned char* address;
 };
 
 ///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
 class b3StackAlloc
 {
 public:
+	b3StackAlloc(unsigned int size)
+	{
+		ctor();
+		create(size);
+	}
+	~b3StackAlloc() { destroy(); }
 
-	b3StackAlloc(unsigned int size)	{ ctor();create(size); }
-	~b3StackAlloc()		{ destroy(); }
-	
-	inline void		create(unsigned int size)
+	inline void create(unsigned int size)
 	{
 		destroy();
-		data		=  (unsigned char*) b3AlignedAlloc(size,16);
-		totalsize	=	size;
+		data = (unsigned char*)b3AlignedAlloc(size, 16);
+		totalsize = size;
 	}
-	inline void		destroy()
+	inline void destroy()
 	{
-		b3Assert(usedsize==0);
+		b3Assert(usedsize == 0);
 		//Raise(L"StackAlloc is still in use");
 
-		if(usedsize==0)
+		if (usedsize == 0)
 		{
-			if(!ischild && data)		
+			if (!ischild && data)
 				b3AlignedFree(data);
 
-			data				=	0;
-			usedsize			=	0;
+			data = 0;
+			usedsize = 0;
 		}
-		
 	}
 
-	int	getAvailableMemory() const
+	int getAvailableMemory() const
 	{
 		return static_cast<int>(totalsize - usedsize);
 	}
 
-	unsigned char*			allocate(unsigned int size)
+	unsigned char* allocate(unsigned int size)
 	{
-		const unsigned int	nus(usedsize+size);
-		if(nus<totalsize)
+		const unsigned int nus(usedsize + size);
+		if (nus < totalsize)
 		{
-			usedsize=nus;
-			return(data+(usedsize-size));
+			usedsize = nus;
+			return (data + (usedsize - size));
 		}
 		b3Assert(0);
 		//&& (L"Not enough memory"));
-		
-		return(0);
+
+		return (0);
 	}
-	B3_FORCE_INLINE b3Block*		beginBlock()
+	B3_FORCE_INLINE b3Block* beginBlock()
 	{
-		b3Block*	pb = (b3Block*)allocate(sizeof(b3Block));
-		pb->previous	=	current;
-		pb->address		=	data+usedsize;
-		current			=	pb;
-		return(pb);
+		b3Block* pb = (b3Block*)allocate(sizeof(b3Block));
+		pb->previous = current;
+		pb->address = data + usedsize;
+		current = pb;
+		return (pb);
 	}
-	B3_FORCE_INLINE void		endBlock(b3Block* block)
+	B3_FORCE_INLINE void endBlock(b3Block* block)
 	{
-		b3Assert(block==current);
+		b3Assert(block == current);
 		//Raise(L"Unmatched blocks");
-		if(block==current)
+		if (block == current)
 		{
-			current		=	block->previous;
-			usedsize	=	(unsigned int)((block->address-data)-sizeof(b3Block));
+			current = block->previous;
+			usedsize = (unsigned int)((block->address - data) - sizeof(b3Block));
 		}
 	}
 
 private:
-	void		ctor()
+	void ctor()
 	{
-		data		=	0;
-		totalsize	=	0;
-		usedsize	=	0;
-		current		=	0;
-		ischild		=	false;
+		data = 0;
+		totalsize = 0;
+		usedsize = 0;
+		current = 0;
+		ischild = false;
 	}
-	unsigned char*		data;
-	unsigned int		totalsize;
-	unsigned int		usedsize;
-	b3Block*	current;
-	bool		ischild;
+	unsigned char* data;
+	unsigned int totalsize;
+	unsigned int usedsize;
+	b3Block* current;
+	bool ischild;
 };
 
-#endif //B3_STACK_ALLOC
+#endif  //B3_STACK_ALLOC

thirdparty/bullet/Bullet3Common/b3Transform.h

@@ -12,11 +12,9 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef B3_TRANSFORM_H
 #define B3_TRANSFORM_H
 
-
 #include "b3Matrix3x3.h"
 
 #ifdef B3_USE_DOUBLE_PRECISION
@@ -25,46 +23,45 @@ subject to the following restrictions:
 #define b3TransformData b3TransformFloatData
 #endif
 
-
-
-
 /**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
  *It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */
-B3_ATTRIBUTE_ALIGNED16(class) b3Transform {
-	
-  ///Storage for the rotation
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Transform
+{
+	///Storage for the rotation
 	b3Matrix3x3 m_basis;
-  ///Storage for the translation
-	b3Vector3   m_origin;
+	///Storage for the translation
+	b3Vector3 m_origin;
 
 public:
-	
-  /**@brief No initialization constructor */
+	/**@brief No initialization constructor */
 	b3Transform() {}
-  /**@brief Constructor from b3Quaternion (optional b3Vector3 )
+	/**@brief Constructor from b3Quaternion (optional b3Vector3 )
    * @param q Rotation from quaternion 
    * @param c Translation from Vector (default 0,0,0) */
-	explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q, 
-		const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0))) 
+	explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q,
+										 const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
 		: m_basis(q),
-		m_origin(c)
-	{}
+		  m_origin(c)
+	{
+	}
 
-  /**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
+	/**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
    * @param b Rotation from Matrix 
    * @param c Translation from Vector default (0,0,0)*/
-	explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b, 
-		const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
+	explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b,
+										 const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
 		: m_basis(b),
-		m_origin(c)
-	{}
-  /**@brief Copy constructor */
-	B3_FORCE_INLINE b3Transform (const b3Transform& other)
+		  m_origin(c)
+	{
+	}
+	/**@brief Copy constructor */
+	B3_FORCE_INLINE b3Transform(const b3Transform& other)
 		: m_basis(other.m_basis),
-		m_origin(other.m_origin)
+		  m_origin(other.m_origin)
 	{
 	}
-  /**@brief Assignment Operator */
+	/**@brief Assignment Operator */
 	B3_FORCE_INLINE b3Transform& operator=(const b3Transform& other)
 	{
 		m_basis = other.m_basis;
@@ -72,70 +69,70 @@ public:
 		return *this;
 	}
 
-
-  /**@brief Set the current transform as the value of the product of two transforms
+	/**@brief Set the current transform as the value of the product of two transforms
    * @param t1 Transform 1
    * @param t2 Transform 2
    * This = Transform1 * Transform2 */
-		B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2) {
-			m_basis = t1.m_basis * t2.m_basis;
-			m_origin = t1(t2.m_origin);
-		}
+	B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2)
+	{
+		m_basis = t1.m_basis * t2.m_basis;
+		m_origin = t1(t2.m_origin);
+	}
 
-/*		void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
+	/*		void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
 			b3Vector3 v = t2.m_origin - t1.m_origin;
 			m_basis = b3MultTransposeLeft(t1.m_basis, t2.m_basis);
 			m_origin = v * t1.m_basis;
 		}
 		*/
 
-/**@brief Return the transform of the vector */
+	/**@brief Return the transform of the vector */
 	B3_FORCE_INLINE b3Vector3 operator()(const b3Vector3& x) const
 	{
-        return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
+		return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
 	}
 
-  /**@brief Return the transform of the vector */
+	/**@brief Return the transform of the vector */
 	B3_FORCE_INLINE b3Vector3 operator*(const b3Vector3& x) const
 	{
 		return (*this)(x);
 	}
 
-  /**@brief Return the transform of the b3Quaternion */
+	/**@brief Return the transform of the b3Quaternion */
 	B3_FORCE_INLINE b3Quaternion operator*(const b3Quaternion& q) const
 	{
 		return getRotation() * q;
 	}
 
-  /**@brief Return the basis matrix for the rotation */
-	B3_FORCE_INLINE b3Matrix3x3&       getBasis()          { return m_basis; }
-  /**@brief Return the basis matrix for the rotation */
-	B3_FORCE_INLINE const b3Matrix3x3& getBasis()    const { return m_basis; }
+	/**@brief Return the basis matrix for the rotation */
+	B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; }
+	/**@brief Return the basis matrix for the rotation */
+	B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; }
 
-  /**@brief Return the origin vector translation */
-	B3_FORCE_INLINE b3Vector3&         getOrigin()         { return m_origin; }
-  /**@brief Return the origin vector translation */
-	B3_FORCE_INLINE const b3Vector3&   getOrigin()   const { return m_origin; }
+	/**@brief Return the origin vector translation */
+	B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; }
+	/**@brief Return the origin vector translation */
+	B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; }
 
-  /**@brief Return a quaternion representing the rotation */
-	b3Quaternion getRotation() const { 
+	/**@brief Return a quaternion representing the rotation */
+	b3Quaternion getRotation() const
+	{
 		b3Quaternion q;
 		m_basis.getRotation(q);
 		return q;
 	}
-	
-	
-  /**@brief Set from an array 
+
+	/**@brief Set from an array 
    * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
-	void setFromOpenGLMatrix(const b3Scalar *m)
+	void setFromOpenGLMatrix(const b3Scalar* m)
 	{
 		m_basis.setFromOpenGLSubMatrix(m);
-		m_origin.setValue(m[12],m[13],m[14]);
+		m_origin.setValue(m[12], m[13], m[14]);
 	}
 
-  /**@brief Fill an array representation
+	/**@brief Fill an array representation
    * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
-	void getOpenGLMatrix(b3Scalar *m) const 
+	void getOpenGLMatrix(b3Scalar * m) const
 	{
 		m_basis.getOpenGLSubMatrix(m);
 		m[12] = m_origin.getX();
@@ -144,80 +141,76 @@ public:
 		m[15] = b3Scalar(1.0);
 	}
 
-  /**@brief Set the translational element
+	/**@brief Set the translational element
    * @param origin The vector to set the translation to */
-	B3_FORCE_INLINE void setOrigin(const b3Vector3& origin) 
-	{ 
+	B3_FORCE_INLINE void setOrigin(const b3Vector3& origin)
+	{
 		m_origin = origin;
 	}
 
 	B3_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const;
 
-
-  /**@brief Set the rotational element by b3Matrix3x3 */
+	/**@brief Set the rotational element by b3Matrix3x3 */
 	B3_FORCE_INLINE void setBasis(const b3Matrix3x3& basis)
-	{ 
+	{
 		m_basis = basis;
 	}
 
-  /**@brief Set the rotational element by b3Quaternion */
+	/**@brief Set the rotational element by b3Quaternion */
 	B3_FORCE_INLINE void setRotation(const b3Quaternion& q)
 	{
 		m_basis.setRotation(q);
 	}
 
-
-  /**@brief Set this transformation to the identity */
+	/**@brief Set this transformation to the identity */
 	void setIdentity()
 	{
 		m_basis.setIdentity();
 		m_origin.setValue(b3Scalar(0.0), b3Scalar(0.0), b3Scalar(0.0));
 	}
 
-  /**@brief Multiply this Transform by another(this = this * another) 
+	/**@brief Multiply this Transform by another(this = this * another) 
    * @param t The other transform */
-	b3Transform& operator*=(const b3Transform& t) 
+	b3Transform& operator*=(const b3Transform& t)
 	{
 		m_origin += m_basis * t.m_origin;
 		m_basis *= t.m_basis;
 		return *this;
 	}
 
-  /**@brief Return the inverse of this transform */
+	/**@brief Return the inverse of this transform */
 	b3Transform inverse() const
-	{ 
+	{
 		b3Matrix3x3 inv = m_basis.transpose();
 		return b3Transform(inv, inv * -m_origin);
 	}
 
-  /**@brief Return the inverse of this transform times the other transform
+	/**@brief Return the inverse of this transform times the other transform
    * @param t The other transform 
    * return this.inverse() * the other */
-	b3Transform inverseTimes(const b3Transform& t) const;  
+	b3Transform inverseTimes(const b3Transform& t) const;
 
-  /**@brief Return the product of this transform and the other */
+	/**@brief Return the product of this transform and the other */
 	b3Transform operator*(const b3Transform& t) const;
 
-  /**@brief Return an identity transform */
-	static const b3Transform&	getIdentity()
+	/**@brief Return an identity transform */
+	static const b3Transform& getIdentity()
 	{
 		static const b3Transform identityTransform(b3Matrix3x3::getIdentity());
 		return identityTransform;
 	}
 
-	void	serialize(struct	b3TransformData& dataOut) const;
-
-	void	serializeFloat(struct	b3TransformFloatData& dataOut) const;
+	void serialize(struct b3TransformData & dataOut) const;
 
-	void	deSerialize(const struct	b3TransformData& dataIn);
+	void serializeFloat(struct b3TransformFloatData & dataOut) const;
 
-	void	deSerializeDouble(const struct	b3TransformDoubleData& dataIn);
+	void deSerialize(const struct b3TransformData& dataIn);
 
-	void	deSerializeFloat(const struct	b3TransformFloatData& dataIn);
+	void deSerializeDouble(const struct b3TransformDoubleData& dataIn);
 
+	void deSerializeFloat(const struct b3TransformFloatData& dataIn);
 };
 
-
 B3_FORCE_INLINE b3Vector3
 b3Transform::invXform(const b3Vector3& inVec) const
 {
@@ -225,80 +218,69 @@ b3Transform::invXform(const b3Vector3& inVec) const
 	return (m_basis.transpose() * v);
 }
 
-B3_FORCE_INLINE b3Transform 
-b3Transform::inverseTimes(const b3Transform& t) const  
+B3_FORCE_INLINE b3Transform
+b3Transform::inverseTimes(const b3Transform& t) const
 {
 	b3Vector3 v = t.getOrigin() - m_origin;
-		return b3Transform(m_basis.transposeTimes(t.m_basis),
-			v * m_basis);
+	return b3Transform(m_basis.transposeTimes(t.m_basis),
+					   v * m_basis);
 }
 
-B3_FORCE_INLINE b3Transform 
-b3Transform::operator*(const b3Transform& t) const
+B3_FORCE_INLINE b3Transform
+	b3Transform::operator*(const b3Transform& t) const
 {
-	return b3Transform(m_basis * t.m_basis, 
-		(*this)(t.m_origin));
+	return b3Transform(m_basis * t.m_basis,
+					   (*this)(t.m_origin));
 }
 
 /**@brief Test if two transforms have all elements equal */
 B3_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2)
 {
-   return ( t1.getBasis()  == t2.getBasis() &&
-            t1.getOrigin() == t2.getOrigin() );
+	return (t1.getBasis() == t2.getBasis() &&
+			t1.getOrigin() == t2.getOrigin());
 }
 
-
 ///for serialization
-struct	b3TransformFloatData
+struct b3TransformFloatData
 {
-	b3Matrix3x3FloatData	m_basis;
-	b3Vector3FloatData	m_origin;
+	b3Matrix3x3FloatData m_basis;
+	b3Vector3FloatData m_origin;
 };
 
-struct	b3TransformDoubleData
+struct b3TransformDoubleData
 {
-	b3Matrix3x3DoubleData	m_basis;
-	b3Vector3DoubleData	m_origin;
+	b3Matrix3x3DoubleData m_basis;
+	b3Vector3DoubleData m_origin;
 };
 
-
-
-B3_FORCE_INLINE	void	b3Transform::serialize(b3TransformData& dataOut) const
+B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const
 {
 	m_basis.serialize(dataOut.m_basis);
 	m_origin.serialize(dataOut.m_origin);
 }
 
-B3_FORCE_INLINE	void	b3Transform::serializeFloat(b3TransformFloatData& dataOut) const
+B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const
 {
 	m_basis.serializeFloat(dataOut.m_basis);
 	m_origin.serializeFloat(dataOut.m_origin);
 }
 
-
-B3_FORCE_INLINE	void	b3Transform::deSerialize(const b3TransformData& dataIn)
+B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn)
 {
 	m_basis.deSerialize(dataIn.m_basis);
 	m_origin.deSerialize(dataIn.m_origin);
 }
 
-B3_FORCE_INLINE	void	b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn)
+B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn)
 {
 	m_basis.deSerializeFloat(dataIn.m_basis);
 	m_origin.deSerializeFloat(dataIn.m_origin);
 }
 
-B3_FORCE_INLINE	void	b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn)
+B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn)
 {
 	m_basis.deSerializeDouble(dataIn.m_basis);
 	m_origin.deSerializeDouble(dataIn.m_origin);
 }
 
-
-#endif //B3_TRANSFORM_H
-
-
-
-
-
-
+#endif  //B3_TRANSFORM_H

thirdparty/bullet/Bullet3Common/b3TransformUtil.h

@@ -12,204 +12,189 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef B3_TRANSFORM_UTIL_H
 #define B3_TRANSFORM_UTIL_H
 
 #include "b3Transform.h"
-#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5)*B3_HALF_PI
-
-
-
+#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5) * B3_HALF_PI
 
-B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents,const b3Vector3& supportDir)
+B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents, const b3Vector3& supportDir)
 {
 	return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
-      supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
-      supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ()); 
+						 supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
+						 supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
 }
 
-
-
-
-
-
 /// Utils related to temporal transforms
 class b3TransformUtil
 {
-
 public:
-
-	static void integrateTransform(const b3Transform& curTrans,const b3Vector3& linvel,const b3Vector3& angvel,b3Scalar timeStep,b3Transform& predictedTransform)
+	static void integrateTransform(const b3Transform& curTrans, const b3Vector3& linvel, const b3Vector3& angvel, b3Scalar timeStep, b3Transform& predictedTransform)
 	{
 		predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
-//	#define QUATERNION_DERIVATIVE
-	#ifdef QUATERNION_DERIVATIVE
+		//	#define QUATERNION_DERIVATIVE
+#ifdef QUATERNION_DERIVATIVE
 		b3Quaternion predictedOrn = curTrans.getRotation();
 		predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5));
 		predictedOrn.normalize();
-	#else
+#else
 		//Exponential map
 		//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
 
 		b3Vector3 axis;
-		b3Scalar	fAngle = angvel.length(); 
+		b3Scalar fAngle = angvel.length();
 		//limit the angular motion
-		if (fAngle*timeStep > B3_ANGULAR_MOTION_THRESHOLD)
+		if (fAngle * timeStep > B3_ANGULAR_MOTION_THRESHOLD)
 		{
 			fAngle = B3_ANGULAR_MOTION_THRESHOLD / timeStep;
 		}
 
-		if ( fAngle < b3Scalar(0.001) )
+		if (fAngle < b3Scalar(0.001))
 		{
 			// use Taylor's expansions of sync function
-			axis   = angvel*( b3Scalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(b3Scalar(0.020833333333))*fAngle*fAngle );
+			axis = angvel * (b3Scalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (b3Scalar(0.020833333333)) * fAngle * fAngle);
 		}
 		else
 		{
 			// sync(fAngle) = sin(c*fAngle)/t
-			axis   = angvel*( b3Sin(b3Scalar(0.5)*fAngle*timeStep)/fAngle );
+			axis = angvel * (b3Sin(b3Scalar(0.5) * fAngle * timeStep) / fAngle);
 		}
-		b3Quaternion dorn (axis.getX(),axis.getY(),axis.getZ(),b3Cos( fAngle*timeStep*b3Scalar(0.5) ));
+		b3Quaternion dorn(axis.getX(), axis.getY(), axis.getZ(), b3Cos(fAngle * timeStep * b3Scalar(0.5)));
 		b3Quaternion orn0 = curTrans.getRotation();
 
 		b3Quaternion predictedOrn = dorn * orn0;
 		predictedOrn.normalize();
-	#endif
+#endif
 		predictedTransform.setRotation(predictedOrn);
 	}
 
-	static void	calculateVelocityQuaternion(const b3Vector3& pos0,const b3Vector3& pos1,const b3Quaternion& orn0,const b3Quaternion& orn1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
+	static void calculateVelocityQuaternion(const b3Vector3& pos0, const b3Vector3& pos1, const b3Quaternion& orn0, const b3Quaternion& orn1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
 	{
 		linVel = (pos1 - pos0) / timeStep;
 		b3Vector3 axis;
-		b3Scalar  angle;
+		b3Scalar angle;
 		if (orn0 != orn1)
 		{
-			calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
+			calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
 			angVel = axis * angle / timeStep;
-		} else
+		}
+		else
 		{
-			angVel.setValue(0,0,0);
+			angVel.setValue(0, 0, 0);
 		}
 	}
 
-	static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle)
+	static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0, const b3Quaternion& orn1a, b3Vector3& axis, b3Scalar& angle)
 	{
 		b3Quaternion orn1 = orn0.nearest(orn1a);
 		b3Quaternion dorn = orn1 * orn0.inverse();
 		angle = dorn.getAngle();
-		axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ());
+		axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
 		axis[3] = b3Scalar(0.);
 		//check for axis length
 		b3Scalar len = axis.length2();
-		if (len < B3_EPSILON*B3_EPSILON)
-			axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
+		if (len < B3_EPSILON * B3_EPSILON)
+			axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
 		else
 			axis /= b3Sqrt(len);
 	}
 
-	static void	calculateVelocity(const b3Transform& transform0,const b3Transform& transform1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
+	static void calculateVelocity(const b3Transform& transform0, const b3Transform& transform1, b3Scalar timeStep, b3Vector3& linVel, b3Vector3& angVel)
 	{
 		linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
 		b3Vector3 axis;
-		b3Scalar  angle;
-		calculateDiffAxisAngle(transform0,transform1,axis,angle);
+		b3Scalar angle;
+		calculateDiffAxisAngle(transform0, transform1, axis, angle);
 		angVel = axis * angle / timeStep;
 	}
 
-	static void calculateDiffAxisAngle(const b3Transform& transform0,const b3Transform& transform1,b3Vector3& axis,b3Scalar& angle)
+	static void calculateDiffAxisAngle(const b3Transform& transform0, const b3Transform& transform1, b3Vector3& axis, b3Scalar& angle)
 	{
 		b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
 		b3Quaternion dorn;
 		dmat.getRotation(dorn);
 
-		///floating point inaccuracy can lead to w component > 1..., which breaks 
+		///floating point inaccuracy can lead to w component > 1..., which breaks
 		dorn.normalize();
-		
+
 		angle = dorn.getAngle();
-		axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ());
+		axis = b3MakeVector3(dorn.getX(), dorn.getY(), dorn.getZ());
 		axis[3] = b3Scalar(0.);
 		//check for axis length
 		b3Scalar len = axis.length2();
-		if (len < B3_EPSILON*B3_EPSILON)
-			axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
+		if (len < B3_EPSILON * B3_EPSILON)
+			axis = b3MakeVector3(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.));
 		else
 			axis /= b3Sqrt(len);
 	}
-
 };
 
-
-///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection 
+///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
 ///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
-class	b3ConvexSeparatingDistanceUtil
+class b3ConvexSeparatingDistanceUtil
 {
-	b3Quaternion	m_ornA;
-	b3Quaternion	m_ornB;
-	b3Vector3	m_posA;
-	b3Vector3	m_posB;
-	
-	b3Vector3	m_separatingNormal;
+	b3Quaternion m_ornA;
+	b3Quaternion m_ornB;
+	b3Vector3 m_posA;
+	b3Vector3 m_posB;
 
-	b3Scalar	m_boundingRadiusA;
-	b3Scalar	m_boundingRadiusB;
-	b3Scalar	m_separatingDistance;
+	b3Vector3 m_separatingNormal;
 
-public:
+	b3Scalar m_boundingRadiusA;
+	b3Scalar m_boundingRadiusB;
+	b3Scalar m_separatingDistance;
 
-	b3ConvexSeparatingDistanceUtil(b3Scalar	boundingRadiusA,b3Scalar	boundingRadiusB)
-		:m_boundingRadiusA(boundingRadiusA),
-		m_boundingRadiusB(boundingRadiusB),
-		m_separatingDistance(0.f)
+public:
+	b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA, b3Scalar boundingRadiusB)
+		: m_boundingRadiusA(boundingRadiusA),
+		  m_boundingRadiusB(boundingRadiusB),
+		  m_separatingDistance(0.f)
 	{
 	}
 
-	b3Scalar	getConservativeSeparatingDistance()
+	b3Scalar getConservativeSeparatingDistance()
 	{
 		return m_separatingDistance;
 	}
 
-	void	updateSeparatingDistance(const b3Transform& transA,const b3Transform& transB)
+	void updateSeparatingDistance(const b3Transform& transA, const b3Transform& transB)
 	{
 		const b3Vector3& toPosA = transA.getOrigin();
 		const b3Vector3& toPosB = transB.getOrigin();
 		b3Quaternion toOrnA = transA.getRotation();
 		b3Quaternion toOrnB = transB.getRotation();
 
-		if (m_separatingDistance>0.f)
+		if (m_separatingDistance > 0.f)
 		{
-			
-
-			b3Vector3 linVelA,angVelA,linVelB,angVelB;
-			b3TransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,b3Scalar(1.),linVelA,angVelA);
-			b3TransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,b3Scalar(1.),linVelB,angVelB);
+			b3Vector3 linVelA, angVelA, linVelB, angVelB;
+			b3TransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, b3Scalar(1.), linVelA, angVelA);
+			b3TransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, b3Scalar(1.), linVelB, angVelB);
 			b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
-			b3Vector3 relLinVel = (linVelB-linVelA);
+			b3Vector3 relLinVel = (linVelB - linVelA);
 			b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
-			if (relLinVelocLength<0.f)
+			if (relLinVelocLength < 0.f)
 			{
 				relLinVelocLength = 0.f;
 			}
-	
-			b3Scalar	projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
+
+			b3Scalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
 			m_separatingDistance -= projectedMotion;
 		}
-	
+
 		m_posA = toPosA;
 		m_posB = toPosB;
 		m_ornA = toOrnA;
 		m_ornB = toOrnB;
 	}
 
-	void	initSeparatingDistance(const b3Vector3& separatingVector,b3Scalar separatingDistance,const b3Transform& transA,const b3Transform& transB)
+	void initSeparatingDistance(const b3Vector3& separatingVector, b3Scalar separatingDistance, const b3Transform& transA, const b3Transform& transB)
 	{
 		m_separatingDistance = separatingDistance;
 
-		if (m_separatingDistance>0.f)
+		if (m_separatingDistance > 0.f)
 		{
 			m_separatingNormal = separatingVector;
-			
+
 			const b3Vector3& toPosA = transA.getOrigin();
 			const b3Vector3& toPosB = transB.getOrigin();
 			b3Quaternion toOrnA = transA.getRotation();
@@ -220,9 +205,6 @@ public:
 			m_ornB = toOrnB;
 		}
 	}
-
 };
 
-
-#endif //B3_TRANSFORM_UTIL_H
-
+#endif  //B3_TRANSFORM_UTIL_H

thirdparty/bullet/Bullet3Common/shared/b3Float4.h

@@ -4,94 +4,87 @@
 #include "Bullet3Common/shared/b3PlatformDefinitions.h"
 
 #ifdef __cplusplus
-	#include "Bullet3Common/b3Vector3.h"
-	#define b3Float4 b3Vector3
-	#define b3Float4ConstArg const b3Vector3&
-	#define b3Dot3F4 b3Dot
-	#define b3Cross3 b3Cross
-	#define	b3MakeFloat4  b3MakeVector3
-	inline b3Vector3 b3Normalized(const b3Vector3& vec)
-	{
-		return vec.normalized();
-	}
-
-	inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
-	{
-		return v.normalized();
-	}
-
-	inline b3Float4 b3MaxFloat4 (const b3Float4& a, const b3Float4& b)
-	{
-		b3Float4 tmp = a;
-		tmp.setMax(b);
-		return tmp;
-	}
-	inline b3Float4 b3MinFloat4 (const b3Float4& a, const b3Float4& b)
-	{
-		b3Float4 tmp = a;
-		tmp.setMin(b);
-		return tmp;
-	}
+#include "Bullet3Common/b3Vector3.h"
+#define b3Float4 b3Vector3
+#define b3Float4ConstArg const b3Vector3&
+#define b3Dot3F4 b3Dot
+#define b3Cross3 b3Cross
+#define b3MakeFloat4 b3MakeVector3
+inline b3Vector3 b3Normalized(const b3Vector3& vec)
+{
+	return vec.normalized();
+}
 
+inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
+{
+	return v.normalized();
+}
 
+inline b3Float4 b3MaxFloat4(const b3Float4& a, const b3Float4& b)
+{
+	b3Float4 tmp = a;
+	tmp.setMax(b);
+	return tmp;
+}
+inline b3Float4 b3MinFloat4(const b3Float4& a, const b3Float4& b)
+{
+	b3Float4 tmp = a;
+	tmp.setMin(b);
+	return tmp;
+}
 
 #else
-	typedef float4	b3Float4;
-	#define b3Float4ConstArg const b3Float4
-	#define b3MakeFloat4 (float4)
-	float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)
-	{
-		float4 a1 = b3MakeFloat4(v0.xyz,0.f);
-		float4 b1 = b3MakeFloat4(v1.xyz,0.f);
-		return dot(a1, b1);
-	}
-	b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)
-	{
-		float4 a1 = b3MakeFloat4(v0.xyz,0.f);
-		float4 b1 = b3MakeFloat4(v1.xyz,0.f);
-		return cross(a1, b1);
-	}
-	#define b3MinFloat4 min
-	#define b3MaxFloat4 max
-
-	#define b3Normalized(a) normalize(a)
+typedef float4 b3Float4;
+#define b3Float4ConstArg const b3Float4
+#define b3MakeFloat4 (float4)
+float b3Dot3F4(b3Float4ConstArg v0, b3Float4ConstArg v1)
+{
+	float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
+	float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
+	return dot(a1, b1);
+}
+b3Float4 b3Cross3(b3Float4ConstArg v0, b3Float4ConstArg v1)
+{
+	float4 a1 = b3MakeFloat4(v0.xyz, 0.f);
+	float4 b1 = b3MakeFloat4(v1.xyz, 0.f);
+	return cross(a1, b1);
+}
+#define b3MinFloat4 min
+#define b3MaxFloat4 max
 
-#endif 
+#define b3Normalized(a) normalize(a)
 
+#endif
 
-		
 inline bool b3IsAlmostZero(b3Float4ConstArg v)
 {
-	if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6)	
+	if (b3Fabs(v.x) > 1e-6 || b3Fabs(v.y) > 1e-6 || b3Fabs(v.z) > 1e-6)
 		return false;
 	return true;
 }
 
-
-inline int    b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )
+inline int b3MaxDot(b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut)
 {
-    float maxDot = -B3_INFINITY;
-    int i = 0;
-    int ptIndex = -1;
-    for( i = 0; i < vecLen; i++ )
-    {
-        float dot = b3Dot3F4(vecArray[i],vec);
-            
-        if( dot > maxDot )
-        {
-            maxDot = dot;
-            ptIndex = i;
-        }
-    }
-	b3Assert(ptIndex>=0);
-    if (ptIndex<0)
+	float maxDot = -B3_INFINITY;
+	int i = 0;
+	int ptIndex = -1;
+	for (i = 0; i < vecLen; i++)
+	{
+		float dot = b3Dot3F4(vecArray[i], vec);
+
+		if (dot > maxDot)
+		{
+			maxDot = dot;
+			ptIndex = i;
+		}
+	}
+	b3Assert(ptIndex >= 0);
+	if (ptIndex < 0)
 	{
 		ptIndex = 0;
 	}
-    *dotOut = maxDot;
-    return ptIndex;
+	*dotOut = maxDot;
+	return ptIndex;
 }
 
-
-
-#endif //B3_FLOAT4_H
+#endif  //B3_FLOAT4_H

thirdparty/bullet/Bullet3Common/shared/b3Int2.h

@@ -20,11 +20,10 @@ subject to the following restrictions:
 
 struct b3UnsignedInt2
 {
-	union
-	{
+	union {
 		struct
 		{
-			unsigned int x,y;
+			unsigned int x, y;
 		};
 		struct
 		{
@@ -35,11 +34,10 @@ struct b3UnsignedInt2
 
 struct b3Int2
 {
-	union
-	{
+	union {
 		struct
 		{
-			int x,y;
+			int x, y;
 		};
 		struct
 		{
@@ -51,7 +49,8 @@ struct b3Int2
 inline b3Int2 b3MakeInt2(int x, int y)
 {
 	b3Int2 v;
-	v.s[0] = x; v.s[1] = y;
+	v.s[0] = x;
+	v.s[1] = y;
 	return v;
 }
 #else
@@ -60,5 +59,5 @@ inline b3Int2 b3MakeInt2(int x, int y)
 #define b3Int2 int2
 #define b3MakeInt2 (int2)
 
-#endif //__cplusplus
+#endif  //__cplusplus
 #endif

thirdparty/bullet/Bullet3Common/shared/b3Int4.h

@@ -5,16 +5,15 @@
 
 #include "Bullet3Common/b3Scalar.h"
 
-
-B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3UnsignedInt4
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	union
-	{
+	union {
 		struct
 		{
-			unsigned int x,y,z,w;
+			unsigned int x, y, z, w;
 		};
 		struct
 		{
@@ -23,15 +22,15 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
 	};
 };
 
-B3_ATTRIBUTE_ALIGNED16(struct) b3Int4
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3Int4
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	union
-	{
+	union {
 		struct
 		{
-			int x,y,z,w;
+			int x, y, z, w;
 		};
 		struct
 		{
@@ -43,26 +42,30 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3Int4
 B3_FORCE_INLINE b3Int4 b3MakeInt4(int x, int y, int z, int w = 0)
 {
 	b3Int4 v;
-	v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w;
+	v.s[0] = x;
+	v.s[1] = y;
+	v.s[2] = z;
+	v.s[3] = w;
 	return v;
 }
 
 B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y, unsigned int z, unsigned int w = 0)
 {
 	b3UnsignedInt4 v;
-	v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w;
+	v.s[0] = x;
+	v.s[1] = y;
+	v.s[2] = z;
+	v.s[3] = w;
 	return v;
 }
 
 #else
 
-
 #define b3UnsignedInt4 uint4
 #define b3Int4 int4
 #define b3MakeInt4 (int4)
 #define b3MakeUnsignedInt4 (uint4)
 
+#endif  //__cplusplus
 
-#endif //__cplusplus
-
-#endif //B3_INT4_H
+#endif  //B3_INT4_H

thirdparty/bullet/Bullet3Common/shared/b3Mat3x3.h

@@ -4,7 +4,6 @@
 
 #include "Bullet3Common/shared/b3Quat.h"
 
-
 #ifdef __cplusplus
 
 #include "Bullet3Common/b3Matrix3x3.h"
@@ -22,43 +21,41 @@ inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg mat)
 	return mat.absolute();
 }
 
-#define b3GetRow(m,row) m.getRow(row)
+#define b3GetRow(m, row) m.getRow(row)
 
-__inline
-b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
+__inline b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
 {
-	return b*a;
+	return b * a;
 }
 
-
 #else
 
 typedef struct
 {
 	b3Float4 m_row[3];
-}b3Mat3x3;
+} b3Mat3x3;
 
 #define b3Mat3x3ConstArg const b3Mat3x3
-#define b3GetRow(m,row) (m.m_row[row])
+#define b3GetRow(m, row) (m.m_row[row])
 
 inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)
 {
-	b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);
+	b3Float4 quat2 = (b3Float4)(quat.x * quat.x, quat.y * quat.y, quat.z * quat.z, 0.f);
 	b3Mat3x3 out;
 
-	out.m_row[0].x=1-2*quat2.y-2*quat2.z;
-	out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;
-	out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;
+	out.m_row[0].x = 1 - 2 * quat2.y - 2 * quat2.z;
+	out.m_row[0].y = 2 * quat.x * quat.y - 2 * quat.w * quat.z;
+	out.m_row[0].z = 2 * quat.x * quat.z + 2 * quat.w * quat.y;
 	out.m_row[0].w = 0.f;
 
-	out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;
-	out.m_row[1].y=1-2*quat2.x-2*quat2.z;
-	out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;
+	out.m_row[1].x = 2 * quat.x * quat.y + 2 * quat.w * quat.z;
+	out.m_row[1].y = 1 - 2 * quat2.x - 2 * quat2.z;
+	out.m_row[1].z = 2 * quat.y * quat.z - 2 * quat.w * quat.x;
 	out.m_row[1].w = 0.f;
 
-	out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;
-	out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;
-	out.m_row[2].z=1-2*quat2.x-2*quat2.y;
+	out.m_row[2].x = 2 * quat.x * quat.z - 2 * quat.w * quat.y;
+	out.m_row[2].y = 2 * quat.y * quat.z + 2 * quat.w * quat.x;
+	out.m_row[2].z = 1 - 2 * quat2.x - 2 * quat2.y;
 	out.m_row[2].w = 0.f;
 
 	return out;
@@ -73,27 +70,19 @@ inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)
 	return out;
 }
 
+__inline b3Mat3x3 mtZero();
 
-__inline
-b3Mat3x3 mtZero();
-
-__inline
-b3Mat3x3 mtIdentity();
+__inline b3Mat3x3 mtIdentity();
 
-__inline
-b3Mat3x3 mtTranspose(b3Mat3x3 m);
+__inline b3Mat3x3 mtTranspose(b3Mat3x3 m);
 
-__inline
-b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
+__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
 
-__inline
-b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
+__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
 
-__inline
-b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
+__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
 
-__inline
-b3Mat3x3 mtZero()
+__inline b3Mat3x3 mtZero()
 {
 	b3Mat3x3 m;
 	m.m_row[0] = (b3Float4)(0.f);
@@ -102,18 +91,16 @@ b3Mat3x3 mtZero()
 	return m;
 }
 
-__inline
-b3Mat3x3 mtIdentity()
+__inline b3Mat3x3 mtIdentity()
 {
 	b3Mat3x3 m;
-	m.m_row[0] = (b3Float4)(1,0,0,0);
-	m.m_row[1] = (b3Float4)(0,1,0,0);
-	m.m_row[2] = (b3Float4)(0,0,1,0);
+	m.m_row[0] = (b3Float4)(1, 0, 0, 0);
+	m.m_row[1] = (b3Float4)(0, 1, 0, 0);
+	m.m_row[2] = (b3Float4)(0, 0, 1, 0);
 	return m;
 }
 
-__inline
-b3Mat3x3 mtTranspose(b3Mat3x3 m)
+__inline b3Mat3x3 mtTranspose(b3Mat3x3 m)
 {
 	b3Mat3x3 out;
 	out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
@@ -122,58 +109,49 @@ b3Mat3x3 mtTranspose(b3Mat3x3 m)
 	return out;
 }
 
-__inline
-b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
+__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
 {
 	b3Mat3x3 transB;
-	transB = mtTranspose( b );
+	transB = mtTranspose(b);
 	b3Mat3x3 ans;
 	//	why this doesn't run when 0ing in the for{}
 	a.m_row[0].w = 0.f;
 	a.m_row[1].w = 0.f;
 	a.m_row[2].w = 0.f;
-	for(int i=0; i<3; i++)
+	for (int i = 0; i < 3; i++)
 	{
-//	a.m_row[i].w = 0.f;
-		ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);
-		ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);
-		ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);
+		//	a.m_row[i].w = 0.f;
+		ans.m_row[i].x = b3Dot3F4(a.m_row[i], transB.m_row[0]);
+		ans.m_row[i].y = b3Dot3F4(a.m_row[i], transB.m_row[1]);
+		ans.m_row[i].z = b3Dot3F4(a.m_row[i], transB.m_row[2]);
 		ans.m_row[i].w = 0.f;
 	}
 	return ans;
 }
 
-__inline
-b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
+__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
 {
 	b3Float4 ans;
-	ans.x = b3Dot3F4( a.m_row[0], b );
-	ans.y = b3Dot3F4( a.m_row[1], b );
-	ans.z = b3Dot3F4( a.m_row[2], b );
+	ans.x = b3Dot3F4(a.m_row[0], b);
+	ans.y = b3Dot3F4(a.m_row[1], b);
+	ans.z = b3Dot3F4(a.m_row[2], b);
 	ans.w = 0.f;
 	return ans;
 }
 
-__inline
-b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
+__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
 {
 	b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
 	b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
 	b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
 
 	b3Float4 ans;
-	ans.x = b3Dot3F4( a, colx );
-	ans.y = b3Dot3F4( a, coly );
-	ans.z = b3Dot3F4( a, colz );
+	ans.x = b3Dot3F4(a, colx);
+	ans.y = b3Dot3F4(a, coly);
+	ans.z = b3Dot3F4(a, colz);
 	return ans;
 }
 
-
 #endif
 
-
-
-
-
-
-#endif //B3_MAT3x3_H
+#endif  //B3_MAT3x3_H

thirdparty/bullet/Bullet3Common/shared/b3PlatformDefinitions.h

@@ -8,18 +8,18 @@ struct MyTest
 
 #ifdef __cplusplus
 //#define b3ConstArray(a) const b3AlignedObjectArray<a>&
-#define b3ConstArray(a) const a*
+#define b3ConstArray(a) const a *
 #define b3AtomicInc(a) ((*a)++)
 
-inline int b3AtomicAdd (volatile int *p, int val)
+inline int b3AtomicAdd(volatile int *p, int val)
 {
 	int oldValue = *p;
-	int newValue = oldValue+val;
+	int newValue = oldValue + val;
 	*p = newValue;
 	return oldValue;
 }
 
-#define __global 
+#define __global
 
 #define B3_STATIC static
 #else
@@ -27,7 +27,7 @@ inline int b3AtomicAdd (volatile int *p, int val)
 #define B3_LARGE_FLOAT 1e18f
 #define B3_INFINITY 1e18f
 #define b3Assert(a)
-#define b3ConstArray(a) __global const a*
+#define b3ConstArray(a) __global const a *
 #define b3AtomicInc atomic_inc
 #define b3AtomicAdd atomic_add
 #define b3Fabs fabs

thirdparty/bullet/Bullet3Common/shared/b3Quat.h

@@ -5,35 +5,34 @@
 #include "Bullet3Common/shared/b3Float4.h"
 
 #ifdef __cplusplus
-	#include "Bullet3Common/b3Quaternion.h"
-	#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
 
-	#define b3Quat b3Quaternion
-	#define b3QuatConstArg const b3Quaternion&
-	inline b3Quat b3QuatInverse(b3QuatConstArg orn)
-	{
-		return orn.inverse();
-	}
+#define b3Quat b3Quaternion
+#define b3QuatConstArg const b3Quaternion&
+inline b3Quat b3QuatInverse(b3QuatConstArg orn)
+{
+	return orn.inverse();
+}
 
-	inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg  orientation)
-	{
-		b3Transform tr;
-		tr.setOrigin(translation);
-		tr.setRotation(orientation);
-		return tr(point);
-	}
+inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
+{
+	b3Transform tr;
+	tr.setOrigin(translation);
+	tr.setRotation(orientation);
+	return tr(point);
+}
 
 #else
-	typedef float4	b3Quat;
-	#define b3QuatConstArg const b3Quat
-	
-	
+typedef float4 b3Quat;
+#define b3QuatConstArg const b3Quat
+
 inline float4 b3FastNormalize4(float4 v)
 {
-	v = (float4)(v.xyz,0.f);
+	v = (float4)(v.xyz, 0.f);
 	return fast_normalize(v);
 }
-	
+
 inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
 inline b3Quat b3QuatNormalized(b3QuatConstArg in);
 inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
@@ -43,20 +42,20 @@ inline b3Quat b3QuatInverse(b3QuatConstArg q);
 inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
 {
 	b3Quat ans;
-	ans = b3Cross3( a, b );
-	ans += a.w*b+b.w*a;
-//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
-	ans.w = a.w*b.w - b3Dot3F4(a, b);
+	ans = b3Cross3(a, b);
+	ans += a.w * b + b.w * a;
+	//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
+	ans.w = a.w * b.w - b3Dot3F4(a, b);
 	return ans;
 }
 
 inline b3Quat b3QuatNormalized(b3QuatConstArg in)
 {
 	b3Quat q;
-	q=in;
+	q = in;
 	//return b3FastNormalize4(in);
 	float len = native_sqrt(dot(q, q));
-	if(len > 0.f)
+	if (len > 0.f)
 	{
 		q *= 1.f / len;
 	}
@@ -69,15 +68,13 @@ inline b3Quat b3QuatNormalized(b3QuatConstArg in)
 }
 inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
 {
-	b3Quat qInv = b3QuatInvert( q );
+	b3Quat qInv = b3QuatInvert(q);
 	float4 vcpy = vec;
 	vcpy.w = 0.f;
-	float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);
+	float4 out = b3QuatMul(b3QuatMul(q, vcpy), qInv);
 	return out;
 }
 
-
-
 inline b3Quat b3QuatInverse(b3QuatConstArg q)
 {
 	return (b3Quat)(-q.xyz, q.w);
@@ -90,14 +87,14 @@ inline b3Quat b3QuatInvert(b3QuatConstArg q)
 
 inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
 {
-	return b3QuatRotate( b3QuatInvert( q ), vec );
+	return b3QuatRotate(b3QuatInvert(q), vec);
 }
 
-inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg  orientation)
+inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
 {
-	return b3QuatRotate( orientation, point ) + (translation);
+	return b3QuatRotate(orientation, point) + (translation);
 }
-	
-#endif 
 
-#endif //B3_QUAT_H
+#endif
+
+#endif  //B3_QUAT_H

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h

@@ -18,7 +18,7 @@ subject to the following restrictions:
 
 #include "Bullet3Common/b3Scalar.h"
 
-enum	b3SolverMode
+enum b3SolverMode
 {
 	B3_SOLVER_RANDMIZE_ORDER = 1,
 	B3_SOLVER_FRICTION_SEPARATE = 2,
@@ -34,45 +34,38 @@ enum	b3SolverMode
 
 struct b3ContactSolverInfoData
 {
-	
-
-	b3Scalar	m_tau;
-	b3Scalar	m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	b3Scalar	m_friction;
-	b3Scalar	m_timeStep;
-	b3Scalar	m_restitution;
-	int		m_numIterations;
-	b3Scalar	m_maxErrorReduction;
-	b3Scalar	m_sor;
-	b3Scalar	m_erp;//used as Baumgarte factor
-	b3Scalar	m_erp2;//used in Split Impulse
-	b3Scalar	m_globalCfm;//constraint force mixing
-	int			m_splitImpulse;
-	b3Scalar	m_splitImpulsePenetrationThreshold;
-	b3Scalar	m_splitImpulseTurnErp;
-	b3Scalar	m_linearSlop;
-	b3Scalar	m_warmstartingFactor;
-
-	int			m_solverMode;
-	int	m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	b3Scalar	m_maxGyroscopicForce;
-	b3Scalar	m_singleAxisRollingFrictionThreshold;
-
-
+	b3Scalar m_tau;
+	b3Scalar m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	b3Scalar m_friction;
+	b3Scalar m_timeStep;
+	b3Scalar m_restitution;
+	int m_numIterations;
+	b3Scalar m_maxErrorReduction;
+	b3Scalar m_sor;
+	b3Scalar m_erp;        //used as Baumgarte factor
+	b3Scalar m_erp2;       //used in Split Impulse
+	b3Scalar m_globalCfm;  //constraint force mixing
+	int m_splitImpulse;
+	b3Scalar m_splitImpulsePenetrationThreshold;
+	b3Scalar m_splitImpulseTurnErp;
+	b3Scalar m_linearSlop;
+	b3Scalar m_warmstartingFactor;
+
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	b3Scalar m_maxGyroscopicForce;
+	b3Scalar m_singleAxisRollingFrictionThreshold;
 };
 
 struct b3ContactSolverInfo : public b3ContactSolverInfoData
 {
-
-	
-
 	inline b3ContactSolverInfo()
 	{
 		m_tau = b3Scalar(0.6);
 		m_damping = b3Scalar(1.0);
 		m_friction = b3Scalar(0.3);
-		m_timeStep = b3Scalar(1.f/60.f);
+		m_timeStep = b3Scalar(1.f / 60.f);
 		m_restitution = b3Scalar(0.);
 		m_maxErrorReduction = b3Scalar(20.);
 		m_numIterations = 10;
@@ -84,76 +77,73 @@ struct b3ContactSolverInfo : public b3ContactSolverInfoData
 		m_splitImpulsePenetrationThreshold = -.04f;
 		m_splitImpulseTurnErp = 0.1f;
 		m_linearSlop = b3Scalar(0.0);
-		m_warmstartingFactor=b3Scalar(0.85);
+		m_warmstartingFactor = b3Scalar(0.85);
 		//m_solverMode =  B3_SOLVER_USE_WARMSTARTING |  B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER;
-		m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_SOLVER_RANDMIZE_ORDER;
-		m_restingContactRestitutionThreshold = 2;//unused as of 2.81
-		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
-		m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
-		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+		m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;  // | B3_SOLVER_RANDMIZE_ORDER;
+		m_restingContactRestitutionThreshold = 2;                    //unused as of 2.81
+		m_minimumSolverBatchSize = 128;                              //try to combine islands until the amount of constraints reaches this limit
+		m_maxGyroscopicForce = 100.f;                                ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
+		m_singleAxisRollingFrictionThreshold = 1e30f;                ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 	}
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct b3ContactSolverInfoDoubleData
 {
-	double		m_tau;
-	double		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	double		m_friction;
-	double		m_timeStep;
-	double		m_restitution;
-	double		m_maxErrorReduction;
-	double		m_sor;
-	double		m_erp;//used as Baumgarte factor
-	double		m_erp2;//used in Split Impulse
-	double		m_globalCfm;//constraint force mixing
-	double		m_splitImpulsePenetrationThreshold;
-	double		m_splitImpulseTurnErp;
-	double		m_linearSlop;
-	double		m_warmstartingFactor;
-	double		m_maxGyroscopicForce;
-	double		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	int			m_splitImpulse;
-	char		m_padding[4];
-
+	double m_tau;
+	double m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	double m_friction;
+	double m_timeStep;
+	double m_restitution;
+	double m_maxErrorReduction;
+	double m_sor;
+	double m_erp;        //used as Baumgarte factor
+	double m_erp2;       //used in Split Impulse
+	double m_globalCfm;  //constraint force mixing
+	double m_splitImpulsePenetrationThreshold;
+	double m_splitImpulseTurnErp;
+	double m_linearSlop;
+	double m_warmstartingFactor;
+	double m_maxGyroscopicForce;
+	double m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	int m_splitImpulse;
+	char m_padding[4];
 };
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct b3ContactSolverInfoFloatData
 {
-	float		m_tau;
-	float		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	float		m_friction;
-	float		m_timeStep;
-
-	float		m_restitution;
-	float		m_maxErrorReduction;
-	float		m_sor;
-	float		m_erp;//used as Baumgarte factor
-
-	float		m_erp2;//used in Split Impulse
-	float		m_globalCfm;//constraint force mixing
-	float		m_splitImpulsePenetrationThreshold;
-	float		m_splitImpulseTurnErp;
-
-	float		m_linearSlop;
-	float		m_warmstartingFactor;
-	float		m_maxGyroscopicForce;
-	float		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-
-	int			m_splitImpulse;
-	char		m_padding[4];
+	float m_tau;
+	float m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	float m_friction;
+	float m_timeStep;
+
+	float m_restitution;
+	float m_maxErrorReduction;
+	float m_sor;
+	float m_erp;  //used as Baumgarte factor
+
+	float m_erp2;       //used in Split Impulse
+	float m_globalCfm;  //constraint force mixing
+	float m_splitImpulsePenetrationThreshold;
+	float m_splitImpulseTurnErp;
+
+	float m_linearSlop;
+	float m_warmstartingFactor;
+	float m_maxGyroscopicForce;
+	float m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+
+	int m_splitImpulse;
+	char m_padding[4];
 };
 
-
-
-#endif //B3_CONTACT_SOLVER_INFO
+#endif  //B3_CONTACT_SOLVER_INFO

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp

@@ -4,105 +4,100 @@
 #include "Bullet3Common/b3TransformUtil.h"
 #include <new>
 
-
-b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB)
-:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB)
+b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB)
+	: b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB)
 {
 	m_pivotInA = frameInA.getOrigin();
 	m_pivotInB = frameInB.getOrigin();
-	m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
-
+	m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse();
 }
 
-b3FixedConstraint::~b3FixedConstraint ()
+b3FixedConstraint::~b3FixedConstraint()
 {
 }
 
-	
-void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
 	info->m_numConstraintRows = 6;
 	info->nub = 6;
 }
 
-void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
 {
 	//fix the 3 linear degrees of freedom
 
 	const b3Vector3& worldPosA = bodies[m_rbA].m_pos;
 	const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat;
-	const b3Vector3& worldPosB= bodies[m_rbB].m_pos;
+	const b3Vector3& worldPosB = bodies[m_rbB].m_pos;
 	const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat;
 
 	info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
 
-	b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA);
+	b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA);
 	{
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		b3Vector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
+
 	if (info->m_J2linearAxis)
 	{
 		info->m_J2linearAxis[0] = -1;
-		info->m_J2linearAxis[info->rowskip+1] = -1;
-		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+		info->m_J2linearAxis[info->rowskip + 1] = -1;
+		info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
 	}
-	
-	b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB);
-   
+
+	b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB);
+
 	{
-	//	b3Vector3 a2n = -a2;
+		//	b3Vector3 a2n = -a2;
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
 
-    // set right hand side for the linear dofs
+	// set right hand side for the linear dofs
 	b3Scalar k = info->fps * info->erp;
-	b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA);
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = linearError[j];
+	b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA);
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = linearError[j];
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
+	}
 
-		//fix the 3 angular degrees of freedom
+	//fix the 3 angular degrees of freedom
 
 	int start_row = 3;
 	int s = info->rowskip;
-    int start_index = start_row * s;
+	int start_index = start_row * s;
 
-    // 3 rows to make body rotations equal
+	// 3 rows to make body rotations equal
 	info->m_J1angularAxis[start_index] = 1;
-    info->m_J1angularAxis[start_index + s + 1] = 1;
-    info->m_J1angularAxis[start_index + s*2+2] = 1;
-    if ( info->m_J2angularAxis)
-    {
-        info->m_J2angularAxis[start_index] = -1;
-        info->m_J2angularAxis[start_index + s+1] = -1;
-        info->m_J2angularAxis[start_index + s*2+2] = -1;
-    }
-
+	info->m_J1angularAxis[start_index + s + 1] = 1;
+	info->m_J1angularAxis[start_index + s * 2 + 2] = 1;
+	if (info->m_J2angularAxis)
+	{
+		info->m_J2angularAxis[start_index] = -1;
+		info->m_J2angularAxis[start_index + s + 1] = -1;
+		info->m_J2angularAxis[start_index + s * 2 + 2] = -1;
+	}
 
-    // set right hand side for the angular dofs
+	// set right hand side for the angular dofs
 
 	b3Vector3 diff;
 	b3Scalar angle;
-	b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse();
-
-	b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
-	diff*=-angle;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
-    }
+	b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse();
 
+	b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle);
+	diff *= -angle;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j];
+	}
 }

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h

@@ -4,32 +4,31 @@
 
 #include "b3TypedConstraint.h"
 
-B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint
+B3_ATTRIBUTE_ALIGNED16(class)
+b3FixedConstraint : public b3TypedConstraint
 {
 	b3Vector3 m_pivotInA;
 	b3Vector3 m_pivotInB;
 	b3Quaternion m_relTargetAB;
 
 public:
-	b3FixedConstraint(int  rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB);
-	
+	b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB);
+
 	virtual ~b3FixedConstraint();
 
-	
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
 
-	virtual	void	setParam(int num, b3Scalar value, int axis = -1)
+	virtual void setParam(int num, b3Scalar value, int axis = -1)
 	{
 		b3Assert(0);
 	}
-	virtual	b3Scalar getParam(int num, int axis = -1) const
+	virtual b3Scalar getParam(int num, int axis = -1) const
 	{
 		b3Assert(0);
 		return 0.f;
 	}
-
 };
 
-#endif //B3_FIXED_CONSTRAINT_H
+#endif  //B3_FIXED_CONSTRAINT_H

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp

@@ -26,69 +26,48 @@ http://gimpact.sf.net
 #include "Bullet3Common/b3TransformUtil.h"
 #include <new>
 
-
-
 #define D6_USE_OBSOLETE_METHOD false
 #define D6_USE_FRAME_OFFSET true
 
-
-
-
-
-
-b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int  rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
-: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
-, m_frameInA(frameInA)
-, m_frameInB(frameInB),
-m_useLinearReferenceFrameA(useLinearReferenceFrameA),
-m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-m_flags(0)
+b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
+	: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
 {
 	calculateTransforms(bodies);
 }
 
-
-
-
-
-
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-
-
 b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
 b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
 {
-	int i = index%3;
-	int j = index/3;
+	int i = index % 3;
+	int j = index / 3;
 	return mat[i][j];
 }
 
-
-
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz);
-bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
 {
 	//	// rot =  cy*cz          -cy*sz           sy
 	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
 	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
 	//
 
-	b3Scalar fi = btGetMatrixElem(mat,2);
+	b3Scalar fi = btGetMatrixElem(mat, 2);
 	if (fi < b3Scalar(1.0f))
 	{
 		if (fi > b3Scalar(-1.0f))
 		{
-			xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
-			xyz[1] = b3Asin(btGetMatrixElem(mat,2));
-			xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+			xyz[1] = b3Asin(btGetMatrixElem(mat, 2));
+			xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 			return true;
 		}
 		else
 		{
 			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-			xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 			xyz[1] = -B3_HALF_PI;
 			xyz[2] = b3Scalar(0.0);
 			return false;
@@ -97,7 +76,7 @@ bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
 	else
 	{
 		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-		xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 		xyz[1] = B3_HALF_PI;
 		xyz[2] = 0.0;
 	}
@@ -108,85 +87,75 @@ bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
 
 int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
 {
-	if(m_loLimit>m_hiLimit)
+	if (m_loLimit > m_hiLimit)
 	{
-		m_currentLimit = 0;//Free from violation
+		m_currentLimit = 0;  //Free from violation
 		return 0;
 	}
 	if (test_value < m_loLimit)
 	{
-		m_currentLimit = 1;//low limit violation
-		m_currentLimitError =  test_value - m_loLimit;
-		if(m_currentLimitError>B3_PI) 
-			m_currentLimitError-=B3_2_PI;
-		else if(m_currentLimitError<-B3_PI) 
-			m_currentLimitError+=B3_2_PI;
+		m_currentLimit = 1;  //low limit violation
+		m_currentLimitError = test_value - m_loLimit;
+		if (m_currentLimitError > B3_PI)
+			m_currentLimitError -= B3_2_PI;
+		else if (m_currentLimitError < -B3_PI)
+			m_currentLimitError += B3_2_PI;
 		return 1;
 	}
-	else if (test_value> m_hiLimit)
+	else if (test_value > m_hiLimit)
 	{
-		m_currentLimit = 2;//High limit violation
+		m_currentLimit = 2;  //High limit violation
 		m_currentLimitError = test_value - m_hiLimit;
-		if(m_currentLimitError>B3_PI) 
-			m_currentLimitError-=B3_2_PI;
-		else if(m_currentLimitError<-B3_PI) 
-			m_currentLimitError+=B3_2_PI;
+		if (m_currentLimitError > B3_PI)
+			m_currentLimitError -= B3_2_PI;
+		else if (m_currentLimitError < -B3_PI)
+			m_currentLimitError += B3_2_PI;
 		return 2;
 	};
 
-	m_currentLimit = 0;//Free from violation
+	m_currentLimit = 0;  //Free from violation
 	return 0;
-
 }
 
-
-
-
 //////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
 
-
-
-
 //////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
 
-
 int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
 {
 	b3Scalar loLimit = m_lowerLimit[limitIndex];
 	b3Scalar hiLimit = m_upperLimit[limitIndex];
-	if(loLimit > hiLimit)
+	if (loLimit > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 0;//Free from violation
+		m_currentLimit[limitIndex] = 0;  //Free from violation
 		m_currentLimitError[limitIndex] = b3Scalar(0.f);
 		return 0;
 	}
 
 	if (test_value < loLimit)
 	{
-		m_currentLimit[limitIndex] = 2;//low limit violation
-		m_currentLimitError[limitIndex] =  test_value - loLimit;
+		m_currentLimit[limitIndex] = 2;  //low limit violation
+		m_currentLimitError[limitIndex] = test_value - loLimit;
 		return 2;
 	}
-	else if (test_value> hiLimit)
+	else if (test_value > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimit[limitIndex] = 1;  //High limit violation
 		m_currentLimitError[limitIndex] = test_value - hiLimit;
 		return 1;
 	};
 
-	m_currentLimit[limitIndex] = 0;//Free from violation
+	m_currentLimit[limitIndex] = 0;  //Free from violation
 	m_currentLimitError[limitIndex] = b3Scalar(0.f);
 	return 0;
 }
 
-
-
 //////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
 
 void b3Generic6DofConstraint::calculateAngleInfo()
 {
-	b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+	b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
+	matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 	// in euler angle mode we do not actually constrain the angular velocity
 	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
 	//
@@ -211,12 +180,11 @@ void b3Generic6DofConstraint::calculateAngleInfo()
 	m_calculatedAxis[0].normalize();
 	m_calculatedAxis[1].normalize();
 	m_calculatedAxis[2].normalize();
-
 }
 
 static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
 {
-	b3Transform tr(body.m_quat,body.m_pos);
+	b3Transform tr(body.m_quat, body.m_pos);
 	return tr;
 }
 
@@ -226,26 +194,26 @@ void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
 	b3Transform transB;
 	transA = getCenterOfMassTransform(bodies[m_rbA]);
 	transB = getCenterOfMassTransform(bodies[m_rbB]);
-	calculateTransforms(transA,transB,bodies);
+	calculateTransforms(transA, transB, bodies);
 }
 
-void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies)
 {
 	m_calculatedTransformA = transA * m_frameInA;
 	m_calculatedTransformB = transB * m_frameInB;
 	calculateLinearInfo();
 	calculateAngleInfo();
-	if(m_useOffsetForConstraintFrame)
-	{	//  get weight factors depending on masses
+	if (m_useOffsetForConstraintFrame)
+	{  //  get weight factors depending on masses
 		b3Scalar miA = bodies[m_rbA].m_invMass;
 		b3Scalar miB = bodies[m_rbB].m_invMass;
 		m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
 		b3Scalar miS = miA + miB;
-		if(miS > b3Scalar(0.f))
+		if (miS > b3Scalar(0.f))
 		{
 			m_factA = miB / miS;
 		}
-		else 
+		else
 		{
 			m_factA = b3Scalar(0.5f);
 		}
@@ -253,12 +221,6 @@ void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,cons
 	}
 }
 
-
-
-
-
-
-
 bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
 {
 	b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
@@ -269,48 +231,43 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
 	return m_angularLimits[axis_index].needApplyTorques();
 }
 
-
-
-
-void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
 	//prepare constraint
-	calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
+	calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies);
 	info->m_numConstraintRows = 0;
 	info->nub = 6;
 	int i;
 	//test linear limits
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
-		if(m_linearLimits.needApplyForce(i))
+		if (m_linearLimits.needApplyForce(i))
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
 		}
 	}
 	//test angular limits
-	for (i=0;i<3 ;i++ )
+	for (i = 0; i < 3; i++)
 	{
-		if(testAngularLimitMotor(i))
+		if (testAngularLimitMotor(i))
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
 		}
 	}
-//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
+	//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
 }
 
-void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
 	//pre-allocate all 6
 	info->m_numConstraintRows = 6;
 	info->nub = 0;
 }
 
-
-void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
 {
-
 	b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
 	b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
 	const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
@@ -318,136 +275,124 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod
 	const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
 	const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
 
-	if(m_useOffsetForConstraintFrame)
-	{ // for stability better to solve angular limits first
-		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
-
 }
 
-
-void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
 {
-	
 	//prepare constraint
-	calculateTransforms(transA,transB,bodies);
+	calculateTransforms(transA, transB, bodies);
 
 	int i;
-	for (i=0;i<3 ;i++ )
+	for (i = 0; i < 3; i++)
 	{
 		testAngularLimitMotor(i);
 	}
 
-	if(m_useOffsetForConstraintFrame)
-	{ // for stability better to solve angular limits first
-		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 }
 
-
-
-int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
+int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
 {
-//	int row = 0;
+	//	int row = 0;
 	//solve linear limits
 	b3RotationalLimitMotor limot;
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(m_linearLimits.needApplyForce(i))
-		{ // re-use rotational motor code
+		if (m_linearLimits.needApplyForce(i))
+		{  // re-use rotational motor code
 			limot.m_bounce = b3Scalar(0.f);
 			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
 			limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
-			limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
-			limot.m_damping  = m_linearLimits.m_damping;
-			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
-			limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
-			limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
-			limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
-			limot.m_maxLimitForce  = b3Scalar(0.f);
-			limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
-			limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
+			limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+			limot.m_damping = m_linearLimits.m_damping;
+			limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+			limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+			limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+			limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxLimitForce = b3Scalar(0.f);
+			limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
 			b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
 			int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
-			limot.m_normalCFM	= (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
-			limot.m_stopCFM		= (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
-			limot.m_stopERP		= (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
-			if(m_useOffsetForConstraintFrame)
+			limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+			limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+			if (m_useOffsetForConstraintFrame)
 			{
 				int indx1 = (i + 1) % 3;
 				int indx2 = (i + 2) % 3;
-				int rotAllowed = 1; // rotations around orthos to current axis
-				if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+				int rotAllowed = 1;  // rotations around orthos to current axis
+				if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
 				{
 					rotAllowed = 0;
 				}
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 			}
 			else
 			{
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
 			}
 		}
 	}
 	return row;
 }
 
-
-
-int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
+int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
 {
-	b3Generic6DofConstraint * d6constraint = this;
+	b3Generic6DofConstraint* d6constraint = this;
 	int row = row_offset;
 	//solve angular limits
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
 		{
 			b3Vector3 axis = d6constraint->getAxis(i);
 			int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
-			if(!(flags & B3_6DOF_FLAGS_CFM_NORM))
+			if (!(flags & B3_6DOF_FLAGS_CFM_NORM))
 			{
 				m_angularLimits[i].m_normalCFM = info->cfm[0];
 			}
-			if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
+			if (!(flags & B3_6DOF_FLAGS_CFM_STOP))
 			{
 				m_angularLimits[i].m_stopCFM = info->cfm[0];
 			}
-			if(!(flags & B3_6DOF_FLAGS_ERP_STOP))
+			if (!(flags & B3_6DOF_FLAGS_ERP_STOP))
 			{
 				m_angularLimits[i].m_stopERP = info->erp;
 			}
 			row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
-												transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+										 transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 		}
 	}
 
 	return row;
 }
 
-
-
-
-void	b3Generic6DofConstraint::updateRHS(b3Scalar	timeStep)
+void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
-void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
 {
 	m_frameInA = frameA;
 	m_frameInB = frameB;
@@ -455,33 +400,27 @@ void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Trans
 	calculateTransforms(bodies);
 }
 
-
-
 b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
 {
 	return m_calculatedAxis[axis_index];
 }
 
-
-b3Scalar	b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
+b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
 {
 	return m_calculatedLinearDiff[axisIndex];
 }
 
-
 b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
 {
 	return m_calculatedAxisAngleDiff[axisIndex];
 }
 
-
-
 void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
 {
 	b3Scalar imA = bodies[m_rbA].m_invMass;
 	b3Scalar imB = bodies[m_rbB].m_invMass;
 	b3Scalar weight;
-	if(imB == b3Scalar(0.0))
+	if (imB == b3Scalar(0.0))
 	{
 		weight = b3Scalar(1.0);
 	}
@@ -495,47 +434,43 @@ void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
 	return;
 }
 
-
-
 void b3Generic6DofConstraint::calculateLinearInfo()
 {
 	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
-	for(int i = 0; i < 3; i++)
+	for (int i = 0; i < 3; i++)
 	{
 		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 	}
 }
 
-
-
 int b3Generic6DofConstraint::get_limit_motor_info2(
-	b3RotationalLimitMotor * limot,
-	const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
-	b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed)
+	b3RotationalLimitMotor* limot,
+	const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
+	b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed)
 {
-    int srow = row * info->rowskip;
-    bool powered = limot->m_enableMotor;
-    int limit = limot->m_currentLimit;
-    if (powered || limit)
-    {   // if the joint is powered, or has joint limits, add in the extra row
-        b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
-        b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+	int srow = row * info->rowskip;
+	bool powered = limot->m_enableMotor;
+	int limit = limot->m_currentLimit;
+	if (powered || limit)
+	{  // if the joint is powered, or has joint limits, add in the extra row
+		b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+		b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
 		if (J1)
 		{
-			J1[srow+0] = ax1[0];
-			J1[srow+1] = ax1[1];
-			J1[srow+2] = ax1[2];
+			J1[srow + 0] = ax1[0];
+			J1[srow + 1] = ax1[1];
+			J1[srow + 2] = ax1[2];
 		}
 		if (J2)
 		{
-			J2[srow+0] = -ax1[0];
-			J2[srow+1] = -ax1[1];
-			J2[srow+2] = -ax1[2];
+			J2[srow + 0] = -ax1[0];
+			J2[srow + 1] = -ax1[1];
+			J2[srow + 2] = -ax1[2];
 		}
-		if((!rotational))
-        {
+		if ((!rotational))
+		{
 			if (m_useOffsetForConstraintFrame)
 			{
 				b3Vector3 tmpA, tmpB, relA, relB;
@@ -558,55 +493,56 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
 				relB = orthoB - totalDist * m_factB;
 				tmpA = relA.cross(ax1);
 				tmpB = relB.cross(ax1);
-				if(m_hasStaticBody && (!rotAllowed))
+				if (m_hasStaticBody && (!rotAllowed))
 				{
 					tmpA *= m_factA;
 					tmpB *= m_factB;
 				}
 				int i;
-				for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
-				for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
-			} else
+				for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
+				for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
+			}
+			else
 			{
-				b3Vector3 ltd;	// Linear Torque Decoupling vector
+				b3Vector3 ltd;  // Linear Torque Decoupling vector
 				b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
 				ltd = c.cross(ax1);
-				info->m_J1angularAxis[srow+0] = ltd[0];
-				info->m_J1angularAxis[srow+1] = ltd[1];
-				info->m_J1angularAxis[srow+2] = ltd[2];
+				info->m_J1angularAxis[srow + 0] = ltd[0];
+				info->m_J1angularAxis[srow + 1] = ltd[1];
+				info->m_J1angularAxis[srow + 2] = ltd[2];
 
 				c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 				ltd = -c.cross(ax1);
-				info->m_J2angularAxis[srow+0] = ltd[0];
-				info->m_J2angularAxis[srow+1] = ltd[1];
-				info->m_J2angularAxis[srow+2] = ltd[2];
+				info->m_J2angularAxis[srow + 0] = ltd[0];
+				info->m_J2angularAxis[srow + 1] = ltd[1];
+				info->m_J2angularAxis[srow + 2] = ltd[2];
 			}
-        }
-        // if we're limited low and high simultaneously, the joint motor is
-        // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
-        info->m_constraintError[srow] = b3Scalar(0.f);
-        if (powered)
-        {
+		}
+		// if we're limited low and high simultaneously, the joint motor is
+		// ineffective
+		if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
+		info->m_constraintError[srow] = b3Scalar(0.f);
+		if (powered)
+		{
 			info->cfm[srow] = limot->m_normalCFM;
-            if(!limit)
-            {
+			if (!limit)
+			{
 				b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
 
-				b3Scalar mot_fact = getMotorFactor(	limot->m_currentPosition, 
-													limot->m_loLimit,
-													limot->m_hiLimit, 
-													tag_vel, 
-													info->fps * limot->m_stopERP);
+				b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition,
+												   limot->m_loLimit,
+												   limot->m_hiLimit,
+												   tag_vel,
+												   info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
-            }
-        }
-        if(limit)
-        {
-            b3Scalar k = info->fps * limot->m_stopERP;
-			if(!rotational)
+				info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+				info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+			}
+		}
+		if (limit)
+		{
+			b3Scalar k = info->fps * limot->m_stopERP;
+			if (!rotational)
 			{
 				info->m_constraintError[srow] += k * limot->m_currentLimitError;
 			}
@@ -615,116 +551,112 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
 				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
 			}
 			info->cfm[srow] = limot->m_stopCFM;
-            if (limot->m_loLimit == limot->m_hiLimit)
-            {   // limited low and high simultaneously
-                info->m_lowerLimit[srow] = -B3_INFINITY;
-                info->m_upperLimit[srow] = B3_INFINITY;
-            }
-            else
-            {
-                if (limit == 1)
-                {
-                    info->m_lowerLimit[srow] = 0;
-                    info->m_upperLimit[srow] = B3_INFINITY;
-                }
-                else
-                {
-                    info->m_lowerLimit[srow] = -B3_INFINITY;
-                    info->m_upperLimit[srow] = 0;
-                }
-                // deal with bounce
-                if (limot->m_bounce > 0)
-                {
-                    // calculate joint velocity
-                    b3Scalar vel;
-                    if (rotational)
-                    {
-                        vel = angVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= angVelB.dot(ax1);
-                    }
-                    else
-                    {
-                        vel = linVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= linVelB.dot(ax1);
-                    }
-                    // only apply bounce if the velocity is incoming, and if the
-                    // resulting c[] exceeds what we already have.
-                    if (limit == 1)
-                    {
-                        if (vel < 0)
-                        {
-                            b3Scalar newc = -limot->m_bounce* vel;
-                            if (newc > info->m_constraintError[srow]) 
+			if (limot->m_loLimit == limot->m_hiLimit)
+			{  // limited low and high simultaneously
+				info->m_lowerLimit[srow] = -B3_INFINITY;
+				info->m_upperLimit[srow] = B3_INFINITY;
+			}
+			else
+			{
+				if (limit == 1)
+				{
+					info->m_lowerLimit[srow] = 0;
+					info->m_upperLimit[srow] = B3_INFINITY;
+				}
+				else
+				{
+					info->m_lowerLimit[srow] = -B3_INFINITY;
+					info->m_upperLimit[srow] = 0;
+				}
+				// deal with bounce
+				if (limot->m_bounce > 0)
+				{
+					// calculate joint velocity
+					b3Scalar vel;
+					if (rotational)
+					{
+						vel = angVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= angVelB.dot(ax1);
+					}
+					else
+					{
+						vel = linVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= linVelB.dot(ax1);
+					}
+					// only apply bounce if the velocity is incoming, and if the
+					// resulting c[] exceeds what we already have.
+					if (limit == 1)
+					{
+						if (vel < 0)
+						{
+							b3Scalar newc = -limot->m_bounce * vel;
+							if (newc > info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                    else
-                    {
-                        if (vel > 0)
-                        {
-                            b3Scalar newc = -limot->m_bounce * vel;
-                            if (newc < info->m_constraintError[srow]) 
+						}
+					}
+					else
+					{
+						if (vel > 0)
+						{
+							b3Scalar newc = -limot->m_bounce * vel;
+							if (newc < info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                }
-            }
-        }
-        return 1;
-    }
-    else return 0;
+						}
+					}
+				}
+			}
+		}
+		return 1;
+	}
+	else
+		return 0;
 }
 
-
-
-
-
-
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
-	///If no axis is provided, it uses the default axis for this constraint.
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
 void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
 {
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				m_linearLimits.m_stopERP[axis] = value;
 				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				m_linearLimits.m_stopCFM[axis] = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				m_linearLimits.m_normalCFM[axis] = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				m_angularLimits[axis - 3].m_stopERP = value;
 				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				m_angularLimits[axis - 3].m_stopCFM = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				m_angularLimits[axis - 3].m_normalCFM = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
@@ -734,47 +666,47 @@ void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
 	}
 }
 
-	///return the local value of parameter
-b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const 
+///return the local value of parameter
+b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
 {
 	b3Scalar retVal = 0;
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopERP[axis];
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopCFM[axis];
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_normalCFM[axis];
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopERP;
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopCFM;
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_normalCFM;
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
@@ -785,23 +717,21 @@ b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
 	return retVal;
 }
 
- 
-
-void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
 {
 	b3Vector3 zAxis = axis1.normalized();
 	b3Vector3 yAxis = axis2.normalized();
-	b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
-	
+	b3Vector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
+
 	b3Transform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
-	                                xAxis[1], yAxis[1], zAxis[1],
-	                               xAxis[2], yAxis[2], zAxis[2]);
-	
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
+
 	// now get constraint frame in local coordinate systems
 	m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
 	m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
-	
+
 	calculateTransforms(bodies);
 }

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h

@@ -23,7 +23,6 @@ email: [email protected]
 http://gimpact.sf.net
 */
 
-
 #ifndef B3_GENERIC_6DOF_CONSTRAINT_H
 #define B3_GENERIC_6DOF_CONSTRAINT_H
 
@@ -33,88 +32,83 @@ http://gimpact.sf.net
 
 struct b3RigidBodyData;
 
-
-
-
 //! Rotation Limit structure for generic joints
 class b3RotationalLimitMotor
 {
 public:
-    //! limit_parameters
-    //!@{
-    b3Scalar m_loLimit;//!< joint limit
-    b3Scalar m_hiLimit;//!< joint limit
-    b3Scalar m_targetVelocity;//!< target motor velocity
-    b3Scalar m_maxMotorForce;//!< max force on motor
-    b3Scalar m_maxLimitForce;//!< max force on limit
-    b3Scalar m_damping;//!< Damping.
-    b3Scalar m_limitSoftness;//! Relaxation factor
-    b3Scalar m_normalCFM;//!< Constraint force mixing factor
-    b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit
-    b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    b3Scalar m_bounce;//!< restitution factor
-    bool m_enableMotor;
-
-    //!@}
-
-    //! temp_variables
-    //!@{
-    b3Scalar m_currentLimitError;//!  How much is violated this limit
-    b3Scalar m_currentPosition;     //!  current value of angle 
-    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
-    b3Scalar m_accumulatedImpulse;
-    //!@}
-
-    b3RotationalLimitMotor()
-    {
-    	m_accumulatedImpulse = 0.f;
-        m_targetVelocity = 0;
-        m_maxMotorForce = 6.0f;
-        m_maxLimitForce = 300.0f;
-        m_loLimit = 1.0f;
-        m_hiLimit = -1.0f;
+	//! limit_parameters
+	//!@{
+	b3Scalar m_loLimit;         //!< joint limit
+	b3Scalar m_hiLimit;         //!< joint limit
+	b3Scalar m_targetVelocity;  //!< target motor velocity
+	b3Scalar m_maxMotorForce;   //!< max force on motor
+	b3Scalar m_maxLimitForce;   //!< max force on limit
+	b3Scalar m_damping;         //!< Damping.
+	b3Scalar m_limitSoftness;   //! Relaxation factor
+	b3Scalar m_normalCFM;       //!< Constraint force mixing factor
+	b3Scalar m_stopERP;         //!< Error tolerance factor when joint is at limit
+	b3Scalar m_stopCFM;         //!< Constraint force mixing factor when joint is at limit
+	b3Scalar m_bounce;          //!< restitution factor
+	bool m_enableMotor;
+
+	//!@}
+
+	//! temp_variables
+	//!@{
+	b3Scalar m_currentLimitError;  //!  How much is violated this limit
+	b3Scalar m_currentPosition;    //!  current value of angle
+	int m_currentLimit;            //!< 0=free, 1=at lo limit, 2=at hi limit
+	b3Scalar m_accumulatedImpulse;
+	//!@}
+
+	b3RotationalLimitMotor()
+	{
+		m_accumulatedImpulse = 0.f;
+		m_targetVelocity = 0;
+		m_maxMotorForce = 6.0f;
+		m_maxLimitForce = 300.0f;
+		m_loLimit = 1.0f;
+		m_hiLimit = -1.0f;
 		m_normalCFM = 0.f;
 		m_stopERP = 0.2f;
 		m_stopCFM = 0.f;
-        m_bounce = 0.0f;
-        m_damping = 1.0f;
-        m_limitSoftness = 0.5f;
-        m_currentLimit = 0;
-        m_currentLimitError = 0;
-        m_enableMotor = false;
-    }
-
-    b3RotationalLimitMotor(const b3RotationalLimitMotor & limot)
-    {
-        m_targetVelocity = limot.m_targetVelocity;
-        m_maxMotorForce = limot.m_maxMotorForce;
-        m_limitSoftness = limot.m_limitSoftness;
-        m_loLimit = limot.m_loLimit;
-        m_hiLimit = limot.m_hiLimit;
+		m_bounce = 0.0f;
+		m_damping = 1.0f;
+		m_limitSoftness = 0.5f;
+		m_currentLimit = 0;
+		m_currentLimitError = 0;
+		m_enableMotor = false;
+	}
+
+	b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
+	{
+		m_targetVelocity = limot.m_targetVelocity;
+		m_maxMotorForce = limot.m_maxMotorForce;
+		m_limitSoftness = limot.m_limitSoftness;
+		m_loLimit = limot.m_loLimit;
+		m_hiLimit = limot.m_hiLimit;
 		m_normalCFM = limot.m_normalCFM;
 		m_stopERP = limot.m_stopERP;
-		m_stopCFM =	limot.m_stopCFM;
-        m_bounce = limot.m_bounce;
-        m_currentLimit = limot.m_currentLimit;
-        m_currentLimitError = limot.m_currentLimitError;
-        m_enableMotor = limot.m_enableMotor;
-    }
-
-
+		m_stopCFM = limot.m_stopCFM;
+		m_bounce = limot.m_bounce;
+		m_currentLimit = limot.m_currentLimit;
+		m_currentLimitError = limot.m_currentLimitError;
+		m_enableMotor = limot.m_enableMotor;
+	}
 
 	//! Is limited
-    bool isLimited()
-    {
-    	if(m_loLimit > m_hiLimit) return false;
-    	return true;
-    }
+	bool isLimited()
+	{
+		if (m_loLimit > m_hiLimit) return false;
+		return true;
+	}
 
 	//! Need apply correction
-    bool needApplyTorques()
-    {
-    	if(m_currentLimit == 0 && m_enableMotor == false) return false;
-    	return true;
-    }
+	bool needApplyTorques()
+	{
+		if (m_currentLimit == 0 && m_enableMotor == false) return false;
+		return true;
+	}
 
 	//! calculates  error
 	/*!
@@ -123,104 +117,98 @@ public:
 	int testLimitValue(b3Scalar test_value);
 
 	//! apply the correction impulses for two bodies
-    b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1);
-
+	b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
 };
 
-
-
 class b3TranslationalLimitMotor
 {
 public:
-	b3Vector3 m_lowerLimit;//!< the constraint lower limits
-    b3Vector3 m_upperLimit;//!< the constraint upper limits
-    b3Vector3 m_accumulatedImpulse;
-    //! Linear_Limit_parameters
-    //!@{
-	b3Vector3	m_normalCFM;//!< Constraint force mixing factor
-    b3Vector3	m_stopERP;//!< Error tolerance factor when joint is at limit
-	b3Vector3	m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    b3Vector3	m_targetVelocity;//!< target motor velocity
-    b3Vector3	m_maxMotorForce;//!< max force on motor
-    b3Vector3	m_currentLimitError;//!  How much is violated this limit
-    b3Vector3	m_currentLinearDiff;//!  Current relative offset of constraint frames
-	b3Scalar	m_limitSoftness;//!< Softness for linear limit
-    b3Scalar	m_damping;//!< Damping for linear limit
-    b3Scalar	m_restitution;//! Bounce parameter for linear limit
+	b3Vector3 m_lowerLimit;  //!< the constraint lower limits
+	b3Vector3 m_upperLimit;  //!< the constraint upper limits
+	b3Vector3 m_accumulatedImpulse;
+	//! Linear_Limit_parameters
+	//!@{
+	b3Vector3 m_normalCFM;          //!< Constraint force mixing factor
+	b3Vector3 m_stopERP;            //!< Error tolerance factor when joint is at limit
+	b3Vector3 m_stopCFM;            //!< Constraint force mixing factor when joint is at limit
+	b3Vector3 m_targetVelocity;     //!< target motor velocity
+	b3Vector3 m_maxMotorForce;      //!< max force on motor
+	b3Vector3 m_currentLimitError;  //!  How much is violated this limit
+	b3Vector3 m_currentLinearDiff;  //!  Current relative offset of constraint frames
+	b3Scalar m_limitSoftness;       //!< Softness for linear limit
+	b3Scalar m_damping;             //!< Damping for linear limit
+	b3Scalar m_restitution;         //! Bounce parameter for linear limit
 	//!@}
-	bool		m_enableMotor[3];
-	int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
-
-    b3TranslationalLimitMotor()
-    {
-    	m_lowerLimit.setValue(0.f,0.f,0.f);
-    	m_upperLimit.setValue(0.f,0.f,0.f);
-    	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+	bool m_enableMotor[3];
+	int m_currentLimit[3];  //!< 0=free, 1=at lower limit, 2=at upper limit
+
+	b3TranslationalLimitMotor()
+	{
+		m_lowerLimit.setValue(0.f, 0.f, 0.f);
+		m_upperLimit.setValue(0.f, 0.f, 0.f);
+		m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
 		m_normalCFM.setValue(0.f, 0.f, 0.f);
 		m_stopERP.setValue(0.2f, 0.2f, 0.2f);
 		m_stopCFM.setValue(0.f, 0.f, 0.f);
 
-    	m_limitSoftness = 0.7f;
-    	m_damping = b3Scalar(1.0f);
-    	m_restitution = b3Scalar(0.5f);
-		for(int i=0; i < 3; i++) 
+		m_limitSoftness = 0.7f;
+		m_damping = b3Scalar(1.0f);
+		m_restitution = b3Scalar(0.5f);
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = false;
 			m_targetVelocity[i] = b3Scalar(0.f);
 			m_maxMotorForce[i] = b3Scalar(0.f);
 		}
-    }
+	}
 
-    b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other )
-    {
-    	m_lowerLimit = other.m_lowerLimit;
-    	m_upperLimit = other.m_upperLimit;
-    	m_accumulatedImpulse = other.m_accumulatedImpulse;
+	b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
+	{
+		m_lowerLimit = other.m_lowerLimit;
+		m_upperLimit = other.m_upperLimit;
+		m_accumulatedImpulse = other.m_accumulatedImpulse;
 
-    	m_limitSoftness = other.m_limitSoftness ;
-    	m_damping = other.m_damping;
-    	m_restitution = other.m_restitution;
+		m_limitSoftness = other.m_limitSoftness;
+		m_damping = other.m_damping;
+		m_restitution = other.m_restitution;
 		m_normalCFM = other.m_normalCFM;
 		m_stopERP = other.m_stopERP;
 		m_stopCFM = other.m_stopCFM;
 
-		for(int i=0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = other.m_enableMotor[i];
 			m_targetVelocity[i] = other.m_targetVelocity[i];
 			m_maxMotorForce[i] = other.m_maxMotorForce[i];
 		}
-    }
+	}
 
-    //! Test limit
+	//! Test limit
 	/*!
     - free means upper < lower,
     - locked means upper == lower
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    inline bool	isLimited(int limitIndex)
-    {
-       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
-    }
-    inline bool needApplyForce(int limitIndex)
-    {
-    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
-    	return true;
-    }
+	inline bool isLimited(int limitIndex)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+	inline bool needApplyForce(int limitIndex)
+	{
+		if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+		return true;
+	}
 	int testLimitValue(int limitIndex, b3Scalar test_value);
 
-
-    b3Scalar solveLinearAxis(
-    	b3Scalar timeStep,
-        b3Scalar jacDiagABInv,
-        b3RigidBodyData& body1,const b3Vector3 &pointInA,
-        b3RigidBodyData& body2,const b3Vector3 &pointInB,
-        int limit_index,
-        const b3Vector3 & axis_normal_on_a,
-		const b3Vector3 & anchorPos);
-
-
+	b3Scalar solveLinearAxis(
+		b3Scalar timeStep,
+		b3Scalar jacDiagABInv,
+		b3RigidBodyData& body1, const b3Vector3& pointInA,
+		b3RigidBodyData& body2, const b3Vector3& pointInB,
+		int limit_index,
+		const b3Vector3& axis_normal_on_a,
+		const b3Vector3& anchorPos);
 };
 
 enum b36DofFlags
@@ -229,8 +217,7 @@ enum b36DofFlags
 	B3_6DOF_FLAGS_CFM_STOP = 2,
 	B3_6DOF_FLAGS_ERP_STOP = 4
 };
-#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
-
+#define B3_6DOF_FLAGS_AXIS_SHIFT 3  // bits per axis
 
 /// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /*!
@@ -268,240 +255,229 @@ This brings support for limit parameters and motors. </li>
 </ul>
 
 */
-B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Generic6DofConstraint : public b3TypedConstraint
 {
 protected:
-
 	//! relative_frames
-    //!@{
-	b3Transform	m_frameInA;//!< the constraint space w.r.t body A
-    b3Transform	m_frameInB;//!< the constraint space w.r.t body B
-    //!@}
+	//!@{
+	b3Transform m_frameInA;  //!< the constraint space w.r.t body A
+	b3Transform m_frameInB;  //!< the constraint space w.r.t body B
+	//!@}
 
-    //! Jacobians
-    //!@{
-//    b3JacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
-//    b3JacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
-    //!@}
+	//! Jacobians
+	//!@{
+	//    b3JacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
+	//    b3JacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
+	//!@}
 
 	//! Linear_Limit_parameters
-    //!@{
-    b3TranslationalLimitMotor m_linearLimits;
-    //!@}
-
-
-    //! hinge_parameters
-    //!@{
-    b3RotationalLimitMotor m_angularLimits[3];
+	//!@{
+	b3TranslationalLimitMotor m_linearLimits;
 	//!@}
 
+	//! hinge_parameters
+	//!@{
+	b3RotationalLimitMotor m_angularLimits[3];
+	//!@}
 
 protected:
-    //! temporal variables
-    //!@{
-    b3Transform m_calculatedTransformA;
-    b3Transform m_calculatedTransformB;
-    b3Vector3 m_calculatedAxisAngleDiff;
-    b3Vector3 m_calculatedAxis[3];
-    b3Vector3 m_calculatedLinearDiff;
-    b3Scalar m_timeStep;
-	b3Scalar	m_factA;
-	b3Scalar	m_factB;
-	bool		m_hasStaticBody;
-    
-	b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
+	//! temporal variables
+	//!@{
+	b3Transform m_calculatedTransformA;
+	b3Transform m_calculatedTransformB;
+	b3Vector3 m_calculatedAxisAngleDiff;
+	b3Vector3 m_calculatedAxis[3];
+	b3Vector3 m_calculatedLinearDiff;
+	b3Scalar m_timeStep;
+	b3Scalar m_factA;
+	b3Scalar m_factB;
+	bool m_hasStaticBody;
 
-    bool	m_useLinearReferenceFrameA;
-	bool	m_useOffsetForConstraintFrame;
-    
-	int		m_flags;
+	b3Vector3 m_AnchorPos;  // point betwen pivots of bodies A and B to solve linear axes
 
-    //!@}
+	bool m_useLinearReferenceFrameA;
+	bool m_useOffsetForConstraintFrame;
 
-    b3Generic6DofConstraint&	operator=(b3Generic6DofConstraint&	other)
-    {
-        b3Assert(0);
-        (void) other;
-        return *this;
-    }
+	int m_flags;
 
+	//!@}
 
-	int setAngularLimits(b3ConstraintInfo2 *info, int row_offset,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
+	b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
+	{
+		b3Assert(0);
+		(void)other;
+		return *this;
+	}
 
-	int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
+	int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
 
+	int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
 
 	// tests linear limits
 	void calculateLinearInfo();
 
 	//! calcs the euler angles between the two bodies.
-    void calculateAngleInfo();
-
-
+	void calculateAngleInfo();
 
 public:
-
 	B3_DECLARE_ALIGNED_ALLOCATOR();
-	
-    b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies);
-    
+
+	b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
+
 	//! Calcs global transform of the offsets
 	/*!
 	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
 	\sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
 	*/
-    void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies);
+	void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
 
 	void calculateTransforms(const b3RigidBodyData* bodies);
 
 	//! Gets the global transform of the offset for body A
-    /*!
+	/*!
     \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
     */
-    const b3Transform & getCalculatedTransformA() const
-    {
-    	return m_calculatedTransformA;
-    }
+	const b3Transform& getCalculatedTransformA() const
+	{
+		return m_calculatedTransformA;
+	}
 
-    //! Gets the global transform of the offset for body B
-    /*!
+	//! Gets the global transform of the offset for body B
+	/*!
     \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
     */
-    const b3Transform & getCalculatedTransformB() const
-    {
-    	return m_calculatedTransformB;
-    }
-
-    const b3Transform & getFrameOffsetA() const
-    {
-    	return m_frameInA;
-    }
-
-    const b3Transform & getFrameOffsetB() const
-    {
-    	return m_frameInB;
-    }
-
-
-    b3Transform & getFrameOffsetA()
-    {
-    	return m_frameInA;
-    }
+	const b3Transform& getCalculatedTransformB() const
+	{
+		return m_calculatedTransformB;
+	}
 
-    b3Transform & getFrameOffsetB()
-    {
-    	return m_frameInB;
-    }
+	const b3Transform& getFrameOffsetA() const
+	{
+		return m_frameInA;
+	}
 
+	const b3Transform& getFrameOffsetB() const
+	{
+		return m_frameInB;
+	}
 
+	b3Transform& getFrameOffsetA()
+	{
+		return m_frameInA;
+	}
 
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	b3Transform& getFrameOffsetB()
+	{
+		return m_frameInB;
+	}
 
-	void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies);
+	void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies);
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
 
+	void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies);
 
-    void	updateRHS(b3Scalar	timeStep);
+	void updateRHS(b3Scalar timeStep);
 
 	//! Get the rotation axis in global coordinates
-    b3Vector3 getAxis(int axis_index) const;
+	b3Vector3 getAxis(int axis_index) const;
 
-    //! Get the relative Euler angle
-    /*!
+	//! Get the relative Euler angle
+	/*!
 	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    b3Scalar getAngle(int axis_index) const;
+	b3Scalar getAngle(int axis_index) const;
 
 	//! Get the relative position of the constraint pivot
-    /*!
+	/*!
 	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 	*/
 	b3Scalar getRelativePivotPosition(int axis_index) const;
 
-	void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyData* bodies);
+	void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
 
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
 	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    bool testAngularLimitMotor(int axis_index);
+	bool testAngularLimitMotor(int axis_index);
 
-    void	setLinearLowerLimit(const b3Vector3& linearLower)
-    {
-    	m_linearLimits.m_lowerLimit = linearLower;
-    }
+	void setLinearLowerLimit(const b3Vector3& linearLower)
+	{
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
 
-	void	getLinearLowerLimit(b3Vector3& linearLower)
+	void getLinearLowerLimit(b3Vector3 & linearLower)
 	{
 		linearLower = m_linearLimits.m_lowerLimit;
 	}
 
-	void	setLinearUpperLimit(const b3Vector3& linearUpper)
+	void setLinearUpperLimit(const b3Vector3& linearUpper)
 	{
 		m_linearLimits.m_upperLimit = linearUpper;
 	}
 
-	void	getLinearUpperLimit(b3Vector3& linearUpper)
+	void getLinearUpperLimit(b3Vector3 & linearUpper)
 	{
 		linearUpper = m_linearLimits.m_upperLimit;
 	}
 
-    void	setAngularLowerLimit(const b3Vector3& angularLower)
-    {
-		for(int i = 0; i < 3; i++) 
+	void setAngularLowerLimit(const b3Vector3& angularLower)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
-    }
+	}
 
-	void	getAngularLowerLimit(b3Vector3& angularLower)
+	void getAngularLowerLimit(b3Vector3 & angularLower)
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			angularLower[i] = m_angularLimits[i].m_loLimit;
 	}
 
-    void	setAngularUpperLimit(const b3Vector3& angularUpper)
-    {
-		for(int i = 0; i < 3; i++)
+	void setAngularUpperLimit(const b3Vector3& angularUpper)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
-    }
+	}
 
-	void	getAngularUpperLimit(b3Vector3& angularUpper)
+	void getAngularUpperLimit(b3Vector3 & angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = m_angularLimits[i].m_hiLimit;
 	}
 
 	//! Retrieves the angular limit informacion
-    b3RotationalLimitMotor * getRotationalLimitMotor(int index)
-    {
-    	return &m_angularLimits[index];
-    }
-
-    //! Retrieves the  limit informacion
-    b3TranslationalLimitMotor * getTranslationalLimitMotor()
-    {
-    	return &m_linearLimits;
-    }
-
-    //first 3 are linear, next 3 are angular
-    void setLimit(int axis, b3Scalar lo, b3Scalar hi)
-    {
-    	if(axis<3)
-    	{
-    		m_linearLimits.m_lowerLimit[axis] = lo;
-    		m_linearLimits.m_upperLimit[axis] = hi;
-    	}
-    	else
-    	{
+	b3RotationalLimitMotor* getRotationalLimitMotor(int index)
+	{
+		return &m_angularLimits[index];
+	}
+
+	//! Retrieves the  limit informacion
+	b3TranslationalLimitMotor* getTranslationalLimitMotor()
+	{
+		return &m_linearLimits;
+	}
+
+	//first 3 are linear, next 3 are angular
+	void setLimit(int axis, b3Scalar lo, b3Scalar hi)
+	{
+		if (axis < 3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
 			lo = b3NormalizeAngle(lo);
 			hi = b3NormalizeAngle(hi);
-    		m_angularLimits[axis-3].m_loLimit = lo;
-    		m_angularLimits[axis-3].m_hiLimit = hi;
-    	}
-    }
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
+		}
+	}
 
 	//! Test limit
 	/*!
@@ -510,41 +486,32 @@ public:
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    bool	isLimited(int limitIndex)
-    {
-    	if(limitIndex<3)
-    	{
+	bool isLimited(int limitIndex)
+	{
+		if (limitIndex < 3)
+		{
 			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex - 3].isLimited();
+	}
 
-    	}
-        return m_angularLimits[limitIndex-3].isLimited();
-    }
-
-	virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
+	virtual void calcAnchorPos(const b3RigidBodyData* bodies);  // overridable
 
-	int get_limit_motor_info2(	b3RotationalLimitMotor * limot,
-								const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
-								b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
+	int get_limit_motor_info2(b3RotationalLimitMotor * limot,
+							  const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
+							  b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
 
 	// access for UseFrameOffset
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void setParam(int num, b3Scalar value, int axis = -1);
+	virtual void setParam(int num, b3Scalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	b3Scalar getParam(int num, int axis = -1) const;
-
-	void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyData* bodies);
+	virtual b3Scalar getParam(int num, int axis = -1) const;
 
-
-
-	
+	void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
 };
 
-
-
-
-
-#endif //B3_GENERIC_6DOF_CONSTRAINT_H
+#endif  //B3_GENERIC_6DOF_CONSTRAINT_H

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h

@@ -18,7 +18,6 @@ subject to the following restrictions:
 
 #include "Bullet3Common/b3Matrix3x3.h"
 
-
 //notes:
 // Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
 // which makes the b3JacobianEntry memory layout 16 bytes
@@ -27,25 +26,26 @@ subject to the following restrictions:
 /// Jacobian entry is an abstraction that allows to describe constraints
 /// it can be used in combination with a constraint solver
 /// Can be used to relate the effect of an impulse to the constraint error
-B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry
+B3_ATTRIBUTE_ALIGNED16(class)
+b3JacobianEntry
 {
 public:
-	b3JacobianEntry() {};
+	b3JacobianEntry(){};
 	//constraint between two different rigidbodies
 	b3JacobianEntry(
 		const b3Matrix3x3& world2A,
 		const b3Matrix3x3& world2B,
-		const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+		const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
 		const b3Vector3& jointAxis,
-		const b3Vector3& inertiaInvA, 
+		const b3Vector3& inertiaInvA,
 		const b3Scalar massInvA,
 		const b3Vector3& inertiaInvB,
 		const b3Scalar massInvB)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
-		m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
+		m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
 
@@ -54,33 +54,31 @@ public:
 
 	//angular constraint between two different rigidbodies
 	b3JacobianEntry(const b3Vector3& jointAxis,
-		const b3Matrix3x3& world2A,
-		const b3Matrix3x3& world2B,
-		const b3Vector3& inertiaInvA,
-		const b3Vector3& inertiaInvB)
-		:m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
+					const b3Matrix3x3& world2A,
+					const b3Matrix3x3& world2B,
+					const b3Vector3& inertiaInvA,
+					const b3Vector3& inertiaInvB)
+		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.)))
 	{
-		m_aJ= world2A*jointAxis;
-		m_bJ = world2B*-jointAxis;
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * jointAxis;
+		m_bJ = world2B * -jointAxis;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		b3Assert(m_Adiag > b3Scalar(0.0));
 	}
 
 	//angular constraint between two different rigidbodies
 	b3JacobianEntry(const b3Vector3& axisInA,
-		const b3Vector3& axisInB,
-		const b3Vector3& inertiaInvA,
-		const b3Vector3& inertiaInvB)
-		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
-		, m_aJ(axisInA)
-		, m_bJ(-axisInB)
+					const b3Vector3& axisInB,
+					const b3Vector3& inertiaInvA,
+					const b3Vector3& inertiaInvB)
+		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
 	{
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		b3Assert(m_Adiag > b3Scalar(0.0));
 	}
@@ -88,25 +86,25 @@ public:
 	//constraint on one rigidbody
 	b3JacobianEntry(
 		const b3Matrix3x3& world2A,
-		const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+		const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
 		const b3Vector3& jointAxis,
-		const b3Vector3& inertiaInvA, 
+		const b3Vector3& inertiaInvA,
 		const b3Scalar massInvA)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ= world2A*(rel_pos1.cross(jointAxis));
-		m_bJ = world2A*(rel_pos2.cross(-jointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
-		m_1MinvJt = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+		m_aJ = world2A * (rel_pos1.cross(jointAxis));
+		m_bJ = world2A * (rel_pos2.cross(-jointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
 
 		b3Assert(m_Adiag > b3Scalar(0.0));
 	}
 
-	b3Scalar	getDiagonal() const { return m_Adiag; }
+	b3Scalar getDiagonal() const { return m_Adiag; }
 
 	// for two constraints on the same rigidbody (for example vehicle friction)
-	b3Scalar	getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
+	b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
 	{
 		const b3JacobianEntry& jacA = *this;
 		b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
@@ -114,42 +112,39 @@ public:
 		return lin + ang;
 	}
 
-	
-
 	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
-	b3Scalar	getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const
+	b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const
 	{
 		const b3JacobianEntry& jacA = *this;
 		b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
 		b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
 		b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
-		b3Vector3 lin0 = massInvA * lin ;
+		b3Vector3 lin0 = massInvA * lin;
 		b3Vector3 lin1 = massInvB * lin;
-		b3Vector3 sum = ang0+ang1+lin0+lin1;
-		return sum[0]+sum[1]+sum[2];
+		b3Vector3 sum = ang0 + ang1 + lin0 + lin1;
+		return sum[0] + sum[1] + sum[2];
 	}
 
-	b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB)
+	b3Scalar getRelativeVelocity(const b3Vector3& linvelA, const b3Vector3& angvelA, const b3Vector3& linvelB, const b3Vector3& angvelB)
 	{
 		b3Vector3 linrel = linvelA - linvelB;
-		b3Vector3 angvela  = angvelA * m_aJ;
-		b3Vector3 angvelb  = angvelB * m_bJ;
+		b3Vector3 angvela = angvelA * m_aJ;
+		b3Vector3 angvelb = angvelB * m_bJ;
 		linrel *= m_linearJointAxis;
 		angvela += angvelb;
 		angvela += linrel;
-		b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		b3Scalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
 		return rel_vel2 + B3_EPSILON;
 	}
-//private:
+	//private:
 
-	b3Vector3	m_linearJointAxis;
-	b3Vector3	m_aJ;
-	b3Vector3	m_bJ;
-	b3Vector3	m_0MinvJt;
-	b3Vector3	m_1MinvJt;
+	b3Vector3 m_linearJointAxis;
+	b3Vector3 m_aJ;
+	b3Vector3 m_bJ;
+	b3Vector3 m_0MinvJt;
+	b3Vector3 m_1MinvJt;
 	//Optimization: can be stored in the w/last component of one of the vectors
-	b3Scalar	m_Adiag;
-
+	b3Scalar m_Adiag;
 };
 
-#endif //B3_JACOBIAN_ENTRY_H
+#endif  //B3_JACOBIAN_ENTRY_H

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h

@@ -1,11 +1,9 @@
 #ifndef B3_PGS_JACOBI_SOLVER
 #define B3_PGS_JACOBI_SOLVER
 
-
 struct b3Contact4;
 struct b3ContactPoint;
 
-
 class b3Dispatcher;
 
 #include "b3TypedConstraint.h"
@@ -18,132 +16,118 @@ struct b3InertiaData;
 
 class b3PgsJacobiSolver
 {
-
 protected:
-	b3AlignedObjectArray<b3SolverBody>      m_tmpSolverBodyPool;
-	b3ConstraintArray			m_tmpSolverContactConstraintPool;
-	b3ConstraintArray			m_tmpSolverNonContactConstraintPool;
-	b3ConstraintArray			m_tmpSolverContactFrictionConstraintPool;
-	b3ConstraintArray			m_tmpSolverContactRollingFrictionConstraintPool;
-
-	b3AlignedObjectArray<int>	m_orderTmpConstraintPool;
-	b3AlignedObjectArray<int>	m_orderNonContactConstraintPool;
-	b3AlignedObjectArray<int>	m_orderFrictionConstraintPool;
+	b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
+	b3ConstraintArray m_tmpSolverContactConstraintPool;
+	b3ConstraintArray m_tmpSolverNonContactConstraintPool;
+	b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
+	b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
+
+	b3AlignedObjectArray<int> m_orderTmpConstraintPool;
+	b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
+	b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
 	b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
-	
-	b3AlignedObjectArray<int>		m_bodyCount;
-	b3AlignedObjectArray<int>		m_bodyCountCheck;
-	
-	b3AlignedObjectArray<b3Vector3>	m_deltaLinearVelocities;
-	b3AlignedObjectArray<b3Vector3>	m_deltaAngularVelocities;
 
-	bool						m_usePgs;
-	void						averageVelocities();
+	b3AlignedObjectArray<int> m_bodyCount;
+	b3AlignedObjectArray<int> m_bodyCountCheck;
+
+	b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
+	b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
+
+	bool m_usePgs;
+	void averageVelocities();
 
-	int							m_maxOverrideNumSolverIterations;
+	int m_maxOverrideNumSolverIterations;
 
-	int							m_numSplitImpulseRecoveries;
+	int m_numSplitImpulseRecoveries;
 
-	b3Scalar	getContactProcessingThreshold(b3Contact4* contact)
+	b3Scalar getContactProcessingThreshold(b3Contact4* contact)
 	{
 		return 0.02f;
 	}
-	void setupFrictionConstraint(	b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
-									b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, 
-									b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
+	void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+								 b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+								 b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+								 b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
 
-	void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,	b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
-									b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, 
-									b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
-
-	b3SolverConstraint&	addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
-	b3SolverConstraint&	addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
+	void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+										b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+										b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+										b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
 
+	b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
+	b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);
 
 	void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
-								b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, 
-								const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, 
+								b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
+								const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
 								b3Vector3& rel_pos1, b3Vector3& rel_pos2);
 
-	void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
-										 b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
+	void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
+									  b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
-	unsigned long	m_btSeed2;
+	unsigned long m_btSeed2;
 
-	
 	b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
 
-	void	convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
-
+	void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
 
-	void	resolveSplitPenetrationSIMD(
-     b3SolverBody& bodyA,b3SolverBody& bodyB,
-        const b3SolverConstraint& contactConstraint);
+	void resolveSplitPenetrationSIMD(
+		b3SolverBody& bodyA, b3SolverBody& bodyB,
+		const b3SolverConstraint& contactConstraint);
 
-	void	resolveSplitPenetrationImpulseCacheFriendly(
-       b3SolverBody& bodyA,b3SolverBody& bodyB,
-        const b3SolverConstraint& contactConstraint);
+	void resolveSplitPenetrationImpulseCacheFriendly(
+		b3SolverBody& bodyA, b3SolverBody& bodyB,
+		const b3SolverConstraint& contactConstraint);
 
 	//internal method
-	int		getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias);
-	void	initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
+	int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
+	void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
 
-	void	resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-
-	void	resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-		
-protected:
+	void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
-	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
+	void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
-	virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
-	virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
-	b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
+protected:
+	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 
-	virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
+	virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+	b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 
+	virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
 
 public:
-
 	B3_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	b3PgsJacobiSolver(bool usePgs);
 	virtual ~b3PgsJacobiSolver();
 
-//	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
-	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
+	//	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
+	void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
 
-	b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 
 	///clear internal cached data and reset random seed
-	virtual	void	reset();
-	
+	virtual void reset();
+
 	unsigned long b3Rand2();
 
-	int b3RandInt2 (int n);
+	int b3RandInt2(int n);
 
-	void	setRandSeed(unsigned long seed)
+	void setRandSeed(unsigned long seed)
 	{
 		m_btSeed2 = seed;
 	}
-	unsigned long	getRandSeed() const
+	unsigned long getRandSeed() const
 	{
 		return m_btSeed2;
 	}
-
-
-
-
 };
 
-#endif //B3_PGS_JACOBI_SOLVER
-
+#endif  //B3_PGS_JACOBI_SOLVER

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp

@@ -13,21 +13,14 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "b3Point2PointConstraint.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
 #include <new>
 
-
-
-
-
-b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB)
-:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
-m_flags(0)
+b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
+	: b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
 {
-
 }
 
 /*
@@ -40,22 +33,18 @@ m_useSolveConstraintObsolete(false)
 }
 */
 
-
-void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
-	getInfo1NonVirtual(info,bodies);
+	getInfo1NonVirtual(info, bodies);
 }
 
-void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
-		info->m_numConstraintRows = 3;
-		info->nub = 3;
+	info->m_numConstraintRows = 3;
+	info->nub = 3;
 }
 
-
-
-
-void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
 {
 	b3Transform trA;
 	trA.setIdentity();
@@ -67,143 +56,135 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
 	trB.setOrigin(bodies[m_rbB].m_pos);
 	trB.setRotation(bodies[m_rbB].m_quat);
 
-	getInfo2NonVirtual(info, trA,trB);
+	getInfo2NonVirtual(info, trA, trB);
 }
 
-void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
+void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
 {
-
-	 //retrieve matrices
+	//retrieve matrices
 
 	// anchor points in global coordinates with respect to body PORs.
-   
-    // set jacobian
-    info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	b3Vector3 a1 = body0_trans.getBasis()*getPivotInA();
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
+
+	b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
 	//b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
 
 	{
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		b3Vector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
+
 	if (info->m_J2linearAxis)
 	{
 		info->m_J2linearAxis[0] = -1;
-		info->m_J2linearAxis[info->rowskip+1] = -1;
-		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+		info->m_J2linearAxis[info->rowskip + 1] = -1;
+		info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
 	}
-	
-	b3Vector3 a2 = body1_trans.getBasis()*getPivotInB();
-   
+
+	b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
+
 	{
-	//	b3Vector3 a2n = -a2;
+		//	b3Vector3 a2n = -a2;
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
 
-
-    // set right hand side
+	// set right hand side
 	b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
-    b3Scalar k = info->fps * currERP;
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+	b3Scalar k = info->fps * currERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
-	if(m_flags & B3_P2P_FLAGS_CFM)
+	}
+	if (m_flags & B3_P2P_FLAGS_CFM)
 	{
-		for (j=0; j<3; j++)
+		for (j = 0; j < 3; j++)
 		{
-			info->cfm[j*info->rowskip] = m_cfm;
+			info->cfm[j * info->rowskip] = m_cfm;
 		}
 	}
 
-	b3Scalar impulseClamp = m_setting.m_impulseClamp;//
-	for (j=0; j<3; j++)
-    {
+	b3Scalar impulseClamp = m_setting.m_impulseClamp;  //
+	for (j = 0; j < 3; j++)
+	{
 		if (m_setting.m_impulseClamp > 0)
 		{
-			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
-			info->m_upperLimit[j*info->rowskip] = impulseClamp;
+			info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
+			info->m_upperLimit[j * info->rowskip] = impulseClamp;
 		}
 	}
 	info->m_damping = m_setting.m_damping;
-	
 }
 
-
-
-void	b3Point2PointConstraint::updateRHS(b3Scalar	timeStep)
+void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 ///If no axis is provided, it uses the default axis for this constraint.
 void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
 {
-	if(axis != -1)
+	if (axis != -1)
 	{
 		b3AssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_ERP :
-			case B3_CONSTRAINT_STOP_ERP :
-				m_erp = value; 
+			case B3_CONSTRAINT_ERP:
+			case B3_CONSTRAINT_STOP_ERP:
+				m_erp = value;
 				m_flags |= B3_P2P_FLAGS_ERP;
 				break;
-			case B3_CONSTRAINT_CFM :
-			case B3_CONSTRAINT_STOP_CFM :
-				m_cfm = value; 
+			case B3_CONSTRAINT_CFM:
+			case B3_CONSTRAINT_STOP_CFM:
+				m_cfm = value;
 				m_flags |= B3_P2P_FLAGS_CFM;
 				break;
-			default: 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
 }
 
 ///return the local value of parameter
-b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const 
+b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
 {
 	b3Scalar retVal(B3_INFINITY);
-	if(axis != -1)
+	if (axis != -1)
 	{
 		b3AssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_ERP :
-			case B3_CONSTRAINT_STOP_ERP :
+			case B3_CONSTRAINT_ERP:
+			case B3_CONSTRAINT_STOP_ERP:
 				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
-				retVal = m_erp; 
+				retVal = m_erp;
 				break;
-			case B3_CONSTRAINT_CFM :
-			case B3_CONSTRAINT_STOP_CFM :
+			case B3_CONSTRAINT_CFM:
+			case B3_CONSTRAINT_STOP_CFM:
 				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
-				retVal = m_cfm; 
+				retVal = m_cfm;
 				break;
-			default: 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
 	return retVal;
 }
-	

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h

@@ -22,26 +22,24 @@ subject to the following restrictions:
 
 class b3RigidBody;
 
-
 #ifdef B3_USE_DOUBLE_PRECISION
-#define b3Point2PointConstraintData	b3Point2PointConstraintDoubleData
-#define b3Point2PointConstraintDataName	"b3Point2PointConstraintDoubleData"
+#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
+#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
 #else
-#define b3Point2PointConstraintData	b3Point2PointConstraintFloatData
-#define b3Point2PointConstraintDataName	"b3Point2PointConstraintFloatData"
-#endif //B3_USE_DOUBLE_PRECISION
+#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
+#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
+#endif  //B3_USE_DOUBLE_PRECISION
 
-struct	b3ConstraintSetting
+struct b3ConstraintSetting
 {
-	b3ConstraintSetting()	:
-		m_tau(b3Scalar(0.3)),
-		m_damping(b3Scalar(1.)),
-		m_impulseClamp(b3Scalar(0.))
+	b3ConstraintSetting() : m_tau(b3Scalar(0.3)),
+							m_damping(b3Scalar(1.)),
+							m_impulseClamp(b3Scalar(0.))
 	{
 	}
-	b3Scalar		m_tau;
-	b3Scalar		m_damping;
-	b3Scalar		m_impulseClamp;
+	b3Scalar m_tau;
+	b3Scalar m_damping;
+	b3Scalar m_impulseClamp;
 };
 
 enum b3Point2PointFlags
@@ -51,47 +49,45 @@ enum b3Point2PointFlags
 };
 
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
-B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Point2PointConstraint : public b3TypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	
-	b3Vector3	m_pivotInA;
-	b3Vector3	m_pivotInB;
-	
-	int			m_flags;
-	b3Scalar	m_erp;
-	b3Scalar	m_cfm;
-	
-public:
 
-	B3_DECLARE_ALIGNED_ALLOCATOR();
+	b3Vector3 m_pivotInA;
+	b3Vector3 m_pivotInB;
 
-	b3ConstraintSetting	m_setting;
+	int m_flags;
+	b3Scalar m_erp;
+	b3Scalar m_cfm;
 
-	b3Point2PointConstraint(int  rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	//b3Point2PointConstraint(int  rbA,const b3Vector3& pivotInA);
+	b3ConstraintSetting m_setting;
 
+	b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB);
 
+	//b3Point2PointConstraint(int  rbA,const b3Vector3& pivotInA);
 
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
 
-	void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
+	void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& body0_trans, const b3Transform& body1_trans);
 
-	void	updateRHS(b3Scalar	timeStep);
+	void updateRHS(b3Scalar timeStep);
 
-	void	setPivotA(const b3Vector3& pivotA)
+	void setPivotA(const b3Vector3& pivotA)
 	{
 		m_pivotInA = pivotA;
 	}
 
-	void	setPivotB(const b3Vector3& pivotB)
+	void setPivotB(const b3Vector3& pivotB)
 	{
 		m_pivotInB = pivotB;
 	}
@@ -106,34 +102,32 @@ public:
 		return m_pivotInB;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, b3Scalar value, int axis = -1);
+	virtual void setParam(int num, b3Scalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	b3Scalar getParam(int num, int axis = -1) const;
+	virtual b3Scalar getParam(int num, int axis = -1) const;
 
-//	virtual	int	calculateSerializeBufferSize() const;
+	//	virtual	int	calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-//	virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
-
-
+	//	virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	b3Point2PointConstraintFloatData
+struct b3Point2PointConstraintFloatData
 {
-	b3TypedConstraintData	m_typeConstraintData;
-	b3Vector3FloatData	m_pivotInA;
-	b3Vector3FloatData	m_pivotInB;
+	b3TypedConstraintData m_typeConstraintData;
+	b3Vector3FloatData m_pivotInA;
+	b3Vector3FloatData m_pivotInB;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	b3Point2PointConstraintDoubleData
+struct b3Point2PointConstraintDoubleData
 {
-	b3TypedConstraintData	m_typeConstraintData;
-	b3Vector3DoubleData	m_pivotInA;
-	b3Vector3DoubleData	m_pivotInB;
+	b3TypedConstraintData m_typeConstraintData;
+	b3Vector3DoubleData m_pivotInA;
+	b3Vector3DoubleData m_pivotInB;
 };
 
 /*
@@ -156,4 +150,4 @@ B3_FORCE_INLINE	const char*	b3Point2PointConstraint::serialize(void* dataBuffer,
 }
 */
 
-#endif //B3_POINT2POINTCONSTRAINT_H
+#endif  //B3_POINT2POINTCONSTRAINT_H

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h

@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef B3_SOLVER_BODY_H
 #define B3_SOLVER_BODY_H
 
-
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Matrix3x3.h"
 
@@ -26,110 +25,104 @@ subject to the following restrictions:
 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
 #ifdef B3_USE_SSE
 #define USE_SIMD 1
-#endif //
-
+#endif  //
 
 #ifdef USE_SIMD
 
-struct	b3SimdScalar
+struct b3SimdScalar
 {
-	B3_FORCE_INLINE	b3SimdScalar()
+	B3_FORCE_INLINE b3SimdScalar()
 	{
-
 	}
 
-	B3_FORCE_INLINE	b3SimdScalar(float	fl)
-	:m_vec128 (_mm_set1_ps(fl))
+	B3_FORCE_INLINE b3SimdScalar(float fl)
+		: m_vec128(_mm_set1_ps(fl))
 	{
 	}
 
-	B3_FORCE_INLINE	b3SimdScalar(__m128 v128)
-		:m_vec128(v128)
+	B3_FORCE_INLINE b3SimdScalar(__m128 v128)
+		: m_vec128(v128)
 	{
 	}
-	union
-	{
-		__m128		m_vec128;
-		float		m_floats[4];
-		float		x,y,z,w;
-		int			m_ints[4];
-		b3Scalar	m_unusedPadding;
+	union {
+		__m128 m_vec128;
+		float m_floats[4];
+		float x, y, z, w;
+		int m_ints[4];
+		b3Scalar m_unusedPadding;
 	};
-	B3_FORCE_INLINE	__m128	get128()
+	B3_FORCE_INLINE __m128 get128()
 	{
 		return m_vec128;
 	}
 
-	B3_FORCE_INLINE	const __m128	get128() const
+	B3_FORCE_INLINE const __m128 get128() const
 	{
 		return m_vec128;
 	}
 
-	B3_FORCE_INLINE	void	set128(__m128 v128)
+	B3_FORCE_INLINE void set128(__m128 v128)
 	{
 		m_vec128 = v128;
 	}
 
-	B3_FORCE_INLINE	operator       __m128()       
-	{ 
-		return m_vec128; 
-	}
-	B3_FORCE_INLINE	operator const __m128() const 
-	{ 
-		return m_vec128; 
+	B3_FORCE_INLINE operator __m128()
+	{
+		return m_vec128;
 	}
-	
-	B3_FORCE_INLINE	operator float() const 
-	{ 
-		return m_floats[0]; 
+	B3_FORCE_INLINE operator const __m128() const
+	{
+		return m_vec128;
 	}
 
+	B3_FORCE_INLINE operator float() const
+	{
+		return m_floats[0];
+	}
 };
 
 ///@brief Return the elementwise product of two b3SimdScalar
-B3_FORCE_INLINE b3SimdScalar 
-operator*(const b3SimdScalar& v1, const b3SimdScalar& v2) 
+B3_FORCE_INLINE b3SimdScalar
+operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
 {
-	return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+	return b3SimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
 }
 
 ///@brief Return the elementwise product of two b3SimdScalar
-B3_FORCE_INLINE b3SimdScalar 
-operator+(const b3SimdScalar& v1, const b3SimdScalar& v2) 
+B3_FORCE_INLINE b3SimdScalar
+operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
 {
-	return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+	return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
 }
 
-
 #else
 #define b3SimdScalar b3Scalar
 #endif
 
 ///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
-B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3SolverBody
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
-	b3Transform		m_worldTransform;
-	b3Vector3		m_deltaLinearVelocity;
-	b3Vector3		m_deltaAngularVelocity;
-	b3Vector3		m_angularFactor;
-	b3Vector3		m_linearFactor;
-	b3Vector3		m_invMass;
-	b3Vector3		m_pushVelocity;
-	b3Vector3		m_turnVelocity;
-	b3Vector3		m_linearVelocity;
-	b3Vector3		m_angularVelocity;
-
-	union 
-	{
-		void*	m_originalBody;
-		int		m_originalBodyIndex;
+	b3Transform m_worldTransform;
+	b3Vector3 m_deltaLinearVelocity;
+	b3Vector3 m_deltaAngularVelocity;
+	b3Vector3 m_angularFactor;
+	b3Vector3 m_linearFactor;
+	b3Vector3 m_invMass;
+	b3Vector3 m_pushVelocity;
+	b3Vector3 m_turnVelocity;
+	b3Vector3 m_linearVelocity;
+	b3Vector3 m_angularVelocity;
+
+	union {
+		void* m_originalBody;
+		int m_originalBodyIndex;
 	};
 
 	int padding[3];
 
-
-	void	setWorldTransform(const b3Transform& worldTransform)
+	void setWorldTransform(const b3Transform& worldTransform)
 	{
 		m_worldTransform = worldTransform;
 	}
@@ -138,45 +131,42 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 	{
 		return m_worldTransform;
 	}
-	
-	B3_FORCE_INLINE void	getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
+
+	B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
 	{
 		if (m_originalBody)
-			velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+			velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 		else
-			velocity.setValue(0,0,0);
+			velocity.setValue(0, 0, 0);
 	}
 
-	B3_FORCE_INLINE void	getAngularVelocity(b3Vector3& angVel) const
+	B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const
 	{
 		if (m_originalBody)
-			angVel =m_angularVelocity+m_deltaAngularVelocity;
+			angVel = m_angularVelocity + m_deltaAngularVelocity;
 		else
-			angVel.setValue(0,0,0);
+			angVel.setValue(0, 0, 0);
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
+	B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-	B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude)
+	B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-
-
 	const b3Vector3& getDeltaLinearVelocity() const
 	{
 		return m_deltaLinearVelocity;
@@ -187,20 +177,19 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 		return m_deltaAngularVelocity;
 	}
 
-	const b3Vector3& getPushVelocity() const 
+	const b3Vector3& getPushVelocity() const
 	{
 		return m_pushVelocity;
 	}
 
-	const b3Vector3& getTurnVelocity() const 
+	const b3Vector3& getTurnVelocity() const
 	{
 		return m_turnVelocity;
 	}
 
-
 	////////////////////////////////////////////////
 	///some internal methods, don't use them
-		
+
 	b3Vector3& internalGetDeltaLinearVelocity()
 	{
 		return m_deltaLinearVelocity;
@@ -225,7 +214,7 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 	{
 		m_invMass = invMass;
 	}
-	
+
 	b3Vector3& internalGetPushVelocity()
 	{
 		return m_pushVelocity;
@@ -236,67 +225,57 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 		return m_turnVelocity;
 	}
 
-	B3_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
+	B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
 	{
-		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+		velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 	}
 
-	B3_FORCE_INLINE void	internalGetAngularVelocity(b3Vector3& angVel) const
+	B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const
 	{
-		angVel = m_angularVelocity+m_deltaAngularVelocity;
+		angVel = m_angularVelocity + m_deltaAngularVelocity;
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
+	B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
 	{
 		//if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
-		
-	
-	
 
-	void	writebackVelocity()
+	void writebackVelocity()
 	{
 		//if (m_originalBody>=0)
 		{
-			m_linearVelocity +=m_deltaLinearVelocity;
+			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
 
-
-	void	writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
+	void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
 	{
-        (void) timeStep;
+		(void)timeStep;
 		if (m_originalBody)
 		{
 			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//correct the position/orientation based on push/turn recovery
 			b3Transform newTransform;
-			if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
+			if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
 			{
-			//	b3Quaternion orn = m_worldTransform.getRotation();
-				b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
+				//	b3Quaternion orn = m_worldTransform.getRotation();
+				b3TransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
 				m_worldTransform = newTransform;
 			}
 			//m_worldTransform.setRotation(orn);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-	
-
-
 };
 
-#endif //B3_SOLVER_BODY_H
-
-
+#endif  //B3_SOLVER_BODY_H

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h

@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef B3_SOLVER_CONSTRAINT_H
 #define B3_SOLVER_CONSTRAINT_H
 
-
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Matrix3x3.h"
 //#include "b3JacobianEntry.h"
@@ -25,56 +24,50 @@ subject to the following restrictions:
 //#define NO_FRICTION_TANGENTIALS 1
 #include "b3SolverBody.h"
 
-
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverConstraint
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3SolverConstraint
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	b3Vector3		m_relpos1CrossNormal;
-	b3Vector3		m_contactNormal;
+	b3Vector3 m_relpos1CrossNormal;
+	b3Vector3 m_contactNormal;
 
-	b3Vector3		m_relpos2CrossNormal;
+	b3Vector3 m_relpos2CrossNormal;
 	//b3Vector3		m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
 
-	b3Vector3		m_angularComponentA;
-	b3Vector3		m_angularComponentB;
-	
-	mutable b3SimdScalar	m_appliedPushImpulse;
-	mutable b3SimdScalar	m_appliedImpulse;
+	b3Vector3 m_angularComponentA;
+	b3Vector3 m_angularComponentB;
+
+	mutable b3SimdScalar m_appliedPushImpulse;
+	mutable b3SimdScalar m_appliedImpulse;
 	int m_padding1;
 	int m_padding2;
-	b3Scalar	m_friction;
-	b3Scalar	m_jacDiagABInv;
-	b3Scalar		m_rhs;
-	b3Scalar		m_cfm;
-	
-    b3Scalar		m_lowerLimit;
-	b3Scalar		m_upperLimit;
-	b3Scalar		m_rhsPenetration;
-    union
-	{
-		void*		m_originalContactPoint;
-		b3Scalar	m_unusedPadding4;
+	b3Scalar m_friction;
+	b3Scalar m_jacDiagABInv;
+	b3Scalar m_rhs;
+	b3Scalar m_cfm;
+
+	b3Scalar m_lowerLimit;
+	b3Scalar m_upperLimit;
+	b3Scalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		b3Scalar m_unusedPadding4;
 	};
 
-	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
 	int m_solverBodyIdA;
 	int m_solverBodyIdB;
 
-    
-	enum		b3SolverConstraintType
+	enum b3SolverConstraintType
 	{
 		B3_SOLVER_CONTACT_1D = 0,
 		B3_SOLVER_FRICTION_1D
 	};
 };
 
-typedef b3AlignedObjectArray<b3SolverConstraint>	b3ConstraintArray;
-
-
-#endif //B3_SOLVER_CONSTRAINT_H
-
-
+typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
 
+#endif  //B3_SOLVER_CONSTRAINT_H

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp

@@ -13,53 +13,46 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "b3TypedConstraint.h"
 //#include "Bullet3Common/b3Serializer.h"
 
-
 #define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
 
-
-
-b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB)
-:b3TypedObject(type),
-m_userConstraintType(-1),
-m_userConstraintPtr((void*)-1),
-m_breakingImpulseThreshold(B3_INFINITY),
-m_isEnabled(true),
-m_needsFeedback(false),
-m_overrideNumSolverIterations(-1),
-m_rbA(rbA),
-m_rbB(rbB),
-m_appliedImpulse(b3Scalar(0.)),
-m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
-m_jointFeedback(0)
+b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
+	: b3TypedObject(type),
+	  m_userConstraintType(-1),
+	  m_userConstraintPtr((void*)-1),
+	  m_breakingImpulseThreshold(B3_INFINITY),
+	  m_isEnabled(true),
+	  m_needsFeedback(false),
+	  m_overrideNumSolverIterations(-1),
+	  m_rbA(rbA),
+	  m_rbB(rbB),
+	  m_appliedImpulse(b3Scalar(0.)),
+	  m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
+	  m_jointFeedback(0)
 {
 }
 
-
-
-
 b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
 {
-	if(lowLim > uppLim)
+	if (lowLim > uppLim)
 	{
 		return b3Scalar(1.0f);
 	}
-	else if(lowLim == uppLim)
+	else if (lowLim == uppLim)
 	{
 		return b3Scalar(0.0f);
 	}
 	b3Scalar lim_fact = b3Scalar(1.0f);
 	b3Scalar delta_max = vel / timeFact;
-	if(delta_max < b3Scalar(0.0f))
+	if (delta_max < b3Scalar(0.0f))
 	{
-		if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
 		{
 			lim_fact = (lowLim - pos) / delta_max;
 		}
-		else if(pos  < lowLim)
+		else if (pos < lowLim)
 		{
 			lim_fact = b3Scalar(0.0f);
 		}
@@ -68,13 +61,13 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
 			lim_fact = b3Scalar(1.0f);
 		}
 	}
-	else if(delta_max > b3Scalar(0.0f))
+	else if (delta_max > b3Scalar(0.0f))
 	{
-		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
 		{
 			lim_fact = (uppLim - pos) / delta_max;
 		}
-		else if(pos  > uppLim)
+		else if (pos > uppLim)
 		{
 			lim_fact = b3Scalar(0.0f);
 		}
@@ -85,18 +78,16 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
 	}
 	else
 	{
-			lim_fact = b3Scalar(0.0f);
+		lim_fact = b3Scalar(0.0f);
 	}
 	return lim_fact;
 }
 
-
-
 void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
 {
 	m_halfRange = (high - low) / 2.0f;
 	m_center = b3NormalizeAngle(low + m_halfRange);
-	m_softness =  _softness;
+	m_softness = _softness;
 	m_biasFactor = _biasFactor;
 	m_relaxationFactor = _relaxationFactor;
 }
@@ -113,7 +104,7 @@ void b3AngularLimit::test(const b3Scalar angle)
 		if (deviation < -m_halfRange)
 		{
 			m_solveLimit = true;
-			m_correction = - (deviation + m_halfRange);
+			m_correction = -(deviation + m_halfRange);
 			m_sign = +1.0f;
 		}
 		else if (deviation > m_halfRange)
@@ -125,7 +116,6 @@ void b3AngularLimit::test(const b3Scalar angle)
 	}
 }
 
-
 b3Scalar b3AngularLimit::getError() const
 {
 	return m_correction * m_sign;

thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h

@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef B3_TYPED_CONSTRAINT_H
 #define B3_TYPED_CONSTRAINT_H
 
-
 #include "Bullet3Common/b3Scalar.h"
 #include "b3SolverConstraint.h"
 
@@ -25,7 +24,7 @@ class b3Serializer;
 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
 enum b3TypedConstraintType
 {
-	B3_POINT2POINT_CONSTRAINT_TYPE=3,
+	B3_POINT2POINT_CONSTRAINT_TYPE = 3,
 	B3_HINGE_CONSTRAINT_TYPE,
 	B3_CONETWIST_CONSTRAINT_TYPE,
 	B3_D6_CONSTRAINT_TYPE,
@@ -37,92 +36,86 @@ enum b3TypedConstraintType
 	B3_MAX_CONSTRAINT_TYPE
 };
 
-
 enum b3ConstraintParams
 {
-	B3_CONSTRAINT_ERP=1,
+	B3_CONSTRAINT_ERP = 1,
 	B3_CONSTRAINT_STOP_ERP,
 	B3_CONSTRAINT_CFM,
 	B3_CONSTRAINT_STOP_CFM
 };
 
 #if 1
-	#define b3AssertConstrParams(_par) b3Assert(_par) 
+#define b3AssertConstrParams(_par) b3Assert(_par)
 #else
-	#define b3AssertConstrParams(_par)
+#define b3AssertConstrParams(_par)
 #endif
 
-
-B3_ATTRIBUTE_ALIGNED16(struct)	b3JointFeedback
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3JointFeedback
 {
-	b3Vector3	m_appliedForceBodyA;
-	b3Vector3	m_appliedTorqueBodyA;
-	b3Vector3	m_appliedForceBodyB;
-	b3Vector3	m_appliedTorqueBodyB;
+	b3Vector3 m_appliedForceBodyA;
+	b3Vector3 m_appliedTorqueBodyA;
+	b3Vector3 m_appliedForceBodyB;
+	b3Vector3 m_appliedTorqueBodyB;
 };
 
-
 struct b3RigidBodyData;
 
-
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
-B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
+B3_ATTRIBUTE_ALIGNED16(class)
+b3TypedConstraint : public b3TypedObject
 {
-	int	m_userConstraintType;
+	int m_userConstraintType;
 
-	union
-	{
-		int	m_userConstraintId;
+	union {
+		int m_userConstraintId;
 		void* m_userConstraintPtr;
 	};
 
-	b3Scalar	m_breakingImpulseThreshold;
-	bool		m_isEnabled;
-	bool		m_needsFeedback;
-	int			m_overrideNumSolverIterations;
+	b3Scalar m_breakingImpulseThreshold;
+	bool m_isEnabled;
+	bool m_needsFeedback;
+	int m_overrideNumSolverIterations;
 
-
-	b3TypedConstraint&	operator=(b3TypedConstraint&	other)
+	b3TypedConstraint& operator=(b3TypedConstraint& other)
 	{
 		b3Assert(0);
-		(void) other;
+		(void)other;
 		return *this;
 	}
 
 protected:
-	int				m_rbA;
-	int				m_rbB;
-	b3Scalar	m_appliedImpulse;
-	b3Scalar	m_dbgDrawSize;
-	b3JointFeedback*	m_jointFeedback;
+	int m_rbA;
+	int m_rbB;
+	b3Scalar m_appliedImpulse;
+	b3Scalar m_dbgDrawSize;
+	b3JointFeedback* m_jointFeedback;
 
 	///internal method used by the constraint solver, don't use them directly
 	b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
-	
 
 public:
-
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	virtual ~b3TypedConstraint() {};
-	b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB);
+	virtual ~b3TypedConstraint(){};
+	b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB);
 
-	struct b3ConstraintInfo1 {
-		int m_numConstraintRows,nub;
+	struct b3ConstraintInfo1
+	{
+		int m_numConstraintRows, nub;
 	};
 
-	
-
-	struct b3ConstraintInfo2 {
+	struct b3ConstraintInfo2
+	{
 		// integrator parameters: frames per second (1/stepsize), default error
 		// reduction parameter (0..1).
-		b3Scalar fps,erp;
+		b3Scalar fps, erp;
 
 		// for the first and second body, pointers to two (linear and angular)
 		// n*3 jacobian sub matrices, stored by rows. these matrices will have
 		// been initialized to 0 on entry. if the second body is zero then the
 		// J2xx pointers may be 0.
-		b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+		b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
 
 		// elements to jump from one row to the next in J's
 		int rowskip;
@@ -130,24 +123,24 @@ public:
 		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
 		// "constraint force mixing" vector. c is set to zero on entry, cfm is
 		// set to a constant value (typically very small or zero) value on entry.
-		b3Scalar *m_constraintError,*cfm;
+		b3Scalar *m_constraintError, *cfm;
 
 		// lo and hi limits for variables (set to -/+ infinity on entry).
-		b3Scalar *m_lowerLimit,*m_upperLimit;
+		b3Scalar *m_lowerLimit, *m_upperLimit;
 
 		// findex vector for variables. see the LCP solver interface for a
 		// description of what this does. this is set to -1 on entry.
 		// note that the returned indexes are relative to the first index of
 		// the constraint.
-		int *findex;
+		int* findex;
 		// number of solver iterations
 		int m_numIterations;
 
 		//damping of the velocity
-		b3Scalar	m_damping;
+		b3Scalar m_damping;
 	};
 
-	int	getOverrideNumSolverIterations() const
+	int getOverrideNumSolverIterations() const
 	{
 		return m_overrideNumSolverIterations;
 	}
@@ -159,59 +152,55 @@ public:
 		m_overrideNumSolverIterations = overideNumIterations;
 	}
 
-
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
+	virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep)
 	{
-        (void)ca;
-        (void)solverBodyA;
-        (void)solverBodyB;
-        (void)timeStep;
+		(void)ca;
+		(void)solverBodyA;
+		(void)solverBodyB;
+		(void)timeStep;
 	}
-	
+
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)=0;
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo2 (b3ConstraintInfo2* info,  const b3RigidBodyData* bodies)=0;
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	void	internalSetAppliedImpulse(b3Scalar appliedImpulse)
+	void internalSetAppliedImpulse(b3Scalar appliedImpulse)
 	{
 		m_appliedImpulse = appliedImpulse;
 	}
 	///internal method used by the constraint solver, don't use them directly
-	b3Scalar	internalGetAppliedImpulse()
+	b3Scalar internalGetAppliedImpulse()
 	{
 		return m_appliedImpulse;
 	}
 
-
-	b3Scalar	getBreakingImpulseThreshold() const
+	b3Scalar getBreakingImpulseThreshold() const
 	{
-		return 	m_breakingImpulseThreshold;
+		return m_breakingImpulseThreshold;
 	}
 
-	void	setBreakingImpulseThreshold(b3Scalar threshold)
+	void setBreakingImpulseThreshold(b3Scalar threshold)
 	{
 		m_breakingImpulseThreshold = threshold;
 	}
 
-	bool	isEnabled() const
+	bool isEnabled() const
 	{
 		return m_isEnabled;
 	}
 
-	void	setEnabled(bool enabled)
+	void setEnabled(bool enabled)
 	{
-		m_isEnabled=enabled;
+		m_isEnabled = enabled;
 	}
 
-
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	solveConstraintObsolete(b3SolverBody& /*bodyA*/,b3SolverBody& /*bodyB*/,b3Scalar	/*timeStep*/) {};
+	virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){};
 
-	
 	int getRigidBodyA() const
 	{
 		return m_rbA;
@@ -221,8 +210,7 @@ public:
 		return m_rbB;
 	}
 
-
-	int getRigidBodyA() 
+	int getRigidBodyA()
 	{
 		return m_rbA;
 	}
@@ -233,15 +221,15 @@ public:
 
 	int getUserConstraintType() const
 	{
-		return m_userConstraintType ;
+		return m_userConstraintType;
 	}
 
-	void	setUserConstraintType(int userConstraintType)
+	void setUserConstraintType(int userConstraintType)
 	{
 		m_userConstraintType = userConstraintType;
 	};
 
-	void	setUserConstraintId(int uid)
+	void setUserConstraintId(int uid)
 	{
 		m_userConstraintId = uid;
 	}
@@ -251,17 +239,17 @@ public:
 		return m_userConstraintId;
 	}
 
-	void	setUserConstraintPtr(void* ptr)
+	void setUserConstraintPtr(void* ptr)
 	{
 		m_userConstraintPtr = ptr;
 	}
 
-	void*	getUserConstraintPtr()
+	void* getUserConstraintPtr()
 	{
 		return m_userConstraintPtr;
 	}
 
-	void	setJointFeedback(b3JointFeedback* jointFeedback)
+	void setJointFeedback(b3JointFeedback * jointFeedback)
 	{
 		m_jointFeedback = jointFeedback;
 	}
@@ -276,37 +264,36 @@ public:
 		return m_jointFeedback;
 	}
 
-
 	int getUid() const
 	{
-		return m_userConstraintId;   
-	} 
+		return m_userConstraintId;
+	}
 
-	bool	needsFeedback() const
+	bool needsFeedback() const
 	{
 		return m_needsFeedback;
 	}
 
 	///enableFeedback will allow to read the applied linear and angular impulse
 	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
-	void	enableFeedback(bool needsFeedback)
+	void enableFeedback(bool needsFeedback)
 	{
 		m_needsFeedback = needsFeedback;
 	}
 
-	///getAppliedImpulse is an estimated total applied impulse. 
+	///getAppliedImpulse is an estimated total applied impulse.
 	///This feedback could be used to determine breaking constraints or playing sounds.
-	b3Scalar	getAppliedImpulse() const
+	b3Scalar getAppliedImpulse() const
 	{
 		b3Assert(m_needsFeedback);
 		return m_appliedImpulse;
 	}
 
-	b3TypedConstraintType getConstraintType () const
+	b3TypedConstraintType getConstraintType() const
 	{
 		return b3TypedConstraintType(m_objectType);
 	}
-	
+
 	void setDbgDrawSize(b3Scalar dbgDrawSize)
 	{
 		m_dbgDrawSize = dbgDrawSize;
@@ -316,35 +303,34 @@ public:
 		return m_dbgDrawSize;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, b3Scalar value, int axis = -1) = 0;
+	virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
 
 	///return the local value of parameter
-	virtual	b3Scalar getParam(int num, int axis = -1) const = 0;
-	
-//	virtual	int	calculateSerializeBufferSize() const;
+	virtual b3Scalar getParam(int num, int axis = -1) const = 0;
+
+	//	virtual	int	calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	//virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
-
 };
 
-// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits 
+// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
 // all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI])
 B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
 {
-	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
 	{
 		return angleInRadians;
 	}
-	else if(angleInRadians < angleLowerLimitInRadians)
+	else if (angleInRadians < angleLowerLimitInRadians)
 	{
 		b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
 		b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
 		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI);
 	}
-	else if(angleInRadians > angleUpperLimitInRadians)
+	else if (angleInRadians > angleUpperLimitInRadians)
 	{
 		b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
 		b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
@@ -356,6 +342,7 @@ B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar
 	}
 }
 
+// clang-format off
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	b3TypedConstraintData
 {
@@ -379,17 +366,18 @@ struct	b3TypedConstraintData
 	
 };
 
+// clang-format on
+
 /*B3_FORCE_INLINE	int	b3TypedConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(b3TypedConstraintData);
 }
 */
 
-
 class b3AngularLimit
 {
 private:
-	b3Scalar 
+	b3Scalar
 		m_center,
 		m_halfRange,
 		m_softness,
@@ -404,15 +392,16 @@ private:
 public:
 	/// Default constructor initializes limit as inactive, allowing free constraint movement
 	b3AngularLimit()
-		:m_center(0.0f),
-		m_halfRange(-1.0f),
-		m_softness(0.9f),
-		m_biasFactor(0.3f),
-		m_relaxationFactor(1.0f),
-		m_correction(0.0f),
-		m_sign(0.0f),
-		m_solveLimit(false)
-	{}
+		: m_center(0.0f),
+		  m_halfRange(-1.0f),
+		  m_softness(0.9f),
+		  m_biasFactor(0.3f),
+		  m_relaxationFactor(1.0f),
+		  m_correction(0.0f),
+		  m_sign(0.0f),
+		  m_solveLimit(false)
+	{
+	}
 
 	/// Sets all limit's parameters.
 	/// When low > high limit becomes inactive.
@@ -441,13 +430,13 @@ public:
 		return m_relaxationFactor;
 	}
 
-	/// Returns correction value evaluated when test() was invoked 
+	/// Returns correction value evaluated when test() was invoked
 	inline b3Scalar getCorrection() const
 	{
 		return m_correction;
 	}
 
-	/// Returns sign value evaluated when test() was invoked 
+	/// Returns sign value evaluated when test() was invoked
 	inline b3Scalar getSign() const
 	{
 		return m_sign;
@@ -475,9 +464,6 @@ public:
 	b3Scalar getLow() const;
 
 	b3Scalar getHigh() const;
-
 };
 
-
-
-#endif //B3_TYPED_CONSTRAINT_H
+#endif  //B3_TYPED_CONSTRAINT_H

thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp

@@ -11,7 +11,6 @@
 #include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
 #include "Bullet3Dynamics/shared/b3Inertia.h"
 
-
 struct b3CpuRigidBodyPipelineInternalData
 {
 	b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
@@ -22,7 +21,6 @@ struct b3CpuRigidBodyPipelineInternalData
 	b3CpuNarrowPhase* m_np;
 	b3Config m_config;
 };
-	
 
 b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
 {
@@ -39,49 +37,43 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
 
 void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
 {
-
-	for (int i=0;i<this->getNumBodies();i++)
+	for (int i = 0; i < this->getNumBodies(); i++)
 	{
 		b3RigidBodyData* body = &m_data->m_rigidBodies[i];
 		b3Float4 position = body->m_pos;
-		b3Quat	orientation = body->m_quat;
+		b3Quat orientation = body->m_quat;
 
 		int collidableIndex = body->m_collidableIdx;
 		b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
 		int shapeIndex = collidable.m_shapeIndex;
-		
-		if (shapeIndex>=0)
-		{
-			
 
+		if (shapeIndex >= 0)
+		{
 			b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
 			b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
-			float margin=0.f;
-			b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec);
-			m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0);
+			float margin = 0.f;
+			b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
+			m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
 		}
 	}
 }
 
-void	b3CpuRigidBodyPipeline::computeOverlappingPairs()
+void b3CpuRigidBodyPipeline::computeOverlappingPairs()
 {
 	int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
 	m_data->m_bp->calculateOverlappingPairs();
 	numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
-	printf("numPairs=%d\n",numPairs);
+	printf("numPairs=%d\n", numPairs);
 }
 
 void b3CpuRigidBodyPipeline::computeContactPoints()
 {
-	
 	b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
-	
-	m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
 
+	m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
 }
-void	b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
+void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
 {
-	
 	//update world space aabb's
 	updateAabbWorldSpace();
 
@@ -92,73 +84,71 @@ void	b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
 	computeContactPoints();
 
 	//solve contacts
-	
+
 	//update transforms
 	integrate(deltaTime);
-	
-	
 }
 
-
-static	inline	float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, 
-					 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
+static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
+								 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
 {
 	return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
 }
 
-
-static	inline	void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
-							 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
+static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
+										 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
 {
 	linear = -n;
 	angular0 = -b3Cross(r0, n);
 	angular1 = b3Cross(r1, n);
 }
 
-
-
-static inline void b3SolveContact(b3ContactConstraint4& cs, 
-	const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
-	const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
-	float maxRambdaDt[4], float minRambdaDt[4])
+static inline void b3SolveContact(b3ContactConstraint4& cs,
+								  const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+								  const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
+								  float maxRambdaDt[4], float minRambdaDt[4])
 {
-
-	b3Vector3 dLinVelA; dLinVelA.setZero();
-	b3Vector3 dAngVelA; dAngVelA.setZero();
-	b3Vector3 dLinVelB; dLinVelB.setZero();
-	b3Vector3 dAngVelB; dAngVelB.setZero();
-
-	for(int ic=0; ic<4; ic++)
+	b3Vector3 dLinVelA;
+	dLinVelA.setZero();
+	b3Vector3 dAngVelA;
+	dAngVelA.setZero();
+	b3Vector3 dLinVelB;
+	dLinVelB.setZero();
+	b3Vector3 dAngVelB;
+	dAngVelB.setZero();
+
+	for (int ic = 0; ic < 4; ic++)
 	{
 		//	dont necessary because this makes change to 0
-		if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
+		if (cs.m_jacCoeffInv[ic] == 0.f) continue;
 
 		{
 			b3Vector3 angular0, angular1, linear;
 			b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
 			b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
-			b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
+			b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
 
-			float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
-				linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
+			float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
+										  linVelA, angVelA, linVelB, angVelB) +
+							 cs.m_b[ic];
 			rambdaDt *= cs.m_jacCoeffInv[ic];
 
 			{
 				float prevSum = cs.m_appliedRambdaDt[ic];
 				float updated = prevSum;
 				updated += rambdaDt;
-				updated = b3Max( updated, minRambdaDt[ic] );
-				updated = b3Min( updated, maxRambdaDt[ic] );
+				updated = b3Max(updated, minRambdaDt[ic]);
+				updated = b3Min(updated, maxRambdaDt[ic]);
 				rambdaDt = updated - prevSum;
 				cs.m_appliedRambdaDt[ic] = updated;
 			}
 
-			b3Vector3 linImp0 = invMassA*linear*rambdaDt;
-			b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
-			b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
-			b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+			b3Vector3 linImp0 = invMassA * linear * rambdaDt;
+			b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
+			b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
+			b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
 #ifdef _WIN32
-            b3Assert(_finite(linImp0.getX()));
+			b3Assert(_finite(linImp0.getX()));
 			b3Assert(_finite(linImp1.getX()));
 #endif
 			{
@@ -169,53 +159,46 @@ static inline void b3SolveContact(b3ContactConstraint4& cs,
 			}
 		}
 	}
-
-
 }
 
-
-
-
-
-static inline void b3SolveFriction(b3ContactConstraint4& cs, 
-		const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
-		const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
-		float maxRambdaDt[4], float minRambdaDt[4])
+static inline void b3SolveFriction(b3ContactConstraint4& cs,
+								   const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+								   const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
+								   float maxRambdaDt[4], float minRambdaDt[4])
 {
-
-	if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
+	if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
 	const b3Vector3& center = (const b3Vector3&)cs.m_center;
 
 	b3Vector3 n = -(const b3Vector3&)cs.m_linear;
 
 	b3Vector3 tangent[2];
 
-	b3PlaneSpace1 (n, tangent[0],tangent[1]);
+	b3PlaneSpace1(n, tangent[0], tangent[1]);
 
 	b3Vector3 angular0, angular1, linear;
 	b3Vector3 r0 = center - posA;
 	b3Vector3 r1 = center - posB;
-	for(int i=0; i<2; i++)
+	for (int i = 0; i < 2; i++)
 	{
-		b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
+		b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
 		float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
-			linVelA, angVelA, linVelB, angVelB );
+									  linVelA, angVelA, linVelB, angVelB);
 		rambdaDt *= cs.m_fJacCoeffInv[i];
 
-			{
-				float prevSum = cs.m_fAppliedRambdaDt[i];
-				float updated = prevSum;
-				updated += rambdaDt;
-				updated = b3Max( updated, minRambdaDt[i] );
-				updated = b3Min( updated, maxRambdaDt[i] );
-				rambdaDt = updated - prevSum;
-				cs.m_fAppliedRambdaDt[i] = updated;
-			}
+		{
+			float prevSum = cs.m_fAppliedRambdaDt[i];
+			float updated = prevSum;
+			updated += rambdaDt;
+			updated = b3Max(updated, minRambdaDt[i]);
+			updated = b3Min(updated, maxRambdaDt[i]);
+			rambdaDt = updated - prevSum;
+			cs.m_fAppliedRambdaDt[i] = updated;
+		}
 
-		b3Vector3 linImp0 = invMassA*linear*rambdaDt;
-		b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
-		b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
-		b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+		b3Vector3 linImp0 = invMassA * linear * rambdaDt;
+		b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
+		b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
+		b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
 #ifdef _WIN32
 		b3Assert(_finite(linImp0.getX()));
 		b3Assert(_finite(linImp1.getX()));
@@ -226,57 +209,45 @@ static inline void b3SolveFriction(b3ContactConstraint4& cs,
 		angVelB += angImp1;
 	}
 
-	{	//	angular damping for point constraint
-		b3Vector3 ab = ( posB - posA ).normalized();
-		b3Vector3 ac = ( center - posA ).normalized();
-		if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
+	{  //	angular damping for point constraint
+		b3Vector3 ab = (posB - posA).normalized();
+		b3Vector3 ac = (center - posA).normalized();
+		if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
 		{
-			float angNA = b3Dot( n, angVelA );
-			float angNB = b3Dot( n, angVelB );
+			float angNA = b3Dot(n, angVelA);
+			float angNB = b3Dot(n, angVelB);
 
-			angVelA -= (angNA*0.1f)*n;
-			angVelB -= (angNB*0.1f)*n;
+			angVelA -= (angNA * 0.1f) * n;
+			angVelB -= (angNB * 0.1f) * n;
 		}
 	}
-
 }
 
-
-
-
-
-struct b3SolveTask// : public ThreadPool::Task
+struct b3SolveTask  // : public ThreadPool::Task
 {
-	b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,  
-				b3AlignedObjectArray<b3Inertia>& shapes, 
+	b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
+				b3AlignedObjectArray<b3Inertia>& shapes,
 				b3AlignedObjectArray<b3ContactConstraint4>& constraints,
 				int start, int nConstraints,
 				int maxNumBatches,
-				b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx
-				)
-		: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), 
-		m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx),
-m_start( start ), 
-	m_nConstraints( nConstraints ),
-	m_solveFriction( true ),
-	m_maxNumBatches(maxNumBatches)
-	{}
-
-	unsigned short int getType(){ return 0; }
+				b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
+		: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
+	{
+	}
+
+	unsigned short int getType() { return 0; }
 
 	void run(int tIdx)
 	{
 		b3AlignedObjectArray<int> usedBodies;
 		//printf("run..............\n");
 
-		
-		for (int bb=0;bb<m_maxNumBatches;bb++)
+		for (int bb = 0; bb < m_maxNumBatches; bb++)
 		{
 			usedBodies.resize(0);
-			for(int ic=m_nConstraints-1; ic>=0; ic--)
+			for (int ic = m_nConstraints - 1; ic >= 0; ic--)
 			//for(int ic=0; ic<m_nConstraints; ic++)
 			{
-				
 				int i = m_start + ic;
 				if (m_constraints[i].m_batchIdx != bb)
 					continue;
@@ -298,87 +269,80 @@ m_start( start ),
 					//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
 				}
 #endif
-				if (aIdx==10)
+				if (aIdx == 10)
 				{
 					//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
 				}
-				if (usedBodies.size()<(aIdx+1))
+				if (usedBodies.size() < (aIdx + 1))
 				{
-					usedBodies.resize(aIdx+1,0);
+					usedBodies.resize(aIdx + 1, 0);
 				}
-		
-				if (usedBodies.size()<(bIdx+1))
+
+				if (usedBodies.size() < (bIdx + 1))
 				{
-					usedBodies.resize(bIdx+1,0);
+					usedBodies.resize(bIdx + 1, 0);
 				}
 
 				if (bodyA.m_invMass)
 				{
-					b3Assert(usedBodies[aIdx]==0);
+					b3Assert(usedBodies[aIdx] == 0);
 					usedBodies[aIdx]++;
 				}
-				
+
 				if (bodyB.m_invMass)
 				{
-					b3Assert(usedBodies[bIdx]==0);
+					b3Assert(usedBodies[bIdx] == 0);
 					usedBodies[bIdx]++;
 				}
-				
 
-				if( !m_solveFriction )
+				if (!m_solveFriction)
 				{
-					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
-					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
-
-					b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 
-							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
-						maxRambdaDt, minRambdaDt );
+					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
+					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
 
+					b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
+								   (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
+								   maxRambdaDt, minRambdaDt);
 				}
 				else
 				{
-					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
-					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
+					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
 
 					float sum = 0;
-					for(int j=0; j<4; j++)
+					for (int j = 0; j < 4; j++)
 					{
-						sum +=m_constraints[i].m_appliedRambdaDt[j];
+						sum += m_constraints[i].m_appliedRambdaDt[j];
 					}
 					frictionCoeff = 0.7f;
-					for(int j=0; j<4; j++)
+					for (int j = 0; j < 4; j++)
 					{
-						maxRambdaDt[j] = frictionCoeff*sum;
+						maxRambdaDt[j] = frictionCoeff * sum;
 						minRambdaDt[j] = -maxRambdaDt[j];
 					}
 
-				b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 
-						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
-						maxRambdaDt, minRambdaDt );
-			
+					b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
+									(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
+									maxRambdaDt, minRambdaDt);
 				}
 			}
 
 			if (m_wgUsedBodies)
 			{
-				if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size())
+				if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
 				{
 					m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
 				}
-				for (int i=0;i<usedBodies.size();i++)
+				for (int i = 0; i < usedBodies.size(); i++)
 				{
 					if (usedBodies[i])
 					{
 						//printf("cell %d uses body %d\n", m_curWgidx,i);
-						m_wgUsedBodies[m_curWgidx][i]=1;
+						m_wgUsedBodies[m_curWgidx][i] = 1;
 					}
 				}
 			}
-
 		}
-
-
-		
 	}
 
 	b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
@@ -397,24 +361,22 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
 	int m_nIterations = 4;
 
 	b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
-//	const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
+	//	const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
 	int n = contactConstraints.size();
 	//convert contacts...
 
-
-	
 	int maxNumBatches = 250;
 
-	for(int iter=0; iter<m_nIterations; iter++)
+	for (int iter = 0; iter < m_nIterations; iter++)
 	{
-		b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
+		b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
 		task.m_solveFriction = false;
 		task.run(0);
 	}
 
-	for(int iter=0; iter<m_nIterations; iter++)
+	for (int iter = 0; iter < m_nIterations; iter++)
 	{
-		b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
+		b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
 		task.m_solveFriction = true;
 		task.run(0);
 	}
@@ -422,53 +384,51 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
 
 void b3CpuRigidBodyPipeline::integrate(float deltaTime)
 {
-	float angDamping=0.f;
-	b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0);
+	float angDamping = 0.f;
+	b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
 
 	//integrate transforms (external forces/gravity should be moved into constraint solver)
-	for (int i=0;i<m_data->m_rigidBodies.size();i++)
+	for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
 	{
-		b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration);
+		b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
 	}
-
 }
 
-int		b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
+int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
 {
 	b3RigidBodyData body;
 	int bodyIndex = m_data->m_rigidBodies.size();
-	body.m_invMass = mass ? 1.f/mass : 0.f;
-	body.m_angVel.setValue(0,0,0);
+	body.m_invMass = mass ? 1.f / mass : 0.f;
+	body.m_angVel.setValue(0, 0, 0);
 	body.m_collidableIdx = collidableIndex;
 	body.m_frictionCoeff = 0.3f;
-	body.m_linVel.setValue(0,0,0);
-	body.m_pos.setValue(position[0],position[1],position[2]);
-	body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
+	body.m_linVel.setValue(0, 0, 0);
+	body.m_pos.setValue(position[0], position[1], position[2]);
+	body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
 	body.m_restituitionCoeff = 0.f;
 
 	m_data->m_rigidBodies.push_back(body);
 
-	
-	if (collidableIndex>=0)
+	if (collidableIndex >= 0)
 	{
 		b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
 
 		b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
-		b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
-		b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
-		
+		b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
+		b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
+
 		b3Scalar margin = 0.01f;
 		b3Transform t;
 		t.setIdentity();
-		t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
-		t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
-		b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
-
-		m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
-//		b3Vector3 aabbMin,aabbMax;
-	//	m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
+		t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
+		t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
+		b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
 
-	} else
+		m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
+		//		b3Vector3 aabbMin,aabbMax;
+		//	m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
+	}
+	else
 	{
 		b3Error("registerPhysicsInstance using invalid collidableIndex\n");
 	}
@@ -476,13 +436,12 @@ int		b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
 	return bodyIndex;
 }
 
-
 const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
 {
 	return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
 }
 
-int	b3CpuRigidBodyPipeline::getNumBodies() const
+int b3CpuRigidBodyPipeline::getNumBodies() const
 {
 	return m_data->m_rigidBodies.size();
 }

thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h

@@ -16,52 +16,47 @@ subject to the following restrictions:
 #ifndef B3_CPU_RIGIDBODY_PIPELINE_H
 #define B3_CPU_RIGIDBODY_PIPELINE_H
 
-
-
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"
 
 class b3CpuRigidBodyPipeline
 {
 protected:
-	struct b3CpuRigidBodyPipelineInternalData*	m_data;
+	struct b3CpuRigidBodyPipelineInternalData* m_data;
 
 	int allocateCollidable();
 
 public:
-
-
 	b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
 	virtual ~b3CpuRigidBodyPipeline();
 
-	virtual void	stepSimulation(float deltaTime);
-	virtual void	integrate(float timeStep);
-	virtual void	updateAabbWorldSpace();
-	virtual void	computeOverlappingPairs();
-	virtual void	computeContactPoints();
-	virtual void	solveContactConstraints();
-
-	int		registerConvexPolyhedron(class b3ConvexUtility* convex);
-
-	int		registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
-	void	writeAllInstancesToGpu();
-	void	copyConstraintsToHost();
-	void	setGravity(const float* grav);
-	void	reset();
-	
-	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold);
+	virtual void stepSimulation(float deltaTime);
+	virtual void integrate(float timeStep);
+	virtual void updateAabbWorldSpace();
+	virtual void computeOverlappingPairs();
+	virtual void computeContactPoints();
+	virtual void solveContactConstraints();
+
+	int registerConvexPolyhedron(class b3ConvexUtility* convex);
+
+	int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
+	void writeAllInstancesToGpu();
+	void copyConstraintsToHost();
+	void setGravity(const float* grav);
+	void reset();
+
+	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold);
 	int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
 	void removeConstraintByUid(int uid);
 
-	void	addConstraint(class b3TypedConstraint* constraint);
-	void	removeConstraint(b3TypedConstraint* constraint);
+	void addConstraint(class b3TypedConstraint* constraint);
+	void removeConstraint(b3TypedConstraint* constraint);
 
-	void	castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults);
+	void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
 
 	const struct b3RigidBodyData* getBodyBuffer() const;
 
-	int	getNumBodies() const;
-
+	int getNumBodies() const;
 };
 
-#endif //B3_CPU_RIGIDBODY_PIPELINE_H
+#endif  //B3_CPU_RIGIDBODY_PIPELINE_H

thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h

@@ -5,30 +5,27 @@
 
 typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
 
-
 struct b3ContactConstraint4
 {
-
-	b3Float4 m_linear;//normal?
+	b3Float4 m_linear;  //normal?
 	b3Float4 m_worldPos[4];
-	b3Float4 m_center;	//	friction
+	b3Float4 m_center;  //	friction
 	float m_jacCoeffInv[4];
 	float m_b[4];
 	float m_appliedRambdaDt[4];
-	float m_fJacCoeffInv[2];	//	friction
-	float m_fAppliedRambdaDt[2];	//	friction
+	float m_fJacCoeffInv[2];      //	friction
+	float m_fAppliedRambdaDt[2];  //	friction
 
 	unsigned int m_bodyA;
 	unsigned int m_bodyB;
-	int			m_batchIdx;
+	int m_batchIdx;
 	unsigned int m_paddings;
-
 };
 
 //inline	void setFrictionCoeff(float value) { m_linear[3] = value; }
-inline	float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) 
+inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
 {
-	return constraint->m_linear.w; 
+	return constraint->m_linear.w;
 }
 
-#endif //B3_CONTACT_CONSTRAINT5_H
+#endif  //B3_CONTACT_CONSTRAINT5_H

thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h

@@ -4,89 +4,84 @@
 #include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
-
-void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);
- void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
 {
-  if (b3Fabs(n.z) > 0.70710678f) {
-    // choose p in y-z plane
-    float a = n.y*n.y + n.z*n.z;
-    float k = 1.f/sqrt(a);
-    p[0].x = 0;
-	p[0].y = -n.z*k;
-	p[0].z = n.y*k;
-    // set q = n x p
-    q[0].x = a*k;
-	q[0].y = -n.x*p[0].z;
-	q[0].z = n.x*p[0].y;
-  }
-  else {
-    // choose p in x-y plane
-    float a = n.x*n.x + n.y*n.y;
-    float k = 1.f/sqrt(a);
-    p[0].x = -n.y*k;
-	p[0].y = n.x*k;
-	p[0].z = 0;
-    // set q = n x p
-    q[0].x = -n.z*p[0].y;
-	q[0].y = n.z*p[0].x;
-	q[0].z = a*k;
-  }
+	if (b3Fabs(n.z) > 0.70710678f)
+	{
+		// choose p in y-z plane
+		float a = n.y * n.y + n.z * n.z;
+		float k = 1.f / sqrt(a);
+		p[0].x = 0;
+		p[0].y = -n.z * k;
+		p[0].z = n.y * k;
+		// set q = n x p
+		q[0].x = a * k;
+		q[0].y = -n.x * p[0].z;
+		q[0].z = n.x * p[0].y;
+	}
+	else
+	{
+		// choose p in x-y plane
+		float a = n.x * n.x + n.y * n.y;
+		float k = 1.f / sqrt(a);
+		p[0].x = -n.y * k;
+		p[0].y = n.x * k;
+		p[0].z = 0;
+		// set q = n x p
+		q[0].x = -n.z * p[0].y;
+		q[0].y = n.z * p[0].x;
+		q[0].z = a * k;
+	}
 }
 
-
- 
-void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
+void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
 {
-	*linear = b3MakeFloat4(n.x,n.y,n.z,0.f);
+	*linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
 	*angular0 = b3Cross3(r0, n);
 	*angular1 = -b3Cross3(r1, n);
 }
 
-
-float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
-	b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )
+float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
+				 b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
 {
 	return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
 }
 
-
 float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
-					float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
+				   float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
 {
 	//	linear0,1 are normlized
-	float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;
-	float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);
-	float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;
-	float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);
-	return -1.f/(jmj0+jmj1+jmj2+jmj3);
+	float jmj0 = invMass0;  //b3Dot3F4(linear0, linear0)*invMass0;
+	float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
+	float jmj2 = invMass1;  //b3Dot3F4(linear1, linear1)*invMass1;
+	float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
+	return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
 }
 
-
-void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
-	b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, 
-	__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
-	b3ContactConstraint4_t* dstC )
+void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
+					b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
+					__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
+					b3ContactConstraint4_t* dstC)
 {
 	dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
 	dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
 
-	float dtInv = 1.f/dt;
-	for(int ic=0; ic<4; ic++)
+	float dtInv = 1.f / dt;
+	for (int ic = 0; ic < 4; ic++)
 	{
 		dstC->m_appliedRambdaDt[ic] = 0.f;
 	}
 	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
 
-
 	dstC->m_linear = src->m_worldNormalOnB;
-	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
-	for(int ic=0; ic<4; ic++)
+	dstC->m_linear.w = 0.7f;  //src->getFrictionCoeff() );
+	for (int ic = 0; ic < 4; ic++)
 	{
 		b3Float4 r0 = src->m_worldPosB[ic] - posA;
 		b3Float4 r1 = src->m_worldPosB[ic] - posB;
 
-		if( ic >= src->m_worldNormalOnB.w )//npoints
+		if (ic >= src->m_worldNormalOnB.w)  //npoints
 		{
 			dstC->m_jacCoeffInv[ic] = 0.f;
 			continue;
@@ -98,56 +93,56 @@ void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4Co
 			setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
 
 			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
-				invMassA, &invInertiaA, invMassB, &invInertiaB );
+												   invMassA, &invInertiaA, invMassB, &invInertiaB);
 
 			relVelN = calcRelVel(linear, -linear, angular0, angular1,
-				linVelA, angVelA, linVelB, angVelB);
+								 linVelA, angVelA, linVelB, angVelB);
 
-			float e = 0.f;//src->getRestituitionCoeff();
-			if( relVelN*relVelN < 0.004f ) e = 0.f;
+			float e = 0.f;  //src->getRestituitionCoeff();
+			if (relVelN * relVelN < 0.004f) e = 0.f;
 
-			dstC->m_b[ic] = e*relVelN;
+			dstC->m_b[ic] = e * relVelN;
 			//float penetration = src->m_worldPosB[ic].w;
-			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
+			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
 			dstC->m_appliedRambdaDt[ic] = 0.f;
 		}
 	}
 
-	if( src->m_worldNormalOnB.w > 0 )//npoints
-	{	//	prepare friction
-		b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);
-		for(int i=0; i<src->m_worldNormalOnB.w; i++) 
+	if (src->m_worldNormalOnB.w > 0)  //npoints
+	{                                 //	prepare friction
+		b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
+		for (int i = 0; i < src->m_worldNormalOnB.w; i++)
 			center += src->m_worldPosB[i];
 		center /= (float)src->m_worldNormalOnB.w;
 
 		b3Float4 tangent[2];
-		b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);
-		
+		b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
+
 		b3Float4 r[2];
 		r[0] = center - posA;
 		r[1] = center - posB;
 
-		for(int i=0; i<2; i++)
+		for (int i = 0; i < 2; i++)
 		{
 			b3Float4 linear, angular0, angular1;
 			setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
 
 			dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
-				invMassA, &invInertiaA, invMassB, &invInertiaB );
+												   invMassA, &invInertiaA, invMassB, &invInertiaB);
 			dstC->m_fAppliedRambdaDt[i] = 0.f;
 		}
 		dstC->m_center = center;
 	}
 
-	for(int i=0; i<4; i++)
+	for (int i = 0; i < 4; i++)
 	{
-		if( i<src->m_worldNormalOnB.w )
+		if (i < src->m_worldNormalOnB.w)
 		{
 			dstC->m_worldPos[i] = src->m_worldPosB[i];
 		}
 		else
 		{
-			dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);
+			dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
 		}
 	}
 }

thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h

@@ -11,5 +11,4 @@ struct b3Inertia
 	b3Mat3x3 m_initInvInertia;
 };
 
-
-#endif //B3_INERTIA_H
+#endif  //B3_INERTIA_H

thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h

@@ -2,11 +2,8 @@
 
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
-
-
-inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
 {
-	
 	if (bodies[nodeID].m_invMass != 0.f)
 	{
 		float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
@@ -18,27 +15,27 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
 			bodies[nodeID].m_angVel.x *= angularDamping;
 			bodies[nodeID].m_angVel.y *= angularDamping;
 			bodies[nodeID].m_angVel.z *= angularDamping;
-			
+
 			b3Float4 angvel = bodies[nodeID].m_angVel;
 
 			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
-			
+
 			//limit the angular motion
-			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
 			{
 				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
 			}
-			if(fAngle < 0.001f)
+			if (fAngle < 0.001f)
 			{
 				// use Taylor's expansions of sync function
-				axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+				axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
 			}
 			else
 			{
 				// sync(fAngle) = sin(c*fAngle)/t
-				axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
+				axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
 			}
-			
+
 			b3Quat dorn;
 			dorn.x = axis.x;
 			dorn.y = axis.y;
@@ -47,23 +44,21 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
 			b3Quat orn0 = bodies[nodeID].m_quat;
 			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
 			predictedOrn = b3QuatNormalized(predictedOrn);
-			bodies[nodeID].m_quat=predictedOrn;
+			bodies[nodeID].m_quat = predictedOrn;
 		}
-		//linear velocity		
-		bodies[nodeID].m_pos +=  bodies[nodeID].m_linVel * timeStep;
-		
+		//linear velocity
+		bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
+
 		//apply gravity
 		bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
-		
 	}
-	
 }
 
-inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
 {
 	float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
-	
-	if( (body->m_invMass != 0.f))
+
+	if ((body->m_invMass != 0.f))
 	{
 		//angular velocity
 		{
@@ -72,23 +67,23 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
 			body->m_angVel.x *= angularDamping;
 			body->m_angVel.y *= angularDamping;
 			body->m_angVel.z *= angularDamping;
-			
+
 			b3Float4 angvel = body->m_angVel;
 			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
 			//limit the angular motion
-			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
 			{
 				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
 			}
-			if(fAngle < 0.001f)
+			if (fAngle < 0.001f)
 			{
 				// use Taylor's expansions of sync function
-				axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+				axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
 			}
 			else
 			{
 				// sync(fAngle) = sin(c*fAngle)/t
-				axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
+				axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
 			}
 			b3Quat dorn;
 			dorn.x = axis.x;
@@ -99,15 +94,13 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
 
 			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
 			predictedOrn = b3QuatNormalized(predictedOrn);
-			body->m_quat=predictedOrn;
+			body->m_quat = predictedOrn;
 		}
 
 		//apply gravity
 		body->m_linVel += gravityAcceleration * timeStep;
 
-		//linear velocity		
-		body->m_pos +=  body->m_linVel * timeStep;
-		
+		//linear velocity
+		body->m_pos += body->m_linVel * timeStep;
 	}
-	
 }

thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h

@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef B3_AABB_UTIL2
 #define B3_AABB_UTIL2
 
@@ -21,20 +19,18 @@ subject to the following restrictions:
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3MinMax.h"
 
-
-
-B3_FORCE_INLINE void b3AabbExpand (b3Vector3& aabbMin,
-								   b3Vector3& aabbMax,
-								   const b3Vector3& expansionMin,
-								   const b3Vector3& expansionMax)
+B3_FORCE_INLINE void b3AabbExpand(b3Vector3& aabbMin,
+								  b3Vector3& aabbMax,
+								  const b3Vector3& expansionMin,
+								  const b3Vector3& expansionMax)
 {
 	aabbMin = aabbMin + expansionMin;
 	aabbMax = aabbMax + expansionMax;
 }
 
 /// conservative test for overlap between two aabbs
-B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
-								const b3Vector3 &point)
+B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
+											 const b3Vector3& point)
 {
 	bool overlap = true;
 	overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
@@ -43,10 +39,9 @@ B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3
 	return overlap;
 }
 
-
 /// conservative test for overlap between two aabbs
-B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1,
-								const b3Vector3 &aabbMin2, const b3Vector3 &aabbMax2)
+B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(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;
@@ -56,52 +51,49 @@ B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3V
 }
 
 /// conservative test for overlap between triangle and aabb
-B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3 *vertices,
-									const b3Vector3 &aabbMin, const b3Vector3 &aabbMax)
+B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3* vertices,
+												const b3Vector3& aabbMin, const b3Vector3& aabbMax)
 {
-	const b3Vector3 &p1 = vertices[0];
-	const b3Vector3 &p2 = vertices[1];
-	const b3Vector3 &p3 = vertices[2];
+	const b3Vector3& p1 = vertices[0];
+	const b3Vector3& p2 = vertices[1];
+	const b3Vector3& p3 = vertices[2];
 
 	if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
 	if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
 
 	if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
 	if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
-  
+
 	if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
 	if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
 	return true;
 }
 
-
-B3_FORCE_INLINE int	b3Outcode(const b3Vector3& p,const b3Vector3& halfExtent) 
+B3_FORCE_INLINE int b3Outcode(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);
+	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);
 }
 
-
-
 B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
-								  const b3Vector3& rayInvDirection,
-								  const unsigned int raySign[3],
-								  const b3Vector3 bounds[2],
-								  b3Scalar& tmin,
-								  b3Scalar lambda_min,
-								  b3Scalar lambda_max)
+								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();
+	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();
+	tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
 
-	if ( (tmin > tymax) || (tymin > tmax) )
+	if ((tmin > tymax) || (tymin > tmax))
 		return false;
 
 	if (tymin > tmin)
@@ -111,59 +103,59 @@ B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
 		tmax = tymax;
 
 	tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
-	tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
+	tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
 
-	if ( (tmin > tzmax) || (tzmin > tmax) )
+	if ((tmin > tzmax) || (tzmin > tmax))
 		return false;
 	if (tzmin > tmin)
 		tmin = tzmin;
 	if (tzmax < tmax)
 		tmax = tzmax;
-	return ( (tmin < lambda_max) && (tmax > lambda_min) );
+	return ((tmin < lambda_max) && (tmax > lambda_min));
 }
 
-B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom, 
-								 const b3Vector3& rayTo, 
-								 const b3Vector3& aabbMin, 
-								 const b3Vector3& aabbMax,
-					  b3Scalar& param, b3Vector3& normal) 
+B3_FORCE_INLINE bool b3RayAabb(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 = b3Outcode(source,aabbHalfExtent);
-	int targetOutcode = b3Outcode(target,aabbHalfExtent);
+	b3Vector3 aabbHalfExtent = (aabbMax - aabbMin) * b3Scalar(0.5);
+	b3Vector3 aabbCenter = (aabbMax + aabbMin) * b3Scalar(0.5);
+	b3Vector3 source = rayFrom - aabbCenter;
+	b3Vector3 target = rayTo - aabbCenter;
+	int sourceOutcode = b3Outcode(source, aabbHalfExtent);
+	int targetOutcode = b3Outcode(target, aabbHalfExtent);
 	if ((sourceOutcode & targetOutcode) == 0x0)
 	{
 		b3Scalar lambda_enter = b3Scalar(0.0);
-		b3Scalar lambda_exit  = param;
+		b3Scalar lambda_exit = param;
 		b3Vector3 r = target - source;
 		int i;
-		b3Scalar	normSign = 1;
-		b3Vector3	hitNormal = b3MakeVector3(0,0,0);
-		int bit=1;
+		b3Scalar normSign = 1;
+		b3Vector3 hitNormal = b3MakeVector3(0, 0, 0);
+		int bit = 1;
 
-		for (int j=0;j<2;j++)
+		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];
+					b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
 					if (lambda_enter <= lambda)
 					{
 						lambda_enter = lambda;
-						hitNormal.setValue(0,0,0);
+						hitNormal.setValue(0, 0, 0);
 						hitNormal[i] = normSign;
 					}
 				}
-				else if (targetOutcode & bit) 
+				else if (targetOutcode & bit)
 				{
-					b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+					b3Scalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
 					b3SetMin(lambda_exit, lambda);
 				}
-				bit<<=1;
+				bit <<= 1;
 			}
 			normSign = b3Scalar(-1.);
 		}
@@ -177,56 +169,49 @@ B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
 	return false;
 }
 
-
-
-B3_FORCE_INLINE	void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
+B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin, const b3Transform& t, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
 {
-	b3Vector3 halfExtentsWithMargin = halfExtents+b3MakeVector3(margin,margin,margin);
-	b3Matrix3x3 abs_b = t.getBasis().absolute();  
+	b3Vector3 halfExtentsWithMargin = halfExtents + b3MakeVector3(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] );
+	b3Vector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
 	aabbMinOut = center - extent;
 	aabbMaxOut = center + extent;
 }
 
-
-B3_FORCE_INLINE	void b3TransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
+B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin, const b3Vector3& localAabbMax, b3Scalar margin, const b3Transform& trans, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
 {
-		//b3Assert(localAabbMin.getX() <= localAabbMax.getX());
-		//b3Assert(localAabbMin.getY() <= localAabbMax.getY());
-		//b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
-		b3Vector3 localHalfExtents = b3Scalar(0.5)*(localAabbMax-localAabbMin);
-		localHalfExtents+=b3MakeVector3(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;
+	//b3Assert(localAabbMin.getX() <= localAabbMax.getX());
+	//b3Assert(localAabbMin.getY() <= localAabbMax.getY());
+	//b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
+	b3Vector3 localHalfExtents = b3Scalar(0.5) * (localAabbMax - localAabbMin);
+	localHalfExtents += b3MakeVector3(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 B3_USE_BANCHLESS 1
 #ifdef B3_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)
-	B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(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>(b3Select((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));
-	}
+//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)
+B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(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>(b3Select((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
-	B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(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 //B3_USE_BANCHLESS
-
-#endif //B3_AABB_UTIL2
-
+B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(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  //B3_USE_BANCHLESS
 
+#endif  //B3_AABB_UTIL2

thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h

@@ -23,58 +23,56 @@ subject to the following restrictions:
 /// 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:
-		b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
+		int next;
+		int reverse;
+		int targetVertex;
 
-	public:
+		friend class b3ConvexHullComputer;
 
-		class Edge
+	public:
+		int getSourceVertex() const
 		{
-			private:
-				int next;
-				int reverse;
-				int targetVertex;
-
-				friend class b3ConvexHullComputer;
-
-			public:
-				int getSourceVertex() const
-				{
-					return (this + reverse)->targetVertex;
-				}
-
-				int getTargetVertex() const
-				{
-					return targetVertex;
-				}
+			return (this + reverse)->targetVertex;
+		}
 
-				const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
-				{
-					return this + next;
-				}
+		int getTargetVertex() const
+		{
+			return targetVertex;
+		}
 
-				const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
-				{
-					return (this + reverse)->getNextEdgeOfVertex();
-				}
+		const Edge* getNextEdgeOfVertex() const  // clockwise list of all edges of a vertex
+		{
+			return this + next;
+		}
 
-				const Edge* getReverseEdge() const
-				{
-					return this + reverse;
-				}
-		};
+		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;
+	// Vertices of the output hull
+	b3AlignedObjectArray<b3Vector3> vertices;
 
-		// Edges of the output hull
-		b3AlignedObjectArray<Edge> edges;
+	// 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;
+	// 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).
@@ -86,18 +84,16 @@ class b3ConvexHullComputer
 
 		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);
-		}
+	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 //B3_CONVEX_HULL_COMPUTER_H
-
+#endif  //B3_CONVEX_HULL_COMPUTER_H

thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp

@@ -12,49 +12,43 @@ subject to the following restrictions:
 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 b3BulletMathProbe ();
+{
+	void b3BulletMathProbe();
 
-	void b3BulletMathProbe () {}
+	void b3BulletMathProbe() {}
 }
 
-
-bool	b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar	margin)
+bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
 {
 	int numbrushes = planeEquations.size();
-	for (int i=0;i<numbrushes;i++)
+	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.))
+		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)
+bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
 {
 	int numvertices = vertices.size();
-	for (int i=0;i<numvertices;i++)
+	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.))
+		b3Scalar dist = b3Scalar(planeNormal.dot(N1)) + b3Scalar(planeNormal[3]) - margin;
+		if (dist > b3Scalar(0.))
 		{
 			return false;
 		}
@@ -62,102 +56,98 @@ bool	b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const
 	return true;
 }
 
-bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray<b3Vector3>& planeEquations);
+bool notExist(const b3Vector3& planeEquation, const b3AlignedObjectArray<b3Vector3>& planeEquations);
 
-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++)
+	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 )
+void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut)
 {
-		const int numvertices = vertices.size();
+	const int numvertices = vertices.size();
 	// brute force:
-	for (int i=0;i<numvertices;i++)
+	for (int i = 0; i < numvertices; i++)
 	{
 		const b3Vector3& N1 = vertices[i];
-		
 
-		for (int j=i+1;j<numvertices;j++)
+		for (int j = i + 1; j < numvertices; j++)
 		{
 			const b3Vector3& N2 = vertices[j];
-				
-			for (int k=j+1;k<numvertices;k++)
-			{
 
+			for (int k = j + 1; k < numvertices; k++)
+			{
 				const b3Vector3& N3 = vertices[k];
 
-				b3Vector3 planeEquation,edge0,edge1;
-				edge0 = N2-N1;
-				edge1 = N3-N1;
+				b3Vector3 planeEquation, edge0, edge1;
+				edge0 = N2 - N1;
+				edge1 = N3 - N1;
 				b3Scalar normalSign = b3Scalar(1.);
-				for (int ww=0;ww<2;ww++)
+				for (int ww = 0; ww < 2; ww++)
 				{
 					planeEquation = normalSign * edge0.cross(edge1);
 					if (planeEquation.length2() > b3Scalar(0.0001))
 					{
 						planeEquation.normalize();
-						if (notExist(planeEquation,planeEquationsOut))
+						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);
-								}
+
+							//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 )
+void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut)
 {
 	const int numbrushes = planeEquations.size();
 	// brute force:
-	for (int i=0;i<numbrushes;i++)
+	for (int i = 0; i < numbrushes; i++)
 	{
 		const b3Vector3& N1 = planeEquations[i];
-		
 
-		for (int j=i+1;j<numbrushes;j++)
+		for (int j = i + 1; j < numbrushes; j++)
 		{
 			const b3Vector3& N2 = planeEquations[j];
-				
-			for (int k=j+1;k<numbrushes;k++)
-			{
 
+			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) ) )
+				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 )  
-
+					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
+					//P =  -------------------------------------------------------------------------
+					//   N1 . ( N2 * N3 )
 
 					b3Scalar quotient = (N1.dot(n2n3));
 					if (b3Fabs(quotient) > b3Scalar(0.000001))
@@ -172,7 +162,7 @@ void	b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
 						potentialVertex *= quotient;
 
 						//check if inside, and replace supportingVertexOut if needed
-						if (isPointInsidePlanes(planeEquations,potentialVertex,b3Scalar(0.01)))
+						if (isPointInsidePlanes(planeEquations, potentialVertex, b3Scalar(0.01)))
 						{
 							verticesOut.push_back(potentialVertex);
 						}
@@ -182,4 +172,3 @@ void	b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
 		}
 	}
 }
-

thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h

@@ -12,7 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef B3_GEOMETRY_UTIL_H
 #define B3_GEOMETRY_UTIL_H
 
@@ -22,21 +21,16 @@ subject to the following restrictions:
 ///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);
+public:
+	static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut);
 
-		static bool	areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar	margin);
+	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);
 
-#endif //B3_GEOMETRY_UTIL_H
+	static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
+};
 
+#endif  //B3_GEOMETRY_UTIL_H

thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h

@@ -13,41 +13,40 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
 #define B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
 
-
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3AlignedObjectArray.h"
 
 struct b3GrahamVector3 : public b3Vector3
 {
 	b3GrahamVector3(const b3Vector3& org, int orgIndex)
-		:b3Vector3(org),
-			m_orgIndex(orgIndex)
+		: b3Vector3(org),
+		  m_orgIndex(orgIndex)
 	{
 	}
-	b3Scalar	m_angle;
+	b3Scalar m_angle;
 	int m_orgIndex;
 };
 
-
-struct b3AngleCompareFunc {
+struct b3AngleCompareFunc
+{
 	b3Vector3 m_anchor;
 	b3AngleCompareFunc(const b3Vector3& anchor)
-	: m_anchor(anchor) 
+		: m_anchor(anchor)
 	{
 	}
-	bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const {
+	bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& 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();
+			b3Scalar al = (a - m_anchor).length2();
+			b3Scalar bl = (b - m_anchor).length2();
 			if (al != bl)
-				return  al < bl;
+				return al < bl;
 			else
 			{
 				return a.m_orgIndex < b.m_orgIndex;
@@ -58,60 +57,60 @@ struct b3AngleCompareFunc {
 
 inline void b3GrahamScanConvexHull2D(b3AlignedObjectArray<b3GrahamVector3>& originalPoints, b3AlignedObjectArray<b3GrahamVector3>& hull, const b3Vector3& normalAxis)
 {
-	b3Vector3 axis0,axis1;
-	b3PlaneSpace1(normalAxis,axis0,axis1);
-	
+	b3Vector3 axis0, axis1;
+	b3PlaneSpace1(normalAxis, axis0, axis1);
 
-	if (originalPoints.size()<=1)
+	if (originalPoints.size() <= 1)
 	{
-		for (int i=0;i<originalPoints.size();i++)
+		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++)
+	for (int i = 0; i < originalPoints.size(); i++)
 	{
-//		const b3Vector3& left = originalPoints[i];
-//		const b3Vector3& right = originalPoints[0];
+		//		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);
+			originalPoints.swap(0, i);
 		}
 	}
 
 	//also precompute angles
 	originalPoints[0].m_angle = -1e30f;
-	for (int i=1;i<originalPoints.size();i++)
+	for (int i = 1; i < originalPoints.size(); i++)
 	{
 		b3Vector3 xvec = axis0;
-		b3Vector3 ar = originalPoints[i]-originalPoints[0];
+		b3Vector3 ar = originalPoints[i] - originalPoints[0];
 		originalPoints[i].m_angle = b3Cross(xvec, ar).dot(normalAxis) / ar.length();
 	}
 
 	//step 2: sort all points, based on 'angle' with this anchor
 	b3AngleCompareFunc comp(originalPoints[0]);
-	originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
+	originalPoints.quickSortInternal(comp, 1, originalPoints.size() - 1);
 
 	int i;
-	for (i = 0; i<2; 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++) 
+	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 = b3Cross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
+		while (!isConvex && hull.size() > 1)
+		{
+			b3Vector3& a = hull[hull.size() - 2];
+			b3Vector3& b = hull[hull.size() - 1];
+			isConvex = b3Cross(a - b, a - originalPoints[i]).dot(normalAxis) > 0;
 			if (!isConvex)
 				hull.pop_back();
-			else 
+			else
 				hull.push_back(originalPoints[i]);
 		}
 	}
 }
 
-#endif //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
+#endif  //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h

@@ -12,33 +12,31 @@
 class b3GpuBroadphaseInterface
 {
 public:
-
-	typedef class b3GpuBroadphaseInterface* (CreateFunc)(cl_context ctx,cl_device_id device, cl_command_queue  q);
+	typedef class b3GpuBroadphaseInterface*(CreateFunc)(cl_context ctx, cl_device_id device, cl_command_queue q);
 
 	virtual ~b3GpuBroadphaseInterface()
 	{
 	}
 
-	virtual void createProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)=0;
-	virtual void createLargeProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)=0;
+	virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) = 0;
+	virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) = 0;
 
-	virtual void  calculateOverlappingPairs(int maxPairs)=0;
-	virtual void  calculateOverlappingPairsHost(int maxPairs)=0;
+	virtual void calculateOverlappingPairs(int maxPairs) = 0;
+	virtual void calculateOverlappingPairsHost(int maxPairs) = 0;
 
 	//call writeAabbsToGpu after done making all changes (createProxy etc)
-	virtual void writeAabbsToGpu()=0;
+	virtual void writeAabbsToGpu() = 0;
+
+	virtual cl_mem getAabbBufferWS() = 0;
+	virtual int getNumOverlap() = 0;
+	virtual cl_mem getOverlappingPairBuffer() = 0;
 
-	virtual cl_mem	getAabbBufferWS()=0;
-	virtual int	getNumOverlap()=0;
-	virtual cl_mem	getOverlappingPairBuffer()=0;
+	virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU() = 0;
+	virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU() = 0;
 
-	virtual b3OpenCLArray<b3SapAabb>&	getAllAabbsGPU()=0;
-	virtual b3AlignedObjectArray<b3SapAabb>&	getAllAabbsCPU()=0;
-	
 	virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU() = 0;
 	virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU() = 0;
 	virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU() = 0;
-
 };
 
-#endif //B3_GPU_BROADPHASE_INTERFACE_H
+#endif  //B3_GPU_BROADPHASE_INTERFACE_H

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp

@@ -5,12 +5,9 @@
 #include "kernels/sapKernels.h"
 //#include "kernels/gridBroadphase.cl"
 
-
 #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 
-
-
 #define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl"
 #define B3_GRID_BROADPHASE_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl"
 
@@ -21,31 +18,25 @@ cl_kernel kFindOverlappingPairs;
 cl_kernel m_copyAabbsKernel;
 cl_kernel m_sap2Kernel;
 
-
-
-
-
 //int maxPairsPerBody = 64;
-int maxBodiesPerCell = 256;//??
+int maxBodiesPerCell = 256;  //??
 
-b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx,cl_device_id device, cl_command_queue  q )
-:m_context(ctx),
-m_device(device),
-m_queue(q),
-m_allAabbsGPU1(ctx,q),
-m_smallAabbsMappingGPU(ctx,q),
-m_largeAabbsMappingGPU(ctx,q),
-m_gpuPairs(ctx,q),
+b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q)
+	: m_context(ctx),
+	  m_device(device),
+	  m_queue(q),
+	  m_allAabbsGPU1(ctx, q),
+	  m_smallAabbsMappingGPU(ctx, q),
+	  m_largeAabbsMappingGPU(ctx, q),
+	  m_gpuPairs(ctx, q),
 
-m_hashGpu(ctx,q),
+	  m_hashGpu(ctx, q),
 
-m_cellStartGpu(ctx,q),
-m_paramsGPU(ctx,q)
+	  m_cellStartGpu(ctx, q),
+	  m_paramsGPU(ctx, q)
 {
-
-	
-	b3Vector3 gridSize = b3MakeVector3(3,3,3);
-	b3Vector3 invGridSize = b3MakeVector3(1.f/gridSize[0],1.f/gridSize[1],1.f/gridSize[2]);
+	b3Vector3 gridSize = b3MakeVector3(3, 3, 3);
+	b3Vector3 invGridSize = b3MakeVector3(1.f / gridSize[0], 1.f / gridSize[1], 1.f / gridSize[2]);
 
 	m_paramsCPU.m_gridSize[0] = 128;
 	m_paramsCPU.m_gridSize[1] = 128;
@@ -58,92 +49,79 @@ m_paramsGPU(ctx,q)
 	m_paramsCPU.m_invCellSize[3] = 0.f;
 	m_paramsGPU.push_back(m_paramsCPU);
 
-	cl_int errNum=0;
+	cl_int errNum = 0;
 
 	{
 		const char* sapSrc = sapCL;
-		cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,sapSrc,&errNum,"",B3_BROADPHASE_SAP_PATH);
-		b3Assert(errNum==CL_SUCCESS);
-		m_copyAabbsKernel= b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "copyAabbsKernel",&errNum,sapProg );
-		m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelTwoArrays",&errNum,sapProg );
-		b3Assert(errNum==CL_SUCCESS);
+		cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, sapSrc, &errNum, "", B3_BROADPHASE_SAP_PATH);
+		b3Assert(errNum == CL_SUCCESS);
+		m_copyAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "copyAabbsKernel", &errNum, sapProg);
+		m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelTwoArrays", &errNum, sapProg);
+		b3Assert(errNum == CL_SUCCESS);
 	}
 
 	{
-		
-		cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,gridBroadphaseCL,&errNum,"",B3_GRID_BROADPHASE_PATH);
-		b3Assert(errNum==CL_SUCCESS);
-
-		kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kCalcHashAABB",&errNum,gridProg);
-		b3Assert(errNum==CL_SUCCESS);
-	
-		kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kClearCellStart",&errNum,gridProg);
-		b3Assert(errNum==CL_SUCCESS);
-
-		kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kFindCellStart",&errNum,gridProg);
-		b3Assert(errNum==CL_SUCCESS);
-
-	
-		kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kFindOverlappingPairs",&errNum,gridProg);
-		b3Assert(errNum==CL_SUCCESS);
-
-		
-		
-		
-	}
+		cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, gridBroadphaseCL, &errNum, "", B3_GRID_BROADPHASE_PATH);
+		b3Assert(errNum == CL_SUCCESS);
 
-	m_sorter = new b3RadixSort32CL(m_context,m_device,m_queue);
+		kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kCalcHashAABB", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
 
+		kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kClearCellStart", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
+
+		kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindCellStart", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
+
+		kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindOverlappingPairs", &errNum, gridProg);
+		b3Assert(errNum == CL_SUCCESS);
+	}
+
+	m_sorter = new b3RadixSort32CL(m_context, m_device, m_queue);
 }
 b3GpuGridBroadphase::~b3GpuGridBroadphase()
 {
-	clReleaseKernel( kCalcHashAABB);
-	clReleaseKernel( kClearCellStart);
-	clReleaseKernel( kFindCellStart);
-	clReleaseKernel( kFindOverlappingPairs);
-	clReleaseKernel( m_sap2Kernel);
-	clReleaseKernel( m_copyAabbsKernel);
-	
-	
-	
+	clReleaseKernel(kCalcHashAABB);
+	clReleaseKernel(kClearCellStart);
+	clReleaseKernel(kFindCellStart);
+	clReleaseKernel(kFindOverlappingPairs);
+	clReleaseKernel(m_sap2Kernel);
+	clReleaseKernel(m_copyAabbsKernel);
+
 	delete m_sorter;
 }
 
-
-
-void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)
+void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
 {
 	b3SapAabb aabb;
 	aabb.m_minVec = aabbMin;
 	aabb.m_maxVec = aabbMax;
 	aabb.m_minIndices[3] = userPtr;
-	aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();//NOT userPtr;
+	aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();  //NOT userPtr;
 	m_smallAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
 
 	m_allAabbsCPU1.push_back(aabb);
-
 }
-void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)
+void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
 {
 	b3SapAabb aabb;
 	aabb.m_minVec = aabbMin;
 	aabb.m_maxVec = aabbMax;
 	aabb.m_minIndices[3] = userPtr;
-	aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();//NOT userPtr;
+	aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();  //NOT userPtr;
 	m_largeAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
 
 	m_allAabbsCPU1.push_back(aabb);
 }
 
-void  b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
+void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
 {
 	B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs");
-	
 
 	if (0)
 	{
 		calculateOverlappingPairsHost(maxPairs);
-	/*
+		/*
 		b3AlignedObjectArray<b3Int4> cpuPairs;
 		m_gpuPairs.copyToHost(cpuPairs);
 		printf("host m_gpuPairs.size()=%d\n",m_gpuPairs.size());
@@ -154,57 +132,50 @@ void  b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
 		*/
 		return;
 	}
-	
-	
-
 
-	
 	int numSmallAabbs = m_smallAabbsMappingGPU.size();
 
-	b3OpenCLArray<int> pairCount(m_context,m_queue);
+	b3OpenCLArray<int> pairCount(m_context, m_queue);
 	pairCount.push_back(0);
-	m_gpuPairs.resize(maxPairs);//numSmallAabbs*maxPairsPerBody);
+	m_gpuPairs.resize(maxPairs);  //numSmallAabbs*maxPairsPerBody);
 
 	{
 		int numLargeAabbs = m_largeAabbsMappingGPU.size();
 		if (numLargeAabbs && numSmallAabbs)
 		{
 			B3_PROFILE("sap2Kernel");
-			b3BufferInfoCL bInfo[] = { 
-				b3BufferInfoCL( m_allAabbsGPU1.getBufferCL() ),
-				b3BufferInfoCL( m_largeAabbsMappingGPU.getBufferCL() ),
-				b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL() ), 
-				b3BufferInfoCL( m_gpuPairs.getBufferCL() ), 
+			b3BufferInfoCL bInfo[] = {
+				b3BufferInfoCL(m_allAabbsGPU1.getBufferCL()),
+				b3BufferInfoCL(m_largeAabbsMappingGPU.getBufferCL()),
+				b3BufferInfoCL(m_smallAabbsMappingGPU.getBufferCL()),
+				b3BufferInfoCL(m_gpuPairs.getBufferCL()),
 				b3BufferInfoCL(pairCount.getBufferCL())};
-			b3LauncherCL launcher(m_queue, m_sap2Kernel,"m_sap2Kernel");
-			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
-			launcher.setConst(   numLargeAabbs  );
-			launcher.setConst( numSmallAabbs);
-			launcher.setConst( 0  );//axis is not used
-			launcher.setConst( maxPairs  );
-	//@todo: use actual maximum work item sizes of the device instead of hardcoded values
-			launcher.launch2D( numLargeAabbs, numSmallAabbs,4,64);
-                
+			b3LauncherCL launcher(m_queue, m_sap2Kernel, "m_sap2Kernel");
+			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
+			launcher.setConst(numLargeAabbs);
+			launcher.setConst(numSmallAabbs);
+			launcher.setConst(0);  //axis is not used
+			launcher.setConst(maxPairs);
+			//@todo: use actual maximum work item sizes of the device instead of hardcoded values
+			launcher.launch2D(numLargeAabbs, numSmallAabbs, 4, 64);
+
 			int numPairs = pairCount.at(0);
-			
-			if (numPairs >maxPairs)
+
+			if (numPairs > maxPairs)
 			{
 				b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
-				numPairs =maxPairs;
+				numPairs = maxPairs;
 			}
 		}
 	}
 
-
-
-
 	if (numSmallAabbs)
 	{
 		B3_PROFILE("gridKernel");
 		m_hashGpu.resize(numSmallAabbs);
 		{
 			B3_PROFILE("kCalcHashAABB");
-			b3LauncherCL launch(m_queue,kCalcHashAABB,"kCalcHashAABB");
+			b3LauncherCL launch(m_queue, kCalcHashAABB, "kCalcHashAABB");
 			launch.setConst(numSmallAabbs);
 			launch.setBuffer(m_allAabbsGPU1.getBufferCL());
 			launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
@@ -214,117 +185,104 @@ void  b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
 		}
 
 		m_sorter->execute(m_hashGpu);
-		
-		int numCells = this->m_paramsCPU.m_gridSize[0]*this->m_paramsCPU.m_gridSize[1]*this->m_paramsCPU.m_gridSize[2];
+
+		int numCells = this->m_paramsCPU.m_gridSize[0] * this->m_paramsCPU.m_gridSize[1] * this->m_paramsCPU.m_gridSize[2];
 		m_cellStartGpu.resize(numCells);
 		//b3AlignedObjectArray<int >			cellStartCpu;
-				
-		
+
 		{
 			B3_PROFILE("kClearCellStart");
-			b3LauncherCL launch(m_queue,kClearCellStart,"kClearCellStart");
+			b3LauncherCL launch(m_queue, kClearCellStart, "kClearCellStart");
 			launch.setConst(numCells);
 			launch.setBuffer(m_cellStartGpu.getBufferCL());
 			launch.launch1D(numCells);
 			//m_cellStartGpu.copyToHost(cellStartCpu);
 			//printf("??\n");
-
 		}
 
-
 		{
 			B3_PROFILE("kFindCellStart");
-			b3LauncherCL launch(m_queue,kFindCellStart,"kFindCellStart");
+			b3LauncherCL launch(m_queue, kFindCellStart, "kFindCellStart");
 			launch.setConst(numSmallAabbs);
 			launch.setBuffer(m_hashGpu.getBufferCL());
 			launch.setBuffer(m_cellStartGpu.getBufferCL());
 			launch.launch1D(numSmallAabbs);
 			//m_cellStartGpu.copyToHost(cellStartCpu);
 			//printf("??\n");
-
 		}
-		
+
 		{
 			B3_PROFILE("kFindOverlappingPairs");
-			
-			
-			b3LauncherCL launch(m_queue,kFindOverlappingPairs,"kFindOverlappingPairs");
+
+			b3LauncherCL launch(m_queue, kFindOverlappingPairs, "kFindOverlappingPairs");
 			launch.setConst(numSmallAabbs);
 			launch.setBuffer(m_allAabbsGPU1.getBufferCL());
 			launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
 			launch.setBuffer(m_hashGpu.getBufferCL());
 			launch.setBuffer(m_cellStartGpu.getBufferCL());
-			
+
 			launch.setBuffer(m_paramsGPU.getBufferCL());
 			//launch.setBuffer(0);
 			launch.setBuffer(pairCount.getBufferCL());
 			launch.setBuffer(m_gpuPairs.getBufferCL());
-			
+
 			launch.setConst(maxPairs);
 			launch.launch1D(numSmallAabbs);
-			
 
 			int numPairs = pairCount.at(0);
-			if (numPairs >maxPairs)
+			if (numPairs > maxPairs)
 			{
 				b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
-				numPairs =maxPairs;
+				numPairs = maxPairs;
 			}
-			
+
 			m_gpuPairs.resize(numPairs);
-	
+
 			if (0)
 			{
 				b3AlignedObjectArray<b3Int4> pairsCpu;
 				m_gpuPairs.copyToHost(pairsCpu);
 
 				int sz = m_gpuPairs.size();
-				printf("m_gpuPairs.size()=%d\n",sz);
-				for (int i=0;i<m_gpuPairs.size();i++)
+				printf("m_gpuPairs.size()=%d\n", sz);
+				for (int i = 0; i < m_gpuPairs.size(); i++)
 				{
-					printf("pair %d = %d,%d\n",i,pairsCpu[i].x,pairsCpu[i].y);
+					printf("pair %d = %d,%d\n", i, pairsCpu[i].x, pairsCpu[i].y);
 				}
 
 				printf("?!?\n");
 			}
-			
 		}
-	
-
 	}
 
-	
-
-
-
 	//calculateOverlappingPairsHost(maxPairs);
 }
-void  b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
+void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
 {
-
 	m_hostPairs.resize(0);
 	m_allAabbsGPU1.copyToHost(m_allAabbsCPU1);
-	for (int i=0;i<m_allAabbsCPU1.size();i++)
+	for (int i = 0; i < m_allAabbsCPU1.size(); i++)
 	{
-		for (int j=i+1;j<m_allAabbsCPU1.size();j++)
+		for (int j = i + 1; j < m_allAabbsCPU1.size(); j++)
 		{
 			if (b3TestAabbAgainstAabb2(m_allAabbsCPU1[i].m_minVec, m_allAabbsCPU1[i].m_maxVec,
-				m_allAabbsCPU1[j].m_minVec,m_allAabbsCPU1[j].m_maxVec))
+									   m_allAabbsCPU1[j].m_minVec, m_allAabbsCPU1[j].m_maxVec))
 			{
 				b3Int4 pair;
 				int a = m_allAabbsCPU1[j].m_minIndices[3];
 				int b = m_allAabbsCPU1[i].m_minIndices[3];
-				if (a<=b)
+				if (a <= b)
 				{
-					pair.x = a; 
-					pair.y = b;//store the original index in the unsorted aabb array
-				} else
+					pair.x = a;
+					pair.y = b;  //store the original index in the unsorted aabb array
+				}
+				else
 				{
 					pair.x = b;
-					pair.y = a;//store the original index in the unsorted aabb array
+					pair.y = a;  //store the original index in the unsorted aabb array
 				}
-					
-				if (m_hostPairs.size()<maxPairs)
+
+				if (m_hostPairs.size() < maxPairs)
 				{
 					m_hostPairs.push_back(pair);
 				}
@@ -332,40 +290,36 @@ void  b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
 		}
 	}
 
-
 	m_gpuPairs.copyFromHost(m_hostPairs);
-
-
 }
 
-	//call writeAabbsToGpu after done making all changes (createProxy etc)
+//call writeAabbsToGpu after done making all changes (createProxy etc)
 void b3GpuGridBroadphase::writeAabbsToGpu()
 {
 	m_allAabbsGPU1.copyFromHost(m_allAabbsCPU1);
 	m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU);
 	m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU);
-
 }
 
-cl_mem	b3GpuGridBroadphase::getAabbBufferWS()
+cl_mem b3GpuGridBroadphase::getAabbBufferWS()
 {
 	return this->m_allAabbsGPU1.getBufferCL();
 }
-int	b3GpuGridBroadphase::getNumOverlap()
+int b3GpuGridBroadphase::getNumOverlap()
 {
 	return m_gpuPairs.size();
 }
-cl_mem	b3GpuGridBroadphase::getOverlappingPairBuffer()
+cl_mem b3GpuGridBroadphase::getOverlappingPairBuffer()
 {
 	return m_gpuPairs.getBufferCL();
 }
 
-b3OpenCLArray<b3SapAabb>&	b3GpuGridBroadphase::getAllAabbsGPU()
+b3OpenCLArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsGPU()
 {
 	return m_allAabbsGPU1;
 }
 
-b3AlignedObjectArray<b3SapAabb>&	b3GpuGridBroadphase::getAllAabbsCPU()
+b3AlignedObjectArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsCPU()
 {
 	return m_allAabbsCPU1;
 }
@@ -382,4 +336,3 @@ b3OpenCLArray<int>& b3GpuGridBroadphase::getLargeAabbIndicesGPU()
 {
 	return m_largeAabbsMappingGPU;
 }
-

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h

@@ -6,83 +6,75 @@
 
 struct b3ParamsGridBroadphaseCL
 {
-
 	float m_invCellSize[4];
-	int   m_gridSize[4];
+	int m_gridSize[4];
 
-	int	getMaxBodiesPerCell() const
+	int getMaxBodiesPerCell() const
 	{
 		return m_gridSize[3];
 	}
 
-	void setMaxBodiesPerCell(int maxOverlap) 
+	void setMaxBodiesPerCell(int maxOverlap)
 	{
 		m_gridSize[3] = maxOverlap;
 	}
 };
 
-
 class b3GpuGridBroadphase : public b3GpuBroadphaseInterface
 {
 protected:
-	cl_context				m_context;
-	cl_device_id			m_device;
-	cl_command_queue		m_queue;
+	cl_context m_context;
+	cl_device_id m_device;
+	cl_command_queue m_queue;
 
-	b3OpenCLArray<b3SapAabb>	m_allAabbsGPU1;
-	b3AlignedObjectArray<b3SapAabb>	m_allAabbsCPU1;
+	b3OpenCLArray<b3SapAabb> m_allAabbsGPU1;
+	b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU1;
 
-	b3OpenCLArray<int>	m_smallAabbsMappingGPU;
+	b3OpenCLArray<int> m_smallAabbsMappingGPU;
 	b3AlignedObjectArray<int> m_smallAabbsMappingCPU;
 
-	b3OpenCLArray<int>	m_largeAabbsMappingGPU;
+	b3OpenCLArray<int> m_largeAabbsMappingGPU;
 	b3AlignedObjectArray<int> m_largeAabbsMappingCPU;
 
 	b3AlignedObjectArray<b3Int4> m_hostPairs;
-	b3OpenCLArray<b3Int4>			m_gpuPairs;
+	b3OpenCLArray<b3Int4> m_gpuPairs;
 
-	b3OpenCLArray<b3SortData>			m_hashGpu;
-	b3OpenCLArray<int>			m_cellStartGpu;
-	
+	b3OpenCLArray<b3SortData> m_hashGpu;
+	b3OpenCLArray<int> m_cellStartGpu;
 
-	b3ParamsGridBroadphaseCL		m_paramsCPU;
-	b3OpenCLArray<b3ParamsGridBroadphaseCL>		m_paramsGPU;
+	b3ParamsGridBroadphaseCL m_paramsCPU;
+	b3OpenCLArray<b3ParamsGridBroadphaseCL> m_paramsGPU;
 
-	class b3RadixSort32CL*			m_sorter;
+	class b3RadixSort32CL* m_sorter;
 
 public:
-
-	b3GpuGridBroadphase(cl_context ctx,cl_device_id device, cl_command_queue  q );
+	b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q);
 	virtual ~b3GpuGridBroadphase();
 
-	static b3GpuBroadphaseInterface* CreateFunc(cl_context ctx,cl_device_id device, cl_command_queue  q)
+	static b3GpuBroadphaseInterface* CreateFunc(cl_context ctx, cl_device_id device, cl_command_queue q)
 	{
-		return new b3GpuGridBroadphase(ctx,device,q);
+		return new b3GpuGridBroadphase(ctx, device, q);
 	}
 
-	
-
+	virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
+	virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
 
-	virtual void createProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
-	virtual void createLargeProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
-
-	virtual void  calculateOverlappingPairs(int maxPairs);
-	virtual void  calculateOverlappingPairsHost(int maxPairs);
+	virtual void calculateOverlappingPairs(int maxPairs);
+	virtual void calculateOverlappingPairsHost(int maxPairs);
 
 	//call writeAabbsToGpu after done making all changes (createProxy etc)
 	virtual void writeAabbsToGpu();
 
-	virtual cl_mem	getAabbBufferWS();
-	virtual int	getNumOverlap();
-	virtual cl_mem	getOverlappingPairBuffer();
+	virtual cl_mem getAabbBufferWS();
+	virtual int getNumOverlap();
+	virtual cl_mem getOverlappingPairBuffer();
+
+	virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU();
+	virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU();
 
-	virtual b3OpenCLArray<b3SapAabb>&	getAllAabbsGPU();
-	virtual b3AlignedObjectArray<b3SapAabb>&	getAllAabbsCPU();
-	
 	virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU();
 	virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU();
 	virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU();
-
 };
 
-#endif //B3_GPU_GRID_BROADPHASE_H
+#endif  //B3_GPU_GRID_BROADPHASE_H

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp

@@ -16,177 +16,174 @@ subject to the following restrictions:
 
 #include "b3GpuParallelLinearBvh.h"
 
-b3GpuParallelLinearBvh::b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue) :
-	m_queue(queue),
-	m_radixSorter(context, device, queue),
-	
-	m_rootNodeIndex(context, queue),
-	m_maxDistanceFromRoot(context, queue),
-	m_temp(context, queue),
-	
-	m_internalNodeAabbs(context, queue),
-	m_internalNodeLeafIndexRanges(context, queue),
-	m_internalNodeChildNodes(context, queue),
-	m_internalNodeParentNodes(context, queue),
-	
-	m_commonPrefixes(context, queue),
-	m_commonPrefixLengths(context, queue),
-	m_distanceFromRoot(context, queue),
-	
-	m_leafNodeParentNodes(context, queue),
-	m_mortonCodesAndAabbIndicies(context, queue),
-	m_mergedAabb(context, queue),
-	m_leafNodeAabbs(context, queue),
-	
-	m_largeAabbs(context, queue)
+b3GpuParallelLinearBvh::b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue) : m_queue(queue),
+																												  m_radixSorter(context, device, queue),
+
+																												  m_rootNodeIndex(context, queue),
+																												  m_maxDistanceFromRoot(context, queue),
+																												  m_temp(context, queue),
+
+																												  m_internalNodeAabbs(context, queue),
+																												  m_internalNodeLeafIndexRanges(context, queue),
+																												  m_internalNodeChildNodes(context, queue),
+																												  m_internalNodeParentNodes(context, queue),
+
+																												  m_commonPrefixes(context, queue),
+																												  m_commonPrefixLengths(context, queue),
+																												  m_distanceFromRoot(context, queue),
+
+																												  m_leafNodeParentNodes(context, queue),
+																												  m_mortonCodesAndAabbIndicies(context, queue),
+																												  m_mergedAabb(context, queue),
+																												  m_leafNodeAabbs(context, queue),
+
+																												  m_largeAabbs(context, queue)
 {
 	m_rootNodeIndex.resize(1);
 	m_maxDistanceFromRoot.resize(1);
 	m_temp.resize(1);
-	
+
 	//
 	const char CL_PROGRAM_PATH[] = "src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl";
-	
-	const char* kernelSource = parallelLinearBvhCL;	//parallelLinearBvhCL.h
+
+	const char* kernelSource = parallelLinearBvhCL;  //parallelLinearBvhCL.h
 	cl_int error;
 	char* additionalMacros = 0;
 	m_parallelLinearBvhProgram = b3OpenCLUtils::compileCLProgramFromString(context, device, kernelSource, &error, additionalMacros, CL_PROGRAM_PATH);
 	b3Assert(m_parallelLinearBvhProgram);
-	
-	m_separateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "separateAabbs", &error, m_parallelLinearBvhProgram, additionalMacros );
+
+	m_separateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "separateAabbs", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_separateAabbsKernel);
-	m_findAllNodesMergedAabbKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findAllNodesMergedAabb", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_findAllNodesMergedAabbKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "findAllNodesMergedAabb", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_findAllNodesMergedAabbKernel);
-	m_assignMortonCodesAndAabbIndiciesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "assignMortonCodesAndAabbIndicies", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_assignMortonCodesAndAabbIndiciesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "assignMortonCodesAndAabbIndicies", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_assignMortonCodesAndAabbIndiciesKernel);
-	
-	m_computeAdjacentPairCommonPrefixKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "computeAdjacentPairCommonPrefix", &error, m_parallelLinearBvhProgram, additionalMacros );
+
+	m_computeAdjacentPairCommonPrefixKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "computeAdjacentPairCommonPrefix", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_computeAdjacentPairCommonPrefixKernel);
-	m_buildBinaryRadixTreeLeafNodesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeLeafNodes", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_buildBinaryRadixTreeLeafNodesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "buildBinaryRadixTreeLeafNodes", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_buildBinaryRadixTreeLeafNodesKernel);
-	m_buildBinaryRadixTreeInternalNodesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeInternalNodes", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_buildBinaryRadixTreeInternalNodesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "buildBinaryRadixTreeInternalNodes", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_buildBinaryRadixTreeInternalNodesKernel);
-	m_findDistanceFromRootKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findDistanceFromRoot", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_findDistanceFromRootKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "findDistanceFromRoot", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_findDistanceFromRootKernel);
-	m_buildBinaryRadixTreeAabbsRecursiveKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeAabbsRecursive", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_buildBinaryRadixTreeAabbsRecursiveKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "buildBinaryRadixTreeAabbsRecursive", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_buildBinaryRadixTreeAabbsRecursiveKernel);
-	
-	m_findLeafIndexRangesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findLeafIndexRanges", &error, m_parallelLinearBvhProgram, additionalMacros );
+
+	m_findLeafIndexRangesKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "findLeafIndexRanges", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_findLeafIndexRangesKernel);
-	
-	m_plbvhCalculateOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhCalculateOverlappingPairs", &error, m_parallelLinearBvhProgram, additionalMacros );
+
+	m_plbvhCalculateOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhCalculateOverlappingPairs", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_plbvhCalculateOverlappingPairsKernel);
-	m_plbvhRayTraverseKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhRayTraverse", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_plbvhRayTraverseKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhRayTraverse", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_plbvhRayTraverseKernel);
-	m_plbvhLargeAabbAabbTestKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhLargeAabbAabbTest", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_plbvhLargeAabbAabbTestKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhLargeAabbAabbTest", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_plbvhLargeAabbAabbTestKernel);
-	m_plbvhLargeAabbRayTestKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhLargeAabbRayTest", &error, m_parallelLinearBvhProgram, additionalMacros );
+	m_plbvhLargeAabbRayTestKernel = b3OpenCLUtils::compileCLKernelFromString(context, device, kernelSource, "plbvhLargeAabbRayTest", &error, m_parallelLinearBvhProgram, additionalMacros);
 	b3Assert(m_plbvhLargeAabbRayTestKernel);
 }
 
-b3GpuParallelLinearBvh::~b3GpuParallelLinearBvh() 
+b3GpuParallelLinearBvh::~b3GpuParallelLinearBvh()
 {
 	clReleaseKernel(m_separateAabbsKernel);
 	clReleaseKernel(m_findAllNodesMergedAabbKernel);
 	clReleaseKernel(m_assignMortonCodesAndAabbIndiciesKernel);
-	
+
 	clReleaseKernel(m_computeAdjacentPairCommonPrefixKernel);
 	clReleaseKernel(m_buildBinaryRadixTreeLeafNodesKernel);
 	clReleaseKernel(m_buildBinaryRadixTreeInternalNodesKernel);
 	clReleaseKernel(m_findDistanceFromRootKernel);
 	clReleaseKernel(m_buildBinaryRadixTreeAabbsRecursiveKernel);
-	
+
 	clReleaseKernel(m_findLeafIndexRangesKernel);
-	
+
 	clReleaseKernel(m_plbvhCalculateOverlappingPairsKernel);
 	clReleaseKernel(m_plbvhRayTraverseKernel);
 	clReleaseKernel(m_plbvhLargeAabbAabbTestKernel);
 	clReleaseKernel(m_plbvhLargeAabbRayTestKernel);
-	
+
 	clReleaseProgram(m_parallelLinearBvhProgram);
 }
 
-void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices, 
-									const b3OpenCLArray<int>& largeAabbIndices)
+void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices,
+								   const b3OpenCLArray<int>& largeAabbIndices)
 {
 	B3_PROFILE("b3ParallelLinearBvh::build()");
-	
+
 	int numLargeAabbs = largeAabbIndices.size();
 	int numSmallAabbs = smallAabbIndices.size();
-	
-	//Since all AABBs(both large and small) are input as a contiguous array, 
+
+	//Since all AABBs(both large and small) are input as a contiguous array,
 	//with 2 additional arrays used to indicate the indices of large and small AABBs,
 	//it is necessary to separate the AABBs so that the large AABBs will not degrade the quality of the BVH.
 	{
 		B3_PROFILE("Separate large and small AABBs");
-		
+
 		m_largeAabbs.resize(numLargeAabbs);
 		m_leafNodeAabbs.resize(numSmallAabbs);
-		
+
 		//Write large AABBs into m_largeAabbs
 		{
-			b3BufferInfoCL bufferInfo[] = 
-			{
-				b3BufferInfoCL( worldSpaceAabbs.getBufferCL() ),
-				b3BufferInfoCL( largeAabbIndices.getBufferCL() ),
-				
-				b3BufferInfoCL( m_largeAabbs.getBufferCL() )
-			};
-			
+			b3BufferInfoCL bufferInfo[] =
+				{
+					b3BufferInfoCL(worldSpaceAabbs.getBufferCL()),
+					b3BufferInfoCL(largeAabbIndices.getBufferCL()),
+
+					b3BufferInfoCL(m_largeAabbs.getBufferCL())};
+
 			b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel");
-			launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+			launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 			launcher.setConst(numLargeAabbs);
-			
+
 			launcher.launch1D(numLargeAabbs);
 		}
-		
+
 		//Write small AABBs into m_leafNodeAabbs
 		{
-			b3BufferInfoCL bufferInfo[] = 
-			{
-				b3BufferInfoCL( worldSpaceAabbs.getBufferCL() ),
-				b3BufferInfoCL( smallAabbIndices.getBufferCL() ),
-				
-				b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() )
-			};
-			
+			b3BufferInfoCL bufferInfo[] =
+				{
+					b3BufferInfoCL(worldSpaceAabbs.getBufferCL()),
+					b3BufferInfoCL(smallAabbIndices.getBufferCL()),
+
+					b3BufferInfoCL(m_leafNodeAabbs.getBufferCL())};
+
 			b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel");
-			launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+			launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 			launcher.setConst(numSmallAabbs);
-			
+
 			launcher.launch1D(numSmallAabbs);
 		}
-		
+
 		clFinish(m_queue);
 	}
-	
+
 	//
-	int numLeaves = numSmallAabbs;	//Number of leaves in the BVH == Number of rigid bodies with small AABBs
+	int numLeaves = numSmallAabbs;  //Number of leaves in the BVH == Number of rigid bodies with small AABBs
 	int numInternalNodes = numLeaves - 1;
-	
-	if(numLeaves < 2)
+
+	if (numLeaves < 2)
 	{
 		//Number of leaf nodes is checked in calculateOverlappingPairs() and testRaysAgainstBvhAabbs(),
 		//so it does not matter if numLeaves == 0 and rootNodeIndex == -1
 		int rootNodeIndex = numLeaves - 1;
 		m_rootNodeIndex.copyFromHostPointer(&rootNodeIndex, 1);
-		
+
 		//Since the AABBs need to be rearranged(sorted) for the BVH construction algorithm,
 		//m_mortonCodesAndAabbIndicies.m_value is used to map a sorted AABB index to the unsorted AABB index
 		//instead of directly moving the AABBs. It needs to be set for the ray cast traversal kernel to work.
 		//( m_mortonCodesAndAabbIndicies[].m_value == unsorted index == index of m_leafNodeAabbs )
-		if(numLeaves == 1)
+		if (numLeaves == 1)
 		{
 			b3SortData leaf;
-			leaf.m_value = 0;		//1 leaf so index is always 0; leaf.m_key does not need to be set
-			
+			leaf.m_value = 0;  //1 leaf so index is always 0; leaf.m_key does not need to be set
+
 			m_mortonCodesAndAabbIndicies.resize(1);
 			m_mortonCodesAndAabbIndicies.copyFromHostPointer(&leaf, 1);
 		}
-		
+
 		return;
 	}
-	
+
 	//
 	{
 		m_internalNodeAabbs.resize(numInternalNodes);
@@ -197,37 +194,37 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
 		m_commonPrefixes.resize(numInternalNodes);
 		m_commonPrefixLengths.resize(numInternalNodes);
 		m_distanceFromRoot.resize(numInternalNodes);
-	
+
 		m_leafNodeParentNodes.resize(numLeaves);
 		m_mortonCodesAndAabbIndicies.resize(numLeaves);
 		m_mergedAabb.resize(numLeaves);
 	}
-	
-	//Find the merged AABB of all small AABBs; this is used to define the size of 
+
+	//Find the merged AABB of all small AABBs; this is used to define the size of
 	//each cell in the virtual grid for the next kernel(2^10 cells in each dimension).
 	{
 		B3_PROFILE("Find AABB of merged nodes");
-	
-		m_mergedAabb.copyFromOpenCLArray(m_leafNodeAabbs);	//Need to make a copy since the kernel modifies the array
-			
-		for(int numAabbsNeedingMerge = numLeaves; numAabbsNeedingMerge >= 2; 
-				numAabbsNeedingMerge = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2)
+
+		m_mergedAabb.copyFromOpenCLArray(m_leafNodeAabbs);  //Need to make a copy since the kernel modifies the array
+
+		for (int numAabbsNeedingMerge = numLeaves; numAabbsNeedingMerge >= 2;
+			 numAabbsNeedingMerge = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2)
 		{
-			b3BufferInfoCL bufferInfo[] = 
-			{
-				b3BufferInfoCL( m_mergedAabb.getBufferCL() )		//Resulting AABB is stored in m_mergedAabb[0]
-			};
-			
+			b3BufferInfoCL bufferInfo[] =
+				{
+					b3BufferInfoCL(m_mergedAabb.getBufferCL())  //Resulting AABB is stored in m_mergedAabb[0]
+				};
+
 			b3LauncherCL launcher(m_queue, m_findAllNodesMergedAabbKernel, "m_findAllNodesMergedAabbKernel");
-			launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+			launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 			launcher.setConst(numAabbsNeedingMerge);
-			
+
 			launcher.launch1D(numAabbsNeedingMerge);
 		}
-		
+
 		clFinish(m_queue);
 	}
-	
+
 	//Insert the center of the AABBs into a virtual grid,
 	//then convert the discrete grid coordinates into a morton code
 	//For each element in m_mortonCodesAndAabbIndicies, set
@@ -235,34 +232,32 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
 	//	m_value == small AABB index
 	{
 		B3_PROFILE("Assign morton codes");
-	
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
-			b3BufferInfoCL( m_mergedAabb.getBufferCL() ),
-			b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
+				b3BufferInfoCL(m_mergedAabb.getBufferCL()),
+				b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_assignMortonCodesAndAabbIndiciesKernel, "m_assignMortonCodesAndAabbIndiciesKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(numLeaves);
-		
+
 		launcher.launch1D(numLeaves);
 		clFinish(m_queue);
 	}
-	
+
 	//
 	{
 		B3_PROFILE("Sort leaves by morton codes");
-	
+
 		m_radixSorter.execute(m_mortonCodesAndAabbIndicies);
 		clFinish(m_queue);
 	}
-	
+
 	//
 	constructBinaryRadixTree();
-	
-	
+
 	//Since it is a sorted binary radix tree, each internal node contains a contiguous subset of leaf node indices.
 	//The root node contains leaf node indices in the range [0, numLeafNodes - 1].
 	//The child nodes of each node split their parent's index range into 2 contiguous halves.
@@ -273,17 +268,16 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
 	//This property can be used for optimizing calculateOverlappingPairs(), to avoid testing each AABB pair twice
 	{
 		B3_PROFILE("m_findLeafIndexRangesKernel");
-	
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeLeafIndexRanges.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_findLeafIndexRangesKernel, "m_findLeafIndexRangesKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(numInternalNodes);
-		
+
 		launcher.launch1D(numInternalNodes);
 		clFinish(m_queue);
 	}
@@ -293,285 +287,271 @@ void b3GpuParallelLinearBvh::calculateOverlappingPairs(b3OpenCLArray<b3Int4>& ou
 {
 	int maxPairs = out_overlappingPairs.size();
 	b3OpenCLArray<int>& numPairsGpu = m_temp;
-	
+
 	int reset = 0;
 	numPairsGpu.copyFromHostPointer(&reset, 1);
-	
+
 	//
-	if( m_leafNodeAabbs.size() > 1 )
+	if (m_leafNodeAabbs.size() > 1)
 	{
 		B3_PROFILE("PLBVH small-small AABB test");
-	
+
 		int numQueryAabbs = m_leafNodeAabbs.size();
-		
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
-			
-			b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() ),
-			b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
-			
-			b3BufferInfoCL( numPairsGpu.getBufferCL() ),
-			b3BufferInfoCL( out_overlappingPairs.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
+
+				b3BufferInfoCL(m_rootNodeIndex.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeAabbs.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeLeafIndexRanges.getBufferCL()),
+				b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
+
+				b3BufferInfoCL(numPairsGpu.getBufferCL()),
+				b3BufferInfoCL(out_overlappingPairs.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_plbvhCalculateOverlappingPairsKernel, "m_plbvhCalculateOverlappingPairsKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(maxPairs);
 		launcher.setConst(numQueryAabbs);
-		
+
 		launcher.launch1D(numQueryAabbs);
 		clFinish(m_queue);
 	}
-	
+
 	int numLargeAabbRigids = m_largeAabbs.size();
-	if( numLargeAabbRigids > 0 && m_leafNodeAabbs.size() > 0 )
+	if (numLargeAabbRigids > 0 && m_leafNodeAabbs.size() > 0)
 	{
 		B3_PROFILE("PLBVH large-small AABB test");
-	
+
 		int numQueryAabbs = m_leafNodeAabbs.size();
-		
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
-			b3BufferInfoCL( m_largeAabbs.getBufferCL() ),
-			
-			b3BufferInfoCL( numPairsGpu.getBufferCL() ),
-			b3BufferInfoCL( out_overlappingPairs.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
+				b3BufferInfoCL(m_largeAabbs.getBufferCL()),
+
+				b3BufferInfoCL(numPairsGpu.getBufferCL()),
+				b3BufferInfoCL(out_overlappingPairs.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_plbvhLargeAabbAabbTestKernel, "m_plbvhLargeAabbAabbTestKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(maxPairs);
 		launcher.setConst(numLargeAabbRigids);
 		launcher.setConst(numQueryAabbs);
-		
+
 		launcher.launch1D(numQueryAabbs);
 		clFinish(m_queue);
 	}
-	
-	
+
 	//
 	int numPairs = -1;
 	numPairsGpu.copyToHostPointer(&numPairs, 1);
-	if(numPairs > maxPairs)
+	if (numPairs > maxPairs)
 	{
 		b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
 		numPairs = maxPairs;
 		numPairsGpu.copyFromHostPointer(&maxPairs, 1);
 	}
-	
+
 	out_overlappingPairs.resize(numPairs);
 }
 
-
-void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays, 
-							b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs)
+void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays,
+													 b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs)
 {
 	B3_PROFILE("PLBVH testRaysAgainstBvhAabbs()");
-	
+
 	int numRays = rays.size();
 	int maxRayRigidPairs = out_rayRigidPairs.size();
-	
+
 	int reset = 0;
 	out_numRayRigidPairs.copyFromHostPointer(&reset, 1);
-	
+
 	//
-	if( m_leafNodeAabbs.size() > 0 )
+	if (m_leafNodeAabbs.size() > 0)
 	{
 		B3_PROFILE("PLBVH ray test small AABB");
-	
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
-			
-			b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() ),
-			b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
-			
-			b3BufferInfoCL( rays.getBufferCL() ),
-			
-			b3BufferInfoCL( out_numRayRigidPairs.getBufferCL() ),
-			b3BufferInfoCL( out_rayRigidPairs.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
+
+				b3BufferInfoCL(m_rootNodeIndex.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeAabbs.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeLeafIndexRanges.getBufferCL()),
+				b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
+
+				b3BufferInfoCL(rays.getBufferCL()),
+
+				b3BufferInfoCL(out_numRayRigidPairs.getBufferCL()),
+				b3BufferInfoCL(out_rayRigidPairs.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_plbvhRayTraverseKernel, "m_plbvhRayTraverseKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(maxRayRigidPairs);
 		launcher.setConst(numRays);
-		
+
 		launcher.launch1D(numRays);
 		clFinish(m_queue);
 	}
-	
+
 	int numLargeAabbRigids = m_largeAabbs.size();
-	if(numLargeAabbRigids > 0)
+	if (numLargeAabbRigids > 0)
 	{
 		B3_PROFILE("PLBVH ray test large AABB");
-		
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_largeAabbs.getBufferCL() ),
-			b3BufferInfoCL( rays.getBufferCL() ),
-			
-			b3BufferInfoCL( out_numRayRigidPairs.getBufferCL() ),
-			b3BufferInfoCL( out_rayRigidPairs.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_largeAabbs.getBufferCL()),
+				b3BufferInfoCL(rays.getBufferCL()),
+
+				b3BufferInfoCL(out_numRayRigidPairs.getBufferCL()),
+				b3BufferInfoCL(out_rayRigidPairs.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_plbvhLargeAabbRayTestKernel, "m_plbvhLargeAabbRayTestKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(numLargeAabbRigids);
 		launcher.setConst(maxRayRigidPairs);
 		launcher.setConst(numRays);
-		
+
 		launcher.launch1D(numRays);
 		clFinish(m_queue);
 	}
-	
+
 	//
 	int numRayRigidPairs = -1;
 	out_numRayRigidPairs.copyToHostPointer(&numRayRigidPairs, 1);
-	
-	if(numRayRigidPairs > maxRayRigidPairs)
+
+	if (numRayRigidPairs > maxRayRigidPairs)
 		b3Error("Error running out of rayRigid pairs: numRayRigidPairs = %d, maxRayRigidPairs = %d.\n", numRayRigidPairs, maxRayRigidPairs);
-	
 }
 
 void b3GpuParallelLinearBvh::constructBinaryRadixTree()
 {
 	B3_PROFILE("b3GpuParallelLinearBvh::constructBinaryRadixTree()");
-	
+
 	int numLeaves = m_leafNodeAabbs.size();
 	int numInternalNodes = numLeaves - 1;
-	
+
 	//Each internal node is placed in between 2 leaf nodes.
 	//By using this arrangement and computing the common prefix between
 	//these 2 adjacent leaf nodes, it is possible to quickly construct a binary radix tree.
 	{
 		B3_PROFILE("m_computeAdjacentPairCommonPrefixKernel");
-		
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
-			b3BufferInfoCL( m_commonPrefixes.getBufferCL() ),
-			b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
+				b3BufferInfoCL(m_commonPrefixes.getBufferCL()),
+				b3BufferInfoCL(m_commonPrefixLengths.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_computeAdjacentPairCommonPrefixKernel, "m_computeAdjacentPairCommonPrefixKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(numInternalNodes);
-		
+
 		launcher.launch1D(numInternalNodes);
 		clFinish(m_queue);
 	}
-	
-	//For each leaf node, select its parent node by 
+
+	//For each leaf node, select its parent node by
 	//comparing the 2 nearest internal nodes and assign child node indices
 	{
 		B3_PROFILE("m_buildBinaryRadixTreeLeafNodesKernel");
-		
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() ),
-			b3BufferInfoCL( m_leafNodeParentNodes.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_commonPrefixLengths.getBufferCL()),
+				b3BufferInfoCL(m_leafNodeParentNodes.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeLeafNodesKernel, "m_buildBinaryRadixTreeLeafNodesKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(numLeaves);
-		
+
 		launcher.launch1D(numLeaves);
 		clFinish(m_queue);
 	}
-	
+
 	//For each internal node, perform 2 binary searches among the other internal nodes
 	//to its left and right to find its potential parent nodes and assign child node indices
 	{
 		B3_PROFILE("m_buildBinaryRadixTreeInternalNodesKernel");
-		
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_commonPrefixes.getBufferCL() ),
-			b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeParentNodes.getBufferCL() ),
-			b3BufferInfoCL( m_rootNodeIndex.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_commonPrefixes.getBufferCL()),
+				b3BufferInfoCL(m_commonPrefixLengths.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeParentNodes.getBufferCL()),
+				b3BufferInfoCL(m_rootNodeIndex.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeInternalNodesKernel, "m_buildBinaryRadixTreeInternalNodesKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(numInternalNodes);
-		
+
 		launcher.launch1D(numInternalNodes);
 		clFinish(m_queue);
 	}
-	
+
 	//Find the number of nodes seperating each internal node and the root node
 	//so that the AABBs can be set using the next kernel.
 	//Also determine the maximum number of nodes separating an internal node and the root node.
 	{
 		B3_PROFILE("m_findDistanceFromRootKernel");
-	
-		b3BufferInfoCL bufferInfo[] = 
-		{
-			b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ),
-			b3BufferInfoCL( m_internalNodeParentNodes.getBufferCL() ),
-			b3BufferInfoCL( m_maxDistanceFromRoot.getBufferCL() ),
-			b3BufferInfoCL( m_distanceFromRoot.getBufferCL() )
-		};
-		
+
+		b3BufferInfoCL bufferInfo[] =
+			{
+				b3BufferInfoCL(m_rootNodeIndex.getBufferCL()),
+				b3BufferInfoCL(m_internalNodeParentNodes.getBufferCL()),
+				b3BufferInfoCL(m_maxDistanceFromRoot.getBufferCL()),
+				b3BufferInfoCL(m_distanceFromRoot.getBufferCL())};
+
 		b3LauncherCL launcher(m_queue, m_findDistanceFromRootKernel, "m_findDistanceFromRootKernel");
-		launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+		launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 		launcher.setConst(numInternalNodes);
-		
+
 		launcher.launch1D(numInternalNodes);
 		clFinish(m_queue);
 	}
-	
+
 	//Starting from the internal nodes nearest to the leaf nodes, recursively move up
 	//the tree towards the root to set the AABBs of each internal node; each internal node
 	//checks its children and merges their AABBs
 	{
 		B3_PROFILE("m_buildBinaryRadixTreeAabbsRecursiveKernel");
-		
+
 		int maxDistanceFromRoot = -1;
 		{
 			B3_PROFILE("copy maxDistanceFromRoot to CPU");
 			m_maxDistanceFromRoot.copyToHostPointer(&maxDistanceFromRoot, 1);
 			clFinish(m_queue);
 		}
-		
-		for(int distanceFromRoot = maxDistanceFromRoot; distanceFromRoot >= 0; --distanceFromRoot)
+
+		for (int distanceFromRoot = maxDistanceFromRoot; distanceFromRoot >= 0; --distanceFromRoot)
 		{
-			b3BufferInfoCL bufferInfo[] = 
-			{
-				b3BufferInfoCL( m_distanceFromRoot.getBufferCL() ),
-				b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ),
-				b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ),
-				b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ),
-				b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() )
-			};
-			
+			b3BufferInfoCL bufferInfo[] =
+				{
+					b3BufferInfoCL(m_distanceFromRoot.getBufferCL()),
+					b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
+					b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
+					b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
+					b3BufferInfoCL(m_internalNodeAabbs.getBufferCL())};
+
 			b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeAabbsRecursiveKernel, "m_buildBinaryRadixTreeAabbsRecursiveKernel");
-			launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) );
+			launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
 			launcher.setConst(maxDistanceFromRoot);
 			launcher.setConst(distanceFromRoot);
 			launcher.setConst(numInternalNodes);
-			
+
 			//It may seem inefficent to launch a thread for each internal node when a
 			//much smaller number of nodes is actually processed, but this is actually
-			//faster than determining the exact nodes that are ready to merge their child AABBs. 
+			//faster than determining the exact nodes that are ready to merge their child AABBs.
 			launcher.launch1D(numInternalNodes);
 		}
-		
+
 		clFinish(m_queue);
 	}
 }
-
-	

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h

@@ -37,10 +37,10 @@ subject to the following restrictions:
 ///"Maximizing Parallelism in the Construction of BVHs, Octrees, and k-d trees" [Karras 2012] \n
 ///@par
 ///The basic algorithm for building the BVH as presented in [Lauterbach et al. 2009] consists of 4 stages:
-/// - [fully parallel] Assign morton codes for each AABB using its center (after quantizing the AABB centers into a virtual grid) 
+/// - [fully parallel] Assign morton codes for each AABB using its center (after quantizing the AABB centers into a virtual grid)
 /// - [fully parallel] Sort morton codes
-/// - [somewhat parallel] Build binary radix tree (assign parent/child pointers for internal nodes of the BVH) 
-/// - [somewhat parallel] Set internal node AABBs 
+/// - [somewhat parallel] Build binary radix tree (assign parent/child pointers for internal nodes of the BVH)
+/// - [somewhat parallel] Set internal node AABBs
 ///@par
 ///[Karras 2012] improves on the algorithm by introducing fully parallel methods for the last 2 stages.
 ///The BVH implementation here shares many concepts with [Karras 2012], but a different method is used for constructing the tree.
@@ -49,75 +49,75 @@ subject to the following restrictions:
 class b3GpuParallelLinearBvh
 {
 	cl_command_queue m_queue;
-	
+
 	cl_program m_parallelLinearBvhProgram;
-	
+
 	cl_kernel m_separateAabbsKernel;
 	cl_kernel m_findAllNodesMergedAabbKernel;
 	cl_kernel m_assignMortonCodesAndAabbIndiciesKernel;
-	
+
 	//Binary radix tree construction kernels
 	cl_kernel m_computeAdjacentPairCommonPrefixKernel;
 	cl_kernel m_buildBinaryRadixTreeLeafNodesKernel;
 	cl_kernel m_buildBinaryRadixTreeInternalNodesKernel;
 	cl_kernel m_findDistanceFromRootKernel;
 	cl_kernel m_buildBinaryRadixTreeAabbsRecursiveKernel;
-	
+
 	cl_kernel m_findLeafIndexRangesKernel;
-	
+
 	//Traversal kernels
 	cl_kernel m_plbvhCalculateOverlappingPairsKernel;
 	cl_kernel m_plbvhRayTraverseKernel;
 	cl_kernel m_plbvhLargeAabbAabbTestKernel;
 	cl_kernel m_plbvhLargeAabbRayTestKernel;
-	
+
 	b3RadixSort32CL m_radixSorter;
-	
+
 	//1 element
-	b3OpenCLArray<int> m_rootNodeIndex;							//Most significant bit(0x80000000) is set to indicate internal node
-	b3OpenCLArray<int> m_maxDistanceFromRoot;					//Max number of internal nodes between an internal node and the root node
-	b3OpenCLArray<int> m_temp;									//Used to hold the number of pairs in calculateOverlappingPairs()
-	
+	b3OpenCLArray<int> m_rootNodeIndex;        //Most significant bit(0x80000000) is set to indicate internal node
+	b3OpenCLArray<int> m_maxDistanceFromRoot;  //Max number of internal nodes between an internal node and the root node
+	b3OpenCLArray<int> m_temp;                 //Used to hold the number of pairs in calculateOverlappingPairs()
+
 	//1 element per internal node (number_of_internal_nodes == number_of_leaves - 1)
 	b3OpenCLArray<b3SapAabb> m_internalNodeAabbs;
-	b3OpenCLArray<b3Int2> m_internalNodeLeafIndexRanges;		//x == min leaf index, y == max leaf index
-	b3OpenCLArray<b3Int2> m_internalNodeChildNodes;				//x == left child, y == right child; msb(0x80000000) is set to indicate internal node
-	b3OpenCLArray<int> m_internalNodeParentNodes;				//For parent node index, msb(0x80000000) is not set since it is always internal
-	
+	b3OpenCLArray<b3Int2> m_internalNodeLeafIndexRanges;  //x == min leaf index, y == max leaf index
+	b3OpenCLArray<b3Int2> m_internalNodeChildNodes;       //x == left child, y == right child; msb(0x80000000) is set to indicate internal node
+	b3OpenCLArray<int> m_internalNodeParentNodes;         //For parent node index, msb(0x80000000) is not set since it is always internal
+
 	//1 element per internal node; for binary radix tree construction
 	b3OpenCLArray<b3Int64> m_commonPrefixes;
 	b3OpenCLArray<int> m_commonPrefixLengths;
-	b3OpenCLArray<int> m_distanceFromRoot;						//Number of internal nodes between this node and the root
-	
+	b3OpenCLArray<int> m_distanceFromRoot;  //Number of internal nodes between this node and the root
+
 	//1 element per leaf node (leaf nodes only include small AABBs)
-	b3OpenCLArray<int> m_leafNodeParentNodes;					//For parent node index, msb(0x80000000) is not set since it is always internal
-	b3OpenCLArray<b3SortData> m_mortonCodesAndAabbIndicies;		//m_key == morton code, m_value == aabb index in m_leafNodeAabbs
-	b3OpenCLArray<b3SapAabb> m_mergedAabb;						//m_mergedAabb[0] contains the merged AABB of all leaf nodes
-	b3OpenCLArray<b3SapAabb> m_leafNodeAabbs;					//Contains only small AABBs
-	
+	b3OpenCLArray<int> m_leafNodeParentNodes;                //For parent node index, msb(0x80000000) is not set since it is always internal
+	b3OpenCLArray<b3SortData> m_mortonCodesAndAabbIndicies;  //m_key == morton code, m_value == aabb index in m_leafNodeAabbs
+	b3OpenCLArray<b3SapAabb> m_mergedAabb;                   //m_mergedAabb[0] contains the merged AABB of all leaf nodes
+	b3OpenCLArray<b3SapAabb> m_leafNodeAabbs;                //Contains only small AABBs
+
 	//1 element per large AABB, which is not stored in the BVH
 	b3OpenCLArray<b3SapAabb> m_largeAabbs;
-	
+
 public:
 	b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue);
 	virtual ~b3GpuParallelLinearBvh();
-	
+
 	///Must be called before any other function
-	void build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices, 
-				const b3OpenCLArray<int>& largeAabbIndices);
-	
+	void build(const b3OpenCLArray<b3SapAabb>& worldSpaceAabbs, const b3OpenCLArray<int>& smallAabbIndices,
+			   const b3OpenCLArray<int>& largeAabbIndices);
+
 	///calculateOverlappingPairs() uses the worldSpaceAabbs parameter of b3GpuParallelLinearBvh::build() as the query AABBs.
 	///@param out_overlappingPairs The size() of this array is used to determine the max number of pairs.
 	///If the number of overlapping pairs is < out_overlappingPairs.size(), out_overlappingPairs is resized.
 	void calculateOverlappingPairs(b3OpenCLArray<b3Int4>& out_overlappingPairs);
-	
+
 	///@param out_numRigidRayPairs Array of length 1; contains the number of detected ray-rigid AABB intersections;
 	///this value may be greater than out_rayRigidPairs.size() if out_rayRigidPairs is not large enough.
 	///@param out_rayRigidPairs Contains an array of rays intersecting rigid AABBs; x == ray index, y == rigid body index.
 	///If the size of this array is insufficient to hold all ray-rigid AABB intersections, additional intersections are discarded.
-	void testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays, 
-								b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs);
-								
+	void testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays,
+								 b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs);
+
 private:
 	void constructBinaryRadixTree();
 };

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp

@@ -13,45 +13,44 @@ subject to the following restrictions:
 
 #include "b3GpuParallelLinearBvhBroadphase.h"
 
-b3GpuParallelLinearBvhBroadphase::b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue) : 
-	m_plbvh(context, device, queue),
-	
-	m_overlappingPairsGpu(context, queue),
-	
-	m_aabbsGpu(context, queue),
-	m_smallAabbsMappingGpu(context, queue),
-	m_largeAabbsMappingGpu(context, queue)
+b3GpuParallelLinearBvhBroadphase::b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue) : m_plbvh(context, device, queue),
+
+																																	  m_overlappingPairsGpu(context, queue),
+
+																																	  m_aabbsGpu(context, queue),
+																																	  m_smallAabbsMappingGpu(context, queue),
+																																	  m_largeAabbsMappingGpu(context, queue)
 {
 }
 
-void b3GpuParallelLinearBvhBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr,  int collisionFilterGroup,  int collisionFilterMask)
+void b3GpuParallelLinearBvhBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
 {
 	int newAabbIndex = m_aabbsCpu.size();
 
 	b3SapAabb aabb;
 	aabb.m_minVec = aabbMin;
 	aabb.m_maxVec = aabbMax;
-	
+
 	aabb.m_minIndices[3] = userPtr;
 	aabb.m_signedMaxIndices[3] = newAabbIndex;
-	
+
 	m_smallAabbsMappingCpu.push_back(newAabbIndex);
-	
+
 	m_aabbsCpu.push_back(aabb);
 }
-void b3GpuParallelLinearBvhBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr,  int collisionFilterGroup,  int collisionFilterMask)
+void b3GpuParallelLinearBvhBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
 {
 	int newAabbIndex = m_aabbsCpu.size();
 
 	b3SapAabb aabb;
 	aabb.m_minVec = aabbMin;
 	aabb.m_maxVec = aabbMax;
-	
+
 	aabb.m_minIndices[3] = userPtr;
 	aabb.m_signedMaxIndices[3] = newAabbIndex;
-	
+
 	m_largeAabbsMappingCpu.push_back(newAabbIndex);
-	
+
 	m_aabbsCpu.push_back(aabb);
 }
 
@@ -59,22 +58,19 @@ void b3GpuParallelLinearBvhBroadphase::calculateOverlappingPairs(int maxPairs)
 {
 	//Reconstruct BVH
 	m_plbvh.build(m_aabbsGpu, m_smallAabbsMappingGpu, m_largeAabbsMappingGpu);
-	
+
 	//
 	m_overlappingPairsGpu.resize(maxPairs);
 	m_plbvh.calculateOverlappingPairs(m_overlappingPairsGpu);
 }
 void b3GpuParallelLinearBvhBroadphase::calculateOverlappingPairsHost(int maxPairs)
 {
-	b3Assert(0);	//CPU version not implemented
+	b3Assert(0);  //CPU version not implemented
 }
 
-void b3GpuParallelLinearBvhBroadphase::writeAabbsToGpu() 
-{ 
-	m_aabbsGpu.copyFromHost(m_aabbsCpu); 
+void b3GpuParallelLinearBvhBroadphase::writeAabbsToGpu()
+{
+	m_aabbsGpu.copyFromHost(m_aabbsCpu);
 	m_smallAabbsMappingGpu.copyFromHost(m_smallAabbsMappingCpu);
 	m_largeAabbsMappingGpu.copyFromHost(m_largeAabbsMappingCpu);
 }
-	
-	
-	

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h

@@ -21,42 +21,42 @@ subject to the following restrictions:
 class b3GpuParallelLinearBvhBroadphase : public b3GpuBroadphaseInterface
 {
 	b3GpuParallelLinearBvh m_plbvh;
-	
+
 	b3OpenCLArray<b3Int4> m_overlappingPairsGpu;
-	
+
 	b3OpenCLArray<b3SapAabb> m_aabbsGpu;
 	b3OpenCLArray<int> m_smallAabbsMappingGpu;
 	b3OpenCLArray<int> m_largeAabbsMappingGpu;
-	
+
 	b3AlignedObjectArray<b3SapAabb> m_aabbsCpu;
 	b3AlignedObjectArray<int> m_smallAabbsMappingCpu;
 	b3AlignedObjectArray<int> m_largeAabbsMappingCpu;
-	
+
 public:
 	b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue);
 	virtual ~b3GpuParallelLinearBvhBroadphase() {}
 
-	virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr,  int collisionFilterGroup,  int collisionFilterMask);
-	virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr,  int collisionFilterGroup,  int collisionFilterMask);
-	
+	virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
+	virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
+
 	virtual void calculateOverlappingPairs(int maxPairs);
 	virtual void calculateOverlappingPairsHost(int maxPairs);
 
 	//call writeAabbsToGpu after done making all changes (createProxy etc)
 	virtual void writeAabbsToGpu();
-	
-	virtual int	getNumOverlap() { return m_overlappingPairsGpu.size(); }
+
+	virtual int getNumOverlap() { return m_overlappingPairsGpu.size(); }
 	virtual cl_mem getOverlappingPairBuffer() { return m_overlappingPairsGpu.getBufferCL(); }
 
 	virtual cl_mem getAabbBufferWS() { return m_aabbsGpu.getBufferCL(); }
 	virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU() { return m_aabbsGpu; }
-	
+
 	virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU() { return m_overlappingPairsGpu; }
 	virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU() { return m_smallAabbsMappingGpu; }
 	virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU() { return m_largeAabbsMappingGpu; }
-	
+
 	virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU() { return m_aabbsCpu; }
-	
+
 	static b3GpuBroadphaseInterface* CreateFunc(cl_context context, cl_device_id device, cl_command_queue queue)
 	{
 		return new b3GpuParallelLinearBvhBroadphase(context, device, queue);

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h

@@ -2,7 +2,7 @@
 #define B3_GPU_SAP_BROADPHASE_H
 
 #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
-#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2
+#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h"  //b3Int2
 class b3Vector3;
 #include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h"
 
@@ -11,141 +11,133 @@ class b3Vector3;
 
 #include "b3GpuBroadphaseInterface.h"
 
-
 class b3GpuSapBroadphase : public b3GpuBroadphaseInterface
 {
-	
-	cl_context				m_context;
-	cl_device_id			m_device;
-	cl_command_queue		m_queue;
-	cl_kernel				m_flipFloatKernel;
-	cl_kernel				m_scatterKernel ;
-	cl_kernel				m_copyAabbsKernel;
-	cl_kernel				m_sapKernel;
-	cl_kernel				m_sap2Kernel;
-	cl_kernel				m_prepareSumVarianceKernel;
-	
+	cl_context m_context;
+	cl_device_id m_device;
+	cl_command_queue m_queue;
+	cl_kernel m_flipFloatKernel;
+	cl_kernel m_scatterKernel;
+	cl_kernel m_copyAabbsKernel;
+	cl_kernel m_sapKernel;
+	cl_kernel m_sap2Kernel;
+	cl_kernel m_prepareSumVarianceKernel;
 
 	class b3RadixSort32CL* m_sorter;
 
 	///test for 3d SAP
-	b3AlignedObjectArray<b3SortData>		m_sortedAxisCPU[3][2];
-	b3AlignedObjectArray<b3UnsignedInt2>	m_objectMinMaxIndexCPU[3][2];
-	b3OpenCLArray<b3UnsignedInt2>			m_objectMinMaxIndexGPUaxis0;
-	b3OpenCLArray<b3UnsignedInt2>			m_objectMinMaxIndexGPUaxis1;
-	b3OpenCLArray<b3UnsignedInt2>			m_objectMinMaxIndexGPUaxis2;
-	b3OpenCLArray<b3UnsignedInt2>			m_objectMinMaxIndexGPUaxis0prev;
-	b3OpenCLArray<b3UnsignedInt2>			m_objectMinMaxIndexGPUaxis1prev;
-	b3OpenCLArray<b3UnsignedInt2>			m_objectMinMaxIndexGPUaxis2prev;
-
-	b3OpenCLArray<b3SortData>				m_sortedAxisGPU0;
-	b3OpenCLArray<b3SortData>				m_sortedAxisGPU1;
-	b3OpenCLArray<b3SortData>				m_sortedAxisGPU2;
-	b3OpenCLArray<b3SortData>				m_sortedAxisGPU0prev;
-	b3OpenCLArray<b3SortData>				m_sortedAxisGPU1prev;
-	b3OpenCLArray<b3SortData>				m_sortedAxisGPU2prev;
-
-
-	b3OpenCLArray<b3Int4>					m_addedHostPairsGPU;
-	b3OpenCLArray<b3Int4>					m_removedHostPairsGPU;
-	b3OpenCLArray<int>						m_addedCountGPU;
-	b3OpenCLArray<int>						m_removedCountGPU;
-	
-	int	m_currentBuffer;
+	b3AlignedObjectArray<b3SortData> m_sortedAxisCPU[3][2];
+	b3AlignedObjectArray<b3UnsignedInt2> m_objectMinMaxIndexCPU[3][2];
+	b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis0;
+	b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis1;
+	b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis2;
+	b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis0prev;
+	b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis1prev;
+	b3OpenCLArray<b3UnsignedInt2> m_objectMinMaxIndexGPUaxis2prev;
+
+	b3OpenCLArray<b3SortData> m_sortedAxisGPU0;
+	b3OpenCLArray<b3SortData> m_sortedAxisGPU1;
+	b3OpenCLArray<b3SortData> m_sortedAxisGPU2;
+	b3OpenCLArray<b3SortData> m_sortedAxisGPU0prev;
+	b3OpenCLArray<b3SortData> m_sortedAxisGPU1prev;
+	b3OpenCLArray<b3SortData> m_sortedAxisGPU2prev;
+
+	b3OpenCLArray<b3Int4> m_addedHostPairsGPU;
+	b3OpenCLArray<b3Int4> m_removedHostPairsGPU;
+	b3OpenCLArray<int> m_addedCountGPU;
+	b3OpenCLArray<int> m_removedCountGPU;
+
+	int m_currentBuffer;
 
 public:
-
 	b3OpenCLArray<int> m_pairCount;
 
+	b3OpenCLArray<b3SapAabb> m_allAabbsGPU;
+	b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU;
 
-	b3OpenCLArray<b3SapAabb>	m_allAabbsGPU;
-	b3AlignedObjectArray<b3SapAabb>	m_allAabbsCPU;
-
-	virtual b3OpenCLArray<b3SapAabb>&	getAllAabbsGPU()
+	virtual b3OpenCLArray<b3SapAabb>& getAllAabbsGPU()
 	{
 		return m_allAabbsGPU;
 	}
-	virtual b3AlignedObjectArray<b3SapAabb>&	getAllAabbsCPU()
+	virtual b3AlignedObjectArray<b3SapAabb>& getAllAabbsCPU()
 	{
 		return m_allAabbsCPU;
 	}
 
-	b3OpenCLArray<b3Vector3>	m_sum;
-	b3OpenCLArray<b3Vector3>	m_sum2;
-	b3OpenCLArray<b3Vector3>	m_dst;
+	b3OpenCLArray<b3Vector3> m_sum;
+	b3OpenCLArray<b3Vector3> m_sum2;
+	b3OpenCLArray<b3Vector3> m_dst;
 
-	b3OpenCLArray<int>	m_smallAabbsMappingGPU;
+	b3OpenCLArray<int> m_smallAabbsMappingGPU;
 	b3AlignedObjectArray<int> m_smallAabbsMappingCPU;
 
-	b3OpenCLArray<int>	m_largeAabbsMappingGPU;
+	b3OpenCLArray<int> m_largeAabbsMappingGPU;
 	b3AlignedObjectArray<int> m_largeAabbsMappingCPU;
 
-	
-	b3OpenCLArray<b3Int4>		m_overlappingPairs;
+	b3OpenCLArray<b3Int4> m_overlappingPairs;
 
 	//temporary gpu work memory
-	b3OpenCLArray<b3SortData>	m_gpuSmallSortData;
-	b3OpenCLArray<b3SapAabb>	m_gpuSmallSortedAabbs;
+	b3OpenCLArray<b3SortData> m_gpuSmallSortData;
+	b3OpenCLArray<b3SapAabb> m_gpuSmallSortedAabbs;
 
-	class b3PrefixScanFloat4CL*		m_prefixScanFloat4;
+	class b3PrefixScanFloat4CL* m_prefixScanFloat4;
 
 	enum b3GpuSapKernelType
 	{
-		B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU=1,
+		B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU = 1,
 		B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU,
 		B3_GPU_SAP_KERNEL_ORIGINAL,
 		B3_GPU_SAP_KERNEL_BARRIER,
 		B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY
 	};
 
-	b3GpuSapBroadphase(cl_context ctx,cl_device_id device, cl_command_queue  q , b3GpuSapKernelType kernelType=B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
+	b3GpuSapBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q, b3GpuSapKernelType kernelType = B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
 	virtual ~b3GpuSapBroadphase();
-	
-	static b3GpuBroadphaseInterface* CreateFuncBruteForceCpu(cl_context ctx,cl_device_id device, cl_command_queue  q)
+
+	static b3GpuBroadphaseInterface* CreateFuncBruteForceCpu(cl_context ctx, cl_device_id device, cl_command_queue q)
 	{
-		return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU);
+		return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU);
 	}
 
-	static b3GpuBroadphaseInterface* CreateFuncBruteForceGpu(cl_context ctx,cl_device_id device, cl_command_queue  q)
+	static b3GpuBroadphaseInterface* CreateFuncBruteForceGpu(cl_context ctx, cl_device_id device, cl_command_queue q)
 	{
-		return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU);
+		return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU);
 	}
 
-	static b3GpuBroadphaseInterface* CreateFuncOriginal(cl_context ctx,cl_device_id device, cl_command_queue  q)
+	static b3GpuBroadphaseInterface* CreateFuncOriginal(cl_context ctx, cl_device_id device, cl_command_queue q)
 	{
-		return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_ORIGINAL);
+		return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_ORIGINAL);
 	}
-	static b3GpuBroadphaseInterface* CreateFuncBarrier(cl_context ctx,cl_device_id device, cl_command_queue  q)
+	static b3GpuBroadphaseInterface* CreateFuncBarrier(cl_context ctx, cl_device_id device, cl_command_queue q)
 	{
-		return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BARRIER);
+		return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_BARRIER);
 	}
-	static b3GpuBroadphaseInterface* CreateFuncLocalMemory(cl_context ctx,cl_device_id device, cl_command_queue  q)
+	static b3GpuBroadphaseInterface* CreateFuncLocalMemory(cl_context ctx, cl_device_id device, cl_command_queue q)
 	{
-		return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
+		return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
 	}
-	
 
-	virtual void  calculateOverlappingPairs(int maxPairs);
-	virtual void  calculateOverlappingPairsHost(int maxPairs);
-	
-	void  reset();
+	virtual void calculateOverlappingPairs(int maxPairs);
+	virtual void calculateOverlappingPairsHost(int maxPairs);
+
+	void reset();
 
 	void init3dSap();
 	virtual void calculateOverlappingPairsHostIncremental3Sap();
 
-	virtual void createProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
-	virtual void createLargeProxy(const b3Vector3& aabbMin,  const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask);
+	virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
+	virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
 
 	//call writeAabbsToGpu after done making all changes (createProxy etc)
 	virtual void writeAabbsToGpu();
 
-	virtual cl_mem	getAabbBufferWS();
-	virtual int	getNumOverlap();
-	virtual cl_mem	getOverlappingPairBuffer();
-	
+	virtual cl_mem getAabbBufferWS();
+	virtual int getNumOverlap();
+	virtual cl_mem getOverlappingPairBuffer();
+
 	virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU();
 	virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU();
 	virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU();
 };
 
-#endif //B3_GPU_SAP_BROADPHASE_H
+#endif  //B3_GPU_SAP_BROADPHASE_H

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h

@@ -5,10 +5,9 @@
 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
 
 ///just make sure that the b3Aabb is 16-byte aligned
-B3_ATTRIBUTE_ALIGNED16(struct) b3SapAabb : public b3Aabb
-{
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3SapAabb : public b3Aabb{
 
-};
+			};
 
-
-#endif //B3_SAP_AABB_H
+#endif  //B3_SAP_AABB_H

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h

@@ -1,199 +1,198 @@
 //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
-static const char* gridBroadphaseCL= \
-"int getPosHash(int4 gridPos, __global float4* pParams)\n"
-"{\n"
-"	int4 gridDim = *((__global int4*)(pParams + 1));\n"
-"	gridPos.x &= gridDim.x - 1;\n"
-"	gridPos.y &= gridDim.y - 1;\n"
-"	gridPos.z &= gridDim.z - 1;\n"
-"	int hash = gridPos.z * gridDim.y * gridDim.x + gridPos.y * gridDim.x + gridPos.x;\n"
-"	return hash;\n"
-"} \n"
-"int4 getGridPos(float4 worldPos, __global float4* pParams)\n"
-"{\n"
-"    int4 gridPos;\n"
-"	int4 gridDim = *((__global int4*)(pParams + 1));\n"
-"    gridPos.x = (int)floor(worldPos.x * pParams[0].x) & (gridDim.x - 1);\n"
-"    gridPos.y = (int)floor(worldPos.y * pParams[0].y) & (gridDim.y - 1);\n"
-"    gridPos.z = (int)floor(worldPos.z * pParams[0].z) & (gridDim.z - 1);\n"
-"    return gridPos;\n"
-"}\n"
-"// calculate grid hash value for each body using its AABB\n"
-"__kernel void kCalcHashAABB(int numObjects, __global float4* allpAABB, __global const int* smallAabbMapping, __global int2* pHash, __global float4* pParams )\n"
-"{\n"
-"    int index = get_global_id(0);\n"
-"    if(index >= numObjects)\n"
-"	{\n"
-"		return;\n"
-"	}\n"
-"	float4 bbMin = allpAABB[smallAabbMapping[index]*2];\n"
-"	float4 bbMax = allpAABB[smallAabbMapping[index]*2 + 1];\n"
-"	float4 pos;\n"
-"	pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
-"	pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
-"	pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
-"	pos.w = 0.f;\n"
-"    // get address in grid\n"
-"    int4 gridPos = getGridPos(pos, pParams);\n"
-"    int gridHash = getPosHash(gridPos, pParams);\n"
-"    // store grid hash and body index\n"
-"    int2 hashVal;\n"
-"    hashVal.x = gridHash;\n"
-"    hashVal.y = index;\n"
-"    pHash[index] = hashVal;\n"
-"}\n"
-"__kernel void kClearCellStart(	int numCells, \n"
-"								__global int* pCellStart )\n"
-"{\n"
-"    int index = get_global_id(0);\n"
-"    if(index >= numCells)\n"
-"	{\n"
-"		return;\n"
-"	}\n"
-"	pCellStart[index] = -1;\n"
-"}\n"
-"__kernel void kFindCellStart(int numObjects, __global int2* pHash, __global int* cellStart )\n"
-"{\n"
-"	__local int sharedHash[513];\n"
-"    int index = get_global_id(0);\n"
-"	int2 sortedData;\n"
-"    if(index < numObjects)\n"
-"	{\n"
-"		sortedData = pHash[index];\n"
-"		// Load hash data into shared memory so that we can look \n"
-"		// at neighboring body's hash value without loading\n"
-"		// two hash values per thread\n"
-"		sharedHash[get_local_id(0) + 1] = sortedData.x;\n"
-"		if((index > 0) && (get_local_id(0) == 0))\n"
-"		{\n"
-"			// first thread in block must load neighbor body hash\n"
-"			sharedHash[0] = pHash[index-1].x;\n"
-"		}\n"
-"	}\n"
-"    barrier(CLK_LOCAL_MEM_FENCE);\n"
-"    if(index < numObjects)\n"
-"	{\n"
-"		if((index == 0) || (sortedData.x != sharedHash[get_local_id(0)]))\n"
-"		{\n"
-"			cellStart[sortedData.x] = index;\n"
-"		}\n"
-"	}\n"
-"}\n"
-"int testAABBOverlap(float4 min0, float4 max0, float4 min1, float4 max1)\n"
-"{\n"
-"	return	(min0.x <= max1.x)&& (min1.x <= max0.x) && \n"
-"			(min0.y <= max1.y)&& (min1.y <= max0.y) && \n"
-"			(min0.z <= max1.z)&& (min1.z <= max0.z); \n"
-"}\n"
-"//search for AABB 'index' against other AABBs' in this cell\n"
-"void findPairsInCell(	int numObjects,\n"
-"						int4	gridPos,\n"
-"						int    index,\n"
-"						__global int2*  pHash,\n"
-"						__global int*   pCellStart,\n"
-"						__global float4* allpAABB, \n"
-"						__global const int* smallAabbMapping,\n"
-"						__global float4* pParams,\n"
-"							volatile  __global int* pairCount,\n"
-"						__global int4*   pPairBuff2,\n"
-"						int maxPairs\n"
-"						)\n"
-"{\n"
-"	int4 pGridDim = *((__global int4*)(pParams + 1));\n"
-"	int maxBodiesPerCell = pGridDim.w;\n"
-"    int gridHash = getPosHash(gridPos, pParams);\n"
-"    // get start of bucket for this cell\n"
-"    int bucketStart = pCellStart[gridHash];\n"
-"    if (bucketStart == -1)\n"
-"	{\n"
-"        return;   // cell empty\n"
-"	}\n"
-"	// iterate over bodies in this cell\n"
-"    int2 sortedData = pHash[index];\n"
-"	int unsorted_indx = sortedData.y;\n"
-"    float4 min0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0]; \n"
-"	float4 max0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
-"	int handleIndex =  as_int(min0.w);\n"
-"	\n"
-"	int bucketEnd = bucketStart + maxBodiesPerCell;\n"
-"	bucketEnd = (bucketEnd > numObjects) ? numObjects : bucketEnd;\n"
-"	for(int index2 = bucketStart; index2 < bucketEnd; index2++) \n"
-"	{\n"
-"        int2 cellData = pHash[index2];\n"
-"        if (cellData.x != gridHash)\n"
-"        {\n"
-"			break;   // no longer in same bucket\n"
-"		}\n"
-"		int unsorted_indx2 = cellData.y;\n"
-"        //if (unsorted_indx2 < unsorted_indx) // check not colliding with self\n"
-"		if (unsorted_indx2 != unsorted_indx) // check not colliding with self\n"
-"        {   \n"
-"			float4 min1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 0];\n"
-"			float4 max1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 1];\n"
-"			if(testAABBOverlap(min0, max0, min1, max1))\n"
-"			{\n"
-"				if (pairCount)\n"
-"				{\n"
-"					int handleIndex2 = as_int(min1.w);\n"
-"					if (handleIndex<handleIndex2)\n"
-"					{\n"
-"						int curPair = atomic_add(pairCount,1);\n"
-"						if (curPair<maxPairs)\n"
-"						{\n"
-"							int4 newpair;\n"
-"							newpair.x = handleIndex;\n"
-"							newpair.y = handleIndex2;\n"
-"							newpair.z = -1;\n"
-"							newpair.w = -1;\n"
-"							pPairBuff2[curPair] = newpair;\n"
-"						}\n"
-"					}\n"
-"				\n"
-"				}\n"
-"			}\n"
-"		}\n"
-"	}\n"
-"}\n"
-"__kernel void kFindOverlappingPairs(	int numObjects,\n"
-"										__global float4* allpAABB, \n"
-"										__global const int* smallAabbMapping,\n"
-"										__global int2* pHash, \n"
-"										__global int* pCellStart, \n"
-"										__global float4* pParams ,\n"
-"										volatile  __global int* pairCount,\n"
-"										__global int4*   pPairBuff2,\n"
-"										int maxPairs\n"
-"										)\n"
-"{\n"
-"    int index = get_global_id(0);\n"
-"    if(index >= numObjects)\n"
-"	{\n"
-"		return;\n"
-"	}\n"
-"    int2 sortedData = pHash[index];\n"
-"	int unsorted_indx = sortedData.y;\n"
-"	float4 bbMin = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0];\n"
-"	float4 bbMax = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
-"	float4 pos;\n"
-"	pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
-"	pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
-"	pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
-"    // get address in grid\n"
-"    int4 gridPosA = getGridPos(pos, pParams);\n"
-"    int4 gridPosB; \n"
-"    // examine only neighbouring cells\n"
-"    for(int z=-1; z<=1; z++) \n"
-"    {\n"
-"		gridPosB.z = gridPosA.z + z;\n"
-"        for(int y=-1; y<=1; y++) \n"
-"        {\n"
-"			gridPosB.y = gridPosA.y + y;\n"
-"            for(int x=-1; x<=1; x++) \n"
-"            {\n"
-"				gridPosB.x = gridPosA.x + x;\n"
-"                findPairsInCell(numObjects, gridPosB, index, pHash, pCellStart, allpAABB,smallAabbMapping, pParams, pairCount,pPairBuff2, maxPairs);\n"
-"            }\n"
-"        }\n"
-"    }\n"
-"}\n"
-;
+static const char* gridBroadphaseCL =
+	"int getPosHash(int4 gridPos, __global float4* pParams)\n"
+	"{\n"
+	"	int4 gridDim = *((__global int4*)(pParams + 1));\n"
+	"	gridPos.x &= gridDim.x - 1;\n"
+	"	gridPos.y &= gridDim.y - 1;\n"
+	"	gridPos.z &= gridDim.z - 1;\n"
+	"	int hash = gridPos.z * gridDim.y * gridDim.x + gridPos.y * gridDim.x + gridPos.x;\n"
+	"	return hash;\n"
+	"} \n"
+	"int4 getGridPos(float4 worldPos, __global float4* pParams)\n"
+	"{\n"
+	"    int4 gridPos;\n"
+	"	int4 gridDim = *((__global int4*)(pParams + 1));\n"
+	"    gridPos.x = (int)floor(worldPos.x * pParams[0].x) & (gridDim.x - 1);\n"
+	"    gridPos.y = (int)floor(worldPos.y * pParams[0].y) & (gridDim.y - 1);\n"
+	"    gridPos.z = (int)floor(worldPos.z * pParams[0].z) & (gridDim.z - 1);\n"
+	"    return gridPos;\n"
+	"}\n"
+	"// calculate grid hash value for each body using its AABB\n"
+	"__kernel void kCalcHashAABB(int numObjects, __global float4* allpAABB, __global const int* smallAabbMapping, __global int2* pHash, __global float4* pParams )\n"
+	"{\n"
+	"    int index = get_global_id(0);\n"
+	"    if(index >= numObjects)\n"
+	"	{\n"
+	"		return;\n"
+	"	}\n"
+	"	float4 bbMin = allpAABB[smallAabbMapping[index]*2];\n"
+	"	float4 bbMax = allpAABB[smallAabbMapping[index]*2 + 1];\n"
+	"	float4 pos;\n"
+	"	pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
+	"	pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
+	"	pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
+	"	pos.w = 0.f;\n"
+	"    // get address in grid\n"
+	"    int4 gridPos = getGridPos(pos, pParams);\n"
+	"    int gridHash = getPosHash(gridPos, pParams);\n"
+	"    // store grid hash and body index\n"
+	"    int2 hashVal;\n"
+	"    hashVal.x = gridHash;\n"
+	"    hashVal.y = index;\n"
+	"    pHash[index] = hashVal;\n"
+	"}\n"
+	"__kernel void kClearCellStart(	int numCells, \n"
+	"								__global int* pCellStart )\n"
+	"{\n"
+	"    int index = get_global_id(0);\n"
+	"    if(index >= numCells)\n"
+	"	{\n"
+	"		return;\n"
+	"	}\n"
+	"	pCellStart[index] = -1;\n"
+	"}\n"
+	"__kernel void kFindCellStart(int numObjects, __global int2* pHash, __global int* cellStart )\n"
+	"{\n"
+	"	__local int sharedHash[513];\n"
+	"    int index = get_global_id(0);\n"
+	"	int2 sortedData;\n"
+	"    if(index < numObjects)\n"
+	"	{\n"
+	"		sortedData = pHash[index];\n"
+	"		// Load hash data into shared memory so that we can look \n"
+	"		// at neighboring body's hash value without loading\n"
+	"		// two hash values per thread\n"
+	"		sharedHash[get_local_id(0) + 1] = sortedData.x;\n"
+	"		if((index > 0) && (get_local_id(0) == 0))\n"
+	"		{\n"
+	"			// first thread in block must load neighbor body hash\n"
+	"			sharedHash[0] = pHash[index-1].x;\n"
+	"		}\n"
+	"	}\n"
+	"    barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"    if(index < numObjects)\n"
+	"	{\n"
+	"		if((index == 0) || (sortedData.x != sharedHash[get_local_id(0)]))\n"
+	"		{\n"
+	"			cellStart[sortedData.x] = index;\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"int testAABBOverlap(float4 min0, float4 max0, float4 min1, float4 max1)\n"
+	"{\n"
+	"	return	(min0.x <= max1.x)&& (min1.x <= max0.x) && \n"
+	"			(min0.y <= max1.y)&& (min1.y <= max0.y) && \n"
+	"			(min0.z <= max1.z)&& (min1.z <= max0.z); \n"
+	"}\n"
+	"//search for AABB 'index' against other AABBs' in this cell\n"
+	"void findPairsInCell(	int numObjects,\n"
+	"						int4	gridPos,\n"
+	"						int    index,\n"
+	"						__global int2*  pHash,\n"
+	"						__global int*   pCellStart,\n"
+	"						__global float4* allpAABB, \n"
+	"						__global const int* smallAabbMapping,\n"
+	"						__global float4* pParams,\n"
+	"							volatile  __global int* pairCount,\n"
+	"						__global int4*   pPairBuff2,\n"
+	"						int maxPairs\n"
+	"						)\n"
+	"{\n"
+	"	int4 pGridDim = *((__global int4*)(pParams + 1));\n"
+	"	int maxBodiesPerCell = pGridDim.w;\n"
+	"    int gridHash = getPosHash(gridPos, pParams);\n"
+	"    // get start of bucket for this cell\n"
+	"    int bucketStart = pCellStart[gridHash];\n"
+	"    if (bucketStart == -1)\n"
+	"	{\n"
+	"        return;   // cell empty\n"
+	"	}\n"
+	"	// iterate over bodies in this cell\n"
+	"    int2 sortedData = pHash[index];\n"
+	"	int unsorted_indx = sortedData.y;\n"
+	"    float4 min0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0]; \n"
+	"	float4 max0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
+	"	int handleIndex =  as_int(min0.w);\n"
+	"	\n"
+	"	int bucketEnd = bucketStart + maxBodiesPerCell;\n"
+	"	bucketEnd = (bucketEnd > numObjects) ? numObjects : bucketEnd;\n"
+	"	for(int index2 = bucketStart; index2 < bucketEnd; index2++) \n"
+	"	{\n"
+	"        int2 cellData = pHash[index2];\n"
+	"        if (cellData.x != gridHash)\n"
+	"        {\n"
+	"			break;   // no longer in same bucket\n"
+	"		}\n"
+	"		int unsorted_indx2 = cellData.y;\n"
+	"        //if (unsorted_indx2 < unsorted_indx) // check not colliding with self\n"
+	"		if (unsorted_indx2 != unsorted_indx) // check not colliding with self\n"
+	"        {   \n"
+	"			float4 min1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 0];\n"
+	"			float4 max1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 1];\n"
+	"			if(testAABBOverlap(min0, max0, min1, max1))\n"
+	"			{\n"
+	"				if (pairCount)\n"
+	"				{\n"
+	"					int handleIndex2 = as_int(min1.w);\n"
+	"					if (handleIndex<handleIndex2)\n"
+	"					{\n"
+	"						int curPair = atomic_add(pairCount,1);\n"
+	"						if (curPair<maxPairs)\n"
+	"						{\n"
+	"							int4 newpair;\n"
+	"							newpair.x = handleIndex;\n"
+	"							newpair.y = handleIndex2;\n"
+	"							newpair.z = -1;\n"
+	"							newpair.w = -1;\n"
+	"							pPairBuff2[curPair] = newpair;\n"
+	"						}\n"
+	"					}\n"
+	"				\n"
+	"				}\n"
+	"			}\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"__kernel void kFindOverlappingPairs(	int numObjects,\n"
+	"										__global float4* allpAABB, \n"
+	"										__global const int* smallAabbMapping,\n"
+	"										__global int2* pHash, \n"
+	"										__global int* pCellStart, \n"
+	"										__global float4* pParams ,\n"
+	"										volatile  __global int* pairCount,\n"
+	"										__global int4*   pPairBuff2,\n"
+	"										int maxPairs\n"
+	"										)\n"
+	"{\n"
+	"    int index = get_global_id(0);\n"
+	"    if(index >= numObjects)\n"
+	"	{\n"
+	"		return;\n"
+	"	}\n"
+	"    int2 sortedData = pHash[index];\n"
+	"	int unsorted_indx = sortedData.y;\n"
+	"	float4 bbMin = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0];\n"
+	"	float4 bbMax = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n"
+	"	float4 pos;\n"
+	"	pos.x = (bbMin.x + bbMax.x) * 0.5f;\n"
+	"	pos.y = (bbMin.y + bbMax.y) * 0.5f;\n"
+	"	pos.z = (bbMin.z + bbMax.z) * 0.5f;\n"
+	"    // get address in grid\n"
+	"    int4 gridPosA = getGridPos(pos, pParams);\n"
+	"    int4 gridPosB; \n"
+	"    // examine only neighbouring cells\n"
+	"    for(int z=-1; z<=1; z++) \n"
+	"    {\n"
+	"		gridPosB.z = gridPosA.z + z;\n"
+	"        for(int y=-1; y<=1; y++) \n"
+	"        {\n"
+	"			gridPosB.y = gridPosA.y + y;\n"
+	"            for(int x=-1; x<=1; x++) \n"
+	"            {\n"
+	"				gridPosB.x = gridPosA.x + x;\n"
+	"                findPairsInCell(numObjects, gridPosB, index, pHash, pCellStart, allpAABB,smallAabbMapping, pParams, pairCount,pPairBuff2, maxPairs);\n"
+	"            }\n"
+	"        }\n"
+	"    }\n"
+	"}\n";

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h

@@ -1,729 +1,728 @@
 //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
-static const char* parallelLinearBvhCL= \
-"/*\n"
-"This software is provided 'as-is', without any express or implied warranty.\n"
-"In no event will the authors be held liable for any damages arising from the use of this software.\n"
-"Permission is granted to anyone to use this software for any purpose,\n"
-"including commercial applications, and to alter it and redistribute it freely,\n"
-"subject to the following restrictions:\n"
-"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.\n"
-"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
-"3. This notice may not be removed or altered from any source distribution.\n"
-"*/\n"
-"//Initial Author Jackson Lee, 2014\n"
-"typedef float b3Scalar;\n"
-"typedef float4 b3Vector3;\n"
-"#define b3Max max\n"
-"#define b3Min min\n"
-"#define b3Sqrt sqrt\n"
-"typedef struct\n"
-"{\n"
-"	unsigned int m_key;\n"
-"	unsigned int m_value;\n"
-"} SortDataCL;\n"
-"typedef struct \n"
-"{\n"
-"	union\n"
-"	{\n"
-"		float4	m_min;\n"
-"		float   m_minElems[4];\n"
-"		int			m_minIndices[4];\n"
-"	};\n"
-"	union\n"
-"	{\n"
-"		float4	m_max;\n"
-"		float   m_maxElems[4];\n"
-"		int			m_maxIndices[4];\n"
-"	};\n"
-"} b3AabbCL;\n"
-"unsigned int interleaveBits(unsigned int x)\n"
-"{\n"
-"	//........ ........ ......12 3456789A	//x\n"
-"	//....1..2 ..3..4.. 5..6..7. .8..9..A	//x after interleaving bits\n"
-"	\n"
-"	//......12 3456789A ......12 3456789A	//x ^ (x << 16)\n"
-"	//11111111 ........ ........ 11111111	//0x FF 00 00 FF\n"
-"	//......12 ........ ........ 3456789A	//x = (x ^ (x << 16)) & 0xFF0000FF;\n"
-"	\n"
-"	//......12 ........ 3456789A 3456789A	//x ^ (x <<  8)\n"
-"	//......11 ........ 1111.... ....1111	//0x 03 00 F0 0F\n"
-"	//......12 ........ 3456.... ....789A	//x = (x ^ (x <<  8)) & 0x0300F00F;\n"
-"	\n"
-"	//..12..12 ....3456 3456.... 789A789A	//x ^ (x <<  4)\n"
-"	//......11 ....11.. ..11.... 11....11	//0x 03 0C 30 C3\n"
-"	//......12 ....34.. ..56.... 78....9A	//x = (x ^ (x <<  4)) & 0x030C30C3;\n"
-"	\n"
-"	//....1212 ..3434.. 5656..78 78..9A9A	//x ^ (x <<  2)\n"
-"	//....1..1 ..1..1.. 1..1..1. .1..1..1	//0x 09 24 92 49\n"
-"	//....1..2 ..3..4.. 5..6..7. .8..9..A	//x = (x ^ (x <<  2)) & 0x09249249;\n"
-"	\n"
-"	//........ ........ ......11 11111111	//0x000003FF\n"
-"	x &= 0x000003FF;		//Clear all bits above bit 10\n"
-"	\n"
-"	x = (x ^ (x << 16)) & 0xFF0000FF;\n"
-"	x = (x ^ (x <<  8)) & 0x0300F00F;\n"
-"	x = (x ^ (x <<  4)) & 0x030C30C3;\n"
-"	x = (x ^ (x <<  2)) & 0x09249249;\n"
-"	\n"
-"	return x;\n"
-"}\n"
-"unsigned int getMortonCode(unsigned int x, unsigned int y, unsigned int z)\n"
-"{\n"
-"	return interleaveBits(x) << 0 | interleaveBits(y) << 1 | interleaveBits(z) << 2;\n"
-"}\n"
-"__kernel void separateAabbs(__global b3AabbCL* unseparatedAabbs, __global int* aabbIndices, __global b3AabbCL* out_aabbs, int numAabbsToSeparate)\n"
-"{\n"
-"	int separatedAabbIndex = get_global_id(0);\n"
-"	if(separatedAabbIndex >= numAabbsToSeparate) return;\n"
-"	int unseparatedAabbIndex = aabbIndices[separatedAabbIndex];\n"
-"	out_aabbs[separatedAabbIndex] = unseparatedAabbs[unseparatedAabbIndex];\n"
-"}\n"
-"//Should replace with an optimized parallel reduction\n"
-"__kernel void findAllNodesMergedAabb(__global b3AabbCL* out_mergedAabb, int numAabbsNeedingMerge)\n"
-"{\n"
-"	//Each time this kernel is added to the command queue, \n"
-"	//the number of AABBs needing to be merged is halved\n"
-"	//\n"
-"	//Example with 159 AABBs:\n"
-"	//	numRemainingAabbs == 159 / 2 + 159 % 2 == 80\n"
-"	//	numMergedAabbs == 159 - 80 == 79\n"
-"	//So, indices [0, 78] are merged with [0 + 80, 78 + 80]\n"
-"	\n"
-"	int numRemainingAabbs = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2;\n"
-"	int numMergedAabbs = numAabbsNeedingMerge - numRemainingAabbs;\n"
-"	\n"
-"	int aabbIndex = get_global_id(0);\n"
-"	if(aabbIndex >= numMergedAabbs) return;\n"
-"	\n"
-"	int otherAabbIndex = aabbIndex + numRemainingAabbs;\n"
-"	\n"
-"	b3AabbCL aabb = out_mergedAabb[aabbIndex];\n"
-"	b3AabbCL otherAabb = out_mergedAabb[otherAabbIndex];\n"
-"		\n"
-"	b3AabbCL mergedAabb;\n"
-"	mergedAabb.m_min = b3Min(aabb.m_min, otherAabb.m_min);\n"
-"	mergedAabb.m_max = b3Max(aabb.m_max, otherAabb.m_max);\n"
-"	out_mergedAabb[aabbIndex] = mergedAabb;\n"
-"}\n"
-"__kernel void assignMortonCodesAndAabbIndicies(__global b3AabbCL* worldSpaceAabbs, __global b3AabbCL* mergedAabbOfAllNodes, \n"
-"												__global SortDataCL* out_mortonCodesAndAabbIndices, int numAabbs)\n"
-"{\n"
-"	int leafNodeIndex = get_global_id(0);	//Leaf node index == AABB index\n"
-"	if(leafNodeIndex >= numAabbs) return;\n"
-"	\n"
-"	b3AabbCL mergedAabb = mergedAabbOfAllNodes[0];\n"
-"	b3Vector3 gridCenter = (mergedAabb.m_min + mergedAabb.m_max) * 0.5f;\n"
-"	b3Vector3 gridCellSize = (mergedAabb.m_max - mergedAabb.m_min) / (float)1024;\n"
-"	\n"
-"	b3AabbCL aabb = worldSpaceAabbs[leafNodeIndex];\n"
-"	b3Vector3 aabbCenter = (aabb.m_min + aabb.m_max) * 0.5f;\n"
-"	b3Vector3 aabbCenterRelativeToGrid = aabbCenter - gridCenter;\n"
-"	\n"
-"	//Quantize into integer coordinates\n"
-"	//floor() is needed to prevent the center cell, at (0,0,0) from being twice the size\n"
-"	b3Vector3 gridPosition = aabbCenterRelativeToGrid / gridCellSize;\n"
-"	\n"
-"	int4 discretePosition;\n"
-"	discretePosition.x = (int)( (gridPosition.x >= 0.0f) ? gridPosition.x : floor(gridPosition.x) );\n"
-"	discretePosition.y = (int)( (gridPosition.y >= 0.0f) ? gridPosition.y : floor(gridPosition.y) );\n"
-"	discretePosition.z = (int)( (gridPosition.z >= 0.0f) ? gridPosition.z : floor(gridPosition.z) );\n"
-"	\n"
-"	//Clamp coordinates into [-512, 511], then convert range from [-512, 511] to [0, 1023]\n"
-"	discretePosition = b3Max( -512, b3Min(discretePosition, 511) );\n"
-"	discretePosition += 512;\n"
-"	\n"
-"	//Interleave bits(assign a morton code, also known as a z-curve)\n"
-"	unsigned int mortonCode = getMortonCode(discretePosition.x, discretePosition.y, discretePosition.z);\n"
-"	\n"
-"	//\n"
-"	SortDataCL mortonCodeIndexPair;\n"
-"	mortonCodeIndexPair.m_key = mortonCode;\n"
-"	mortonCodeIndexPair.m_value = leafNodeIndex;\n"
-"	\n"
-"	out_mortonCodesAndAabbIndices[leafNodeIndex] = mortonCodeIndexPair;\n"
-"}\n"
-"#define B3_PLVBH_TRAVERSE_MAX_STACK_SIZE 128\n"
-"//The most significant bit(0x80000000) of a int32 is used to distinguish between leaf and internal nodes.\n"
-"//If it is set, then the index is for an internal node; otherwise, it is a leaf node. \n"
-"//In both cases, the bit should be cleared to access the actual node index.\n"
-"int isLeafNode(int index) { return (index >> 31 == 0); }\n"
-"int getIndexWithInternalNodeMarkerRemoved(int index) { return index & (~0x80000000); }\n"
-"int getIndexWithInternalNodeMarkerSet(int isLeaf, int index) { return (isLeaf) ? index : (index | 0x80000000); }\n"
-"//From sap.cl\n"
-"#define NEW_PAIR_MARKER -1\n"
-"bool TestAabbAgainstAabb2(const b3AabbCL* aabb1, const b3AabbCL* aabb2)\n"
-"{\n"
-"	bool overlap = true;\n"
-"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
-"	return overlap;\n"
-"}\n"
-"//From sap.cl\n"
-"__kernel void plbvhCalculateOverlappingPairs(__global b3AabbCL* rigidAabbs, \n"
-"											__global int* rootNodeIndex, \n"
-"											__global int2* internalNodeChildIndices, \n"
-"											__global b3AabbCL* internalNodeAabbs,\n"
-"											__global int2* internalNodeLeafIndexRanges,\n"
-"											\n"
-"											__global SortDataCL* mortonCodesAndAabbIndices,\n"
-"											__global int* out_numPairs, __global int4* out_overlappingPairs, \n"
-"											int maxPairs, int numQueryAabbs)\n"
-"{\n"
-"	//Using get_group_id()/get_local_id() is Faster than get_global_id(0) since\n"
-"	//mortonCodesAndAabbIndices[] contains rigid body indices sorted along the z-curve (more spatially coherent)\n"
-"	int queryBvhNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0);\n"
-"	if(queryBvhNodeIndex >= numQueryAabbs) return;\n"
-"	\n"
-"	int queryRigidIndex = mortonCodesAndAabbIndices[queryBvhNodeIndex].m_value;\n"
-"	b3AabbCL queryAabb = rigidAabbs[queryRigidIndex];\n"
-"	\n"
-"	int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE];\n"
-"	\n"
-"	int stackSize = 1;\n"
-"	stack[0] = *rootNodeIndex;\n"
-"	\n"
-"	while(stackSize)\n"
-"	{\n"
-"		int internalOrLeafNodeIndex = stack[ stackSize - 1 ];\n"
-"		--stackSize;\n"
-"		\n"
-"		int isLeaf = isLeafNode(internalOrLeafNodeIndex);	//Internal node if false\n"
-"		int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex);\n"
-"		\n"
-"		//Optimization - if the BVH is structured as a binary radix tree, then\n"
-"		//each internal node corresponds to a contiguous range of leaf nodes(internalNodeLeafIndexRanges[]).\n"
-"		//This can be used to avoid testing each AABB-AABB pair twice, including preventing each node from colliding with itself.\n"
-"		{\n"
-"			int highestLeafIndex = (isLeaf) ? bvhNodeIndex : internalNodeLeafIndexRanges[bvhNodeIndex].y;\n"
-"			if(highestLeafIndex <= queryBvhNodeIndex) continue;\n"
-"		}\n"
-"		\n"
-"		//bvhRigidIndex is not used if internal node\n"
-"		int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1;\n"
-"	\n"
-"		b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex];\n"
-"		if( TestAabbAgainstAabb2(&queryAabb, &bvhNodeAabb) )\n"
-"		{\n"
-"			if(isLeaf)\n"
-"			{\n"
-"				int4 pair;\n"
-"				pair.x = rigidAabbs[queryRigidIndex].m_minIndices[3];\n"
-"				pair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3];\n"
-"				pair.z = NEW_PAIR_MARKER;\n"
-"				pair.w = NEW_PAIR_MARKER;\n"
-"				\n"
-"				int pairIndex = atomic_inc(out_numPairs);\n"
-"				if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair;\n"
-"			}\n"
-"			\n"
-"			if(!isLeaf)	//Internal node\n"
-"			{\n"
-"				if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE)\n"
-"				{\n"
-"					//Error\n"
-"				}\n"
-"				else\n"
-"				{\n"
-"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x;\n"
-"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y;\n"
-"				}\n"
-"			}\n"
-"		}\n"
-"		\n"
-"	}\n"
-"}\n"
-"//From rayCastKernels.cl\n"
-"typedef struct\n"
-"{\n"
-"	float4 m_from;\n"
-"	float4 m_to;\n"
-"} b3RayInfo;\n"
-"//From rayCastKernels.cl\n"
-"b3Vector3 b3Vector3_normalize(b3Vector3 v)\n"
-"{\n"
-"	b3Vector3 normal = (b3Vector3){v.x, v.y, v.z, 0.f};\n"
-"	return normalize(normal);	//OpenCL normalize == vector4 normalize\n"
-"}\n"
-"b3Scalar b3Vector3_length2(b3Vector3 v) { return v.x*v.x + v.y*v.y + v.z*v.z; }\n"
-"b3Scalar b3Vector3_dot(b3Vector3 a, b3Vector3 b) { return a.x*b.x + a.y*b.y + a.z*b.z; }\n"
-"int rayIntersectsAabb(b3Vector3 rayOrigin, b3Scalar rayLength, b3Vector3 rayNormalizedDirection, b3AabbCL aabb)\n"
-"{\n"
-"	//AABB is considered as 3 pairs of 2 planes( {x_min, x_max}, {y_min, y_max}, {z_min, z_max} ).\n"
-"	//t_min is the point of intersection with the closer plane, t_max is the point of intersection with the farther plane.\n"
-"	//\n"
-"	//if (rayNormalizedDirection.x < 0.0f), then max.x will be the near plane \n"
-"	//and min.x will be the far plane; otherwise, it is reversed.\n"
-"	//\n"
-"	//In order for there to be a collision, the t_min and t_max of each pair must overlap.\n"
-"	//This can be tested for by selecting the highest t_min and lowest t_max and comparing them.\n"
-"	\n"
-"	int4 isNegative = isless( rayNormalizedDirection, ((b3Vector3){0.0f, 0.0f, 0.0f, 0.0f}) );	//isless(x,y) returns (x < y)\n"
-"	\n"
-"	//When using vector types, the select() function checks the most signficant bit, \n"
-"	//but isless() sets the least significant bit.\n"
-"	isNegative <<= 31;\n"
-"	//select(b, a, condition) == condition ? a : b\n"
-"	//When using select() with vector types, (condition[i]) is true if its most significant bit is 1\n"
-"	b3Vector3 t_min = ( select(aabb.m_min, aabb.m_max, isNegative) - rayOrigin ) / rayNormalizedDirection;\n"
-"	b3Vector3 t_max = ( select(aabb.m_max, aabb.m_min, isNegative) - rayOrigin ) / rayNormalizedDirection;\n"
-"	\n"
-"	b3Scalar t_min_final = 0.0f;\n"
-"	b3Scalar t_max_final = rayLength;\n"
-"	\n"
-"	//Must use fmin()/fmax(); if one of the parameters is NaN, then the parameter that is not NaN is returned. \n"
-"	//Behavior of min()/max() with NaNs is undefined. (See OpenCL Specification 1.2 [6.12.2] and [6.12.4])\n"
-"	//Since the innermost fmin()/fmax() is always not NaN, this should never return NaN.\n"
-"	t_min_final = fmax( t_min.z, fmax(t_min.y, fmax(t_min.x, t_min_final)) );\n"
-"	t_max_final = fmin( t_max.z, fmin(t_max.y, fmin(t_max.x, t_max_final)) );\n"
-"	\n"
-"	return (t_min_final <= t_max_final);\n"
-"}\n"
-"__kernel void plbvhRayTraverse(__global b3AabbCL* rigidAabbs,\n"
-"								__global int* rootNodeIndex, \n"
-"								__global int2* internalNodeChildIndices, \n"
-"								__global b3AabbCL* internalNodeAabbs,\n"
-"								__global int2* internalNodeLeafIndexRanges,\n"
-"								__global SortDataCL* mortonCodesAndAabbIndices,\n"
-"								\n"
-"								__global b3RayInfo* rays,\n"
-"								\n"
-"								__global int* out_numRayRigidPairs, \n"
-"								__global int2* out_rayRigidPairs,\n"
-"								int maxRayRigidPairs, int numRays)\n"
-"{\n"
-"	int rayIndex = get_global_id(0);\n"
-"	if(rayIndex >= numRays) return;\n"
-"	\n"
-"	//\n"
-"	b3Vector3 rayFrom = rays[rayIndex].m_from;\n"
-"	b3Vector3 rayTo = rays[rayIndex].m_to;\n"
-"	b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom);\n"
-"	b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) );\n"
-"	\n"
-"	//\n"
-"	int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE];\n"
-"	\n"
-"	int stackSize = 1;\n"
-"	stack[0] = *rootNodeIndex;\n"
-"	\n"
-"	while(stackSize)\n"
-"	{\n"
-"		int internalOrLeafNodeIndex = stack[ stackSize - 1 ];\n"
-"		--stackSize;\n"
-"		\n"
-"		int isLeaf = isLeafNode(internalOrLeafNodeIndex);	//Internal node if false\n"
-"		int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex);\n"
-"		\n"
-"		//bvhRigidIndex is not used if internal node\n"
-"		int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1;\n"
-"	\n"
-"		b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex];\n"
-"		if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, bvhNodeAabb)  )\n"
-"		{\n"
-"			if(isLeaf)\n"
-"			{\n"
-"				int2 rayRigidPair;\n"
-"				rayRigidPair.x = rayIndex;\n"
-"				rayRigidPair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3];\n"
-"				\n"
-"				int pairIndex = atomic_inc(out_numRayRigidPairs);\n"
-"				if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair;\n"
-"			}\n"
-"			\n"
-"			if(!isLeaf)	//Internal node\n"
-"			{\n"
-"				if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE)\n"
-"				{\n"
-"					//Error\n"
-"				}\n"
-"				else\n"
-"				{\n"
-"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x;\n"
-"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y;\n"
-"				}\n"
-"			}\n"
-"		}\n"
-"	}\n"
-"}\n"
-"__kernel void plbvhLargeAabbAabbTest(__global b3AabbCL* smallAabbs, __global b3AabbCL* largeAabbs, \n"
-"									__global int* out_numPairs, __global int4* out_overlappingPairs, \n"
-"									int maxPairs, int numLargeAabbRigids, int numSmallAabbRigids)\n"
-"{\n"
-"	int smallAabbIndex = get_global_id(0);\n"
-"	if(smallAabbIndex >= numSmallAabbRigids) return;\n"
-"	\n"
-"	b3AabbCL smallAabb = smallAabbs[smallAabbIndex];\n"
-"	for(int i = 0; i < numLargeAabbRigids; ++i)\n"
-"	{\n"
-"		b3AabbCL largeAabb = largeAabbs[i];\n"
-"		if( TestAabbAgainstAabb2(&smallAabb, &largeAabb) )\n"
-"		{\n"
-"			int4 pair;\n"
-"			pair.x = largeAabb.m_minIndices[3];\n"
-"			pair.y = smallAabb.m_minIndices[3];\n"
-"			pair.z = NEW_PAIR_MARKER;\n"
-"			pair.w = NEW_PAIR_MARKER;\n"
-"			\n"
-"			int pairIndex = atomic_inc(out_numPairs);\n"
-"			if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair;\n"
-"		}\n"
-"	}\n"
-"}\n"
-"__kernel void plbvhLargeAabbRayTest(__global b3AabbCL* largeRigidAabbs, __global b3RayInfo* rays,\n"
-"									__global int* out_numRayRigidPairs,  __global int2* out_rayRigidPairs,\n"
-"									int numLargeAabbRigids, int maxRayRigidPairs, int numRays)\n"
-"{\n"
-"	int rayIndex = get_global_id(0);\n"
-"	if(rayIndex >= numRays) return;\n"
-"	\n"
-"	b3Vector3 rayFrom = rays[rayIndex].m_from;\n"
-"	b3Vector3 rayTo = rays[rayIndex].m_to;\n"
-"	b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom);\n"
-"	b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) );\n"
-"	\n"
-"	for(int i = 0; i < numLargeAabbRigids; ++i)\n"
-"	{\n"
-"		b3AabbCL rigidAabb = largeRigidAabbs[i];\n"
-"		if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, rigidAabb) )\n"
-"		{\n"
-"			int2 rayRigidPair;\n"
-"			rayRigidPair.x = rayIndex;\n"
-"			rayRigidPair.y = rigidAabb.m_minIndices[3];\n"
-"			\n"
-"			int pairIndex = atomic_inc(out_numRayRigidPairs);\n"
-"			if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair;\n"
-"		}\n"
-"	}\n"
-"}\n"
-"//Set so that it is always greater than the actual common prefixes, and never selected as a parent node.\n"
-"//If there are no duplicates, then the highest common prefix is 32 or 64, depending on the number of bits used for the z-curve.\n"
-"//Duplicate common prefixes increase the highest common prefix at most by the number of bits used to index the leaf node.\n"
-"//Since 32 bit ints are used to index leaf nodes, the max prefix is 64(32 + 32 bit z-curve) or 96(32 + 64 bit z-curve).\n"
-"#define B3_PLBVH_INVALID_COMMON_PREFIX 128\n"
-"#define B3_PLBVH_ROOT_NODE_MARKER -1\n"
-"#define b3Int64 long\n"
-"int computeCommonPrefixLength(b3Int64 i, b3Int64 j) { return (int)clz(i ^ j); }\n"
-"b3Int64 computeCommonPrefix(b3Int64 i, b3Int64 j) \n"
-"{\n"
-"	//This function only needs to return (i & j) in order for the algorithm to work,\n"
-"	//but it may help with debugging to mask out the lower bits.\n"
-"	b3Int64 commonPrefixLength = (b3Int64)computeCommonPrefixLength(i, j);\n"
-"	b3Int64 sharedBits = i & j;\n"
-"	b3Int64 bitmask = ((b3Int64)(~0)) << (64 - commonPrefixLength);	//Set all bits after the common prefix to 0\n"
-"	\n"
-"	return sharedBits & bitmask;\n"
-"}\n"
-"//Same as computeCommonPrefixLength(), but allows for prefixes with different lengths\n"
-"int getSharedPrefixLength(b3Int64 prefixA, int prefixLengthA, b3Int64 prefixB, int prefixLengthB)\n"
-"{\n"
-"	return b3Min( computeCommonPrefixLength(prefixA, prefixB), b3Min(prefixLengthA, prefixLengthB) );\n"
-"}\n"
-"__kernel void computeAdjacentPairCommonPrefix(__global SortDataCL* mortonCodesAndAabbIndices,\n"
-"											__global b3Int64* out_commonPrefixes,\n"
-"											__global int* out_commonPrefixLengths,\n"
-"											int numInternalNodes)\n"
-"{\n"
-"	int internalNodeIndex = get_global_id(0);\n"
-"	if (internalNodeIndex >= numInternalNodes) return;\n"
-"	\n"
-"	//Here, (internalNodeIndex + 1) is never out of bounds since it is a leaf node index,\n"
-"	//and the number of internal nodes is always numLeafNodes - 1\n"
-"	int leftLeafIndex = internalNodeIndex;\n"
-"	int rightLeafIndex = internalNodeIndex + 1;\n"
-"	\n"
-"	int leftLeafMortonCode = mortonCodesAndAabbIndices[leftLeafIndex].m_key;\n"
-"	int rightLeafMortonCode = mortonCodesAndAabbIndices[rightLeafIndex].m_key;\n"
-"	\n"
-"	//Binary radix tree construction algorithm does not work if there are duplicate morton codes.\n"
-"	//Append the index of each leaf node to each morton code so that there are no duplicates.\n"
-"	//The algorithm also requires that the morton codes are sorted in ascending order; this requirement\n"
-"	//is also satisfied with this method, as (leftLeafIndex < rightLeafIndex) is always true.\n"
-"	//\n"
-"	//upsample(a, b) == ( ((b3Int64)a) << 32) | b\n"
-"	b3Int64 nonduplicateLeftMortonCode = upsample(leftLeafMortonCode, leftLeafIndex);\n"
-"	b3Int64 nonduplicateRightMortonCode = upsample(rightLeafMortonCode, rightLeafIndex);\n"
-"	\n"
-"	out_commonPrefixes[internalNodeIndex] = computeCommonPrefix(nonduplicateLeftMortonCode, nonduplicateRightMortonCode);\n"
-"	out_commonPrefixLengths[internalNodeIndex] = computeCommonPrefixLength(nonduplicateLeftMortonCode, nonduplicateRightMortonCode);\n"
-"}\n"
-"__kernel void buildBinaryRadixTreeLeafNodes(__global int* commonPrefixLengths, __global int* out_leafNodeParentNodes,\n"
-"											__global int2* out_childNodes, int numLeafNodes)\n"
-"{\n"
-"	int leafNodeIndex = get_global_id(0);\n"
-"	if (leafNodeIndex >= numLeafNodes) return;\n"
-"	\n"
-"	int numInternalNodes = numLeafNodes - 1;\n"
-"	\n"
-"	int leftSplitIndex = leafNodeIndex - 1;\n"
-"	int rightSplitIndex = leafNodeIndex;\n"
-"	\n"
-"	int leftCommonPrefix = (leftSplitIndex >= 0) ? commonPrefixLengths[leftSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
-"	int rightCommonPrefix = (rightSplitIndex < numInternalNodes) ? commonPrefixLengths[rightSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
-"	\n"
-"	//Parent node is the highest adjacent common prefix that is lower than the node's common prefix\n"
-"	//Leaf nodes are considered as having the highest common prefix\n"
-"	int isLeftHigherCommonPrefix = (leftCommonPrefix > rightCommonPrefix);\n"
-"	\n"
-"	//Handle cases for the edge nodes; the first and last node\n"
-"	//For leaf nodes, leftCommonPrefix and rightCommonPrefix should never both be B3_PLBVH_INVALID_COMMON_PREFIX\n"
-"	if(leftCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = false;\n"
-"	if(rightCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = true;\n"
-"	\n"
-"	int parentNodeIndex = (isLeftHigherCommonPrefix) ? leftSplitIndex : rightSplitIndex;\n"
-"	out_leafNodeParentNodes[leafNodeIndex] = parentNodeIndex;\n"
-"	\n"
-"	int isRightChild = (isLeftHigherCommonPrefix);	//If the left node is the parent, then this node is its right child and vice versa\n"
-"	\n"
-"	//out_childNodesAsInt[0] == int2.x == left child\n"
-"	//out_childNodesAsInt[1] == int2.y == right child\n"
-"	int isLeaf = 1;\n"
-"	__global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]);\n"
-"	out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, leafNodeIndex);\n"
-"}\n"
-"__kernel void buildBinaryRadixTreeInternalNodes(__global b3Int64* commonPrefixes, __global int* commonPrefixLengths,\n"
-"												__global int2* out_childNodes,\n"
-"												__global int* out_internalNodeParentNodes, __global int* out_rootNodeIndex,\n"
-"												int numInternalNodes)\n"
-"{\n"
-"	int internalNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0);\n"
-"	if(internalNodeIndex >= numInternalNodes) return;\n"
-"	\n"
-"	b3Int64 nodePrefix = commonPrefixes[internalNodeIndex];\n"
-"	int nodePrefixLength = commonPrefixLengths[internalNodeIndex];\n"
-"	\n"
-"//#define USE_LINEAR_SEARCH\n"
-"#ifdef USE_LINEAR_SEARCH\n"
-"	int leftIndex = -1;\n"
-"	int rightIndex = -1;\n"
-"	\n"
-"	//Find nearest element to left with a lower common prefix\n"
-"	for(int i = internalNodeIndex - 1; i >= 0; --i)\n"
-"	{\n"
-"		int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]);\n"
-"		if(nodeLeftSharedPrefixLength < nodePrefixLength)\n"
-"		{\n"
-"			leftIndex = i;\n"
-"			break;\n"
-"		}\n"
-"	}\n"
-"	\n"
-"	//Find nearest element to right with a lower common prefix\n"
-"	for(int i = internalNodeIndex + 1; i < numInternalNodes; ++i)\n"
-"	{\n"
-"		int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]);\n"
-"		if(nodeRightSharedPrefixLength < nodePrefixLength)\n"
-"		{\n"
-"			rightIndex = i;\n"
-"			break;\n"
-"		}\n"
-"	}\n"
-"	\n"
-"#else //Use binary search\n"
-"	//Find nearest element to left with a lower common prefix\n"
-"	int leftIndex = -1;\n"
-"	{\n"
-"		int lower = 0;\n"
-"		int upper = internalNodeIndex - 1;\n"
-"		\n"
-"		while(lower <= upper)\n"
-"		{\n"
-"			int mid = (lower + upper) / 2;\n"
-"			b3Int64 midPrefix = commonPrefixes[mid];\n"
-"			int midPrefixLength = commonPrefixLengths[mid];\n"
-"			\n"
-"			int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength);\n"
-"			if(nodeMidSharedPrefixLength < nodePrefixLength) \n"
-"			{\n"
-"				int right = mid + 1;\n"
-"				if(right < internalNodeIndex)\n"
-"				{\n"
-"					b3Int64 rightPrefix = commonPrefixes[right];\n"
-"					int rightPrefixLength = commonPrefixLengths[right];\n"
-"					\n"
-"					int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, rightPrefix, rightPrefixLength);\n"
-"					if(nodeRightSharedPrefixLength < nodePrefixLength) \n"
-"					{\n"
-"						lower = right;\n"
-"						leftIndex = right;\n"
-"					}\n"
-"					else \n"
-"					{\n"
-"						leftIndex = mid;\n"
-"						break;\n"
-"					}\n"
-"				}\n"
-"				else \n"
-"				{\n"
-"					leftIndex = mid;\n"
-"					break;\n"
-"				}\n"
-"			}\n"
-"			else upper = mid - 1;\n"
-"		}\n"
-"	}\n"
-"	\n"
-"	//Find nearest element to right with a lower common prefix\n"
-"	int rightIndex = -1;\n"
-"	{\n"
-"		int lower = internalNodeIndex + 1;\n"
-"		int upper = numInternalNodes - 1;\n"
-"		\n"
-"		while(lower <= upper)\n"
-"		{\n"
-"			int mid = (lower + upper) / 2;\n"
-"			b3Int64 midPrefix = commonPrefixes[mid];\n"
-"			int midPrefixLength = commonPrefixLengths[mid];\n"
-"			\n"
-"			int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength);\n"
-"			if(nodeMidSharedPrefixLength < nodePrefixLength) \n"
-"			{\n"
-"				int left = mid - 1;\n"
-"				if(left > internalNodeIndex)\n"
-"				{\n"
-"					b3Int64 leftPrefix = commonPrefixes[left];\n"
-"					int leftPrefixLength = commonPrefixLengths[left];\n"
-"				\n"
-"					int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, leftPrefix, leftPrefixLength);\n"
-"					if(nodeLeftSharedPrefixLength < nodePrefixLength) \n"
-"					{\n"
-"						upper = left;\n"
-"						rightIndex = left;\n"
-"					}\n"
-"					else \n"
-"					{\n"
-"						rightIndex = mid;\n"
-"						break;\n"
-"					}\n"
-"				}\n"
-"				else \n"
-"				{\n"
-"					rightIndex = mid;\n"
-"					break;\n"
-"				}\n"
-"			}\n"
-"			else lower = mid + 1;\n"
-"		}\n"
-"	}\n"
-"#endif\n"
-"	\n"
-"	//Select parent\n"
-"	{\n"
-"		int leftPrefixLength = (leftIndex != -1) ? commonPrefixLengths[leftIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
-"		int rightPrefixLength =  (rightIndex != -1) ? commonPrefixLengths[rightIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
-"		\n"
-"		int isLeftHigherPrefixLength = (leftPrefixLength > rightPrefixLength);\n"
-"		\n"
-"		if(leftPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = false;\n"
-"		else if(rightPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = true;\n"
-"		\n"
-"		int parentNodeIndex = (isLeftHigherPrefixLength) ? leftIndex : rightIndex;\n"
-"		\n"
-"		int isRootNode = (leftIndex == -1 && rightIndex == -1);\n"
-"		out_internalNodeParentNodes[internalNodeIndex] = (!isRootNode) ? parentNodeIndex : B3_PLBVH_ROOT_NODE_MARKER;\n"
-"		\n"
-"		int isLeaf = 0;\n"
-"		if(!isRootNode)\n"
-"		{\n"
-"			int isRightChild = (isLeftHigherPrefixLength);	//If the left node is the parent, then this node is its right child and vice versa\n"
-"			\n"
-"			//out_childNodesAsInt[0] == int2.x == left child\n"
-"			//out_childNodesAsInt[1] == int2.y == right child\n"
-"			__global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]);\n"
-"			out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex);\n"
-"		}\n"
-"		else *out_rootNodeIndex = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex);\n"
-"	}\n"
-"}\n"
-"__kernel void findDistanceFromRoot(__global int* rootNodeIndex, __global int* internalNodeParentNodes,\n"
-"									__global int* out_maxDistanceFromRoot, __global int* out_distanceFromRoot, int numInternalNodes)\n"
-"{\n"
-"	if( get_global_id(0) == 0 ) atomic_xchg(out_maxDistanceFromRoot, 0);\n"
-"	int internalNodeIndex = get_global_id(0);\n"
-"	if(internalNodeIndex >= numInternalNodes) return;\n"
-"	\n"
-"	//\n"
-"	int distanceFromRoot = 0;\n"
-"	{\n"
-"		int parentIndex = internalNodeParentNodes[internalNodeIndex];\n"
-"		while(parentIndex != B3_PLBVH_ROOT_NODE_MARKER)\n"
-"		{\n"
-"			parentIndex = internalNodeParentNodes[parentIndex];\n"
-"			++distanceFromRoot;\n"
-"		}\n"
-"	}\n"
-"	out_distanceFromRoot[internalNodeIndex] = distanceFromRoot;\n"
-"	\n"
-"	//\n"
-"	__local int localMaxDistanceFromRoot;\n"
-"	if( get_local_id(0) == 0 ) localMaxDistanceFromRoot = 0;\n"
-"	barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	\n"
-"	atomic_max(&localMaxDistanceFromRoot, distanceFromRoot);\n"
-"	barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	\n"
-"	if( get_local_id(0) == 0 ) atomic_max(out_maxDistanceFromRoot, localMaxDistanceFromRoot);\n"
-"}\n"
-"__kernel void buildBinaryRadixTreeAabbsRecursive(__global int* distanceFromRoot, __global SortDataCL* mortonCodesAndAabbIndices,\n"
-"												__global int2* childNodes,\n"
-"												__global b3AabbCL* leafNodeAabbs, __global b3AabbCL* internalNodeAabbs,\n"
-"												int maxDistanceFromRoot, int processedDistance, int numInternalNodes)\n"
-"{\n"
-"	int internalNodeIndex = get_global_id(0);\n"
-"	if(internalNodeIndex >= numInternalNodes) return;\n"
-"	\n"
-"	int distance = distanceFromRoot[internalNodeIndex];\n"
-"	\n"
-"	if(distance == processedDistance)\n"
-"	{\n"
-"		int leftChildIndex = childNodes[internalNodeIndex].x;\n"
-"		int rightChildIndex = childNodes[internalNodeIndex].y;\n"
-"		\n"
-"		int isLeftChildLeaf = isLeafNode(leftChildIndex);\n"
-"		int isRightChildLeaf = isLeafNode(rightChildIndex);\n"
-"		\n"
-"		leftChildIndex = getIndexWithInternalNodeMarkerRemoved(leftChildIndex);\n"
-"		rightChildIndex = getIndexWithInternalNodeMarkerRemoved(rightChildIndex);\n"
-"		\n"
-"		//leftRigidIndex/rightRigidIndex is not used if internal node\n"
-"		int leftRigidIndex = (isLeftChildLeaf) ? mortonCodesAndAabbIndices[leftChildIndex].m_value : -1;\n"
-"		int rightRigidIndex = (isRightChildLeaf) ? mortonCodesAndAabbIndices[rightChildIndex].m_value : -1;\n"
-"		\n"
-"		b3AabbCL leftChildAabb = (isLeftChildLeaf) ? leafNodeAabbs[leftRigidIndex] : internalNodeAabbs[leftChildIndex];\n"
-"		b3AabbCL rightChildAabb = (isRightChildLeaf) ? leafNodeAabbs[rightRigidIndex] : internalNodeAabbs[rightChildIndex];\n"
-"		\n"
-"		b3AabbCL mergedAabb;\n"
-"		mergedAabb.m_min = b3Min(leftChildAabb.m_min, rightChildAabb.m_min);\n"
-"		mergedAabb.m_max = b3Max(leftChildAabb.m_max, rightChildAabb.m_max);\n"
-"		internalNodeAabbs[internalNodeIndex] = mergedAabb;\n"
-"	}\n"
-"}\n"
-"__kernel void findLeafIndexRanges(__global int2* internalNodeChildNodes, __global int2* out_leafIndexRanges, int numInternalNodes)\n"
-"{\n"
-"	int internalNodeIndex = get_global_id(0);\n"
-"	if(internalNodeIndex >= numInternalNodes) return;\n"
-"	\n"
-"	int numLeafNodes = numInternalNodes + 1;\n"
-"	\n"
-"	int2 childNodes = internalNodeChildNodes[internalNodeIndex];\n"
-"	\n"
-"	int2 leafIndexRange;	//x == min leaf index, y == max leaf index\n"
-"	\n"
-"	//Find lowest leaf index covered by this internal node\n"
-"	{\n"
-"		int lowestIndex = childNodes.x;		//childNodes.x == Left child\n"
-"		while( !isLeafNode(lowestIndex) ) lowestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(lowestIndex) ].x;\n"
-"		leafIndexRange.x = lowestIndex;\n"
-"	}\n"
-"	\n"
-"	//Find highest leaf index covered by this internal node\n"
-"	{\n"
-"		int highestIndex = childNodes.y;	//childNodes.y == Right child\n"
-"		while( !isLeafNode(highestIndex) ) highestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(highestIndex) ].y;\n"
-"		leafIndexRange.y = highestIndex;\n"
-"	}\n"
-"	\n"
-"	//\n"
-"	out_leafIndexRanges[internalNodeIndex] = leafIndexRange;\n"
-"}\n"
-;
+static const char* parallelLinearBvhCL =
+	"/*\n"
+	"This software is provided 'as-is', without any express or implied warranty.\n"
+	"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+	"Permission is granted to anyone to use this software for any purpose,\n"
+	"including commercial applications, and to alter it and redistribute it freely,\n"
+	"subject to the following restrictions:\n"
+	"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.\n"
+	"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+	"3. This notice may not be removed or altered from any source distribution.\n"
+	"*/\n"
+	"//Initial Author Jackson Lee, 2014\n"
+	"typedef float b3Scalar;\n"
+	"typedef float4 b3Vector3;\n"
+	"#define b3Max max\n"
+	"#define b3Min min\n"
+	"#define b3Sqrt sqrt\n"
+	"typedef struct\n"
+	"{\n"
+	"	unsigned int m_key;\n"
+	"	unsigned int m_value;\n"
+	"} SortDataCL;\n"
+	"typedef struct \n"
+	"{\n"
+	"	union\n"
+	"	{\n"
+	"		float4	m_min;\n"
+	"		float   m_minElems[4];\n"
+	"		int			m_minIndices[4];\n"
+	"	};\n"
+	"	union\n"
+	"	{\n"
+	"		float4	m_max;\n"
+	"		float   m_maxElems[4];\n"
+	"		int			m_maxIndices[4];\n"
+	"	};\n"
+	"} b3AabbCL;\n"
+	"unsigned int interleaveBits(unsigned int x)\n"
+	"{\n"
+	"	//........ ........ ......12 3456789A	//x\n"
+	"	//....1..2 ..3..4.. 5..6..7. .8..9..A	//x after interleaving bits\n"
+	"	\n"
+	"	//......12 3456789A ......12 3456789A	//x ^ (x << 16)\n"
+	"	//11111111 ........ ........ 11111111	//0x FF 00 00 FF\n"
+	"	//......12 ........ ........ 3456789A	//x = (x ^ (x << 16)) & 0xFF0000FF;\n"
+	"	\n"
+	"	//......12 ........ 3456789A 3456789A	//x ^ (x <<  8)\n"
+	"	//......11 ........ 1111.... ....1111	//0x 03 00 F0 0F\n"
+	"	//......12 ........ 3456.... ....789A	//x = (x ^ (x <<  8)) & 0x0300F00F;\n"
+	"	\n"
+	"	//..12..12 ....3456 3456.... 789A789A	//x ^ (x <<  4)\n"
+	"	//......11 ....11.. ..11.... 11....11	//0x 03 0C 30 C3\n"
+	"	//......12 ....34.. ..56.... 78....9A	//x = (x ^ (x <<  4)) & 0x030C30C3;\n"
+	"	\n"
+	"	//....1212 ..3434.. 5656..78 78..9A9A	//x ^ (x <<  2)\n"
+	"	//....1..1 ..1..1.. 1..1..1. .1..1..1	//0x 09 24 92 49\n"
+	"	//....1..2 ..3..4.. 5..6..7. .8..9..A	//x = (x ^ (x <<  2)) & 0x09249249;\n"
+	"	\n"
+	"	//........ ........ ......11 11111111	//0x000003FF\n"
+	"	x &= 0x000003FF;		//Clear all bits above bit 10\n"
+	"	\n"
+	"	x = (x ^ (x << 16)) & 0xFF0000FF;\n"
+	"	x = (x ^ (x <<  8)) & 0x0300F00F;\n"
+	"	x = (x ^ (x <<  4)) & 0x030C30C3;\n"
+	"	x = (x ^ (x <<  2)) & 0x09249249;\n"
+	"	\n"
+	"	return x;\n"
+	"}\n"
+	"unsigned int getMortonCode(unsigned int x, unsigned int y, unsigned int z)\n"
+	"{\n"
+	"	return interleaveBits(x) << 0 | interleaveBits(y) << 1 | interleaveBits(z) << 2;\n"
+	"}\n"
+	"__kernel void separateAabbs(__global b3AabbCL* unseparatedAabbs, __global int* aabbIndices, __global b3AabbCL* out_aabbs, int numAabbsToSeparate)\n"
+	"{\n"
+	"	int separatedAabbIndex = get_global_id(0);\n"
+	"	if(separatedAabbIndex >= numAabbsToSeparate) return;\n"
+	"	int unseparatedAabbIndex = aabbIndices[separatedAabbIndex];\n"
+	"	out_aabbs[separatedAabbIndex] = unseparatedAabbs[unseparatedAabbIndex];\n"
+	"}\n"
+	"//Should replace with an optimized parallel reduction\n"
+	"__kernel void findAllNodesMergedAabb(__global b3AabbCL* out_mergedAabb, int numAabbsNeedingMerge)\n"
+	"{\n"
+	"	//Each time this kernel is added to the command queue, \n"
+	"	//the number of AABBs needing to be merged is halved\n"
+	"	//\n"
+	"	//Example with 159 AABBs:\n"
+	"	//	numRemainingAabbs == 159 / 2 + 159 % 2 == 80\n"
+	"	//	numMergedAabbs == 159 - 80 == 79\n"
+	"	//So, indices [0, 78] are merged with [0 + 80, 78 + 80]\n"
+	"	\n"
+	"	int numRemainingAabbs = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2;\n"
+	"	int numMergedAabbs = numAabbsNeedingMerge - numRemainingAabbs;\n"
+	"	\n"
+	"	int aabbIndex = get_global_id(0);\n"
+	"	if(aabbIndex >= numMergedAabbs) return;\n"
+	"	\n"
+	"	int otherAabbIndex = aabbIndex + numRemainingAabbs;\n"
+	"	\n"
+	"	b3AabbCL aabb = out_mergedAabb[aabbIndex];\n"
+	"	b3AabbCL otherAabb = out_mergedAabb[otherAabbIndex];\n"
+	"		\n"
+	"	b3AabbCL mergedAabb;\n"
+	"	mergedAabb.m_min = b3Min(aabb.m_min, otherAabb.m_min);\n"
+	"	mergedAabb.m_max = b3Max(aabb.m_max, otherAabb.m_max);\n"
+	"	out_mergedAabb[aabbIndex] = mergedAabb;\n"
+	"}\n"
+	"__kernel void assignMortonCodesAndAabbIndicies(__global b3AabbCL* worldSpaceAabbs, __global b3AabbCL* mergedAabbOfAllNodes, \n"
+	"												__global SortDataCL* out_mortonCodesAndAabbIndices, int numAabbs)\n"
+	"{\n"
+	"	int leafNodeIndex = get_global_id(0);	//Leaf node index == AABB index\n"
+	"	if(leafNodeIndex >= numAabbs) return;\n"
+	"	\n"
+	"	b3AabbCL mergedAabb = mergedAabbOfAllNodes[0];\n"
+	"	b3Vector3 gridCenter = (mergedAabb.m_min + mergedAabb.m_max) * 0.5f;\n"
+	"	b3Vector3 gridCellSize = (mergedAabb.m_max - mergedAabb.m_min) / (float)1024;\n"
+	"	\n"
+	"	b3AabbCL aabb = worldSpaceAabbs[leafNodeIndex];\n"
+	"	b3Vector3 aabbCenter = (aabb.m_min + aabb.m_max) * 0.5f;\n"
+	"	b3Vector3 aabbCenterRelativeToGrid = aabbCenter - gridCenter;\n"
+	"	\n"
+	"	//Quantize into integer coordinates\n"
+	"	//floor() is needed to prevent the center cell, at (0,0,0) from being twice the size\n"
+	"	b3Vector3 gridPosition = aabbCenterRelativeToGrid / gridCellSize;\n"
+	"	\n"
+	"	int4 discretePosition;\n"
+	"	discretePosition.x = (int)( (gridPosition.x >= 0.0f) ? gridPosition.x : floor(gridPosition.x) );\n"
+	"	discretePosition.y = (int)( (gridPosition.y >= 0.0f) ? gridPosition.y : floor(gridPosition.y) );\n"
+	"	discretePosition.z = (int)( (gridPosition.z >= 0.0f) ? gridPosition.z : floor(gridPosition.z) );\n"
+	"	\n"
+	"	//Clamp coordinates into [-512, 511], then convert range from [-512, 511] to [0, 1023]\n"
+	"	discretePosition = b3Max( -512, b3Min(discretePosition, 511) );\n"
+	"	discretePosition += 512;\n"
+	"	\n"
+	"	//Interleave bits(assign a morton code, also known as a z-curve)\n"
+	"	unsigned int mortonCode = getMortonCode(discretePosition.x, discretePosition.y, discretePosition.z);\n"
+	"	\n"
+	"	//\n"
+	"	SortDataCL mortonCodeIndexPair;\n"
+	"	mortonCodeIndexPair.m_key = mortonCode;\n"
+	"	mortonCodeIndexPair.m_value = leafNodeIndex;\n"
+	"	\n"
+	"	out_mortonCodesAndAabbIndices[leafNodeIndex] = mortonCodeIndexPair;\n"
+	"}\n"
+	"#define B3_PLVBH_TRAVERSE_MAX_STACK_SIZE 128\n"
+	"//The most significant bit(0x80000000) of a int32 is used to distinguish between leaf and internal nodes.\n"
+	"//If it is set, then the index is for an internal node; otherwise, it is a leaf node. \n"
+	"//In both cases, the bit should be cleared to access the actual node index.\n"
+	"int isLeafNode(int index) { return (index >> 31 == 0); }\n"
+	"int getIndexWithInternalNodeMarkerRemoved(int index) { return index & (~0x80000000); }\n"
+	"int getIndexWithInternalNodeMarkerSet(int isLeaf, int index) { return (isLeaf) ? index : (index | 0x80000000); }\n"
+	"//From sap.cl\n"
+	"#define NEW_PAIR_MARKER -1\n"
+	"bool TestAabbAgainstAabb2(const b3AabbCL* aabb1, const b3AabbCL* aabb2)\n"
+	"{\n"
+	"	bool overlap = true;\n"
+	"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
+	"	return overlap;\n"
+	"}\n"
+	"//From sap.cl\n"
+	"__kernel void plbvhCalculateOverlappingPairs(__global b3AabbCL* rigidAabbs, \n"
+	"											__global int* rootNodeIndex, \n"
+	"											__global int2* internalNodeChildIndices, \n"
+	"											__global b3AabbCL* internalNodeAabbs,\n"
+	"											__global int2* internalNodeLeafIndexRanges,\n"
+	"											\n"
+	"											__global SortDataCL* mortonCodesAndAabbIndices,\n"
+	"											__global int* out_numPairs, __global int4* out_overlappingPairs, \n"
+	"											int maxPairs, int numQueryAabbs)\n"
+	"{\n"
+	"	//Using get_group_id()/get_local_id() is Faster than get_global_id(0) since\n"
+	"	//mortonCodesAndAabbIndices[] contains rigid body indices sorted along the z-curve (more spatially coherent)\n"
+	"	int queryBvhNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0);\n"
+	"	if(queryBvhNodeIndex >= numQueryAabbs) return;\n"
+	"	\n"
+	"	int queryRigidIndex = mortonCodesAndAabbIndices[queryBvhNodeIndex].m_value;\n"
+	"	b3AabbCL queryAabb = rigidAabbs[queryRigidIndex];\n"
+	"	\n"
+	"	int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE];\n"
+	"	\n"
+	"	int stackSize = 1;\n"
+	"	stack[0] = *rootNodeIndex;\n"
+	"	\n"
+	"	while(stackSize)\n"
+	"	{\n"
+	"		int internalOrLeafNodeIndex = stack[ stackSize - 1 ];\n"
+	"		--stackSize;\n"
+	"		\n"
+	"		int isLeaf = isLeafNode(internalOrLeafNodeIndex);	//Internal node if false\n"
+	"		int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex);\n"
+	"		\n"
+	"		//Optimization - if the BVH is structured as a binary radix tree, then\n"
+	"		//each internal node corresponds to a contiguous range of leaf nodes(internalNodeLeafIndexRanges[]).\n"
+	"		//This can be used to avoid testing each AABB-AABB pair twice, including preventing each node from colliding with itself.\n"
+	"		{\n"
+	"			int highestLeafIndex = (isLeaf) ? bvhNodeIndex : internalNodeLeafIndexRanges[bvhNodeIndex].y;\n"
+	"			if(highestLeafIndex <= queryBvhNodeIndex) continue;\n"
+	"		}\n"
+	"		\n"
+	"		//bvhRigidIndex is not used if internal node\n"
+	"		int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1;\n"
+	"	\n"
+	"		b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex];\n"
+	"		if( TestAabbAgainstAabb2(&queryAabb, &bvhNodeAabb) )\n"
+	"		{\n"
+	"			if(isLeaf)\n"
+	"			{\n"
+	"				int4 pair;\n"
+	"				pair.x = rigidAabbs[queryRigidIndex].m_minIndices[3];\n"
+	"				pair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3];\n"
+	"				pair.z = NEW_PAIR_MARKER;\n"
+	"				pair.w = NEW_PAIR_MARKER;\n"
+	"				\n"
+	"				int pairIndex = atomic_inc(out_numPairs);\n"
+	"				if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair;\n"
+	"			}\n"
+	"			\n"
+	"			if(!isLeaf)	//Internal node\n"
+	"			{\n"
+	"				if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE)\n"
+	"				{\n"
+	"					//Error\n"
+	"				}\n"
+	"				else\n"
+	"				{\n"
+	"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x;\n"
+	"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y;\n"
+	"				}\n"
+	"			}\n"
+	"		}\n"
+	"		\n"
+	"	}\n"
+	"}\n"
+	"//From rayCastKernels.cl\n"
+	"typedef struct\n"
+	"{\n"
+	"	float4 m_from;\n"
+	"	float4 m_to;\n"
+	"} b3RayInfo;\n"
+	"//From rayCastKernels.cl\n"
+	"b3Vector3 b3Vector3_normalize(b3Vector3 v)\n"
+	"{\n"
+	"	b3Vector3 normal = (b3Vector3){v.x, v.y, v.z, 0.f};\n"
+	"	return normalize(normal);	//OpenCL normalize == vector4 normalize\n"
+	"}\n"
+	"b3Scalar b3Vector3_length2(b3Vector3 v) { return v.x*v.x + v.y*v.y + v.z*v.z; }\n"
+	"b3Scalar b3Vector3_dot(b3Vector3 a, b3Vector3 b) { return a.x*b.x + a.y*b.y + a.z*b.z; }\n"
+	"int rayIntersectsAabb(b3Vector3 rayOrigin, b3Scalar rayLength, b3Vector3 rayNormalizedDirection, b3AabbCL aabb)\n"
+	"{\n"
+	"	//AABB is considered as 3 pairs of 2 planes( {x_min, x_max}, {y_min, y_max}, {z_min, z_max} ).\n"
+	"	//t_min is the point of intersection with the closer plane, t_max is the point of intersection with the farther plane.\n"
+	"	//\n"
+	"	//if (rayNormalizedDirection.x < 0.0f), then max.x will be the near plane \n"
+	"	//and min.x will be the far plane; otherwise, it is reversed.\n"
+	"	//\n"
+	"	//In order for there to be a collision, the t_min and t_max of each pair must overlap.\n"
+	"	//This can be tested for by selecting the highest t_min and lowest t_max and comparing them.\n"
+	"	\n"
+	"	int4 isNegative = isless( rayNormalizedDirection, ((b3Vector3){0.0f, 0.0f, 0.0f, 0.0f}) );	//isless(x,y) returns (x < y)\n"
+	"	\n"
+	"	//When using vector types, the select() function checks the most signficant bit, \n"
+	"	//but isless() sets the least significant bit.\n"
+	"	isNegative <<= 31;\n"
+	"	//select(b, a, condition) == condition ? a : b\n"
+	"	//When using select() with vector types, (condition[i]) is true if its most significant bit is 1\n"
+	"	b3Vector3 t_min = ( select(aabb.m_min, aabb.m_max, isNegative) - rayOrigin ) / rayNormalizedDirection;\n"
+	"	b3Vector3 t_max = ( select(aabb.m_max, aabb.m_min, isNegative) - rayOrigin ) / rayNormalizedDirection;\n"
+	"	\n"
+	"	b3Scalar t_min_final = 0.0f;\n"
+	"	b3Scalar t_max_final = rayLength;\n"
+	"	\n"
+	"	//Must use fmin()/fmax(); if one of the parameters is NaN, then the parameter that is not NaN is returned. \n"
+	"	//Behavior of min()/max() with NaNs is undefined. (See OpenCL Specification 1.2 [6.12.2] and [6.12.4])\n"
+	"	//Since the innermost fmin()/fmax() is always not NaN, this should never return NaN.\n"
+	"	t_min_final = fmax( t_min.z, fmax(t_min.y, fmax(t_min.x, t_min_final)) );\n"
+	"	t_max_final = fmin( t_max.z, fmin(t_max.y, fmin(t_max.x, t_max_final)) );\n"
+	"	\n"
+	"	return (t_min_final <= t_max_final);\n"
+	"}\n"
+	"__kernel void plbvhRayTraverse(__global b3AabbCL* rigidAabbs,\n"
+	"								__global int* rootNodeIndex, \n"
+	"								__global int2* internalNodeChildIndices, \n"
+	"								__global b3AabbCL* internalNodeAabbs,\n"
+	"								__global int2* internalNodeLeafIndexRanges,\n"
+	"								__global SortDataCL* mortonCodesAndAabbIndices,\n"
+	"								\n"
+	"								__global b3RayInfo* rays,\n"
+	"								\n"
+	"								__global int* out_numRayRigidPairs, \n"
+	"								__global int2* out_rayRigidPairs,\n"
+	"								int maxRayRigidPairs, int numRays)\n"
+	"{\n"
+	"	int rayIndex = get_global_id(0);\n"
+	"	if(rayIndex >= numRays) return;\n"
+	"	\n"
+	"	//\n"
+	"	b3Vector3 rayFrom = rays[rayIndex].m_from;\n"
+	"	b3Vector3 rayTo = rays[rayIndex].m_to;\n"
+	"	b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom);\n"
+	"	b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) );\n"
+	"	\n"
+	"	//\n"
+	"	int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE];\n"
+	"	\n"
+	"	int stackSize = 1;\n"
+	"	stack[0] = *rootNodeIndex;\n"
+	"	\n"
+	"	while(stackSize)\n"
+	"	{\n"
+	"		int internalOrLeafNodeIndex = stack[ stackSize - 1 ];\n"
+	"		--stackSize;\n"
+	"		\n"
+	"		int isLeaf = isLeafNode(internalOrLeafNodeIndex);	//Internal node if false\n"
+	"		int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex);\n"
+	"		\n"
+	"		//bvhRigidIndex is not used if internal node\n"
+	"		int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1;\n"
+	"	\n"
+	"		b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex];\n"
+	"		if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, bvhNodeAabb)  )\n"
+	"		{\n"
+	"			if(isLeaf)\n"
+	"			{\n"
+	"				int2 rayRigidPair;\n"
+	"				rayRigidPair.x = rayIndex;\n"
+	"				rayRigidPair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3];\n"
+	"				\n"
+	"				int pairIndex = atomic_inc(out_numRayRigidPairs);\n"
+	"				if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair;\n"
+	"			}\n"
+	"			\n"
+	"			if(!isLeaf)	//Internal node\n"
+	"			{\n"
+	"				if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE)\n"
+	"				{\n"
+	"					//Error\n"
+	"				}\n"
+	"				else\n"
+	"				{\n"
+	"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x;\n"
+	"					stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y;\n"
+	"				}\n"
+	"			}\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"__kernel void plbvhLargeAabbAabbTest(__global b3AabbCL* smallAabbs, __global b3AabbCL* largeAabbs, \n"
+	"									__global int* out_numPairs, __global int4* out_overlappingPairs, \n"
+	"									int maxPairs, int numLargeAabbRigids, int numSmallAabbRigids)\n"
+	"{\n"
+	"	int smallAabbIndex = get_global_id(0);\n"
+	"	if(smallAabbIndex >= numSmallAabbRigids) return;\n"
+	"	\n"
+	"	b3AabbCL smallAabb = smallAabbs[smallAabbIndex];\n"
+	"	for(int i = 0; i < numLargeAabbRigids; ++i)\n"
+	"	{\n"
+	"		b3AabbCL largeAabb = largeAabbs[i];\n"
+	"		if( TestAabbAgainstAabb2(&smallAabb, &largeAabb) )\n"
+	"		{\n"
+	"			int4 pair;\n"
+	"			pair.x = largeAabb.m_minIndices[3];\n"
+	"			pair.y = smallAabb.m_minIndices[3];\n"
+	"			pair.z = NEW_PAIR_MARKER;\n"
+	"			pair.w = NEW_PAIR_MARKER;\n"
+	"			\n"
+	"			int pairIndex = atomic_inc(out_numPairs);\n"
+	"			if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair;\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"__kernel void plbvhLargeAabbRayTest(__global b3AabbCL* largeRigidAabbs, __global b3RayInfo* rays,\n"
+	"									__global int* out_numRayRigidPairs,  __global int2* out_rayRigidPairs,\n"
+	"									int numLargeAabbRigids, int maxRayRigidPairs, int numRays)\n"
+	"{\n"
+	"	int rayIndex = get_global_id(0);\n"
+	"	if(rayIndex >= numRays) return;\n"
+	"	\n"
+	"	b3Vector3 rayFrom = rays[rayIndex].m_from;\n"
+	"	b3Vector3 rayTo = rays[rayIndex].m_to;\n"
+	"	b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom);\n"
+	"	b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) );\n"
+	"	\n"
+	"	for(int i = 0; i < numLargeAabbRigids; ++i)\n"
+	"	{\n"
+	"		b3AabbCL rigidAabb = largeRigidAabbs[i];\n"
+	"		if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, rigidAabb) )\n"
+	"		{\n"
+	"			int2 rayRigidPair;\n"
+	"			rayRigidPair.x = rayIndex;\n"
+	"			rayRigidPair.y = rigidAabb.m_minIndices[3];\n"
+	"			\n"
+	"			int pairIndex = atomic_inc(out_numRayRigidPairs);\n"
+	"			if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair;\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"//Set so that it is always greater than the actual common prefixes, and never selected as a parent node.\n"
+	"//If there are no duplicates, then the highest common prefix is 32 or 64, depending on the number of bits used for the z-curve.\n"
+	"//Duplicate common prefixes increase the highest common prefix at most by the number of bits used to index the leaf node.\n"
+	"//Since 32 bit ints are used to index leaf nodes, the max prefix is 64(32 + 32 bit z-curve) or 96(32 + 64 bit z-curve).\n"
+	"#define B3_PLBVH_INVALID_COMMON_PREFIX 128\n"
+	"#define B3_PLBVH_ROOT_NODE_MARKER -1\n"
+	"#define b3Int64 long\n"
+	"int computeCommonPrefixLength(b3Int64 i, b3Int64 j) { return (int)clz(i ^ j); }\n"
+	"b3Int64 computeCommonPrefix(b3Int64 i, b3Int64 j) \n"
+	"{\n"
+	"	//This function only needs to return (i & j) in order for the algorithm to work,\n"
+	"	//but it may help with debugging to mask out the lower bits.\n"
+	"	b3Int64 commonPrefixLength = (b3Int64)computeCommonPrefixLength(i, j);\n"
+	"	b3Int64 sharedBits = i & j;\n"
+	"	b3Int64 bitmask = ((b3Int64)(~0)) << (64 - commonPrefixLength);	//Set all bits after the common prefix to 0\n"
+	"	\n"
+	"	return sharedBits & bitmask;\n"
+	"}\n"
+	"//Same as computeCommonPrefixLength(), but allows for prefixes with different lengths\n"
+	"int getSharedPrefixLength(b3Int64 prefixA, int prefixLengthA, b3Int64 prefixB, int prefixLengthB)\n"
+	"{\n"
+	"	return b3Min( computeCommonPrefixLength(prefixA, prefixB), b3Min(prefixLengthA, prefixLengthB) );\n"
+	"}\n"
+	"__kernel void computeAdjacentPairCommonPrefix(__global SortDataCL* mortonCodesAndAabbIndices,\n"
+	"											__global b3Int64* out_commonPrefixes,\n"
+	"											__global int* out_commonPrefixLengths,\n"
+	"											int numInternalNodes)\n"
+	"{\n"
+	"	int internalNodeIndex = get_global_id(0);\n"
+	"	if (internalNodeIndex >= numInternalNodes) return;\n"
+	"	\n"
+	"	//Here, (internalNodeIndex + 1) is never out of bounds since it is a leaf node index,\n"
+	"	//and the number of internal nodes is always numLeafNodes - 1\n"
+	"	int leftLeafIndex = internalNodeIndex;\n"
+	"	int rightLeafIndex = internalNodeIndex + 1;\n"
+	"	\n"
+	"	int leftLeafMortonCode = mortonCodesAndAabbIndices[leftLeafIndex].m_key;\n"
+	"	int rightLeafMortonCode = mortonCodesAndAabbIndices[rightLeafIndex].m_key;\n"
+	"	\n"
+	"	//Binary radix tree construction algorithm does not work if there are duplicate morton codes.\n"
+	"	//Append the index of each leaf node to each morton code so that there are no duplicates.\n"
+	"	//The algorithm also requires that the morton codes are sorted in ascending order; this requirement\n"
+	"	//is also satisfied with this method, as (leftLeafIndex < rightLeafIndex) is always true.\n"
+	"	//\n"
+	"	//upsample(a, b) == ( ((b3Int64)a) << 32) | b\n"
+	"	b3Int64 nonduplicateLeftMortonCode = upsample(leftLeafMortonCode, leftLeafIndex);\n"
+	"	b3Int64 nonduplicateRightMortonCode = upsample(rightLeafMortonCode, rightLeafIndex);\n"
+	"	\n"
+	"	out_commonPrefixes[internalNodeIndex] = computeCommonPrefix(nonduplicateLeftMortonCode, nonduplicateRightMortonCode);\n"
+	"	out_commonPrefixLengths[internalNodeIndex] = computeCommonPrefixLength(nonduplicateLeftMortonCode, nonduplicateRightMortonCode);\n"
+	"}\n"
+	"__kernel void buildBinaryRadixTreeLeafNodes(__global int* commonPrefixLengths, __global int* out_leafNodeParentNodes,\n"
+	"											__global int2* out_childNodes, int numLeafNodes)\n"
+	"{\n"
+	"	int leafNodeIndex = get_global_id(0);\n"
+	"	if (leafNodeIndex >= numLeafNodes) return;\n"
+	"	\n"
+	"	int numInternalNodes = numLeafNodes - 1;\n"
+	"	\n"
+	"	int leftSplitIndex = leafNodeIndex - 1;\n"
+	"	int rightSplitIndex = leafNodeIndex;\n"
+	"	\n"
+	"	int leftCommonPrefix = (leftSplitIndex >= 0) ? commonPrefixLengths[leftSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
+	"	int rightCommonPrefix = (rightSplitIndex < numInternalNodes) ? commonPrefixLengths[rightSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
+	"	\n"
+	"	//Parent node is the highest adjacent common prefix that is lower than the node's common prefix\n"
+	"	//Leaf nodes are considered as having the highest common prefix\n"
+	"	int isLeftHigherCommonPrefix = (leftCommonPrefix > rightCommonPrefix);\n"
+	"	\n"
+	"	//Handle cases for the edge nodes; the first and last node\n"
+	"	//For leaf nodes, leftCommonPrefix and rightCommonPrefix should never both be B3_PLBVH_INVALID_COMMON_PREFIX\n"
+	"	if(leftCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = false;\n"
+	"	if(rightCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = true;\n"
+	"	\n"
+	"	int parentNodeIndex = (isLeftHigherCommonPrefix) ? leftSplitIndex : rightSplitIndex;\n"
+	"	out_leafNodeParentNodes[leafNodeIndex] = parentNodeIndex;\n"
+	"	\n"
+	"	int isRightChild = (isLeftHigherCommonPrefix);	//If the left node is the parent, then this node is its right child and vice versa\n"
+	"	\n"
+	"	//out_childNodesAsInt[0] == int2.x == left child\n"
+	"	//out_childNodesAsInt[1] == int2.y == right child\n"
+	"	int isLeaf = 1;\n"
+	"	__global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]);\n"
+	"	out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, leafNodeIndex);\n"
+	"}\n"
+	"__kernel void buildBinaryRadixTreeInternalNodes(__global b3Int64* commonPrefixes, __global int* commonPrefixLengths,\n"
+	"												__global int2* out_childNodes,\n"
+	"												__global int* out_internalNodeParentNodes, __global int* out_rootNodeIndex,\n"
+	"												int numInternalNodes)\n"
+	"{\n"
+	"	int internalNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0);\n"
+	"	if(internalNodeIndex >= numInternalNodes) return;\n"
+	"	\n"
+	"	b3Int64 nodePrefix = commonPrefixes[internalNodeIndex];\n"
+	"	int nodePrefixLength = commonPrefixLengths[internalNodeIndex];\n"
+	"	\n"
+	"//#define USE_LINEAR_SEARCH\n"
+	"#ifdef USE_LINEAR_SEARCH\n"
+	"	int leftIndex = -1;\n"
+	"	int rightIndex = -1;\n"
+	"	\n"
+	"	//Find nearest element to left with a lower common prefix\n"
+	"	for(int i = internalNodeIndex - 1; i >= 0; --i)\n"
+	"	{\n"
+	"		int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]);\n"
+	"		if(nodeLeftSharedPrefixLength < nodePrefixLength)\n"
+	"		{\n"
+	"			leftIndex = i;\n"
+	"			break;\n"
+	"		}\n"
+	"	}\n"
+	"	\n"
+	"	//Find nearest element to right with a lower common prefix\n"
+	"	for(int i = internalNodeIndex + 1; i < numInternalNodes; ++i)\n"
+	"	{\n"
+	"		int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]);\n"
+	"		if(nodeRightSharedPrefixLength < nodePrefixLength)\n"
+	"		{\n"
+	"			rightIndex = i;\n"
+	"			break;\n"
+	"		}\n"
+	"	}\n"
+	"	\n"
+	"#else //Use binary search\n"
+	"	//Find nearest element to left with a lower common prefix\n"
+	"	int leftIndex = -1;\n"
+	"	{\n"
+	"		int lower = 0;\n"
+	"		int upper = internalNodeIndex - 1;\n"
+	"		\n"
+	"		while(lower <= upper)\n"
+	"		{\n"
+	"			int mid = (lower + upper) / 2;\n"
+	"			b3Int64 midPrefix = commonPrefixes[mid];\n"
+	"			int midPrefixLength = commonPrefixLengths[mid];\n"
+	"			\n"
+	"			int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength);\n"
+	"			if(nodeMidSharedPrefixLength < nodePrefixLength) \n"
+	"			{\n"
+	"				int right = mid + 1;\n"
+	"				if(right < internalNodeIndex)\n"
+	"				{\n"
+	"					b3Int64 rightPrefix = commonPrefixes[right];\n"
+	"					int rightPrefixLength = commonPrefixLengths[right];\n"
+	"					\n"
+	"					int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, rightPrefix, rightPrefixLength);\n"
+	"					if(nodeRightSharedPrefixLength < nodePrefixLength) \n"
+	"					{\n"
+	"						lower = right;\n"
+	"						leftIndex = right;\n"
+	"					}\n"
+	"					else \n"
+	"					{\n"
+	"						leftIndex = mid;\n"
+	"						break;\n"
+	"					}\n"
+	"				}\n"
+	"				else \n"
+	"				{\n"
+	"					leftIndex = mid;\n"
+	"					break;\n"
+	"				}\n"
+	"			}\n"
+	"			else upper = mid - 1;\n"
+	"		}\n"
+	"	}\n"
+	"	\n"
+	"	//Find nearest element to right with a lower common prefix\n"
+	"	int rightIndex = -1;\n"
+	"	{\n"
+	"		int lower = internalNodeIndex + 1;\n"
+	"		int upper = numInternalNodes - 1;\n"
+	"		\n"
+	"		while(lower <= upper)\n"
+	"		{\n"
+	"			int mid = (lower + upper) / 2;\n"
+	"			b3Int64 midPrefix = commonPrefixes[mid];\n"
+	"			int midPrefixLength = commonPrefixLengths[mid];\n"
+	"			\n"
+	"			int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength);\n"
+	"			if(nodeMidSharedPrefixLength < nodePrefixLength) \n"
+	"			{\n"
+	"				int left = mid - 1;\n"
+	"				if(left > internalNodeIndex)\n"
+	"				{\n"
+	"					b3Int64 leftPrefix = commonPrefixes[left];\n"
+	"					int leftPrefixLength = commonPrefixLengths[left];\n"
+	"				\n"
+	"					int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, leftPrefix, leftPrefixLength);\n"
+	"					if(nodeLeftSharedPrefixLength < nodePrefixLength) \n"
+	"					{\n"
+	"						upper = left;\n"
+	"						rightIndex = left;\n"
+	"					}\n"
+	"					else \n"
+	"					{\n"
+	"						rightIndex = mid;\n"
+	"						break;\n"
+	"					}\n"
+	"				}\n"
+	"				else \n"
+	"				{\n"
+	"					rightIndex = mid;\n"
+	"					break;\n"
+	"				}\n"
+	"			}\n"
+	"			else lower = mid + 1;\n"
+	"		}\n"
+	"	}\n"
+	"#endif\n"
+	"	\n"
+	"	//Select parent\n"
+	"	{\n"
+	"		int leftPrefixLength = (leftIndex != -1) ? commonPrefixLengths[leftIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
+	"		int rightPrefixLength =  (rightIndex != -1) ? commonPrefixLengths[rightIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n"
+	"		\n"
+	"		int isLeftHigherPrefixLength = (leftPrefixLength > rightPrefixLength);\n"
+	"		\n"
+	"		if(leftPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = false;\n"
+	"		else if(rightPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = true;\n"
+	"		\n"
+	"		int parentNodeIndex = (isLeftHigherPrefixLength) ? leftIndex : rightIndex;\n"
+	"		\n"
+	"		int isRootNode = (leftIndex == -1 && rightIndex == -1);\n"
+	"		out_internalNodeParentNodes[internalNodeIndex] = (!isRootNode) ? parentNodeIndex : B3_PLBVH_ROOT_NODE_MARKER;\n"
+	"		\n"
+	"		int isLeaf = 0;\n"
+	"		if(!isRootNode)\n"
+	"		{\n"
+	"			int isRightChild = (isLeftHigherPrefixLength);	//If the left node is the parent, then this node is its right child and vice versa\n"
+	"			\n"
+	"			//out_childNodesAsInt[0] == int2.x == left child\n"
+	"			//out_childNodesAsInt[1] == int2.y == right child\n"
+	"			__global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]);\n"
+	"			out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex);\n"
+	"		}\n"
+	"		else *out_rootNodeIndex = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex);\n"
+	"	}\n"
+	"}\n"
+	"__kernel void findDistanceFromRoot(__global int* rootNodeIndex, __global int* internalNodeParentNodes,\n"
+	"									__global int* out_maxDistanceFromRoot, __global int* out_distanceFromRoot, int numInternalNodes)\n"
+	"{\n"
+	"	if( get_global_id(0) == 0 ) atomic_xchg(out_maxDistanceFromRoot, 0);\n"
+	"	int internalNodeIndex = get_global_id(0);\n"
+	"	if(internalNodeIndex >= numInternalNodes) return;\n"
+	"	\n"
+	"	//\n"
+	"	int distanceFromRoot = 0;\n"
+	"	{\n"
+	"		int parentIndex = internalNodeParentNodes[internalNodeIndex];\n"
+	"		while(parentIndex != B3_PLBVH_ROOT_NODE_MARKER)\n"
+	"		{\n"
+	"			parentIndex = internalNodeParentNodes[parentIndex];\n"
+	"			++distanceFromRoot;\n"
+	"		}\n"
+	"	}\n"
+	"	out_distanceFromRoot[internalNodeIndex] = distanceFromRoot;\n"
+	"	\n"
+	"	//\n"
+	"	__local int localMaxDistanceFromRoot;\n"
+	"	if( get_local_id(0) == 0 ) localMaxDistanceFromRoot = 0;\n"
+	"	barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	\n"
+	"	atomic_max(&localMaxDistanceFromRoot, distanceFromRoot);\n"
+	"	barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	\n"
+	"	if( get_local_id(0) == 0 ) atomic_max(out_maxDistanceFromRoot, localMaxDistanceFromRoot);\n"
+	"}\n"
+	"__kernel void buildBinaryRadixTreeAabbsRecursive(__global int* distanceFromRoot, __global SortDataCL* mortonCodesAndAabbIndices,\n"
+	"												__global int2* childNodes,\n"
+	"												__global b3AabbCL* leafNodeAabbs, __global b3AabbCL* internalNodeAabbs,\n"
+	"												int maxDistanceFromRoot, int processedDistance, int numInternalNodes)\n"
+	"{\n"
+	"	int internalNodeIndex = get_global_id(0);\n"
+	"	if(internalNodeIndex >= numInternalNodes) return;\n"
+	"	\n"
+	"	int distance = distanceFromRoot[internalNodeIndex];\n"
+	"	\n"
+	"	if(distance == processedDistance)\n"
+	"	{\n"
+	"		int leftChildIndex = childNodes[internalNodeIndex].x;\n"
+	"		int rightChildIndex = childNodes[internalNodeIndex].y;\n"
+	"		\n"
+	"		int isLeftChildLeaf = isLeafNode(leftChildIndex);\n"
+	"		int isRightChildLeaf = isLeafNode(rightChildIndex);\n"
+	"		\n"
+	"		leftChildIndex = getIndexWithInternalNodeMarkerRemoved(leftChildIndex);\n"
+	"		rightChildIndex = getIndexWithInternalNodeMarkerRemoved(rightChildIndex);\n"
+	"		\n"
+	"		//leftRigidIndex/rightRigidIndex is not used if internal node\n"
+	"		int leftRigidIndex = (isLeftChildLeaf) ? mortonCodesAndAabbIndices[leftChildIndex].m_value : -1;\n"
+	"		int rightRigidIndex = (isRightChildLeaf) ? mortonCodesAndAabbIndices[rightChildIndex].m_value : -1;\n"
+	"		\n"
+	"		b3AabbCL leftChildAabb = (isLeftChildLeaf) ? leafNodeAabbs[leftRigidIndex] : internalNodeAabbs[leftChildIndex];\n"
+	"		b3AabbCL rightChildAabb = (isRightChildLeaf) ? leafNodeAabbs[rightRigidIndex] : internalNodeAabbs[rightChildIndex];\n"
+	"		\n"
+	"		b3AabbCL mergedAabb;\n"
+	"		mergedAabb.m_min = b3Min(leftChildAabb.m_min, rightChildAabb.m_min);\n"
+	"		mergedAabb.m_max = b3Max(leftChildAabb.m_max, rightChildAabb.m_max);\n"
+	"		internalNodeAabbs[internalNodeIndex] = mergedAabb;\n"
+	"	}\n"
+	"}\n"
+	"__kernel void findLeafIndexRanges(__global int2* internalNodeChildNodes, __global int2* out_leafIndexRanges, int numInternalNodes)\n"
+	"{\n"
+	"	int internalNodeIndex = get_global_id(0);\n"
+	"	if(internalNodeIndex >= numInternalNodes) return;\n"
+	"	\n"
+	"	int numLeafNodes = numInternalNodes + 1;\n"
+	"	\n"
+	"	int2 childNodes = internalNodeChildNodes[internalNodeIndex];\n"
+	"	\n"
+	"	int2 leafIndexRange;	//x == min leaf index, y == max leaf index\n"
+	"	\n"
+	"	//Find lowest leaf index covered by this internal node\n"
+	"	{\n"
+	"		int lowestIndex = childNodes.x;		//childNodes.x == Left child\n"
+	"		while( !isLeafNode(lowestIndex) ) lowestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(lowestIndex) ].x;\n"
+	"		leafIndexRange.x = lowestIndex;\n"
+	"	}\n"
+	"	\n"
+	"	//Find highest leaf index covered by this internal node\n"
+	"	{\n"
+	"		int highestIndex = childNodes.y;	//childNodes.y == Right child\n"
+	"		while( !isLeafNode(highestIndex) ) highestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(highestIndex) ].y;\n"
+	"		leafIndexRange.y = highestIndex;\n"
+	"	}\n"
+	"	\n"
+	"	//\n"
+	"	out_leafIndexRanges[internalNodeIndex] = leafIndexRange;\n"
+	"}\n";

thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h

@@ -1,342 +1,341 @@
 //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
-static const char* sapCL= \
-"/*\n"
-"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
-"This software is provided 'as-is', without any express or implied warranty.\n"
-"In no event will the authors be held liable for any damages arising from the use of this software.\n"
-"Permission is granted to anyone to use this software for any purpose, \n"
-"including commercial applications, and to alter it and redistribute it freely, \n"
-"subject to the following restrictions:\n"
-"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.\n"
-"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
-"3. This notice may not be removed or altered from any source distribution.\n"
-"*/\n"
-"//Originally written by Erwin Coumans\n"
-"#define NEW_PAIR_MARKER -1\n"
-"typedef struct \n"
-"{\n"
-"	union\n"
-"	{\n"
-"		float4	m_min;\n"
-"		float   m_minElems[4];\n"
-"		int			m_minIndices[4];\n"
-"	};\n"
-"	union\n"
-"	{\n"
-"		float4	m_max;\n"
-"		float   m_maxElems[4];\n"
-"		int			m_maxIndices[4];\n"
-"	};\n"
-"} btAabbCL;\n"
-"/// conservative test for overlap between two aabbs\n"
-"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2);\n"
-"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2)\n"
-"{\n"
-"	bool overlap = true;\n"
-"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
-"	return overlap;\n"
-"}\n"
-"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
-"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
-"{\n"
-"	bool overlap = true;\n"
-"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
-"	return overlap;\n"
-"}\n"
-"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
-"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
-"{\n"
-"	bool overlap = true;\n"
-"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
-"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
-"	return overlap;\n"
-"}\n"
-"__kernel void   computePairsKernelTwoArrays( __global const btAabbCL* unsortedAabbs, __global const int* unsortedAabbMapping,  __global const int* unsortedAabbMapping2, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numUnsortedAabbs, int numUnSortedAabbs2, int axis, int maxPairs)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	if (i>=numUnsortedAabbs)\n"
-"		return;\n"
-"	int j = get_global_id(1);\n"
-"	if (j>=numUnSortedAabbs2)\n"
-"		return;\n"
-"	__global const btAabbCL* unsortedAabbPtr = &unsortedAabbs[unsortedAabbMapping[i]];\n"
-"	__global const btAabbCL* unsortedAabbPtr2 = &unsortedAabbs[unsortedAabbMapping2[j]];\n"
-"	if (TestAabbAgainstAabb2GlobalGlobal(unsortedAabbPtr,unsortedAabbPtr2))\n"
-"	{\n"
-"		int4 myPair;\n"
-"		\n"
-"		int xIndex = unsortedAabbPtr[0].m_minIndices[3];\n"
-"		int yIndex = unsortedAabbPtr2[0].m_minIndices[3];\n"
-"		if (xIndex>yIndex)\n"
-"		{\n"
-"			int tmp = xIndex;\n"
-"			xIndex=yIndex;\n"
-"			yIndex=tmp;\n"
-"		}\n"
-"		\n"
-"		myPair.x = xIndex;\n"
-"		myPair.y = yIndex;\n"
-"		myPair.z = NEW_PAIR_MARKER;\n"
-"		myPair.w = NEW_PAIR_MARKER;\n"
-"		int curPair = atomic_inc (pairCount);\n"
-"		if (curPair<maxPairs)\n"
-"		{\n"
-"				pairsOut[curPair] = myPair; //flush to main memory\n"
-"		}\n"
-"	}\n"
-"}\n"
-"__kernel void   computePairsKernelBruteForce( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	if (i>=numObjects)\n"
-"		return;\n"
-"	for (int j=i+1;j<numObjects;j++)\n"
-"	{\n"
-"		if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
-"		{\n"
-"			int4 myPair;\n"
-"			myPair.x = aabbs[i].m_minIndices[3];\n"
-"			myPair.y = aabbs[j].m_minIndices[3];\n"
-"			myPair.z = NEW_PAIR_MARKER;\n"
-"			myPair.w = NEW_PAIR_MARKER;\n"
-"			int curPair = atomic_inc (pairCount);\n"
-"			if (curPair<maxPairs)\n"
-"			{\n"
-"					pairsOut[curPair] = myPair; //flush to main memory\n"
-"			}\n"
-"		}\n"
-"	}\n"
-"}\n"
-"__kernel void   computePairsKernelOriginal( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	if (i>=numObjects)\n"
-"		return;\n"
-"	for (int j=i+1;j<numObjects;j++)\n"
-"	{\n"
-"  	if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
-"		{\n"
-"			break;\n"
-"		}\n"
-"		if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
-"		{\n"
-"			int4 myPair;\n"
-"			myPair.x = aabbs[i].m_minIndices[3];\n"
-"			myPair.y = aabbs[j].m_minIndices[3];\n"
-"			myPair.z = NEW_PAIR_MARKER;\n"
-"			myPair.w = NEW_PAIR_MARKER;\n"
-"			int curPair = atomic_inc (pairCount);\n"
-"			if (curPair<maxPairs)\n"
-"			{\n"
-"					pairsOut[curPair] = myPair; //flush to main memory\n"
-"			}\n"
-"		}\n"
-"	}\n"
-"}\n"
-"__kernel void   computePairsKernelBarrier( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	int localId = get_local_id(0);\n"
-"	__local int numActiveWgItems[1];\n"
-"	__local int breakRequest[1];\n"
-"	if (localId==0)\n"
-"	{\n"
-"		numActiveWgItems[0] = 0;\n"
-"		breakRequest[0] = 0;\n"
-"	}\n"
-"	barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	atomic_inc(numActiveWgItems);\n"
-"	barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	int localBreak = 0;\n"
-"	int j=i+1;\n"
-"	do\n"
-"	{\n"
-"		barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	\n"
-"		if (j<numObjects)\n"
-"		{\n"
-"	  	if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
-"			{\n"
-"				if (!localBreak)\n"
-"				{\n"
-"					atomic_inc(breakRequest);\n"
-"					localBreak = 1;\n"
-"				}\n"
-"			}\n"
-"		}\n"
-"		\n"
-"		barrier(CLK_LOCAL_MEM_FENCE);\n"
-"		\n"
-"		if (j>=numObjects && !localBreak)\n"
-"		{\n"
-"			atomic_inc(breakRequest);\n"
-"			localBreak = 1;\n"
-"		}\n"
-"		barrier(CLK_LOCAL_MEM_FENCE);\n"
-"		\n"
-"		if (!localBreak)\n"
-"		{\n"
-"			if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
-"			{\n"
-"				int4 myPair;\n"
-"				myPair.x = aabbs[i].m_minIndices[3];\n"
-"				myPair.y = aabbs[j].m_minIndices[3];\n"
-"				myPair.z = NEW_PAIR_MARKER;\n"
-"				myPair.w = NEW_PAIR_MARKER;\n"
-"				int curPair = atomic_inc (pairCount);\n"
-"				if (curPair<maxPairs)\n"
-"				{\n"
-"						pairsOut[curPair] = myPair; //flush to main memory\n"
-"				}\n"
-"			}\n"
-"		}\n"
-"		j++;\n"
-"	} while (breakRequest[0]<numActiveWgItems[0]);\n"
-"}\n"
-"__kernel void   computePairsKernelLocalSharedMemory( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	int localId = get_local_id(0);\n"
-"	__local int numActiveWgItems[1];\n"
-"	__local int breakRequest[1];\n"
-"	__local btAabbCL localAabbs[128];// = aabbs[i];\n"
-"	\n"
-"	btAabbCL myAabb;\n"
-"	\n"
-"	myAabb = (i<numObjects)? aabbs[i]:aabbs[0];\n"
-"	float testValue = 	myAabb.m_maxElems[axis];\n"
-"	\n"
-"	if (localId==0)\n"
-"	{\n"
-"		numActiveWgItems[0] = 0;\n"
-"		breakRequest[0] = 0;\n"
-"	}\n"
-"	int localCount=0;\n"
-"	int block=0;\n"
-"	localAabbs[localId] = (i+block)<numObjects? aabbs[i+block] : aabbs[0];\n"
-"	localAabbs[localId+64] = (i+block+64)<numObjects? aabbs[i+block+64]: aabbs[0];\n"
-"	\n"
-"	barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	atomic_inc(numActiveWgItems);\n"
-"	barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	int localBreak = 0;\n"
-"	\n"
-"	int j=i+1;\n"
-"	do\n"
-"	{\n"
-"		barrier(CLK_LOCAL_MEM_FENCE);\n"
-"	\n"
-"		if (j<numObjects)\n"
-"		{\n"
-"	  	if(testValue < (localAabbs[localCount+localId+1].m_minElems[axis])) \n"
-"			{\n"
-"				if (!localBreak)\n"
-"				{\n"
-"					atomic_inc(breakRequest);\n"
-"					localBreak = 1;\n"
-"				}\n"
-"			}\n"
-"		}\n"
-"		\n"
-"		barrier(CLK_LOCAL_MEM_FENCE);\n"
-"		\n"
-"		if (j>=numObjects && !localBreak)\n"
-"		{\n"
-"			atomic_inc(breakRequest);\n"
-"			localBreak = 1;\n"
-"		}\n"
-"		barrier(CLK_LOCAL_MEM_FENCE);\n"
-"		\n"
-"		if (!localBreak)\n"
-"		{\n"
-"			if (TestAabbAgainstAabb2(&myAabb,&localAabbs[localCount+localId+1]))\n"
-"			{\n"
-"				int4 myPair;\n"
-"				myPair.x = myAabb.m_minIndices[3];\n"
-"				myPair.y = localAabbs[localCount+localId+1].m_minIndices[3];\n"
-"				myPair.z = NEW_PAIR_MARKER;\n"
-"				myPair.w = NEW_PAIR_MARKER;\n"
-"				int curPair = atomic_inc (pairCount);\n"
-"				if (curPair<maxPairs)\n"
-"				{\n"
-"						pairsOut[curPair] = myPair; //flush to main memory\n"
-"				}\n"
-"			}\n"
-"		}\n"
-"		\n"
-"		barrier(CLK_LOCAL_MEM_FENCE);\n"
-"		localCount++;\n"
-"		if (localCount==64)\n"
-"		{\n"
-"			localCount = 0;\n"
-"			block+=64;			\n"
-"			localAabbs[localId] = ((i+block)<numObjects) ? aabbs[i+block] : aabbs[0];\n"
-"			localAabbs[localId+64] = ((i+64+block)<numObjects) ? aabbs[i+block+64] : aabbs[0];\n"
-"		}\n"
-"		j++;\n"
-"		\n"
-"	} while (breakRequest[0]<numActiveWgItems[0]);\n"
-"	\n"
-"}\n"
-"//http://stereopsis.com/radix.html\n"
-"unsigned int FloatFlip(float fl);\n"
-"unsigned int FloatFlip(float fl)\n"
-"{\n"
-"	unsigned int f = *(unsigned int*)&fl;\n"
-"	unsigned int mask = -(int)(f >> 31) | 0x80000000;\n"
-"	return f ^ mask;\n"
-"}\n"
-"float IFloatFlip(unsigned int f);\n"
-"float IFloatFlip(unsigned int f)\n"
-"{\n"
-"	unsigned int mask = ((f >> 31) - 1) | 0x80000000;\n"
-"	unsigned int fl = f ^ mask;\n"
-"	return *(float*)&fl;\n"
-"}\n"
-"__kernel void   copyAabbsKernel( __global const btAabbCL* allAabbs, __global btAabbCL* destAabbs, int numObjects)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	if (i>=numObjects)\n"
-"		return;\n"
-"	int src = destAabbs[i].m_maxIndices[3];\n"
-"	destAabbs[i] = allAabbs[src];\n"
-"	destAabbs[i].m_maxIndices[3] = src;\n"
-"}\n"
-"__kernel void   flipFloatKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global int2* sortData, int numObjects, int axis)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	if (i>=numObjects)\n"
-"		return;\n"
-"	\n"
-"	\n"
-"	sortData[i].x = FloatFlip(allAabbs[smallAabbMapping[i]].m_minElems[axis]);\n"
-"	sortData[i].y = i;\n"
-"		\n"
-"}\n"
-"__kernel void   scatterKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, volatile __global const int2* sortData, __global btAabbCL* sortedAabbs, int numObjects)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	if (i>=numObjects)\n"
-"		return;\n"
-"	\n"
-"	sortedAabbs[i] = allAabbs[smallAabbMapping[sortData[i].y]];\n"
-"}\n"
-"__kernel void   prepareSumVarianceKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global float4* sum, __global float4* sum2,int numAabbs)\n"
-"{\n"
-"	int i = get_global_id(0);\n"
-"	if (i>=numAabbs)\n"
-"		return;\n"
-"	\n"
-"	btAabbCL smallAabb = allAabbs[smallAabbMapping[i]];\n"
-"	\n"
-"	float4 s;\n"
-"	s = (smallAabb.m_max+smallAabb.m_min)*0.5f;\n"
-"	sum[i]=s;\n"
-"	sum2[i]=s*s;	\n"
-"}\n"
-;
+static const char* sapCL =
+	"/*\n"
+	"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+	"This software is provided 'as-is', without any express or implied warranty.\n"
+	"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+	"Permission is granted to anyone to use this software for any purpose, \n"
+	"including commercial applications, and to alter it and redistribute it freely, \n"
+	"subject to the following restrictions:\n"
+	"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.\n"
+	"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+	"3. This notice may not be removed or altered from any source distribution.\n"
+	"*/\n"
+	"//Originally written by Erwin Coumans\n"
+	"#define NEW_PAIR_MARKER -1\n"
+	"typedef struct \n"
+	"{\n"
+	"	union\n"
+	"	{\n"
+	"		float4	m_min;\n"
+	"		float   m_minElems[4];\n"
+	"		int			m_minIndices[4];\n"
+	"	};\n"
+	"	union\n"
+	"	{\n"
+	"		float4	m_max;\n"
+	"		float   m_maxElems[4];\n"
+	"		int			m_maxIndices[4];\n"
+	"	};\n"
+	"} btAabbCL;\n"
+	"/// conservative test for overlap between two aabbs\n"
+	"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2);\n"
+	"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2)\n"
+	"{\n"
+	"	bool overlap = true;\n"
+	"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
+	"	return overlap;\n"
+	"}\n"
+	"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
+	"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
+	"{\n"
+	"	bool overlap = true;\n"
+	"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
+	"	return overlap;\n"
+	"}\n"
+	"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n"
+	"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n"
+	"{\n"
+	"	bool overlap = true;\n"
+	"	overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n"
+	"	overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n"
+	"	return overlap;\n"
+	"}\n"
+	"__kernel void   computePairsKernelTwoArrays( __global const btAabbCL* unsortedAabbs, __global const int* unsortedAabbMapping,  __global const int* unsortedAabbMapping2, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numUnsortedAabbs, int numUnSortedAabbs2, int axis, int maxPairs)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	if (i>=numUnsortedAabbs)\n"
+	"		return;\n"
+	"	int j = get_global_id(1);\n"
+	"	if (j>=numUnSortedAabbs2)\n"
+	"		return;\n"
+	"	__global const btAabbCL* unsortedAabbPtr = &unsortedAabbs[unsortedAabbMapping[i]];\n"
+	"	__global const btAabbCL* unsortedAabbPtr2 = &unsortedAabbs[unsortedAabbMapping2[j]];\n"
+	"	if (TestAabbAgainstAabb2GlobalGlobal(unsortedAabbPtr,unsortedAabbPtr2))\n"
+	"	{\n"
+	"		int4 myPair;\n"
+	"		\n"
+	"		int xIndex = unsortedAabbPtr[0].m_minIndices[3];\n"
+	"		int yIndex = unsortedAabbPtr2[0].m_minIndices[3];\n"
+	"		if (xIndex>yIndex)\n"
+	"		{\n"
+	"			int tmp = xIndex;\n"
+	"			xIndex=yIndex;\n"
+	"			yIndex=tmp;\n"
+	"		}\n"
+	"		\n"
+	"		myPair.x = xIndex;\n"
+	"		myPair.y = yIndex;\n"
+	"		myPair.z = NEW_PAIR_MARKER;\n"
+	"		myPair.w = NEW_PAIR_MARKER;\n"
+	"		int curPair = atomic_inc (pairCount);\n"
+	"		if (curPair<maxPairs)\n"
+	"		{\n"
+	"				pairsOut[curPair] = myPair; //flush to main memory\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"__kernel void   computePairsKernelBruteForce( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	if (i>=numObjects)\n"
+	"		return;\n"
+	"	for (int j=i+1;j<numObjects;j++)\n"
+	"	{\n"
+	"		if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
+	"		{\n"
+	"			int4 myPair;\n"
+	"			myPair.x = aabbs[i].m_minIndices[3];\n"
+	"			myPair.y = aabbs[j].m_minIndices[3];\n"
+	"			myPair.z = NEW_PAIR_MARKER;\n"
+	"			myPair.w = NEW_PAIR_MARKER;\n"
+	"			int curPair = atomic_inc (pairCount);\n"
+	"			if (curPair<maxPairs)\n"
+	"			{\n"
+	"					pairsOut[curPair] = myPair; //flush to main memory\n"
+	"			}\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"__kernel void   computePairsKernelOriginal( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	if (i>=numObjects)\n"
+	"		return;\n"
+	"	for (int j=i+1;j<numObjects;j++)\n"
+	"	{\n"
+	"  	if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
+	"		{\n"
+	"			break;\n"
+	"		}\n"
+	"		if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
+	"		{\n"
+	"			int4 myPair;\n"
+	"			myPair.x = aabbs[i].m_minIndices[3];\n"
+	"			myPair.y = aabbs[j].m_minIndices[3];\n"
+	"			myPair.z = NEW_PAIR_MARKER;\n"
+	"			myPair.w = NEW_PAIR_MARKER;\n"
+	"			int curPair = atomic_inc (pairCount);\n"
+	"			if (curPair<maxPairs)\n"
+	"			{\n"
+	"					pairsOut[curPair] = myPair; //flush to main memory\n"
+	"			}\n"
+	"		}\n"
+	"	}\n"
+	"}\n"
+	"__kernel void   computePairsKernelBarrier( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	int localId = get_local_id(0);\n"
+	"	__local int numActiveWgItems[1];\n"
+	"	__local int breakRequest[1];\n"
+	"	if (localId==0)\n"
+	"	{\n"
+	"		numActiveWgItems[0] = 0;\n"
+	"		breakRequest[0] = 0;\n"
+	"	}\n"
+	"	barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	atomic_inc(numActiveWgItems);\n"
+	"	barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	int localBreak = 0;\n"
+	"	int j=i+1;\n"
+	"	do\n"
+	"	{\n"
+	"		barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	\n"
+	"		if (j<numObjects)\n"
+	"		{\n"
+	"	  	if(aabbs[i].m_maxElems[axis] < (aabbs[j].m_minElems[axis])) \n"
+	"			{\n"
+	"				if (!localBreak)\n"
+	"				{\n"
+	"					atomic_inc(breakRequest);\n"
+	"					localBreak = 1;\n"
+	"				}\n"
+	"			}\n"
+	"		}\n"
+	"		\n"
+	"		barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"		\n"
+	"		if (j>=numObjects && !localBreak)\n"
+	"		{\n"
+	"			atomic_inc(breakRequest);\n"
+	"			localBreak = 1;\n"
+	"		}\n"
+	"		barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"		\n"
+	"		if (!localBreak)\n"
+	"		{\n"
+	"			if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n"
+	"			{\n"
+	"				int4 myPair;\n"
+	"				myPair.x = aabbs[i].m_minIndices[3];\n"
+	"				myPair.y = aabbs[j].m_minIndices[3];\n"
+	"				myPair.z = NEW_PAIR_MARKER;\n"
+	"				myPair.w = NEW_PAIR_MARKER;\n"
+	"				int curPair = atomic_inc (pairCount);\n"
+	"				if (curPair<maxPairs)\n"
+	"				{\n"
+	"						pairsOut[curPair] = myPair; //flush to main memory\n"
+	"				}\n"
+	"			}\n"
+	"		}\n"
+	"		j++;\n"
+	"	} while (breakRequest[0]<numActiveWgItems[0]);\n"
+	"}\n"
+	"__kernel void   computePairsKernelLocalSharedMemory( __global const btAabbCL* aabbs, volatile __global int4* pairsOut,volatile  __global int* pairCount, int numObjects, int axis, int maxPairs)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	int localId = get_local_id(0);\n"
+	"	__local int numActiveWgItems[1];\n"
+	"	__local int breakRequest[1];\n"
+	"	__local btAabbCL localAabbs[128];// = aabbs[i];\n"
+	"	\n"
+	"	btAabbCL myAabb;\n"
+	"	\n"
+	"	myAabb = (i<numObjects)? aabbs[i]:aabbs[0];\n"
+	"	float testValue = 	myAabb.m_maxElems[axis];\n"
+	"	\n"
+	"	if (localId==0)\n"
+	"	{\n"
+	"		numActiveWgItems[0] = 0;\n"
+	"		breakRequest[0] = 0;\n"
+	"	}\n"
+	"	int localCount=0;\n"
+	"	int block=0;\n"
+	"	localAabbs[localId] = (i+block)<numObjects? aabbs[i+block] : aabbs[0];\n"
+	"	localAabbs[localId+64] = (i+block+64)<numObjects? aabbs[i+block+64]: aabbs[0];\n"
+	"	\n"
+	"	barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	atomic_inc(numActiveWgItems);\n"
+	"	barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	int localBreak = 0;\n"
+	"	\n"
+	"	int j=i+1;\n"
+	"	do\n"
+	"	{\n"
+	"		barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"	\n"
+	"		if (j<numObjects)\n"
+	"		{\n"
+	"	  	if(testValue < (localAabbs[localCount+localId+1].m_minElems[axis])) \n"
+	"			{\n"
+	"				if (!localBreak)\n"
+	"				{\n"
+	"					atomic_inc(breakRequest);\n"
+	"					localBreak = 1;\n"
+	"				}\n"
+	"			}\n"
+	"		}\n"
+	"		\n"
+	"		barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"		\n"
+	"		if (j>=numObjects && !localBreak)\n"
+	"		{\n"
+	"			atomic_inc(breakRequest);\n"
+	"			localBreak = 1;\n"
+	"		}\n"
+	"		barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"		\n"
+	"		if (!localBreak)\n"
+	"		{\n"
+	"			if (TestAabbAgainstAabb2(&myAabb,&localAabbs[localCount+localId+1]))\n"
+	"			{\n"
+	"				int4 myPair;\n"
+	"				myPair.x = myAabb.m_minIndices[3];\n"
+	"				myPair.y = localAabbs[localCount+localId+1].m_minIndices[3];\n"
+	"				myPair.z = NEW_PAIR_MARKER;\n"
+	"				myPair.w = NEW_PAIR_MARKER;\n"
+	"				int curPair = atomic_inc (pairCount);\n"
+	"				if (curPair<maxPairs)\n"
+	"				{\n"
+	"						pairsOut[curPair] = myPair; //flush to main memory\n"
+	"				}\n"
+	"			}\n"
+	"		}\n"
+	"		\n"
+	"		barrier(CLK_LOCAL_MEM_FENCE);\n"
+	"		localCount++;\n"
+	"		if (localCount==64)\n"
+	"		{\n"
+	"			localCount = 0;\n"
+	"			block+=64;			\n"
+	"			localAabbs[localId] = ((i+block)<numObjects) ? aabbs[i+block] : aabbs[0];\n"
+	"			localAabbs[localId+64] = ((i+64+block)<numObjects) ? aabbs[i+block+64] : aabbs[0];\n"
+	"		}\n"
+	"		j++;\n"
+	"		\n"
+	"	} while (breakRequest[0]<numActiveWgItems[0]);\n"
+	"	\n"
+	"}\n"
+	"//http://stereopsis.com/radix.html\n"
+	"unsigned int FloatFlip(float fl);\n"
+	"unsigned int FloatFlip(float fl)\n"
+	"{\n"
+	"	unsigned int f = *(unsigned int*)&fl;\n"
+	"	unsigned int mask = -(int)(f >> 31) | 0x80000000;\n"
+	"	return f ^ mask;\n"
+	"}\n"
+	"float IFloatFlip(unsigned int f);\n"
+	"float IFloatFlip(unsigned int f)\n"
+	"{\n"
+	"	unsigned int mask = ((f >> 31) - 1) | 0x80000000;\n"
+	"	unsigned int fl = f ^ mask;\n"
+	"	return *(float*)&fl;\n"
+	"}\n"
+	"__kernel void   copyAabbsKernel( __global const btAabbCL* allAabbs, __global btAabbCL* destAabbs, int numObjects)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	if (i>=numObjects)\n"
+	"		return;\n"
+	"	int src = destAabbs[i].m_maxIndices[3];\n"
+	"	destAabbs[i] = allAabbs[src];\n"
+	"	destAabbs[i].m_maxIndices[3] = src;\n"
+	"}\n"
+	"__kernel void   flipFloatKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global int2* sortData, int numObjects, int axis)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	if (i>=numObjects)\n"
+	"		return;\n"
+	"	\n"
+	"	\n"
+	"	sortData[i].x = FloatFlip(allAabbs[smallAabbMapping[i]].m_minElems[axis]);\n"
+	"	sortData[i].y = i;\n"
+	"		\n"
+	"}\n"
+	"__kernel void   scatterKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, volatile __global const int2* sortData, __global btAabbCL* sortedAabbs, int numObjects)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	if (i>=numObjects)\n"
+	"		return;\n"
+	"	\n"
+	"	sortedAabbs[i] = allAabbs[smallAabbMapping[sortData[i].y]];\n"
+	"}\n"
+	"__kernel void   prepareSumVarianceKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global float4* sum, __global float4* sum2,int numAabbs)\n"
+	"{\n"
+	"	int i = get_global_id(0);\n"
+	"	if (i>=numAabbs)\n"
+	"		return;\n"
+	"	\n"
+	"	btAabbCL smallAabb = allAabbs[smallAabbMapping[i]];\n"
+	"	\n"
+	"	float4 s;\n"
+	"	s = (smallAabb.m_max+smallAabb.m_min)*0.5f;\n"
+	"	sum[i]=s;\n"
+	"	sum2[i]=s*s;	\n"
+	"}\n";