Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

17 files changed, 12335 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/LowLevelAABB/include/BpAABBManagerTasks.h b/PhysX_3.4/Source/LowLevelAABB/include/BpAABBManagerTasks.h
new file mode 100644
index 00000000..2ff814db
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/include/BpAABBManagerTasks.h
@@ -0,0 +1,107 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef BP_AABB_MANAGER_TASKS_H
+#define BP_AABB_MANAGER_TASKS_H
+
+#include "PsUserAllocated.h"
+#include "CmTask.h"
+
+namespace physx
+{
+	class PxcScratchAllocator;
+namespace Bp
+{
+	class SimpleAABBManager;
+	class Aggregate;
+
+	class AggregateBoundsComputationTask : public Cm::Task, public shdfnd::UserAllocated
+	{
+		public:
+										AggregateBoundsComputationTask() :
+											mManager	(NULL),
+											mStart		(0),
+											mNbToGo		(0),
+											mAggregates	(NULL)
+										{}
+										~AggregateBoundsComputationTask()	{}
+
+		virtual const char*				getName() const { return "AggregateBoundsComputationTask"; }
+		virtual void					runInternal();
+
+				void					Init(SimpleAABBManager* manager, PxU32 start, PxU32 nb, Aggregate** aggregates)
+										{
+											mManager	= manager;
+											mStart		= start;
+											mNbToGo		= nb;
+											mAggregates	= aggregates;
+										}
+		private:
+				SimpleAABBManager*		mManager;
+				PxU32					mStart;
+				PxU32					mNbToGo;
+				Aggregate**				mAggregates;
+
+		AggregateBoundsComputationTask& operator=(const AggregateBoundsComputationTask&);
+	};
+
+	class FinalizeUpdateTask : public Cm::Task, public shdfnd::UserAllocated
+	{
+		public:
+										FinalizeUpdateTask() :
+											mManager				(NULL),
+											mNumCpuTasks			(0),
+											mScratchAllocator		(NULL),
+											mNarrowPhaseUnlockTask	(NULL)
+										{}
+										~FinalizeUpdateTask()	{}
+
+		virtual const char*				getName() const { return "FinalizeUpdateTask"; }
+		virtual void					runInternal();
+
+				void					Init(SimpleAABBManager* manager, PxU32 numCpuTasks, PxcScratchAllocator* scratchAllocator, PxBaseTask* narrowPhaseUnlockTask)
+										{
+											mManager				= manager;
+											mNumCpuTasks			= numCpuTasks;
+											mScratchAllocator		= scratchAllocator;
+											mNarrowPhaseUnlockTask	= narrowPhaseUnlockTask;
+										}
+		private:
+				SimpleAABBManager*		mManager;
+				PxU32					mNumCpuTasks;
+				PxcScratchAllocator*	mScratchAllocator;
+				PxBaseTask*				mNarrowPhaseUnlockTask;
+
+		FinalizeUpdateTask& operator=(const FinalizeUpdateTask&);
+	};
+
+}
+} //namespace physx
+
+#endif // BP_AABB_MANAGER_TASKS_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/include/BpBroadPhase.h b/PhysX_3.4/Source/LowLevelAABB/include/BpBroadPhase.h
new file mode 100644
index 00000000..25990f57
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/include/BpBroadPhase.h
@@ -0,0 +1,336 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef BP_BROADPHASE_H
+#define BP_BROADPHASE_H
+
+#include "foundation/PxUnionCast.h"
+#include "PxBroadPhase.h"
+#include "BpSimpleAABBManager.h"
+
+namespace physx
+{
+class PxcScratchAllocator;
+
+namespace Bp
+{
+
+class BroadPhaseUpdateData;
+class BPMemoryAllocator;
+
+
+/**
+\brief Base broad phase class.  Functions only relevant to MBP.
+*/
+class BroadPhaseBase
+{
+	public:
+									BroadPhaseBase()						{}
+	virtual							~BroadPhaseBase()						{}
+
+	/**
+	\brief Gets broad-phase caps.
+
+	\param[out]	caps	Broad-phase caps
+	\return True if success
+	*/
+	virtual	bool					getCaps(PxBroadPhaseCaps& caps)			const 
+	{
+		caps.maxNbRegions = 0;
+		caps.maxNbObjects = 0;
+		caps.needsPredefinedBounds = false;
+		return true;
+	}
+
+	/**
+	\brief Returns number of regions currently registered in the broad-phase.
+
+	\return Number of regions
+	*/
+	virtual	PxU32					getNbRegions()							const 
+	{
+		return 0;	
+	}
+
+	/**
+	\brief Gets broad-phase regions.
+
+	\param[out]	userBuffer	Returned broad-phase regions
+	\param[in]	bufferSize	Size of userBuffer
+	\param[in]	startIndex	Index of first desired region, in [0 ; getNbRegions()[
+	\return Number of written out regions
+	*/
+	virtual	PxU32					getRegions(PxBroadPhaseRegionInfo* userBuffer, PxU32 bufferSize, PxU32 startIndex=0) const
+	{
+		PX_UNUSED(userBuffer);
+		PX_UNUSED(bufferSize);
+		PX_UNUSED(startIndex);
+		return 0;
+	}
+
+	/**
+	\brief Adds a new broad-phase region.
+
+	The bounds for the new region must be non-empty, otherwise an error occurs and the call is ignored.
+
+	The total number of regions is limited to 256. If that number is exceeded, the call is ignored.
+
+	The newly added region will be automatically populated with already existing SDK objects that touch it, if the
+	'populateRegion' parameter is set to true. Otherwise the newly added region will be empty, and it will only be
+	populated with objects when those objects are added to the simulation, or updated if they already exist.
+
+	Using 'populateRegion=true' has a cost, so it is best to avoid it if possible. In particular it is more efficient
+	to create the empty regions first (with populateRegion=false) and then add the objects afterwards (rather than
+	the opposite).
+
+	Objects automatically move from one region to another during their lifetime. The system keeps tracks of what
+	regions a given object is in. It is legal for an object to be in an arbitrary number of regions. However if an
+	object leaves all regions, or is created outside of all regions, several things happen:
+		- collisions get disabled for this object
+		- if a PxBroadPhaseCallback object is provided, an "out-of-bounds" event is generated via that callback
+		- if a PxBroadPhaseCallback object is not provided, a warning/error message is sent to the error stream
+
+	If an object goes out-of-bounds and user deletes it during the same frame, neither the out-of-bounds event nor the
+	error message is generated.
+
+	If an out-of-bounds object, whose collisions are disabled, re-enters a valid broadphase region, then collisions
+	are re-enabled for that object.
+
+	\param[in]	region			User-provided region data
+	\param[in]	populateRegion	True to automatically populate the newly added region with existing objects touching it
+
+	\return Handle for newly created region, or 0xffffffff in case of failure.
+	*/
+	virtual	PxU32					addRegion(const PxBroadPhaseRegion& region, bool populateRegion)
+	{
+		PX_UNUSED(region);
+		PX_UNUSED(populateRegion);
+		return 0xffffffff;	
+	}
+
+	/**
+	\brief Removes a new broad-phase region.
+
+	If the region still contains objects, and if those objects do not overlap any region any more, they are not
+	automatically removed from the simulation. Instead, the PxBroadPhaseCallback::onObjectOutOfBounds notification
+	is used for each object. Users are responsible for removing the objects from the simulation if this is the
+	desired behavior.
+
+	If the handle is invalid, or if a valid handle is removed twice, an error message is sent to the error stream.
+
+	\param[in]	handle	Region's handle, as returned by PxScene::addBroadPhaseRegion.
+	\return True if success
+	*/
+	virtual	bool					removeRegion(PxU32 handle)	
+	{
+		PX_UNUSED(handle);
+		return false;	
+	}
+
+	/*
+	\brief Return the number of objects that are not in any region.
+	*/
+	virtual	PxU32						getNbOutOfBoundsObjects()	const	
+	{ 
+		return 0;						
+	}
+
+	/*
+	\brief Return an array of objects that are not in any region.
+	*/
+	virtual	const PxU32*				getOutOfBoundsObjects()		const	
+	{ 
+		return NULL;					
+	}
+};
+
+
+/*
+\brief Structure used to report created and deleted broadphase pairs
+\note The indices mVolA and mVolB correspond to the bounds indices 
+BroadPhaseUpdateData::mCreated used by BroadPhase::update
+@see BroadPhase::getCreatedPairs, BroadPhase::getDeletedPairs
+*/
+struct BroadPhasePair
+{
+	BroadPhasePair(ShapeHandle volA, ShapeHandle volB, void* userData)
+	{
+		mVolA=PxMin(volA,volB);
+		mVolB=PxMax(volA,volB);
+		mUserData = userData;
+	}
+	BroadPhasePair()
+		: mVolA(BP_INVALID_BP_HANDLE),
+		  mVolB(BP_INVALID_BP_HANDLE),
+		  mUserData(NULL)
+	{
+	}
+
+	ShapeHandle		mVolA;		// NB: mVolA < mVolB
+	ShapeHandle		mVolB;
+	void*			mUserData;
+};
+
+struct BroadPhasePairReport : public BroadPhasePair
+{
+	PxU32 mHandle;
+
+	BroadPhasePairReport(ShapeHandle volA, ShapeHandle volB, void* userData, PxU32 handle) : BroadPhasePair(volA, volB, userData),  mHandle(handle)
+	{
+	}
+
+	BroadPhasePairReport() : BroadPhasePair(), mHandle(BP_INVALID_BP_HANDLE)
+	{
+	}
+};
+
+
+class BroadPhase : public BroadPhaseBase
+{
+public:
+
+	virtual	PxBroadPhaseType::Enum	getType() const = 0;
+
+	/**
+	\brief Instantiate a BroadPhase instance.
+	\param[in] bpType - the bp type (either mbp or sap).  This is typically specified in PxSceneDesc. 
+	\param[in] maxNbRegions is the expected maximum number of broad-phase regions.
+	\param[in] maxNbBroadPhaseOverlaps is the expected maximum number of broad-phase overlaps.
+	\param[in] maxNbStaticShapes is the expected maximum number of static shapes.
+	\param[in] maxNbDynamicShapes is the expected maximum number of dynamic shapes.
+	\param[in] contextID is the context ID parameter sent to the profiler
+	\return The instantiated BroadPhase.
+	\note maxNbRegions is only used if mbp is the chosen broadphase (PxBroadPhaseType::eMBP)
+	\note maxNbRegions, maxNbBroadPhaseOverlaps, maxNbStaticShapes and maxNbDynamicShapes are typically specified in PxSceneLimits
+	*/
+	static BroadPhase* create(
+		const PxBroadPhaseType::Enum bpType,
+		const PxU32 maxNbRegions,
+		const PxU32 maxNbBroadPhaseOverlaps,
+		const PxU32 maxNbStaticShapes,
+		const PxU32 maxNbDynamicShapes,
+		PxU64 contextID);
+
+
+	/**
+	\brief Shutdown of the broadphase.
+	*/
+	virtual	void							destroy() = 0;
+
+	/**
+	\brief Update the broadphase and compute the lists of created/deleted pairs.
+
+	\param[in] numCpuTasks the number of cpu tasks that can be used for the broadphase update.
+	
+	\param[in] scratchAllocator - a PxcScratchAllocator instance used for temporary memory allocations.
+	This must be non-null.
+
+	\param[in] updateData a description of changes to the collection of aabbs since the last broadphase update.
+	The changes detail the indices of the bounds that have been added/updated/removed as well as an array of all
+	bound coordinates and an array of group ids used to filter pairs with the same id.
+	@see BroadPhaseUpdateData
+
+	\param[in] continuation the task that is in the queue to be executed immediately after the broadphase has completed its update. NULL is not supported.
+	\param[in] nPhaseUnlockTask this task will have its ref count decremented when it is safe to permit NP to run in parallel with BP. NULL is supported.
+
+	\note In PX_CHECKED and PX_DEBUG build configurations illegal input data (that does not conform to the BroadPhaseUpdateData specifications) triggers 
+	a special code-path that entirely bypasses the broadphase and issues a warning message to the error stream.  No guarantees can be made about the 
+	correctness/consistency of broadphase behavior with illegal input data in PX_RELEASE and PX_PROFILE configs because validity checks are not active 
+	in these builds.
+	*/
+
+	virtual	void					update(const PxU32 numCpuTasks, PxcScratchAllocator* scratchAllocator, const BroadPhaseUpdateData& updateData, physx::PxBaseTask* continuation, physx::PxBaseTask* nPhaseUnlockTask) = 0;
+
+	/*
+	\brief Return the number of created aabb overlap pairs computed in the execution of update() that has just completed.
+	*/
+	virtual	PxU32					getNbCreatedPairs()		const	= 0;
+
+	/*
+	\brief Return the array of created aabb overlap pairs computed in the execution of update() that has just completed.
+	Note that each overlap pair is reported only on the frame when the overlap first occurs. The overlap persists
+	until the pair appears in the list of deleted pairs or either of the bounds in the pair is removed from the broadphase.  
+	A created overlap must involve at least one of the bounds of the overlap pair appearing in either the created or updated list.
+	It is impossible for the same pair to appear simultaneously in the list of created and deleted overlap pairs.
+	An overlap is defined as a pair of bounds that overlap on all three axes; that is when maxA > minB and maxB > minA for all three axes. 
+	The rule that minima(maxima) are even(odd) (see BroadPhaseUpdateData) removes the ambiguity of touching bounds. 
+
+	*/
+	virtual	BroadPhasePairReport*			getCreatedPairs()					= 0;
+
+	/**
+	\brief Return the number of deleted overlap pairs computed in the execution of update() that has just completed.
+	*/
+	virtual	PxU32					getNbDeletedPairs()		const	= 0;
+
+	/**
+	\brief Return the number of deleted overlap pairs computed in the execution of update() that has just completed.
+	Note that a deleted pair can only be reported if that pair has already appeared in the list of created pairs in an earlier update.
+	A lost overlap occurs when a pair of bounds previously overlapped on all three axes but have now separated on at least one axis.
+	A lost overlap must involve at least one of the bounds of the lost overlap pair appearing in the updated list.
+	Lost overlaps arising from removal of bounds from the broadphase do not appear in the list of deleted pairs.
+	It is impossible for the same pair to appear simultaneously in the list of created and deleted pairs.
+	The test for overlap is conservative throughout, meaning that deleted pairs do not include touching pairs.
+	*/
+	virtual	BroadPhasePairReport*			getDeletedPairs()					= 0;
+
+	/**
+	\brief After the broadphase has completed its update() function and the created/deleted pairs have been queried
+	with getCreatedPairs/getDeletedPairs it is desirable to free any memory that was temporarily acquired for the update but is 
+	is no longer required post-update.  This can be achieved with the function freeBuffers().
+	*/
+	virtual	void					freeBuffers()						= 0;
+
+	/**
+	\brief Adjust internal structures after all bounds have been adjusted due to a scene origin shift.
+	*/
+	virtual void					shiftOrigin(const PxVec3& shift) = 0;
+
+	/**
+	\brief Test that the created/updated/removed lists obey the rules that 
+	1. object ids can only feature in the created list if they have never been previously added or if they were previously removed.
+	2. object ids can only be added to the updated list if they have been previously added without being removed.
+	3. objects ids can only be added to the removed list if they have been previously added without being removed.
+	*/
+#if PX_CHECKED
+	virtual bool					isValid(const BroadPhaseUpdateData& updateData) const = 0;
+#endif
+
+	virtual BroadPhasePair*				getBroadPhasePairs() const = 0;
+
+	virtual void						deletePairs() = 0;
+
+};
+
+} //namespace Bp
+
+} //namespace physx
+
+#endif //BP_BROADPHASE_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/include/BpBroadPhaseUpdate.h b/PhysX_3.4/Source/LowLevelAABB/include/BpBroadPhaseUpdate.h
new file mode 100644
index 00000000..cf0d65a9
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/include/BpBroadPhaseUpdate.h
@@ -0,0 +1,438 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef BP_BROADPHASE_UPDATE_H
+#define BP_BROADPHASE_UPDATE_H
+
+#include "foundation/PxAssert.h"
+#include "foundation/PxUnionCast.h"
+#include "CmPhysXCommon.h"
+#include "PxBroadPhase.h"
+#include "Ps.h"
+
+namespace physx
+{
+
+namespace Bp
+{
+
+#if PX_USE_16_BIT_HANDLES
+	typedef PxU16 ShapeHandle;
+	typedef PxU16 BpHandle;
+#define BP_INVALID_BP_HANDLE	0xffff
+#else
+	typedef PxU32 ShapeHandle;
+	typedef PxU32 BpHandle;
+#define BP_INVALID_BP_HANDLE	0x3fffffff
+#endif
+
+#define ALIGN_SIZE_16(size) ((unsigned(size)+15)&(unsigned(~15)))
+
+
+	/*
+	\brief Encode a single float value with lossless encoding to integer
+	*/
+	PX_FORCE_INLINE PxU32 encodeFloat(PxU32 ir)
+	{
+		//we may need to check on -0 and 0
+		//But it should make no practical difference.
+		if(ir & PX_SIGN_BITMASK) //negative?
+			return ~ir;//reverse sequence of negative numbers
+		else
+			return ir | PX_SIGN_BITMASK; // flip sign
+	}
+
+	/*
+	\brief Encode a single float value with lossless encoding to integer
+	*/
+	PX_FORCE_INLINE PxU32 decodeFloat(PxU32 ir)
+	{
+		if(ir & PX_SIGN_BITMASK) //positive?
+			return ir & ~PX_SIGN_BITMASK; //flip sign
+		else
+			return ~ir; //undo reversal
+	}
+
+
+/**
+\brief Integer representation of PxBounds3 used by BroadPhase
+@see BroadPhaseUpdateData
+*/
+
+
+typedef PxU32 ValType;
+
+class IntegerAABB
+{
+public:
+
+	enum
+	{
+		MIN_X = 0,
+		MIN_Y,
+		MIN_Z,
+		MAX_X,
+		MAX_Y,
+		MAX_Z
+	};
+
+	IntegerAABB();
+
+	IntegerAABB(const PxBounds3& b, PxReal contactDistance)
+	{
+		encode(PxBounds3(b.minimum-PxVec3(contactDistance), b.maximum+PxVec3(contactDistance)));
+	}
+
+	/*
+	\brief Return the minimum along a specified axis
+	\param[in] i is the axis
+	*/
+	PX_FORCE_INLINE ValType	getMin(PxU32 i)	const	{	return (mMinMax)[MIN_X+i];	}
+
+	/*
+	\brief Return the maximum along a specified axis
+	\param[in] i is the axis
+	*/
+	PX_FORCE_INLINE ValType	getMax(PxU32 i)	const	{	return (mMinMax)[MAX_X+i];	}
+
+	/*
+	\brief Return one of the six min/max values of the bound 
+	\param[in] isMax determines whether a min or max value is returned
+	\param[in] index is the axis 
+	*/
+	PX_FORCE_INLINE ValType	getExtent(PxU32 isMax, PxU32 index) const
+	{
+		PX_ASSERT(isMax<=1);
+		return (mMinMax)[3*isMax+index];
+	}
+
+	/*
+	\brief Return the minimum on the x axis
+	*/
+	PX_FORCE_INLINE ValType getMinX() const { return mMinMax[MIN_X]; }
+
+	/*
+	\brief Return the minimum on the y axis
+	*/
+	PX_FORCE_INLINE ValType getMinY() const { return mMinMax[MIN_Y]; }
+
+	/*
+	\brief Return the minimum on the z axis
+	*/
+	PX_FORCE_INLINE ValType getMinZ() const { return mMinMax[MIN_Z]; }
+
+	/*
+	\brief Return the maximum on the x axis
+	*/
+	PX_FORCE_INLINE ValType getMaxX() const { return mMinMax[MAX_X]; }
+
+	/*
+	\brief Return the maximum on the y axis
+	*/
+	PX_FORCE_INLINE ValType getMaxY() const { return mMinMax[MAX_Y]; }
+
+	/*
+	\brief Return the maximum on the z axis
+	*/
+	PX_FORCE_INLINE ValType getMaxZ() const { return mMinMax[MAX_Z]; }
+
+	/*
+	\brief Encode float bounds so they are stored as integer bounds
+	\param[in] bounds is the bounds to be encoded 
+	\note The integer values of minima are always even, while the integer values of maxima are always odd
+	\note The encoding process masks off the last four bits for minima and masks on the last four bits for maxima.
+	This keeps the bounds constant when its shape is subjected to small global pose perturbations.  In turn, this helps 
+	reduce computational effort in the broadphase update by reducing the amount of sorting required on near-stationary 
+	bodies that are aligned along one or more axis.
+	@see decode
+	*/
+	PX_FORCE_INLINE	void encode(const PxBounds3& bounds)
+	{
+		const PxU32* PX_RESTRICT min = PxUnionCast<const PxU32*, const PxF32*>(&bounds.minimum.x);
+		const PxU32* PX_RESTRICT max = PxUnionCast<const PxU32*, const PxF32*>(&bounds.maximum.x);
+		//Avoid min=max by enforcing the rule that mins are even and maxs are odd.
+		mMinMax[MIN_X] = encodeFloatMin(min[0]);
+		mMinMax[MIN_Y] = encodeFloatMin(min[1]);
+		mMinMax[MIN_Z] = encodeFloatMin(min[2]);
+		mMinMax[MAX_X] = encodeFloatMax(max[0]) | (1<<2);
+		mMinMax[MAX_Y] = encodeFloatMax(max[1]) | (1<<2);
+		mMinMax[MAX_Z] = encodeFloatMax(max[2]) | (1<<2);
+	}
+
+	/*
+	\brief Decode from integer bounds to float bounds	
+	\param[out] bounds is the decoded float bounds
+	\note Encode followed by decode will produce a float bound larger than the original
+	due to the masking in encode.
+	@see encode
+	*/
+	PX_FORCE_INLINE	void decode(PxBounds3& bounds)	const
+	{
+		PxU32* PX_RESTRICT min = PxUnionCast<PxU32*, PxF32*>(&bounds.minimum.x);
+		PxU32* PX_RESTRICT max = PxUnionCast<PxU32*, PxF32*>(&bounds.maximum.x);
+		min[0] = decodeFloat(mMinMax[MIN_X]);
+		min[1] = decodeFloat(mMinMax[MIN_Y]);
+		min[2] = decodeFloat(mMinMax[MIN_Z]);
+		max[0] = decodeFloat(mMinMax[MAX_X]);
+		max[1] = decodeFloat(mMinMax[MAX_Y]);
+		max[2] = decodeFloat(mMinMax[MAX_Z]);
+	}
+
+	/*
+	\brief Encode a single minimum value from integer bounds to float bounds	
+	\note The encoding process masks off the last four bits for minima
+	@see encode
+	*/
+	static PX_FORCE_INLINE ValType encodeFloatMin(PxU32 source)
+	{
+		return ((encodeFloat(source) >> eGRID_SNAP_VAL) - 1) << eGRID_SNAP_VAL;
+	}
+
+	/*
+	\brief Encode a single maximum value from integer bounds to float bounds	
+	\note The encoding process masks on the last four bits for maxima
+	@see encode
+	*/
+	static PX_FORCE_INLINE ValType encodeFloatMax(PxU32 source)
+	{
+		return ((encodeFloat(source) >> eGRID_SNAP_VAL) + 1) << eGRID_SNAP_VAL;
+	}
+
+	/*
+	\brief Shift the encoded bounds by a specified vector
+	\param[in] shift is the vector used to shift the bounds
+	*/
+	PX_FORCE_INLINE void shift(const PxVec3& shift)
+	{
+		::physx::PxBounds3 elemBounds;
+		decode(elemBounds);
+		elemBounds.minimum -= shift;
+		elemBounds.maximum -= shift;
+		encode(elemBounds);
+	}
+
+	/*
+	\brief Test if this aabb lies entirely inside another aabb
+	\param[in] box is the other box
+	\return True if this aabb lies entirely inside box
+	*/
+	PX_INLINE bool isInside(const IntegerAABB& box) const
+	{
+		if(box.mMinMax[MIN_X]>mMinMax[MIN_X])	return false;
+		if(box.mMinMax[MIN_Y]>mMinMax[MIN_Y])	return false;
+		if(box.mMinMax[MIN_Z]>mMinMax[MIN_Z])	return false;
+		if(box.mMinMax[MAX_X]<mMinMax[MAX_X])	return false;
+		if(box.mMinMax[MAX_Y]<mMinMax[MAX_Y])	return false;
+		if(box.mMinMax[MAX_Z]<mMinMax[MAX_Z])	return false;
+		return true;
+	}
+
+	/*
+	\brief Test if this aabb and another intersect
+	\param[in] b is the other box
+	\return True if this aabb and b intersect
+	*/
+	PX_FORCE_INLINE bool intersects(const IntegerAABB& b) const
+	{
+		return !(b.mMinMax[MIN_X] > mMinMax[MAX_X] || mMinMax[MIN_X] > b.mMinMax[MAX_X] ||
+			b.mMinMax[MIN_Y] > mMinMax[MAX_Y] || mMinMax[MIN_Y] > b.mMinMax[MAX_Y] ||
+			b.mMinMax[MIN_Z] > mMinMax[MAX_Z] || mMinMax[MIN_Z] > b.mMinMax[MAX_Z]);
+	}
+
+	PX_FORCE_INLINE bool intersects1D(const IntegerAABB& b, const PxU32 axis) const
+	{
+		const PxU32 maxAxis = axis + 3;
+		return !(b.mMinMax[axis] > mMinMax[maxAxis] || mMinMax[axis] > b.mMinMax[maxAxis]);
+	}
+
+
+	/*
+	\brief Expand bounds to include another
+	\note This is used to compute the aggregate bounds of multiple shape bounds
+	\param[in] b is the bounds to be included
+	*/
+	PX_FORCE_INLINE void include(const IntegerAABB& b)
+	{
+		mMinMax[MIN_X] = PxMin(mMinMax[MIN_X], b.mMinMax[MIN_X]);
+		mMinMax[MIN_Y] = PxMin(mMinMax[MIN_Y], b.mMinMax[MIN_Y]);
+		mMinMax[MIN_Z] = PxMin(mMinMax[MIN_Z], b.mMinMax[MIN_Z]);
+		mMinMax[MAX_X] = PxMax(mMinMax[MAX_X], b.mMinMax[MAX_X]);
+		mMinMax[MAX_Y] = PxMax(mMinMax[MAX_Y], b.mMinMax[MAX_Y]);
+		mMinMax[MAX_Z] = PxMax(mMinMax[MAX_Z], b.mMinMax[MAX_Z]);
+	}
+
+	/*
+	\brief Set the bounds to (max, max, max), (min, min, min)
+	*/
+	PX_INLINE void setEmpty()
+	{
+		mMinMax[MIN_X] = mMinMax[MIN_Y] = mMinMax[MIN_Z] = 0xff7fffff;  //PX_IR(PX_MAX_F32);
+		mMinMax[MAX_X] = mMinMax[MAX_Y] = mMinMax[MAX_Z] = 0x00800000;	///PX_IR(0.0f);
+	}
+
+
+	ValType mMinMax[6];
+
+private:
+
+	enum
+	{
+		eGRID_SNAP_VAL = 4
+	};
+};
+
+PX_FORCE_INLINE ValType encodeMin(const PxBounds3& bounds, PxU32 axis, PxReal contactDistance)
+{
+	const PxReal val = bounds.minimum[axis] - contactDistance;
+	const PxU32 min = PxUnionCast<PxU32, PxF32>(val);
+	const PxU32 m = IntegerAABB::encodeFloatMin(min);
+	return m;
+
+}
+
+PX_FORCE_INLINE ValType encodeMax(const PxBounds3& bounds, PxU32 axis, PxReal contactDistance)
+{
+	const PxReal val = bounds.maximum[axis] + contactDistance;
+	const PxU32 max = PxUnionCast<PxU32, PxF32>(val);
+	const PxU32 m = IntegerAABB::encodeFloatMax(max) | (1<<2);
+	return m;
+}
+
+class BroadPhase;
+
+class BroadPhaseUpdateData
+{
+public:
+
+     /**
+	 \brief A structure detailing the changes to the collection of aabbs, whose overlaps are computed in the broadphase.
+	 The structure consists of per-object arrays of object bounds and object groups, and three arrays that index
+	 into the per-object arrays, denoting the bounds which are to be created, updated and removed in the broad phase.
+
+	 * each entry in the object arrays represents the same shape or aggregate from frame to frame.
+	 * each entry in an index array must be less than the capacity of the per-object arrays.
+	 * no index value may appear in more than one index array, and may not occur more than once in that array.
+
+	 An index value is said to be "in use" if it has appeared in a created list in a previous update, and has not
+	 since occurred in a removed list.
+
+	 \param[in] created an array of indices describing the bounds that must be inserted into the broadphase.
+	 Each index in the array must not be in use.
+
+	 \param[in] updated an array of indices (referencing the boxBounds and boxGroups arrays) describing the bounds
+	 that have moved since the last broadphase update. Each index in the array must be in use, and each object
+	 whose index is in use and whose AABB has changed must appear in the update list.
+
+	 \param[in] removed an array of indices describing the bounds that must be removed from the broad phase. Each index in
+	 the array must be in use.
+
+	 \param[in] boxBounds an array of bounds coordinates for the AABBs to be processed by the broadphase.
+
+	 An entry is valid if its values are integer bitwise representations of floating point numbers that satisfy max>min in each dimension,
+	 along with a further rule that minima(maxima) must have even(odd) values. 
+
+	 Each entry whose index is either in use or appears in the created array must be valid. An entry whose index is either not in use or
+	 appears in the removed array need not be valid.
+
+	 \param[in]  boxGroups an array of group ids, one for each bound, used for pair filtering.  Bounds with the same group id will not be
+	 reported as overlap pairs by the broad phase.  Zero is reserved for static bounds.
+
+	 Entries in this array are immutable: the only way to change the group of an object is to remove it from the broad phase and reinsert
+	 it at a different index (recall that each index must appear at most once in the created/updated/removed lists).
+
+	 \param[in]  boxesCapacity the length of the boxBounds and boxGroups arrays.
+
+	 @see BroadPhase::update
+	 */
+	BroadPhaseUpdateData(
+		const ShapeHandle* created, const PxU32 createdSize, 
+		const ShapeHandle* updated, const PxU32 updatedSize, 
+		const ShapeHandle* removed, const PxU32 removedSize, 
+		const PxBounds3* boxBounds, const ShapeHandle* boxGroups, const PxReal* boxContactDistances, const PxU32 boxesCapacity,
+		const bool stateChanged) :
+		mCreated		(created),
+		mCreatedSize	(createdSize),
+		mUpdated		(updated),
+		mUpdatedSize	(updatedSize),
+		mRemoved		(removed),
+		mRemovedSize	(removedSize),
+		mBoxBounds		(boxBounds),
+		mBoxGroups		(boxGroups),
+		mContactDistance(boxContactDistances),
+		mBoxesCapacity	(boxesCapacity),
+		mStateChanged	(stateChanged)
+	{
+	}
+
+	PX_FORCE_INLINE	const ShapeHandle*	getCreatedHandles()		const { return mCreated;			}
+	PX_FORCE_INLINE	PxU32				getNumCreatedHandles()	const { return mCreatedSize;		}
+
+	PX_FORCE_INLINE	const ShapeHandle*	getUpdatedHandles()		const { return mUpdated;			}
+	PX_FORCE_INLINE	PxU32				getNumUpdatedHandles()	const { return mUpdatedSize;		}
+
+	PX_FORCE_INLINE	const ShapeHandle*	getRemovedHandles()		const { return mRemoved;			}
+	PX_FORCE_INLINE	PxU32				getNumRemovedHandles()	const { return mRemovedSize;		}
+
+	PX_FORCE_INLINE	const PxBounds3*	getAABBs()				const { return mBoxBounds;			}
+	PX_FORCE_INLINE	const ShapeHandle*	getGroups()				const { return mBoxGroups;			}
+	PX_FORCE_INLINE	PxU32				getCapacity()			const { return mBoxesCapacity;		}
+
+	PX_FORCE_INLINE	const PxReal*		getContactDistance()	const { return mContactDistance;	}
+
+	PX_FORCE_INLINE	bool				getStateChanged()		const { return mStateChanged;		}
+
+#if PX_CHECKED
+	static bool isValid(const BroadPhaseUpdateData& updateData, const BroadPhase& bp);
+	bool isValid() const;
+#endif
+
+private:
+
+	const ShapeHandle*	mCreated;
+	PxU32				mCreatedSize;
+
+	const ShapeHandle*	mUpdated;
+	PxU32				mUpdatedSize;
+
+	const ShapeHandle*	mRemoved;
+	PxU32				mRemovedSize;
+
+	const PxBounds3*	mBoxBounds;
+	const ShapeHandle*	mBoxGroups;
+	const PxReal*		mContactDistance;
+	PxU32				mBoxesCapacity;
+	bool				mStateChanged;
+};
+
+} //namespace Bp
+
+} //namespace physx
+
+#endif //BP_BROADPHASE_UPDATE_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/include/BpSimpleAABBManager.h b/PhysX_3.4/Source/LowLevelAABB/include/BpSimpleAABBManager.h
new file mode 100644
index 00000000..55901bf3
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/include/BpSimpleAABBManager.h
@@ -0,0 +1,539 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef BP_SIMPLEAABBMANAGER_H
+#define BP_SIMPLEAABBMANAGER_H
+
+#include "CmPhysXCommon.h"
+#include "BpBroadPhaseUpdate.h"
+#include "GuGeometryUnion.h"
+#include "CmBitMap.h"
+#include "CmTask.h"
+#include "PsAllocator.h"
+#include "GuBounds.h"
+#include "PsHashMap.h"
+#include "CmRadixSortBuffered.h"
+#include "PsFoundation.h"
+#include "BpAABBManagerTasks.h"
+#include "PsHashSet.h"
+
+
+/**
+\brief The maximum number of bounds allowed in an aggregate
+*/
+#define BP_MAX_AGGREGATE_BOUND_SIZE 128	//Max number of bounds in an aggregate bound.
+
+namespace physx
+{
+class PxcScratchAllocator;
+struct PxBroadPhaseType;
+
+namespace Cm
+{
+	class RenderOutput;
+	class EventProfiler;
+	class FlushPool;
+}
+
+namespace Bp
+{
+	typedef PxU32 BoundsIndex;
+	typedef PxU32 AggregateHandle;	// PT: currently an index in mAggregates array
+	typedef PxU32 ActorHandle;
+
+	struct BroadPhasePairReport;
+
+	struct VolumeBuckets
+	{
+		enum Enum
+		{
+			eSHAPE,
+			ePARTICLE,
+			eCLOTH,
+			eTRIGGER,
+			eCOUNT
+		};
+	};
+
+	/**
+	\brief Changes to the configuration of overlap pairs are reported as void* pairs.
+	\note Each void* in the pair corresponds to the void* passed to AABBManager::createVolume.
+	@see AABBManager::createVolume, AABBManager::getCreatedOverlaps, AABBManager::getDestroyedOverlaps
+	*/
+	struct AABBOverlap
+	{
+		PX_FORCE_INLINE AABBOverlap()	{}
+		PX_FORCE_INLINE AABBOverlap(void* userData0, void* userData1, ActorHandle pairHandle) : mUserData0(userData0), mUserData1(userData1), mPairHandle(pairHandle)	{}
+
+		void*	mUserData0;
+		void*	mUserData1;
+		union
+		{
+			ActorHandle	mPairHandle;		//For created pairs, this is the index into the pair in the pair manager
+			void*		mUserData;			//For deleted pairs, this is the user data written by the application to the pair
+		};
+	};
+
+	/*
+	\brief AABBManager volumes with the same filter group value are guaranteed never to generate an overlap pair.
+	\note To ensure that static pairs never overlap, add static shapes with eSTATICS.
+	To ensure that particle bounds never overlap, add particle bounds with ePARTICLES.
+	To ensure that cloth bounds never overlap, add cloth bounds with  eCLOTH.   
+	To ensure that cloth bounds never overlap with cloth or particle bounds, use eCLOTH_NO_PARTICLE_INTERACTION.
+	The value eDYNAMICS_BASE provides a minimum recommended group value for dynamic shapes.
+	If dynamics shapes are assigned group values greater than or equal to eDYNAMICS_BASE then
+	they are allowed to generate broadphase overlaps with particles and statics, and other dynamic shapes provided 
+	they have different group values.
+	@see AABBManager::createVolume
+	*/
+	struct FilterGroup
+	{
+		enum Enum
+		{
+			eSTATICS,
+			ePARTICLES,
+			eCLOTH_NO_PARTICLE_INTERACTION = ePARTICLES,
+			eCLOTH,
+			eDYNAMICS_BASE
+		};
+	};
+
+	class BoundsArray : public Ps::UserAllocated
+	{
+		PX_NOCOPY(BoundsArray)
+
+	public:
+		BoundsArray(Ps::VirtualAllocator& allocator) : mBounds(allocator)
+		{
+		}
+
+		PX_FORCE_INLINE void initEntry(PxU32 index)
+		{
+			const PxU32 oldCapacity = mBounds.capacity();
+			if(index>=oldCapacity)
+			{
+				const PxU32 newCapacity = Ps::nextPowerOfTwo(index);
+				mBounds.reserve(newCapacity);
+				mBounds.forceSize_Unsafe(newCapacity);
+			}
+		}
+
+		PX_FORCE_INLINE void updateBounds(const PxTransform& transform, const Gu::GeometryUnion& geom, PxU32 index, bool extrudeHeightfieldBounds)
+		{
+			Gu::computeBounds(mBounds[index], geom.getGeometry(), transform, 0.0f, NULL, 1.0f, extrudeHeightfieldBounds);
+			mHasAnythingChanged = true;
+		}
+
+
+		PX_FORCE_INLINE const PxBounds3& getBounds(PxU32 index) const
+		{
+			return mBounds[index];
+		}
+
+		PX_FORCE_INLINE void setBounds(const PxBounds3& bounds, PxU32 index)
+		{
+//			PX_CHECK_AND_RETURN(bounds.isValid() && !bounds.isEmpty(), "BoundsArray::setBounds - illegal bounds\n");
+			mBounds[index] = bounds;
+			mHasAnythingChanged = true;
+		}
+
+		PX_FORCE_INLINE const PxBounds3* begin()	const
+		{
+			return mBounds.begin();
+		}
+
+		PX_FORCE_INLINE PxBounds3* begin()
+		{
+			return mBounds.begin();
+		}
+
+		PX_FORCE_INLINE Ps::Array<PxBounds3, Ps::VirtualAllocator>& getBounds()
+		{
+			return mBounds;
+		}
+
+		PX_FORCE_INLINE PxU32 getCapacity()
+		{
+			return mBounds.size();
+		}
+
+		void shiftOrigin(const PxVec3& shift)
+		{
+			// we shift some potential NaNs here because we don't know what's active, but should be harmless
+			for(PxU32 i=0;i<mBounds.size();i++)
+			{
+				mBounds[i].minimum -= shift;
+				mBounds[i].maximum -= shift;
+			}
+			mHasAnythingChanged = true;
+		}
+
+		PX_FORCE_INLINE	bool hasChanged()	const	{ return mHasAnythingChanged;	}
+		PX_FORCE_INLINE	void resetChangedState()	{ mHasAnythingChanged = false;	}
+		PX_FORCE_INLINE	void setChangedState()		{ mHasAnythingChanged = true;	}
+
+	private:
+		Ps::Array<PxBounds3, Ps::VirtualAllocator>	mBounds;
+		bool										mHasAnythingChanged;
+	};
+
+	struct VolumeData
+	{ 
+		void*	mUserData;
+
+		PX_FORCE_INLINE VolumeData() : mUserData(NULL)
+		{
+		}
+
+		PX_FORCE_INLINE	void			reset()
+										{
+											mAggregate = PX_INVALID_U32;
+											mUserData = NULL;
+										}
+
+		PX_FORCE_INLINE	void			setSingleActor()						{ mAggregate = PX_INVALID_U32;				}
+		PX_FORCE_INLINE	bool			isSingleActor()	const					{ return mAggregate == PX_INVALID_U32;		}
+
+		PX_FORCE_INLINE void			setUserData(void* userData)				{ mUserData = userData; }
+
+		PX_FORCE_INLINE void*			getUserData() const						{ return reinterpret_cast<void*>(reinterpret_cast<size_t>(mUserData)& (~size_t(0xF))); }
+
+		PX_FORCE_INLINE void			setVolumeType(PxU8 volumeType)			{ PX_ASSERT(volumeType < 16);  mUserData = reinterpret_cast<void*>(reinterpret_cast<size_t>(getUserData()) | static_cast<size_t>(volumeType)); }
+		PX_FORCE_INLINE PxU8			getVolumeType() const					{ return (reinterpret_cast<size_t>(mUserData)& 0xF); }
+
+		PX_FORCE_INLINE	void			setAggregate(AggregateHandle handle)
+		{
+			PX_ASSERT(handle!=PX_INVALID_U32);
+			mAggregate = (handle<<1)|1;
+		}
+		PX_FORCE_INLINE	bool			isAggregate()	const					{ return !isSingleActor() && ((mAggregate&1)!=0);	}
+
+		PX_FORCE_INLINE	void			setAggregated(AggregateHandle handle)
+		{
+			PX_ASSERT(handle!=PX_INVALID_U32);
+			mAggregate = (handle<<1)|0;
+		}
+
+		PX_FORCE_INLINE	bool			isAggregated()	const
+		{
+			return !isSingleActor() && ((mAggregate&1)==0);
+		}
+
+		PX_FORCE_INLINE	AggregateHandle	getAggregateOwner()	const		{ return mAggregate>>1;					}
+		PX_FORCE_INLINE	AggregateHandle	getAggregate()		const		{ return mAggregate>>1;					}
+
+		private:
+		// PT: TODO: consider moving this to a separate array, which wouldn't be allocated at all for people not using aggregates.
+		// PT: current encoding:
+		// aggregate == PX_INVALID_U32 => single actor
+		// aggregate != PX_INVALID_U32 => aggregate index<<1|LSB. LSB==1 for aggregates, LSB==0 for aggregated actors.
+		AggregateHandle mAggregate;
+	};
+
+	// PT: TODO: revisit this.....
+	class Aggregate;
+	class PersistentPairs;
+	class PersistentActorAggregatePair;
+	class PersistentAggregateAggregatePair;
+	class PersistentSelfCollisionPairs;
+	struct AggPair
+	{
+		PX_FORCE_INLINE	AggPair(ShapeHandle index0, ShapeHandle index1) : mIndex0(index0), mIndex1(index1)	{}
+		ShapeHandle	mIndex0;
+		ShapeHandle	mIndex1;
+
+		PX_FORCE_INLINE bool operator==(const AggPair& p) const
+		{
+			return (p.mIndex0 == mIndex0) && (p.mIndex1 == mIndex1);
+		}
+	};
+	typedef Ps::CoalescedHashMap<AggPair, PersistentPairs*> AggPairMap;
+
+	// PT: TODO: isn't there a generic pair structure somewhere? refactor with AggPair anyway
+	struct Pair
+	{
+		PX_FORCE_INLINE	Pair(PxU32 id0, PxU32 id1) : mID0(id0), mID1(id1)	{}
+		PX_FORCE_INLINE Pair(){}
+
+			PX_FORCE_INLINE bool operator<(const Pair& p) const
+			{
+				const PxU64 value0 = *reinterpret_cast<const PxU64*>(this);
+				const PxU64 value1 = *reinterpret_cast<const PxU64*>(&p);
+				return value0 < value1;
+			}
+
+			PX_FORCE_INLINE bool operator==(const Pair& p) const
+			{
+				return (p.mID0 == mID0) && (p.mID1 == mID1);
+			}
+
+			PX_FORCE_INLINE bool operator!=(const Pair& p) const
+			{
+				return (p.mID0 != mID0) || (p.mID1 != mID1);
+			}
+
+		PxU32	mID0;
+		PxU32	mID1;
+	};
+
+
+	/**
+	\brief A structure responsible for:
+	* storing an aabb representation for each active shape in the related scene
+	* managing the creation/removal of aabb representations when their related shapes are created/removed
+	* updating all aabbs that require an update due to modification of shape geometry or transform
+	* updating the aabb of all aggregates from the union of the aabbs of all shapes that make up each aggregate
+	* computing and reporting the incremental changes to the set of overlapping aabb pairs 
+	*/
+	class SimpleAABBManager : public Ps::UserAllocated
+	{
+		PX_NOCOPY(SimpleAABBManager)
+	public:
+
+		SimpleAABBManager(BroadPhase& bp, BoundsArray& boundsArray, Ps::Array<PxReal, Ps::VirtualAllocator>& contactDistance, PxU32 maxNbAggregates, PxU32 maxNbShapes, Ps::VirtualAllocator& allocator, PxU64 contextID);
+
+		void	destroy();
+
+		AggregateHandle	createAggregate(BoundsIndex index, void* userData, const bool selfCollisions);
+		BoundsIndex		destroyAggregate(AggregateHandle aggregateHandle);
+
+		bool			addBounds(BoundsIndex index, PxReal contactDistance, PxU32 group, void* userdata, AggregateHandle aggregateHandle, PxU8 volumeType);
+		void			removeBounds(BoundsIndex index);
+
+		void			setContactOffset(BoundsIndex handle, PxReal offset)
+		{
+			// PT: this works even for aggregated shapes, since the corresponding bit will also be set in the 'updated' map.
+			mContactDistance.begin()[handle] = offset;
+			mPersistentStateChanged = true;
+			mChangedHandleMap.growAndSet(handle);
+		}
+
+		void setVolumeType(BoundsIndex handle, PxU8 volumeType)
+		{
+			mVolumeData[handle].setVolumeType(volumeType);
+		}
+
+		// PT: TODO: revisit name: we don't "update AABBs" here anymore
+		void			updateAABBsAndBP(	PxU32 numCpuTasks,
+											Cm::FlushPool& flushPool,
+											PxcScratchAllocator* scratchAllocator,
+											bool hasContactDistanceUpdated,
+											PxBaseTask* continuation,
+											PxBaseTask* narrowPhaseUnlockTask);
+
+		void			finalizeUpdate(		PxU32 numCpuTasks,
+											PxcScratchAllocator* scratchAllocator,
+											PxBaseTask* continuation,
+											PxBaseTask* narrowPhaseUnlockTask);
+
+		AABBOverlap*				getCreatedOverlaps(PxU32 type, PxU32& count)
+		{
+			PX_ASSERT(type < VolumeBuckets::eCOUNT);
+			count = mCreatedOverlaps[type].size();
+			return mCreatedOverlaps[type].begin();
+		}
+
+		AABBOverlap*				getDestroyedOverlaps(PxU32 type, PxU32& count)
+		{
+			PX_ASSERT(type < VolumeBuckets::eCOUNT);
+			count = mDestroyedOverlaps[type].size();
+			return mDestroyedOverlaps[type].begin();
+		}
+
+		void						freeBuffers();
+
+		void**						getOutOfBoundsObjects(PxU32& nbOutOfBoundsObjects)
+		{
+			nbOutOfBoundsObjects = mOutOfBoundsObjects.size();
+			return mOutOfBoundsObjects.begin();
+		}
+
+		void						clearOutOfBoundsObjects()
+		{
+			mOutOfBoundsObjects.clear();
+		}
+
+		void**						getOutOfBoundsAggregates(PxU32& nbOutOfBoundsAggregates)
+		{
+			nbOutOfBoundsAggregates = mOutOfBoundsAggregates.size();
+			return mOutOfBoundsAggregates.begin();
+		}
+
+		void						clearOutOfBoundsAggregates()
+		{
+			mOutOfBoundsAggregates.clear();
+		}
+
+		void						shiftOrigin(const PxVec3& shift);
+
+		void						visualize(Cm::RenderOutput& out);
+
+		PX_FORCE_INLINE	BroadPhase*					getBroadPhase()				const	{ return &mBroadPhase;				}
+		PX_FORCE_INLINE	BoundsArray&				getBoundsArray()					{ return mBoundsArray;				}
+		PX_FORCE_INLINE	PxU32						getNbActiveAggregates()		const	{ return mNbAggregates;				}
+		PX_FORCE_INLINE	const float*				getContactDistances()		const	{ return mContactDistance.begin();	}
+		PX_FORCE_INLINE	Cm::BitMapPinned&			getChangedAABBMgActorHandleMap()	{ return mChangedHandleMap;			}
+
+		PX_FORCE_INLINE void*						getUserData(const BoundsIndex index) const { if (index < mVolumeData.size()) return mVolumeData[index].getUserData(); return NULL; }
+		PX_FORCE_INLINE	PxU64						getContextId()				const	{ return mContextID;				}
+
+	private:
+		void reserveShapeSpace(PxU32 nbShapes);
+		void postBroadPhase(PxBaseTask*);
+
+		Cm::DelegateTask<SimpleAABBManager, &SimpleAABBManager::postBroadPhase>			mPostBroadPhase;
+
+		FinalizeUpdateTask				mFinalizeUpdateTask;
+
+		// PT: we have bitmaps here probably to quickly handle added/removed objects during same frame.
+		// PT: TODO: consider replacing with plain arrays (easier to parse, already existing below, etc)
+		Cm::BitMap					mAddedHandleMap;		// PT: indexed by BoundsIndex
+		Cm::BitMap					mRemovedHandleMap;		// PT: indexed by BoundsIndex
+		Cm::BitMapPinned			mChangedHandleMap;
+
+		PX_FORCE_INLINE void removeBPEntry(BoundsIndex index)	// PT: only for objects passed to the BP
+		{
+			if(mAddedHandleMap.test(index))		// PT: if object had been added this frame...
+				mAddedHandleMap.reset(index);	// PT: ...then simply revert the previous operation locally (it hasn't been passed to the BP yet).
+			else
+				mRemovedHandleMap.set(index);	// PT: else we need to remove it from the BP
+		}
+
+		// PT: TODO: when do we need 'Ps::VirtualAllocator' and when don't we? When memory is passed to GPU BP?
+		// PT: TODO: this group array won't compile on platforms where PX_USE_16_BIT_HANDLES is true, because it's silently
+		// passed to BroadPhaseUpdateData, which expects ShapeHandle for groups (not PxU32). I cannot change it immediately
+		// because of the comment below, saying it sticks PX_INVALID_U32 in there. Not sure if 0xffff would work.
+		//ML: we create mGroups and mContactDistance in the SimpleAABBManager constructor. Ps::Array will take Ps::VirtualAllocator as a parameter. Therefore, if GPU BP is using,
+		//we will passed a pinned host memory allocator, otherwise, we will just pass a normal allocator.
+		Ps::Array<PxU32,	Ps::VirtualAllocator>				mGroups;				// NOTE: we stick PX_INVALID_U32 in this slot to indicate that the entry is invalid (removed or never inserted.)
+		Ps::Array<PxReal, Ps::VirtualAllocator>& 				mContactDistance;
+		Ps::Array<VolumeData>									mVolumeData;
+
+		PX_FORCE_INLINE void initEntry(BoundsIndex index, PxReal contactDistance, PxU32 group, void* userData)
+		{
+			if(index >= mVolumeData.size())
+				reserveShapeSpace(index);
+
+			// PT: TODO: why is this needed at all? Why aren't size() and capacity() enough?
+			mUsedSize = PxMax(index+1, mUsedSize);
+
+			PX_ASSERT(group != PX_INVALID_U32);	// PT: we use group == PX_INVALID_U32 to mark removed/invalid entries
+			mGroups[index] = group;
+			mContactDistance.begin()[index] = contactDistance;
+			mVolumeData[index].setUserData(userData);
+		}
+
+		PX_FORCE_INLINE void resetEntry(BoundsIndex index)
+		{
+			mGroups[index] = PX_INVALID_U32;
+			mContactDistance.begin()[index] = 0.0f;
+			mVolumeData[index].reset();
+		}
+
+		// PT: TODO: remove confusion between BoundsIndex and ShapeHandle here!
+		Ps::Array<ShapeHandle, Ps::VirtualAllocator>			mAddedHandles;
+		Ps::Array<ShapeHandle, Ps::VirtualAllocator>			mUpdatedHandles;
+		Ps::Array<ShapeHandle, Ps::VirtualAllocator>			mRemovedHandles;
+
+		BroadPhase&					mBroadPhase;
+		BoundsArray&				mBoundsArray;
+
+		Ps::Array<void*>			mOutOfBoundsObjects;
+		Ps::Array<void*>			mOutOfBoundsAggregates;
+		Ps::Array<AABBOverlap>		mCreatedOverlaps[VolumeBuckets::eCOUNT];
+		Ps::Array<AABBOverlap>		mDestroyedOverlaps[VolumeBuckets::eCOUNT];
+
+		PxcScratchAllocator*		mScratchAllocator;
+
+		PxBaseTask*					mNarrowPhaseUnblockTask;
+		PxU32						mUsedSize;				// highest used value + 1
+		bool						mOriginShifted;
+		bool						mPersistentStateChanged;
+
+		PxU32						mNbAggregates;
+		PxU32						mFirstFreeAggregate;
+		Ps::Array<Aggregate*>		mAggregates;		// PT: indexed by AggregateHandle
+		Ps::Array<Aggregate*>		mDirtyAggregates;
+
+		PxU32						mTimestamp;
+
+		AggPairMap					mActorAggregatePairs;
+		AggPairMap					mAggregateAggregatePairs;
+
+		// PT: TODO: even in the 3.4 trunk this stuff is a clumsy mess: groups are "BpHandle" suddenly passed
+		// to BroadPhaseUpdateData as "ShapeHandle".
+		//Free aggregate group ids.
+		BpHandle					mAggregateGroupTide;
+		Ps::Array<BpHandle>			mFreeAggregateGroups;	// PT: TODO: remove this useless array
+
+		Ps::HashSet<Pair>			mCreatedPairs;
+
+		PxU64						mContextID;
+
+		PX_FORCE_INLINE Aggregate* getAggregateFromHandle(AggregateHandle handle)
+		{
+			PX_ASSERT(handle<mAggregates.size());
+			return mAggregates[handle];
+		}
+
+		PX_FORCE_INLINE void		releaseAggregateGroup(const BpHandle group) 
+		{
+			PX_ASSERT(group != BP_INVALID_BP_HANDLE);
+			mFreeAggregateGroups.pushBack(group);
+		}
+
+		PX_FORCE_INLINE BpHandle	getAggregateGroup()
+		{
+			BpHandle group;
+			if(mFreeAggregateGroups.size())
+				group = mFreeAggregateGroups.popBack();
+			else
+				group = mAggregateGroupTide--;
+			PX_ASSERT(group!=BP_INVALID_BP_HANDLE);
+			return group;
+		}
+
+		void startAggregateBoundsComputationTasks(PxU32 nbToGo, PxU32 numCpuTasks, Cm::FlushPool& flushPool);
+		PersistentActorAggregatePair* createPersistentActorAggregatePair(ShapeHandle volA, ShapeHandle volB);
+		PersistentAggregateAggregatePair* createPersistentAggregateAggregatePair(ShapeHandle volA, ShapeHandle volB);
+		void updatePairs(PersistentPairs& p);
+		void handleOriginShift();
+		public:
+		void processBPCreatedPair(const BroadPhasePairReport& pair);
+		void processBPDeletedPair(const BroadPhasePairReport& pair);
+//		bool checkID(ShapeHandle id);
+		friend class PersistentActorAggregatePair;
+		friend class PersistentAggregateAggregatePair;
+	};
+
+} //namespace Bp
+
+} //namespace physx
+
+#endif //BP_AABBMANAGER_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhase.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhase.cpp
new file mode 100644
index 00000000..93586279
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhase.cpp
@@ -0,0 +1,188 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "BpBroadPhase.h"
+#include "BpBroadPhaseSap.h"
+#include "BpBroadPhaseMBP.h"
+#include "PxSceneDesc.h"
+#include "BpSimpleAABBManager.h"
+
+using namespace physx;
+using namespace Bp;
+
+BroadPhase* BroadPhase::create(
+	const PxBroadPhaseType::Enum bpType,
+	const PxU32 maxNbRegions,
+	const PxU32 maxNbBroadPhaseOverlaps,
+	const PxU32 maxNbStaticShapes,
+	const PxU32 maxNbDynamicShapes,
+	PxU64 contextID)
+{
+	PX_ASSERT(bpType==PxBroadPhaseType::eMBP || bpType == PxBroadPhaseType::eSAP);
+
+	if(bpType==PxBroadPhaseType::eMBP)
+		return PX_NEW(BroadPhaseMBP)(maxNbRegions, maxNbBroadPhaseOverlaps, maxNbStaticShapes, maxNbDynamicShapes);
+	else
+		return PX_NEW(BroadPhaseSap)(maxNbBroadPhaseOverlaps, maxNbStaticShapes, maxNbDynamicShapes, contextID);
+}
+
+#if PX_CHECKED
+bool BroadPhaseUpdateData::isValid(const BroadPhaseUpdateData& updateData, const BroadPhase& bp)
+{
+	return (updateData.isValid() && bp.isValid(updateData));
+}
+#endif
+
+#if PX_CHECKED
+bool BroadPhaseUpdateData::isValid() const 
+{
+	const BpHandle* created=getCreatedHandles();
+	const BpHandle* updated=getUpdatedHandles();
+	const BpHandle* removed=getRemovedHandles();
+	const PxU32 createdSize=getNumCreatedHandles();
+	const PxU32 updatedSize=getNumUpdatedHandles();
+	const PxU32 removedSize=getNumRemovedHandles();
+	const PxBounds3* bounds=getAABBs();
+	const BpHandle* groups=getGroups();
+	const PxU32 boxesCapacity=getCapacity();
+
+	if(NULL==created && createdSize>0)
+	{
+		return false;
+	}
+	if(NULL==updated && updatedSize>0)
+	{
+		return false;
+	}
+	if(NULL==removed && removedSize>0)
+	{
+		return false;
+	}
+
+	ValType minVal=0;
+	ValType maxVal=0xffffffff;
+	PX_UNUSED(minVal);
+	PX_UNUSED(maxVal);
+
+	for(PxU32 i=0;i<createdSize;i++)
+	{
+		if(created[i]>=boxesCapacity)
+			return false;
+
+		//Created array in ascending order of id.
+		if(i>0 && (created[i] < created[i-1]))
+		{
+			return false;
+		}
+		for(PxU32 j=0;j<3;j++)
+		{
+			//Max must be greater than min.
+			if(bounds[created[i]].minimum[j]>bounds[created[i]].maximum[j])
+				return false;
+
+#if 0
+			//Bounds have an upper limit.
+			if(bounds[created[i]].getMax(j)>=maxVal)
+				return false;
+
+			//Bounds have a lower limit.
+			if(bounds[created[i]].getMin(j)<=minVal)
+				return false;
+
+			//Max must be odd.
+			if(4 != (bounds[created[i]].getMax(j) & 4))
+				return false;
+
+			//Min must be even.
+			if(0 != (bounds[created[i]].getMin(j) & 4))
+				return false;
+#endif
+		}
+
+		//Group ids must be less than BP_INVALID_BP_HANDLE.
+		if(groups[created[i]]>=BP_INVALID_BP_HANDLE)
+			return false;
+	}
+
+	for(PxU32 i=0;i<updatedSize;i++)
+	{
+		if(updated[i]>=boxesCapacity)
+			return false;
+
+		//Updated array in ascending order of id
+		if(i>0 && (updated[i] < updated[i-1]))
+		{
+			return false;
+		}
+
+		for(PxU32 j=0;j<3;j++)
+		{
+			//Max must be greater than min.
+			if(bounds[updated[i]].minimum[j]>bounds[updated[i]].maximum[j])
+				return false;
+#if 0
+			//Bounds have an upper limit.
+			if(bounds[updated[i]].getMax(j)>=maxVal)
+				return false;
+
+			//Bounds have a lower limit.
+			if(bounds[updated[i]].getMin(j)<=minVal)
+				return false;
+
+			//Max must be odd.
+			if(4 != (bounds[updated[i]].getMax(j) & 4))
+				return false;
+
+			//Min must be even.
+			if(0 != (bounds[updated[i]].getMin(j) & 4))
+				return false;
+#endif
+		}
+
+		//Group ids must be less than BP_INVALID_BP_HANDLE.
+		if(groups[updated[i]]>=BP_INVALID_BP_HANDLE)
+			return false;
+	}
+
+	for(PxU32 i=0;i<removedSize;i++)
+	{
+		if(removed[i]>=boxesCapacity)
+			return false;
+
+		//Removed array in ascending order of id
+		if(i>0 && (removed[i] < removed[i-1]))
+		{
+			return false;
+		}
+	}
+
+	return true;
+}
+#endif
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.cpp
new file mode 100644
index 00000000..5227c7a4
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.cpp
@@ -0,0 +1,3822 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "foundation/PxProfiler.h"
+#include "PsHash.h"
+#include "BpBroadPhaseMBP.h"
+#include "CmRadixSortBuffered.h"
+#include "CmUtils.h"
+#include "PsUtilities.h"
+#include "PsFoundation.h"
+#include "PsVecMath.h"
+
+
+using namespace physx::shdfnd::aos;
+
+//#define CHECK_NB_OVERLAPS
+//#define USE_SINGLE_THREADED_REFERENCE_CODE
+#define USE_FULLY_INSIDE_FLAG
+
+//HWSCAN: reverse bits in fully-inside-flag bitmaps because the code gives us indices for which bits are set (and we want the opposite)
+#define HWSCAN
+
+using namespace physx;
+using namespace Bp;
+using namespace Cm;
+
+	template<class T>
+	static PX_INLINE T* reserveContainerMemory(Ps::Array<T>& container, PxU32 nb)
+	{
+		const PxU32 maxNbEntries = container.capacity();
+		const PxU32 requiredSize = container.size() + nb;
+
+		if(requiredSize>maxNbEntries)
+		{
+			const PxU32 naturalGrowthSize = maxNbEntries ? maxNbEntries*2 : 2;
+			const PxU32 newSize = PxMax(requiredSize, naturalGrowthSize);
+			container.reserve(newSize);
+		}
+
+		T* buf = container.end();
+		container.forceSize_Unsafe(requiredSize);
+		return buf;
+	}
+
+	static PX_FORCE_INLINE void storeDwords(PxU32* dest, PxU32 nb, PxU32 value)
+	{
+		while(nb--)
+			*dest++ = value;
+	}
+
+#define MBP_ALLOC(x)		PX_ALLOC(x, "MBP")
+#define MBP_ALLOC_TMP(x)	PX_ALLOC_TEMP(x, "MBP_TMP")
+#define MBP_FREE(x)			if(x)	PX_FREE_AND_RESET(x)
+#define DELETESINGLE(x)		if (x) { delete x;		x = NULL; }
+#define DELETEARRAY(x)		if (x) { delete []x;	x = NULL; }
+
+#define	INVALID_ID	0xffffffff
+
+	typedef	MBP_Index*	MBP_Mapping;
+
+/*	PX_FORCE_INLINE PxU32 encodeFloat(const float val)
+	{
+		// We may need to check on -0 and 0
+		// But it should make no practical difference.
+		PxU32 ir = IR(val);
+
+		if(ir & 0x80000000) //negative?
+			ir = ~ir;//reverse sequence of negative numbers
+		else
+			ir |= 0x80000000; // flip sign
+
+		return ir;
+	}*/
+
+struct RegionHandle : public Ps::UserAllocated
+{
+	PxU16	mHandle;			// Handle from region
+	PxU16	mInternalBPHandle;	// Index of region data within mRegions
+};
+
+enum MBPFlags
+{
+	MBP_FLIP_FLOP	= (1<<1),
+	MBP_REMOVED		= (1<<2)	// ### added for TA24714, not needed otherwise
+};
+
+// We have one of those for each of the "200K" objects so we should optimize this size as much as possible
+struct MBP_Object : public Ps::UserAllocated
+{
+	BpHandle	mUserID;		// ### added for PhysX integration
+	PxU16		mNbHandles;		// Number of regions the object is part of
+	PxU16		mFlags;			// MBPFlags ### only 1 bit used in the end
+
+	PX_FORCE_INLINE	bool	getFlipFlop()	const	{ return (mFlags & MBP_FLIP_FLOP)==0;	}
+
+	union
+	{
+		RegionHandle	mHandle;
+		PxU32			mHandlesIndex;
+	};
+};
+
+// This one is used in each Region
+struct MBPEntry : public MBPEntry_Data, public Ps::UserAllocated
+{
+	PX_FORCE_INLINE	MBPEntry()
+	{
+		mMBPHandle = INVALID_ID;
+	}
+};
+
+///////////////////////////////////////////////////////////////////////////////
+
+//#define BIT_ARRAY_STACK	512
+
+	static PX_FORCE_INLINE PxU32 bitsToDwords(PxU32 nbBits)
+	{
+		return (nbBits>>5) + ((nbBits&31) ? 1 : 0);
+	}
+
+	// Use that one instead of an array of bools. Takes less ram, nearly as fast [no bounds checkings and so on].
+	class BitArray
+	{
+		public:
+										BitArray();
+										BitArray(PxU32 nbBits);
+										~BitArray();
+
+						bool			init(PxU32 nbBits);
+						void			empty();
+						void			resize(PxU32 nbBits);
+
+		PX_FORCE_INLINE	void			setBitChecked(PxU32 bitNumber)
+										{
+											const PxU32 index = bitNumber>>5;
+											if(index>=mSize)
+												resize(bitNumber);
+											mBits[index] |= 1<<(bitNumber&31);
+										}
+
+		PX_FORCE_INLINE	void			clearBitChecked(PxU32 bitNumber)
+										{
+											const PxU32 index = bitNumber>>5;
+											if(index>=mSize)
+												resize(bitNumber);
+											mBits[index] &= ~(1<<(bitNumber&31));
+										}
+		// Data management
+		PX_FORCE_INLINE	void			setBit(PxU32 bitNumber)					{ mBits[bitNumber>>5] |= 1<<(bitNumber&31);				}
+		PX_FORCE_INLINE	void			clearBit(PxU32 bitNumber)				{ mBits[bitNumber>>5] &= ~(1<<(bitNumber&31));			}
+		PX_FORCE_INLINE	void			toggleBit(PxU32 bitNumber)				{ mBits[bitNumber>>5] ^= 1<<(bitNumber&31);				}
+
+		PX_FORCE_INLINE	void			clearAll()								{ PxMemZero(mBits, mSize*4);							}
+		PX_FORCE_INLINE	void			setAll()								{ PxMemSet(mBits, 0xff, mSize*4);						}
+
+		// Data access
+		PX_FORCE_INLINE	Ps::IntBool		isSet(PxU32 bitNumber)			const	{ return Ps::IntBool(mBits[bitNumber>>5] & (1<<(bitNumber&31)));		}
+		PX_FORCE_INLINE	Ps::IntBool		isSetChecked(PxU32 bitNumber)	const
+										{
+											const PxU32 index = bitNumber>>5;
+											if(index>=mSize)
+												return 0;
+											return Ps::IntBool(mBits[index] & (1<<(bitNumber&31)));
+										}
+
+		PX_FORCE_INLINE	const PxU32*	getBits()						const	{ return mBits;											}
+		PX_FORCE_INLINE	PxU32			getSize()						const	{ return mSize;											}
+
+		// PT: replicate Cm::BitMap stuff for temp testing
+						PxU32			findLast()						const
+										{
+											for(PxU32 i = mSize; i-- > 0;)
+											{
+												if(mBits[i])
+													return (i<<5)+Ps::highestSetBit(mBits[i]);
+											}
+											return PxU32(0); 
+										}
+		protected:
+						PxU32*			mBits;		//!< Array of bits
+						PxU32			mSize;		//!< Size of the array in dwords
+#ifdef BIT_ARRAY_STACK
+						PxU32			mStack[BIT_ARRAY_STACK];
+#endif
+	};
+
+///////////////////////////////////////////////////////////////////////////////
+
+BitArray::BitArray() : mBits(NULL), mSize(0)
+{
+}
+
+BitArray::BitArray(PxU32 nbBits) : mBits(NULL), mSize(0)
+{
+	init(nbBits);
+}
+
+BitArray::~BitArray()
+{
+	empty();
+}
+
+void BitArray::empty()
+{
+#ifdef BIT_ARRAY_STACK
+	if(mBits!=mStack)
+#endif
+		MBP_FREE(mBits);
+	mBits = NULL;
+	mSize = 0;
+}
+
+bool BitArray::init(PxU32 nbBits)
+{
+	mSize = bitsToDwords(nbBits);
+	// Get ram for n bits
+#ifdef BIT_ARRAY_STACK
+	if(mBits!=mStack)
+#endif
+		MBP_FREE(mBits);
+#ifdef BIT_ARRAY_STACK
+	if(mSize>BIT_ARRAY_STACK)
+#endif
+		mBits = reinterpret_cast<PxU32*>(MBP_ALLOC(sizeof(PxU32)*mSize));
+#ifdef BIT_ARRAY_STACK
+	else
+		mBits = mStack;
+#endif
+
+	// Set all bits to 0
+	clearAll();
+	return true;
+}
+
+void BitArray::resize(PxU32 nbBits)
+{
+	const PxU32 newSize = bitsToDwords(nbBits+128);
+	PxU32* newBits = NULL;
+#ifdef BIT_ARRAY_STACK
+	if(newSize>BIT_ARRAY_STACK)
+#endif
+	{
+		// Old buffer was stack or allocated, new buffer is allocated
+		newBits = reinterpret_cast<PxU32*>(MBP_ALLOC(sizeof(PxU32)*newSize));
+		if(mSize)
+			PxMemCopy(newBits, mBits, sizeof(PxU32)*mSize);
+	}
+#ifdef BIT_ARRAY_STACK
+	else
+	{
+		newBits = mStack;
+		if(mSize>BIT_ARRAY_STACK)
+		{
+			// Old buffer was allocated, new buffer is stack => copy to stack, shrink
+			CopyMemory(newBits, mBits, sizeof(PxU32)*BIT_ARRAY_STACK);
+		}
+		else
+		{
+			// Old buffer was stack, new buffer is stack => keep working on the same stack buffer, nothing to do
+		}
+	}
+#endif
+	const PxU32 remain = newSize - mSize;
+	if(remain)
+		PxMemZero(newBits + mSize, remain*sizeof(PxU32));
+
+#ifdef BIT_ARRAY_STACK
+	if(mBits!=mStack)
+#endif
+		MBP_FREE(mBits);
+	mBits = newBits;
+	mSize = newSize;
+}
+
+#ifdef USE_FULLY_INSIDE_FLAG
+static PX_FORCE_INLINE void setBit(BitArray& bitmap, MBP_ObjectIndex objectIndex)
+{
+	#ifdef HWSCAN
+	bitmap.clearBitChecked(objectIndex);	//HWSCAN
+	#else
+	bitmap.setBitChecked(objectIndex);
+	#endif
+}
+
+static PX_FORCE_INLINE void clearBit(BitArray& bitmap, MBP_ObjectIndex objectIndex)
+{
+	#ifdef HWSCAN
+	bitmap.setBitChecked(objectIndex);	// HWSCAN
+	#else
+	bitmap.clearBitChecked(objectIndex);
+	#endif
+}
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+
+#ifdef MBP_SIMD_OVERLAP
+	typedef	SIMD_AABB	MBP_AABB;
+#else
+	typedef	IAABB		MBP_AABB;
+#endif
+
+	struct MBP_Pair : public Ps::UserAllocated
+	{
+		PxU32		id0;
+		PxU32		id1;
+		void*		usrData;
+		// TODO: optimize memory here
+		bool		isNew;
+		bool		isUpdated;
+	};
+
+	struct MBPEntry;
+	struct RegionHandle;
+	struct MBP_Object;
+
+	class MBP_PairManager : public Ps::UserAllocated
+	{
+		public:
+											MBP_PairManager();
+											~MBP_PairManager();
+
+						void				purge();
+						void				shrinkMemory();
+
+						MBP_Pair*			addPair				(PxU32 id0, PxU32 id1, const BpHandle* PX_RESTRICT groups = NULL, const MBP_Object* objects = NULL);
+						bool				removePair			(PxU32 id0, PxU32 id1);
+//						bool				removePairs			(const BitArray& array);
+						bool				removeMarkedPairs	(const MBP_Object* objects, BroadPhaseMBP* mbp, const BitArray& updated, const BitArray& removed);
+		PX_FORCE_INLINE	PxU32				getPairIndex		(const MBP_Pair* pair)		const
+											{
+												return (PxU32((size_t(pair) - size_t(mActivePairs)))/sizeof(MBP_Pair));
+											}
+
+						PxU32				mHashSize;
+						PxU32				mMask;
+						PxU32				mNbActivePairs;
+						PxU32*				mHashTable;
+						PxU32*				mNext;
+						MBP_Pair*			mActivePairs;
+						PxU32				mReservedMemory;
+
+		PX_FORCE_INLINE	MBP_Pair*			findPair(PxU32 id0, PxU32 id1, PxU32 hashValue) const;
+						void				removePair(PxU32 id0, PxU32 id1, PxU32 hashValue, PxU32 pairIndex);
+						void				reallocPairs();
+						void				reserveMemory(PxU32 memSize);
+	};
+
+	///////////////////////////////////////////////////////////////////////////
+
+	#define STACK_BUFFER_SIZE	256
+	struct MBPOS_TmpBuffers
+	{
+					MBPOS_TmpBuffers();
+					~MBPOS_TmpBuffers();
+
+		void		allocateSleeping(PxU32 nbSleeping, PxU32 nbSentinels);
+		void		allocateUpdated(PxU32 nbUpdated, PxU32 nbSentinels);
+
+		// PT: wtf, why doesn't the 128 version compile?
+//		MBP_AABB	PX_ALIGN(128, mSleepingDynamicBoxes_Stack[STACK_BUFFER_SIZE]);
+//		MBP_AABB	PX_ALIGN(128, mUpdatedDynamicBoxes_Stack[STACK_BUFFER_SIZE]);
+		MBP_AABB	PX_ALIGN(16, mSleepingDynamicBoxes_Stack[STACK_BUFFER_SIZE]);
+		MBP_AABB	PX_ALIGN(16, mUpdatedDynamicBoxes_Stack[STACK_BUFFER_SIZE]);
+		MBP_Index	mInToOut_Dynamic_Sleeping_Stack[STACK_BUFFER_SIZE];
+
+		PxU32		mNbSleeping;
+		PxU32		mNbUpdated;
+		MBP_Index*	mInToOut_Dynamic_Sleeping;
+		MBP_AABB*	mSleepingDynamicBoxes;
+		MBP_AABB*	mUpdatedDynamicBoxes;
+	};
+
+	struct BIP_Input
+	{
+		BIP_Input() :
+			mObjects		(NULL),
+			mNbUpdatedBoxes	(0),
+			mNbStaticBoxes	(0),
+			mDynamicBoxes	(NULL),
+			mStaticBoxes	(NULL),
+			mInToOut_Static	(NULL),
+			mInToOut_Dynamic(NULL),
+			mNeeded			(false)
+		{
+		}
+
+		const MBPEntry*		mObjects;
+		PxU32				mNbUpdatedBoxes;
+		PxU32				mNbStaticBoxes;
+		const MBP_AABB*		mDynamicBoxes;
+		const MBP_AABB*		mStaticBoxes;
+		const MBP_Index*	mInToOut_Static;
+		const MBP_Index*	mInToOut_Dynamic;
+		bool				mNeeded;
+	};
+
+	struct BoxPruning_Input
+	{
+		BoxPruning_Input() :
+			mObjects					(NULL),
+			mUpdatedDynamicBoxes		(NULL),
+			mSleepingDynamicBoxes		(NULL),
+			mInToOut_Dynamic			(NULL),
+			mInToOut_Dynamic_Sleeping	(NULL),
+			mNbUpdated					(0),
+			mNbNonUpdated				(0),
+			mNeeded						(false)
+		{
+		}
+
+		const MBPEntry*		mObjects;
+		const MBP_AABB*		mUpdatedDynamicBoxes;
+		const MBP_AABB*		mSleepingDynamicBoxes;
+		const MBP_Index*	mInToOut_Dynamic;
+		const MBP_Index*	mInToOut_Dynamic_Sleeping;
+		PxU32				mNbUpdated;
+		PxU32				mNbNonUpdated;
+		bool				mNeeded;
+
+		BIP_Input			mBIPInput;
+	};
+
+	class Region : public Ps::UserAllocated
+	{
+							PX_NOCOPY(Region)
+		public:
+							Region();
+							~Region();
+
+		void				updateObject(const MBP_AABB& bounds, MBP_Index handle);
+		MBP_Index			addObject(const MBP_AABB& bounds, MBP_Handle mbpHandle, bool isStatic);
+		void				removeObject(MBP_Index handle);
+		MBP_Handle			retrieveBounds(MBP_AABB& bounds, MBP_Index handle)	const;
+		void				setBounds(MBP_Index handle, const MBP_AABB& bounds);
+#ifdef USE_SINGLE_THREADED_REFERENCE_CODE
+		void				prepareOverlaps();
+		void				findOverlaps(MBP_PairManager& pairManager, MBPOS_TmpBuffers& buffers);
+#endif
+		void				prepareOverlapsMT();
+		void				findOverlapsMT(MBP_PairManager& pairManager, const BpHandle* PX_RESTRICT groups, const MBP_Object* PX_RESTRICT mbpObjects);
+
+//		private:
+		BoxPruning_Input	PX_ALIGN(16, mInput);
+		PxU32				mNbObjects;
+		PxU32				mMaxNbObjects;
+		PxU32				mFirstFree;
+		MBPEntry*			mObjects;			// All objects, indexed by user handle
+		PxU32				mMaxNbStaticBoxes;
+		PxU32				mNbStaticBoxes;
+		PxU32				mMaxNbDynamicBoxes;
+		PxU32				mNbDynamicBoxes;
+		MBP_AABB*			mStaticBoxes;
+		MBP_AABB*			mDynamicBoxes;
+		MBP_Mapping			mInToOut_Static;	// Maps static boxes to mObjects
+		MBP_Mapping			mInToOut_Dynamic;	// Maps dynamic boxes to mObjects
+		PxU32*				mPosList;
+		PxU32				mNbUpdatedBoxes;
+		PxU32				mPrevNbUpdatedBoxes;
+		BitArray			mStaticBits;
+		RadixSortBuffered	mRS;
+		bool				mNeedsSorting;
+		bool				mNeedsSortingSleeping;
+				
+		MBPOS_TmpBuffers	mTmpBuffers;
+
+		void				optimizeMemory();
+		void				resizeObjects();
+		void				staticSort();
+		void				preparePruning(MBPOS_TmpBuffers& buffers);
+		void				prepareBIPPruning(const MBPOS_TmpBuffers& buffers);
+	};
+
+	///////////////////////////////////////////////////////////////////////////
+
+// We have one of those for each Region within the MBP
+struct RegionData : public Ps::UserAllocated
+{
+	MBP_AABB	mBox;		// Volume of space controlled by this Region
+	Region*		mBP;		// Pointer to Region itself
+	Ps::IntBool	mOverlap;	// True if overlaps other regions
+	void*		mUserData;	// Region identifier, reused to contain "first free ID"
+};
+
+	#define MAX_NB_MBP	256
+//	#define MAX_NB_MBP	16
+
+	class MBP : public Ps::UserAllocated
+	{
+		public:
+												MBP();
+												~MBP();
+
+						void					preallocate(PxU32 nbRegions, PxU32 nbObjects, PxU32 maxNbOverlaps);
+						void					reset();
+						void					freeBuffers();
+
+						PxU32					addRegion(const PxBroadPhaseRegion& region, bool populateRegion);
+						bool					removeRegion(PxU32 handle);
+						const Region*			getRegion(PxU32 i)		const;
+		PX_FORCE_INLINE	PxU32					getNbRegions()			const	{ return mNbRegions;	}
+
+						MBP_Handle				addObject(const MBP_AABB& box, BpHandle userID, bool isStatic=false);
+						bool					removeObject(MBP_Handle handle);
+						bool					updateObject(MBP_Handle handle, const MBP_AABB& box);
+						bool					updateObjectAfterRegionRemoval(MBP_Handle handle, Region* removedRegion);
+						bool					updateObjectAfterNewRegionAdded(MBP_Handle handle, const MBP_AABB& box, Region* addedRegion, PxU32 regionIndex);
+#ifdef USE_SINGLE_THREADED_REFERENCE_CODE
+						void					prepareOverlaps();
+						void					findOverlaps();
+#endif
+						void					prepareOverlapsMT();
+						void					findOverlapsMT(const BpHandle* PX_RESTRICT groups);
+						PxU32					finalize(BroadPhaseMBP* mbp);
+						void					shiftOrigin(const PxVec3& shift);
+
+						void					setTransientBounds(const PxBounds3* bounds, const PxReal* contactDistance); 
+//		private:
+						PxU32					mNbPairs;
+						PxU32					mNbRegions;
+						MBP_ObjectIndex			mFirstFreeIndex;	// First free recycled index for mMBP_Objects
+						PxU32					mFirstFreeIndexBP;	// First free recycled index for mRegions
+						Ps::Array<RegionData>	mRegions;
+						Ps::Array<MBP_Object>	mMBP_Objects;
+						MBP_PairManager			mPairManager;
+
+						BitArray				mUpdatedObjects;	// Indexed by MBP_ObjectIndex
+						BitArray				mRemoved;			// Indexed by MBP_ObjectIndex
+						Ps::Array<PxU32>   		mHandles[MAX_NB_MBP+1];
+						PxU32					mFirstFree[MAX_NB_MBP+1];
+		PX_FORCE_INLINE	RegionHandle*			getHandles(MBP_Object& currentObject, PxU32 nbHandles);
+						void					purgeHandles(MBP_Object* PX_RESTRICT object, PxU32 nbHandles);
+						void					storeHandles(MBP_Object* PX_RESTRICT object, PxU32 nbHandles, const RegionHandle* PX_RESTRICT handles);
+
+						Ps::Array<PxU32>		mOutOfBoundsObjects;	// These are BpHandle but the BP interface expects PxU32s
+						void					addToOutOfBoundsArray(BpHandle id);
+
+						const					PxBounds3* mTransientBounds;
+						const					PxReal* mTransientContactDistance;
+
+#ifdef USE_FULLY_INSIDE_FLAG
+						BitArray				mFullyInsideBitmap;	// Indexed by MBP_ObjectIndex
+#endif
+						void					populateNewRegion(const MBP_AABB& box, Region* addedRegion, PxU32 regionIndex);
+
+
+	};
+
+#ifdef MBP_SIMD_OVERLAP
+	#define MBP_OVERLAP_TEST(x)	SIMD_OVERLAP_TEST(x)
+#else
+	#define MBP_OVERLAP_TEST(x)	if(intersect2D(box0, x))
+#endif
+
+#define DEFAULT_NB_ENTRIES	128
+#define INVALID_USER_ID	0xffffffff
+
+#ifdef MBP_USE_SENTINELS
+#ifdef MBP_SIMD_OVERLAP
+static PX_FORCE_INLINE void initSentinel(SIMD_AABB& box)
+{
+//	box.mMinX = encodeFloat(FLT_MAX)>>1;
+	box.mMinX = 0xffffffff;
+}
+#if PX_DEBUG
+static PX_FORCE_INLINE bool isSentinel(const SIMD_AABB& box)
+{
+	return box.mMinX == 0xffffffff;
+}
+#endif
+#else
+static PX_FORCE_INLINE void initSentinel(MBP_AABB& box)
+{
+//	box.mMinX = encodeFloat(FLT_MAX)>>1;
+	box.mMinX = 0xffffffff;
+}
+#if PX_DEBUG
+static PX_FORCE_INLINE bool isSentinel(const MBP_AABB& box)
+{
+	return box.mMinX == 0xffffffff;
+}
+#endif
+#endif
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+
+static PX_FORCE_INLINE void sort(PxU32& id0, PxU32& id1)							{ if(id0>id1)	Ps::swap(id0, id1);		}
+static PX_FORCE_INLINE bool differentPair(const MBP_Pair& p, PxU32 id0, PxU32 id1)	{ return (id0!=p.id0) || (id1!=p.id1);	}
+
+///////////////////////////////////////////////////////////////////////////////
+
+MBP_PairManager::MBP_PairManager() :
+	mHashSize		(0),
+	mMask			(0),
+	mNbActivePairs	(0),
+	mHashTable		(NULL),
+	mNext			(NULL),
+	mActivePairs	(NULL),
+	mReservedMemory (0)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+MBP_PairManager::~MBP_PairManager()
+{
+	purge();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::purge()
+{
+	MBP_FREE(mNext);
+	MBP_FREE(mActivePairs);
+	MBP_FREE(mHashTable);
+	mHashSize		= 0;
+	mMask			= 0;
+	mNbActivePairs	= 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+	static PX_FORCE_INLINE PxU32 hash(PxU32 id0, PxU32 id1)
+	{
+		return PxU32(Ps::hash( (id0&0xffff)|(id1<<16)) );
+	}
+
+///////////////////////////////////////////////////////////////////////////////
+
+// Internal version saving hash computation
+PX_FORCE_INLINE MBP_Pair* MBP_PairManager::findPair(PxU32 id0, PxU32 id1, PxU32 hashValue) const
+{
+	if(!mHashTable)
+		return NULL;	// Nothing has been allocated yet
+
+	MBP_Pair* PX_RESTRICT activePairs = mActivePairs;
+	const PxU32* PX_RESTRICT next = mNext;
+
+	// Look for it in the table
+	PxU32 offset = mHashTable[hashValue];
+	while(offset!=INVALID_ID && differentPair(activePairs[offset], id0, id1))
+	{
+		PX_ASSERT(activePairs[offset].id0!=INVALID_USER_ID);
+		offset = next[offset];		// Better to have a separate array for this
+	}
+	if(offset==INVALID_ID)
+		return NULL;
+	PX_ASSERT(offset<mNbActivePairs);
+	// Match mActivePairs[offset] => the pair is persistent
+	return &activePairs[offset];
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+MBP_Pair* MBP_PairManager::addPair(PxU32 id0, PxU32 id1, const BpHandle* PX_RESTRICT groups, const MBP_Object* objects)
+{
+	PX_ASSERT(id0!=INVALID_ID);
+	PX_ASSERT(id1!=INVALID_ID);
+
+	if(groups)
+	{
+		const MBP_ObjectIndex index0 = decodeHandle_Index(id0);
+		const MBP_ObjectIndex index1 = decodeHandle_Index(id1);
+
+		const BpHandle object0 = objects[index0].mUserID;
+		const BpHandle object1 = objects[index1].mUserID;
+
+		if(groups[object0] == groups[object1])
+			return NULL;
+	}
+
+	// Order the ids
+	sort(id0, id1);
+
+	const PxU32 fullHashValue = hash(id0, id1);
+	PxU32 hashValue = fullHashValue & mMask;
+
+	{
+		MBP_Pair* PX_RESTRICT p = findPair(id0, id1, hashValue);
+		if(p)
+		{
+			p->isUpdated = true;
+			return p;	// Persistent pair
+		}
+	}
+
+	// This is a new pair
+	if(mNbActivePairs >= mHashSize)
+	{
+		// Get more entries
+		mHashSize = Ps::nextPowerOfTwo(mNbActivePairs+1);
+		mMask = mHashSize-1;
+
+		reallocPairs();
+
+		// Recompute hash value with new hash size
+		hashValue = fullHashValue & mMask;
+	}
+
+	MBP_Pair* PX_RESTRICT p = &mActivePairs[mNbActivePairs];
+	p->id0		= id0;	// ### CMOVs would be nice here
+	p->id1		= id1;
+	p->isNew	= true;
+	p->isUpdated= false;
+	mNext[mNbActivePairs] = mHashTable[hashValue];
+	mHashTable[hashValue] = mNbActivePairs++;
+	return p;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::removePair(PxU32 /*id0*/, PxU32 /*id1*/, PxU32 hashValue, PxU32 pairIndex)
+{
+	// Walk the hash table to fix mNext
+	{
+		PxU32 offset = mHashTable[hashValue];
+		PX_ASSERT(offset!=INVALID_ID);
+
+		PxU32 previous=INVALID_ID;
+		while(offset!=pairIndex)
+		{
+			previous = offset;
+			offset = mNext[offset];
+		}
+
+		// Let us go/jump us
+		if(previous!=INVALID_ID)
+		{
+			PX_ASSERT(mNext[previous]==pairIndex);
+			mNext[previous] = mNext[pairIndex];
+		}
+		// else we were the first
+		else mHashTable[hashValue] = mNext[pairIndex];
+		// we're now free to reuse mNext[pairIndex] without breaking the list
+	}
+#if PX_DEBUG
+	mNext[pairIndex]=INVALID_ID;
+#endif
+	// Invalidate entry
+
+	// Fill holes
+	{
+		// 1) Remove last pair
+		const PxU32 lastPairIndex = mNbActivePairs-1;
+		if(lastPairIndex==pairIndex)
+		{
+			mNbActivePairs--;
+		}
+		else
+		{
+			const MBP_Pair* last = &mActivePairs[lastPairIndex];
+			const PxU32 lastHashValue = hash(last->id0, last->id1) & mMask;
+
+			// Walk the hash table to fix mNext
+			PxU32 offset = mHashTable[lastHashValue];
+			PX_ASSERT(offset!=INVALID_ID);
+
+			PxU32 previous=INVALID_ID;
+			while(offset!=lastPairIndex)
+			{
+				previous = offset;
+				offset = mNext[offset];
+			}
+
+			// Let us go/jump us
+			if(previous!=INVALID_ID)
+			{
+				PX_ASSERT(mNext[previous]==lastPairIndex);
+				mNext[previous] = mNext[lastPairIndex];
+			}
+			// else we were the first
+			else mHashTable[lastHashValue] = mNext[lastPairIndex];
+			// we're now free to reuse mNext[lastPairIndex] without breaking the list
+
+#if PX_DEBUG
+			mNext[lastPairIndex]=INVALID_ID;
+#endif
+
+			// Don't invalidate entry since we're going to shrink the array
+
+			// 2) Re-insert in free slot
+			mActivePairs[pairIndex] = mActivePairs[lastPairIndex];
+#if PX_DEBUG
+			PX_ASSERT(mNext[pairIndex]==INVALID_ID);
+#endif
+			mNext[pairIndex] = mHashTable[lastHashValue];
+			mHashTable[lastHashValue] = pairIndex;
+
+			mNbActivePairs--;
+		}
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+bool MBP_PairManager::removePair(PxU32 id0, PxU32 id1)
+{
+	// Order the ids
+	sort(id0, id1);
+
+	const PxU32 hashValue = hash(id0, id1) & mMask;
+	const MBP_Pair* p = findPair(id0, id1, hashValue);
+	if(!p)
+		return false;
+	PX_ASSERT(p->id0==id0);
+	PX_ASSERT(p->id1==id1);
+
+	removePair(id0, id1, hashValue, getPairIndex(p));
+
+	shrinkMemory();
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::shrinkMemory()
+{
+	// Check correct memory against actually used memory
+	const PxU32 correctHashSize = Ps::nextPowerOfTwo(mNbActivePairs);
+	if(mHashSize==correctHashSize)
+		return;
+
+	if(mReservedMemory && correctHashSize < mReservedMemory)
+		return;
+
+	// Reduce memory used
+	mHashSize = correctHashSize;
+	mMask = mHashSize-1;
+
+	reallocPairs();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::reallocPairs()
+{
+	MBP_FREE(mHashTable);
+	mHashTable = reinterpret_cast<PxU32*>(MBP_ALLOC(mHashSize*sizeof(PxU32)));
+	storeDwords(mHashTable, mHashSize, INVALID_ID);
+
+	// Get some bytes for new entries
+	MBP_Pair* newPairs	= reinterpret_cast<MBP_Pair*>(MBP_ALLOC(mHashSize * sizeof(MBP_Pair)));	PX_ASSERT(newPairs);
+	PxU32* newNext		= reinterpret_cast<PxU32*>(MBP_ALLOC(mHashSize * sizeof(PxU32)));		PX_ASSERT(newNext);
+
+	// Copy old data if needed
+	if(mNbActivePairs)
+		PxMemCopy(newPairs, mActivePairs, mNbActivePairs*sizeof(MBP_Pair));
+	// ### check it's actually needed... probably only for pairs whose hash value was cut by the and
+	// yeah, since hash(id0, id1) is a constant
+	// However it might not be needed to recompute them => only less efficient but still ok
+	for(PxU32 i=0;i<mNbActivePairs;i++)
+	{
+		const PxU32 hashValue = hash(mActivePairs[i].id0, mActivePairs[i].id1) & mMask;	// New hash value with new mask
+		newNext[i] = mHashTable[hashValue];
+		mHashTable[hashValue] = i;
+	}
+
+	// Delete old data
+	MBP_FREE(mNext);
+	MBP_FREE(mActivePairs);
+
+	// Assign new pointer
+	mActivePairs = newPairs;
+	mNext = newNext;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::reserveMemory(PxU32 memSize)
+{
+	if(!memSize)
+		return;
+
+	if(!Ps::isPowerOfTwo(memSize))
+		memSize = Ps::nextPowerOfTwo(memSize);
+
+	mHashSize = memSize;
+	mMask = mHashSize-1;
+
+	mReservedMemory = memSize;
+
+	reallocPairs();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+#ifdef MBP_SIMD_OVERLAP
+	#define SIMD_OVERLAP_PRELOAD_BOX0												\
+	__m128i b = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&box0.mMinY));		\
+	b = _mm_shuffle_epi32(b, 78);
+
+	// PT: technically we don't need the 16 bits from _mm_movemask_epi8, we only
+	// need the 4 bits from _mm_movemask_ps. Would it be faster? In any case this
+	// works thanks to the _mm_cmpgt_epi32 which puts the same values in each byte
+	// of each separate 32bits components.
+	#define SIMD_OVERLAP_TEST(x)													\
+	const __m128i a = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&x.mMinY));	\
+	const __m128i d = _mm_cmpgt_epi32(a, b);										\
+	const int mask = _mm_movemask_epi8(d);											\
+	if(mask==0x0000ff00)
+
+#else
+	#define SIMD_OVERLAP_PRELOAD_BOX0
+#endif
+
+
+
+
+#ifdef MBP_USE_NO_CMP_OVERLAP
+/*static PX_FORCE_INLINE void initBox(IAABB& box, const PxBounds3& src)
+{
+	box.initFrom2(src);
+}*/
+#else
+static PX_FORCE_INLINE void initBox(IAABB& box, const PxBounds3& src)
+{
+	box.initFrom(src);
+}
+#endif
+
+Region::Region() :
+	mNbObjects				(0),
+	mMaxNbObjects			(0),
+	mFirstFree				(INVALID_ID),
+	mObjects				(NULL),
+	mMaxNbStaticBoxes		(0),
+	mNbStaticBoxes			(0),
+	mMaxNbDynamicBoxes		(0),
+	mNbDynamicBoxes			(0),
+	mStaticBoxes			(NULL),
+	mDynamicBoxes			(NULL),
+	mInToOut_Static			(NULL),
+	mInToOut_Dynamic		(NULL),
+	mPosList				(NULL),
+	mNbUpdatedBoxes			(0),
+	mPrevNbUpdatedBoxes		(0),
+	mNeedsSorting			(false),
+	mNeedsSortingSleeping	(true)
+{
+}
+
+Region::~Region()
+{
+	DELETEARRAY(mObjects);
+	MBP_FREE(mPosList);
+	MBP_FREE(mInToOut_Dynamic);
+	MBP_FREE(mInToOut_Static);
+	DELETEARRAY(mDynamicBoxes);
+	DELETEARRAY(mStaticBoxes);
+}
+
+// Pre-sort static boxes
+#define STACK_BUFFER_SIZE_STATIC_SORT	8192
+	#define DEFAULT_NUM_DYNAMIC_BOXES 1024
+
+void Region::staticSort()
+{
+	// For now this version is only compatible with:
+	// MBP_USE_WORDS
+	// MBP_USE_SENTINELS
+
+	mNeedsSorting = false;
+
+	const PxU32 nbStaticBoxes = mNbStaticBoxes;
+	if(!nbStaticBoxes)
+	{
+		mStaticBits.empty();
+		return;
+	}
+
+//	PxU32 Time;
+//	StartProfile(Time);
+
+	// Roadmap:
+	// - gather updated/modified static boxes
+	// - sort those, and those only
+	// - merge sorted set with previously existing (and previously sorted set)
+
+	// Separate things-to-sort and things-already-sorted
+	const PxU32 totalSize = sizeof(PxU32)*nbStaticBoxes*4;
+	PxU8 stackBuffer[STACK_BUFFER_SIZE_STATIC_SORT];
+	PxU8* tempMemory = totalSize<=STACK_BUFFER_SIZE_STATIC_SORT ? stackBuffer : reinterpret_cast<PxU8*>(MBP_ALLOC_TMP(totalSize));
+	PxU32* minPosList_ToSort = reinterpret_cast<PxU32*>(tempMemory);
+	PxU32* minPosList_Sorted = reinterpret_cast<PxU32*>(tempMemory + sizeof(PxU32)*nbStaticBoxes);
+	PxU32* boxIndices_ToSort = reinterpret_cast<PxU32*>(tempMemory + sizeof(PxU32)*nbStaticBoxes*2);
+	PxU32* boxIndices_Sorted = reinterpret_cast<PxU32*>(tempMemory + sizeof(PxU32)*nbStaticBoxes*3);
+	PxU32 nbToSort = 0;
+	PxU32 nbSorted = 0;
+	for(PxU32 i=0;i<nbStaticBoxes;i++)
+	{
+		if(mStaticBits.isSetChecked(i))	// ### optimize check in that thing
+		{
+			minPosList_ToSort[nbToSort] = mStaticBoxes[i].mMinX;
+			boxIndices_ToSort[nbToSort] = i;
+			nbToSort++;
+		}
+		else
+		{
+			minPosList_Sorted[nbSorted] = mStaticBoxes[i].mMinX;
+			boxIndices_Sorted[nbSorted] = i;
+			PX_ASSERT(nbSorted==0 || minPosList_Sorted[nbSorted-1]<=minPosList_Sorted[nbSorted]);
+			nbSorted++;
+		}
+	}
+	PX_ASSERT(nbSorted+nbToSort==nbStaticBoxes);
+
+//	EndProfile(Time);
+//	printf("Part1: %d\n", Time);
+
+//	StartProfile(Time);
+
+	// Sort things that need sorting
+	const PxU32* sorted;
+	RadixSortBuffered RS;
+	if(nbToSort<DEFAULT_NUM_DYNAMIC_BOXES)
+	{
+		sorted = mRS.Sort(minPosList_ToSort, nbToSort, RADIX_UNSIGNED).GetRanks();
+	}
+	else
+	{
+		sorted = RS.Sort(minPosList_ToSort, nbToSort, RADIX_UNSIGNED).GetRanks();
+	}
+
+//	EndProfile(Time);
+//	printf("Part2: %d\n", Time);
+
+//	StartProfile(Time);
+
+	// Allocate final buffers that wil contain the 2 (merged) streams
+	MBP_Index* newMapping = reinterpret_cast<MBP_Index*>(MBP_ALLOC(sizeof(MBP_Index)*mMaxNbStaticBoxes));
+	const PxU32 nbStaticSentinels = 2;
+	MBP_AABB* sortedBoxes = PX_NEW(MBP_AABB)[mMaxNbStaticBoxes+nbStaticSentinels];
+	initSentinel(sortedBoxes[nbStaticBoxes]);
+	initSentinel(sortedBoxes[nbStaticBoxes+1]);
+
+//	EndProfile(Time);
+//	printf("Part2b: %d\n", Time);
+
+//	StartProfile(Time);
+
+	// Merge streams to final buffers
+	PxU32 offsetSorted = 0;
+	PxU32 offsetNonSorted = 0;
+
+	PxU32 nextCandidateNonSorted = offsetNonSorted<nbToSort ? minPosList_ToSort[sorted[offsetNonSorted]] : 0xffffffff;
+	PxU32 nextCandidateSorted = offsetSorted<nbSorted ? minPosList_Sorted[offsetSorted] : 0xffffffff;
+
+	for(PxU32 i=0;i<nbStaticBoxes;i++)
+	{
+		PxU32 boxIndex;
+		{
+//			minPosList_Sorted[offsetSorted] = mStaticBoxes[boxIndices_Sorted[offsetSorted]].mMinX;
+
+			if(nextCandidateNonSorted<nextCandidateSorted)
+			{
+				boxIndex = boxIndices_ToSort[sorted[offsetNonSorted]];
+				offsetNonSorted++;
+
+				nextCandidateNonSorted = offsetNonSorted<nbToSort ? minPosList_ToSort[sorted[offsetNonSorted]] : 0xffffffff;
+			}
+			else
+			{
+				boxIndex = boxIndices_Sorted[offsetSorted];
+				offsetSorted++;
+
+				nextCandidateSorted = offsetSorted<nbSorted ? minPosList_Sorted[offsetSorted] : 0xffffffff;
+			}
+		}
+
+		const MBP_Index OwnerIndex = mInToOut_Static[boxIndex];
+		sortedBoxes[i] = mStaticBoxes[boxIndex];
+		newMapping[i] = OwnerIndex;
+
+		PX_ASSERT(mObjects[OwnerIndex].mIndex==boxIndex);
+		PX_ASSERT(mObjects[OwnerIndex].isStatic());
+		mObjects[OwnerIndex].mIndex = i;
+	}
+	PX_ASSERT(offsetSorted+offsetNonSorted==nbStaticBoxes);
+
+//	EndProfile(Time);
+//	printf("Part3: %d\n", Time);
+
+//	StartProfile(Time);
+
+	if(tempMemory!=stackBuffer)
+		MBP_FREE(tempMemory);
+
+	DELETEARRAY(mStaticBoxes);
+	mStaticBoxes = sortedBoxes;
+
+	MBP_FREE(mInToOut_Static);
+	mInToOut_Static = newMapping;
+
+	mStaticBits.empty();
+
+//	EndProfile(Time);
+//	printf("Part4: %d\n", Time);
+}
+
+void Region::optimizeMemory()
+{
+	// TODO: resize static boxes/mapping, dynamic boxes/mapping, object array
+}
+
+void Region::resizeObjects()
+{
+	const PxU32 newMaxNbOjects = mMaxNbObjects ? mMaxNbObjects + DEFAULT_NB_ENTRIES : DEFAULT_NB_ENTRIES;
+	MBPEntry* newObjects = PX_NEW(MBPEntry)[newMaxNbOjects];
+	if(mNbObjects)
+		PxMemCopy(newObjects, mObjects, mNbObjects*sizeof(MBPEntry));
+#if PX_DEBUG
+	for(PxU32 i=mNbObjects;i<newMaxNbOjects;i++)
+		newObjects[i].mUpdated = false;
+#endif
+	DELETEARRAY(mObjects);
+	mObjects = newObjects;
+	mMaxNbObjects = newMaxNbOjects;
+}
+
+static MBP_AABB* resizeBoxes(PxU32 oldNbBoxes, PxU32 newNbBoxes, const MBP_AABB* boxes)
+{
+	MBP_AABB* newBoxes = PX_NEW(MBP_AABB)[newNbBoxes];
+	if(oldNbBoxes)
+		PxMemCopy(newBoxes, boxes, oldNbBoxes*sizeof(MBP_AABB));
+	DELETEARRAY(boxes);
+	return newBoxes;
+}
+
+static MBP_Index* resizeMapping(PxU32 oldNbBoxes, PxU32 newNbBoxes, MBP_Index* mapping)
+{
+	MBP_Index* newMapping = reinterpret_cast<MBP_Index*>(MBP_ALLOC(sizeof(MBP_Index)*newNbBoxes));
+	if(oldNbBoxes)
+		PxMemCopy(newMapping, mapping, oldNbBoxes*sizeof(MBP_Index));
+	MBP_FREE(mapping);
+	return newMapping;
+}
+
+static PX_FORCE_INLINE void MTF(MBP_AABB* PX_RESTRICT dynamicBoxes, MBP_Index* PX_RESTRICT inToOut_Dynamic, MBPEntry* PX_RESTRICT objects, const MBP_AABB& bounds, PxU32 frontIndex, MBPEntry& updatedObject)
+{
+	const PxU32 updatedIndex = updatedObject.mIndex;
+	if(frontIndex!=updatedIndex)
+	{
+		const MBP_AABB box0 = dynamicBoxes[frontIndex];
+		dynamicBoxes[frontIndex] = bounds;
+		dynamicBoxes[updatedIndex] = box0;
+
+		const MBP_Index index0 = inToOut_Dynamic[frontIndex];
+		inToOut_Dynamic[frontIndex] = inToOut_Dynamic[updatedIndex];
+		inToOut_Dynamic[updatedIndex] = index0;
+
+		objects[index0].mIndex = updatedIndex;
+		updatedObject.mIndex = frontIndex;
+	}
+	else
+	{
+		dynamicBoxes[frontIndex] = bounds;
+	}
+}
+
+MBP_Index Region::addObject(const MBP_AABB& bounds, MBP_Handle mbpHandle, bool isStatic)
+{
+	PX_ASSERT((decodeHandle_IsStatic(mbpHandle) && isStatic) || (!decodeHandle_IsStatic(mbpHandle) && !isStatic));
+
+	MBP_Index handle;
+	if(mFirstFree!=INVALID_ID)
+	{
+		handle = MBP_Index(mFirstFree);
+		mFirstFree = mObjects[handle].mIndex;
+	}
+	else
+	{
+		if(mMaxNbObjects==mNbObjects)
+			resizeObjects();
+
+		handle = MBP_Index(mNbObjects);
+	}
+	mNbObjects++;
+#ifdef MBP_USE_WORDS
+	PX_ASSERT(mNbObjects<0xffff);
+#endif
+	///
+
+	PxU32 boxIndex;
+	if(isStatic)
+	{
+		if(mMaxNbStaticBoxes==mNbStaticBoxes)
+		{
+			const PxU32 newMaxNbBoxes = mMaxNbStaticBoxes ? mMaxNbStaticBoxes + DEFAULT_NB_ENTRIES : DEFAULT_NB_ENTRIES;
+			mStaticBoxes = resizeBoxes(mNbStaticBoxes, newMaxNbBoxes, mStaticBoxes);
+			mInToOut_Static = resizeMapping(mNbStaticBoxes, newMaxNbBoxes, mInToOut_Static);
+			mMaxNbStaticBoxes = newMaxNbBoxes;
+		}
+
+		boxIndex = mNbStaticBoxes++;
+		mStaticBoxes[boxIndex] = bounds;
+		mInToOut_Static[boxIndex] = handle;
+		mNeedsSorting = true;
+		mStaticBits.setBitChecked(boxIndex);
+	}
+	else
+	{
+		if(mMaxNbDynamicBoxes==mNbDynamicBoxes)
+		{
+			const PxU32 newMaxNbBoxes = mMaxNbDynamicBoxes ? mMaxNbDynamicBoxes + DEFAULT_NB_ENTRIES : DEFAULT_NB_ENTRIES;
+			mDynamicBoxes = resizeBoxes(mNbDynamicBoxes, newMaxNbBoxes, mDynamicBoxes);
+			mInToOut_Dynamic = resizeMapping(mNbDynamicBoxes, newMaxNbBoxes, mInToOut_Dynamic);
+			mMaxNbDynamicBoxes = newMaxNbBoxes;
+
+			MBP_FREE(mPosList);
+			mPosList = reinterpret_cast<PxU32*>(MBP_ALLOC((newMaxNbBoxes+1)*sizeof(PxU32)));
+		}
+
+		boxIndex = mNbDynamicBoxes++;
+		mDynamicBoxes[boxIndex] = bounds;
+		mInToOut_Dynamic[boxIndex] = handle;
+	}
+
+	mObjects[handle].mIndex = boxIndex;
+	mObjects[handle].mMBPHandle = mbpHandle;
+#if PX_DEBUG
+	mObjects[handle].mUpdated = !isStatic;
+#endif
+
+	if(!isStatic)
+	{
+		MTF(mDynamicBoxes, mInToOut_Dynamic, mObjects, bounds, mNbUpdatedBoxes, mObjects[handle]);
+		mNbUpdatedBoxes++;
+		mPrevNbUpdatedBoxes = 0;
+		mNeedsSortingSleeping = true;
+		PX_ASSERT(mNbUpdatedBoxes<=mNbDynamicBoxes);
+	}
+	return handle;
+}
+
+// Moves box 'lastIndex' to location 'removedBoxIndex'
+static PX_FORCE_INLINE void remove(MBPEntry* PX_RESTRICT objects, MBP_Index* PX_RESTRICT mapping, MBP_AABB* PX_RESTRICT boxes, PxU32 removedBoxIndex, PxU32 lastIndex)
+{
+	const PxU32 movedBoxHandle = mapping[lastIndex];
+	boxes[removedBoxIndex] = boxes[lastIndex];				// Relocate box data
+	mapping[removedBoxIndex] = MBP_Index(movedBoxHandle);	// Relocate mapping data
+	MBPEntry& movedObject = objects[movedBoxHandle];
+	PX_ASSERT(movedObject.mIndex==lastIndex);				// Checks index of moved box was indeed its old location
+	movedObject.mIndex = removedBoxIndex;					// Adjust index of moved box to reflect its new location
+}
+
+void Region::removeObject(MBP_Index handle)
+{
+	PX_ASSERT(handle<mMaxNbObjects);
+
+	MBPEntry& object = mObjects[handle];
+	/*const*/ PxU32 removedBoxIndex = object.mIndex;
+
+	MBP_Index* PX_RESTRICT mapping;
+	MBP_AABB* PX_RESTRICT boxes;
+	PxU32 lastIndex;
+	PxU32 maxNbBoxes;
+	if(!object.isStatic())
+	{
+		mPrevNbUpdatedBoxes = 0;
+		mNeedsSortingSleeping = true;
+
+		PX_ASSERT(mInToOut_Dynamic[removedBoxIndex]==handle);
+		const bool isUpdated = removedBoxIndex<mNbUpdatedBoxes;
+		PX_ASSERT(isUpdated==object.mUpdated);
+		if(isUpdated)
+		{
+			PX_ASSERT(mNbUpdatedBoxes);
+			if(mNbUpdatedBoxes!=mNbDynamicBoxes)
+			{
+				// Removing the object will create this pattern, which is wrong:
+				// UUUUUUUUUUUNNNNNNNNN......... original
+				// UUUUUU.UUUUNNNNNNNNN......... remove U
+				// UUUUUUNUUUUNNNNNNNN.......... move N
+				//
+				// What we want instead is:
+				// UUUUUUUUUUUNNNNNNNNN......... original
+				// UUUUUU.UUUUNNNNNNNNN......... remove U
+				// UUUUUUUUUU.NNNNNNNNN......... move U
+				// UUUUUUUUUUNNNNNNNNN.......... move N
+				const PxU32 lastUpdatedIndex = mNbUpdatedBoxes-1;
+
+				remove(mObjects, mInToOut_Dynamic, mDynamicBoxes, removedBoxIndex, lastUpdatedIndex);	// Move last U to removed U
+				//Remove(mObjects, mInToOut_Dynamic, mDynamicBoxes, lastUpdatedIndex, --mNbDynamicBoxes);	// Move last N to last U
+				removedBoxIndex = lastUpdatedIndex;
+			}
+			mNbUpdatedBoxes--;
+		}
+
+//		remove(mObjects, mInToOut_Dynamic, mDynamicBoxes, removedBoxIndex, --mNbDynamicBoxes);
+		mapping = mInToOut_Dynamic;
+		boxes = mDynamicBoxes;
+		lastIndex = --mNbDynamicBoxes;
+		maxNbBoxes = mMaxNbDynamicBoxes;
+
+		// ### adjust size of mPosList ?
+	}
+	else
+	{
+		PX_ASSERT(mInToOut_Static[removedBoxIndex]==handle);
+
+		mNeedsSorting = true;
+		mStaticBits.setBitChecked(removedBoxIndex);
+
+//		remove(mObjects, mInToOut_Static, mStaticBoxes, removedBoxIndex, --mNbStaticBoxes);
+		mapping = mInToOut_Static;
+		boxes = mStaticBoxes;
+		lastIndex = --mNbStaticBoxes;
+		maxNbBoxes = mMaxNbStaticBoxes;
+	}
+	remove(mObjects, mapping, boxes, removedBoxIndex, lastIndex);
+
+	PX_UNUSED(maxNbBoxes);
+/*	if(lastIndex+DEFAULT_NB_ENTRIES<maxNbBoxes/2)
+	{
+		int stop=1;
+	}*/
+
+	object.mIndex		= mFirstFree;
+	object.mMBPHandle	= INVALID_ID;
+//	printf("Invalid: %d\n", handle);
+	mFirstFree			= handle;
+	mNbObjects--;
+
+#if PX_DEBUG
+	object.mUpdated = false;
+#endif
+}
+
+void Region::updateObject(const MBP_AABB& bounds, MBP_Index handle)
+{
+	PX_ASSERT(handle<mMaxNbObjects);
+
+	MBPEntry& object = mObjects[handle];
+	if(!object.isStatic())
+	{
+		// MTF on updated box
+		const bool isContinuouslyUpdated = object.mIndex<mPrevNbUpdatedBoxes;
+		if(!isContinuouslyUpdated)
+			mNeedsSortingSleeping = true;
+//		printf("%d: %d\n", handle, isContinuouslyUpdated);
+
+		const bool isUpdated = object.mIndex<mNbUpdatedBoxes;
+		PX_ASSERT(isUpdated==object.mUpdated);
+		if(!isUpdated)
+		{
+#if PX_DEBUG
+			object.mUpdated = true;
+#endif
+			MTF(mDynamicBoxes, mInToOut_Dynamic, mObjects, bounds, mNbUpdatedBoxes, object);
+			mNbUpdatedBoxes++;
+			PX_ASSERT(mNbUpdatedBoxes<=mNbDynamicBoxes);
+		}
+		else
+		{
+			mDynamicBoxes[object.mIndex] = bounds;
+		}
+	}
+	else
+	{
+		mStaticBoxes[object.mIndex] = bounds;
+		mNeedsSorting = true;	// ### not always!
+		mStaticBits.setBitChecked(object.mIndex);
+	}
+}
+
+MBP_Handle Region::retrieveBounds(MBP_AABB& bounds, MBP_Index handle) const
+{
+	PX_ASSERT(handle<mMaxNbObjects);
+
+	const MBPEntry& object = mObjects[handle];
+	if(!object.isStatic())
+		bounds = mDynamicBoxes[object.mIndex];
+	else
+		bounds = mStaticBoxes[object.mIndex];
+
+	return object.mMBPHandle;
+}
+
+void Region::setBounds(MBP_Index handle, const MBP_AABB& bounds)
+{
+	PX_ASSERT(handle<mMaxNbObjects);
+
+	const MBPEntry& object = mObjects[handle];
+	if(!object.isStatic())
+	{
+		PX_ASSERT(object.mIndex < mNbDynamicBoxes);
+		mDynamicBoxes[object.mIndex] = bounds;
+	}
+	else
+	{
+		PX_ASSERT(object.mIndex < mNbStaticBoxes);
+		mStaticBoxes[object.mIndex] = bounds;
+	}
+}
+
+#ifndef MBP_SIMD_OVERLAP
+static PX_FORCE_INLINE Ps::IntBool intersect2D(const MBP_AABB& a, const MBP_AABB& b)
+{
+#ifdef MBP_USE_NO_CMP_OVERLAP
+		// PT: warning, only valid with the special encoding in InitFrom2
+		const PxU32 bits0 = (b.mMaxY - a.mMinY)&0x80000000;
+		const PxU32 bits1 = (b.mMaxZ - a.mMinZ)&0x80000000;
+		const PxU32 bits2 = (a.mMaxY - b.mMinY)&0x80000000;
+		const PxU32 bits3 = (a.mMaxZ - b.mMinZ)&0x80000000;
+		const PxU32 mask = bits0|(bits1>>1)|(bits2>>2)|(bits3>>3);
+		return !mask;
+
+	/*	const PxU32 d0 = (b.mMaxY<<16)|a.mMaxY;
+		const PxU32 d0b = (b.mMaxZ<<16)|a.mMaxZ;
+		const PxU32 d1 = (a.mMinY<<16)|b.mMinY;
+		const PxU32 d1b = (a.mMinZ<<16)|b.mMinZ;
+		const PxU32 mask = (d0 - d1) | (d0b - d1b);
+		return !(mask & 0x80008000);*/
+#else
+	if(//mMaxX < a.mMinX || a.mMaxX < mMinX
+//		||
+		b.mMaxY < a.mMinY || a.mMaxY < b.mMinY
+	||
+		b.mMaxZ < a.mMinZ || a.mMaxZ < b.mMinZ
+	)
+		return FALSE;
+	return TRUE;
+#endif
+}
+#endif
+
+#ifdef MBP_USE_NO_CMP_OVERLAP_3D
+static PX_FORCE_INLINE BOOL intersect3D(const IAABB& a, const IAABB& b)
+{
+	// PT: warning, only valid with the special encoding in InitFrom2
+	const PxU32 bits0 = (b.mMaxY - a.mMinY)&0x80000000;
+	const PxU32 bits1 = (b.mMaxZ - a.mMinZ)&0x80000000;
+	const PxU32 bits2 = (a.mMaxY - b.mMinY)&0x80000000;
+	const PxU32 bits3 = (a.mMaxZ - b.mMinZ)&0x80000000;
+	const PxU32 bits4 = (b.mMaxX - a.mMinX)&0x80000000;
+	const PxU32 bits5 = (a.mMaxX - b.mMinX)&0x80000000;
+	const PxU32 mask = bits0|(bits1>>1)|(bits2>>2)|(bits3>>3)|(bits4>>4)|(bits5>>5);
+	return !mask;
+}
+#endif
+
+#ifdef CHECK_NB_OVERLAPS
+static PxU32 gNbOverlaps = 0;
+#endif
+
+static PX_FORCE_INLINE void outputPair_DynamicDynamic(
+												MBP_PairManager& pairManager,
+												PxU32 index0, PxU32 index1,
+												const MBP_Index* PX_RESTRICT inToOut_Dynamic,
+												const MBPEntry* PX_RESTRICT objects,
+												const BpHandle* PX_RESTRICT groups, const MBP_Object* mbpObjects
+												)
+{
+#ifdef CHECK_NB_OVERLAPS
+	gNbOverlaps++;
+#endif
+	PX_ASSERT(index0!=index1);
+	const PxU32 objectIndex0 = inToOut_Dynamic[index0];
+	const PxU32 objectIndex1 = inToOut_Dynamic[index1];
+	const MBP_Handle id0 = objects[objectIndex0].mMBPHandle;
+	const MBP_Handle id1 = objects[objectIndex1].mMBPHandle;
+	pairManager.addPair(id0, id1, groups, mbpObjects);
+}
+
+static PX_FORCE_INLINE void outputPair_DynamicStatic(
+												MBP_PairManager& pairManager,
+												PxU32 index0, PxU32 index1,
+												const MBP_Index* PX_RESTRICT inToOut_Dynamic, const MBP_Index* PX_RESTRICT inToOut_Static,
+												const MBPEntry* PX_RESTRICT objects,
+												const BpHandle* PX_RESTRICT groups, const MBP_Object* mbpObjects
+												)
+{
+#ifdef CHECK_NB_OVERLAPS
+	gNbOverlaps++;
+#endif
+	const PxU32 objectIndex0 = inToOut_Dynamic[index0];
+	const PxU32 objectIndex1 = inToOut_Static[index1];
+	const MBP_Handle id0 = objects[objectIndex0].mMBPHandle;
+	const MBP_Handle id1 = objects[objectIndex1].mMBPHandle;
+//	printf("2: %d %d\n", index0, index1);
+//	printf("3: %d %d\n", objectIndex0, objectIndex1);
+	pairManager.addPair(id0, id1, groups, mbpObjects);
+}
+
+
+
+MBPOS_TmpBuffers::MBPOS_TmpBuffers() :
+	mNbSleeping					(0),
+	mNbUpdated					(0),
+	mInToOut_Dynamic_Sleeping	(NULL),
+	mSleepingDynamicBoxes		(NULL),
+	mUpdatedDynamicBoxes		(NULL)
+{
+}
+
+MBPOS_TmpBuffers::~MBPOS_TmpBuffers()
+{
+//	printf("mNbSleeping: %d\n", mNbSleeping);
+	if(mInToOut_Dynamic_Sleeping!=mInToOut_Dynamic_Sleeping_Stack)
+		MBP_FREE(mInToOut_Dynamic_Sleeping);
+
+	if(mSleepingDynamicBoxes!=mSleepingDynamicBoxes_Stack)
+	{
+		DELETEARRAY(mSleepingDynamicBoxes);
+	}
+
+	if(mUpdatedDynamicBoxes!=mUpdatedDynamicBoxes_Stack)
+	{
+		DELETEARRAY(mUpdatedDynamicBoxes);
+	}
+	mNbSleeping = 0;
+	mNbUpdated = 0;
+}
+
+void MBPOS_TmpBuffers::allocateSleeping(PxU32 nbSleeping, PxU32 nbSentinels)
+{
+	if(nbSleeping>mNbSleeping)
+	{
+		if(mInToOut_Dynamic_Sleeping!=mInToOut_Dynamic_Sleeping_Stack)
+			MBP_FREE(mInToOut_Dynamic_Sleeping);
+		if(mSleepingDynamicBoxes!=mSleepingDynamicBoxes_Stack)
+		{
+			DELETEARRAY(mSleepingDynamicBoxes);
+		}
+
+		if(nbSleeping+nbSentinels<=STACK_BUFFER_SIZE)
+		{
+			mSleepingDynamicBoxes = mSleepingDynamicBoxes_Stack;
+			mInToOut_Dynamic_Sleeping = mInToOut_Dynamic_Sleeping_Stack;
+		}
+		else
+		{
+			mSleepingDynamicBoxes = PX_NEW_TEMP(MBP_AABB)[nbSleeping+nbSentinels];
+			mInToOut_Dynamic_Sleeping = reinterpret_cast<MBP_Index*>(MBP_ALLOC(sizeof(MBP_Index)*nbSleeping));
+		}
+		mNbSleeping = nbSleeping;
+	}
+}
+
+void MBPOS_TmpBuffers::allocateUpdated(PxU32 nbUpdated, PxU32 nbSentinels)
+{
+	if(nbUpdated>mNbUpdated)
+	{
+		if(mUpdatedDynamicBoxes!=mUpdatedDynamicBoxes_Stack)
+		{
+			DELETEARRAY(mUpdatedDynamicBoxes);
+		}
+
+		if(nbUpdated+nbSentinels<=STACK_BUFFER_SIZE)
+			mUpdatedDynamicBoxes = mUpdatedDynamicBoxes_Stack;
+		else
+		{
+			mUpdatedDynamicBoxes = PX_NEW_TEMP(MBP_AABB)[nbUpdated+nbSentinels];
+		}
+		mNbUpdated = nbUpdated;
+	}
+}
+
+//#define TEST2
+
+void Region::preparePruning(MBPOS_TmpBuffers& buffers)
+{
+PxU32 _saved = mNbUpdatedBoxes;
+mNbUpdatedBoxes = 0;
+
+	if(mPrevNbUpdatedBoxes!=_saved)
+		mNeedsSortingSleeping = true;
+
+	PxU32 nb = mNbDynamicBoxes;
+	if(!nb)
+	{
+		mInput.mNeeded = false;
+		mPrevNbUpdatedBoxes = 0;
+		mNeedsSortingSleeping = true;
+		return;
+	}
+	const MBP_AABB* PX_RESTRICT dynamicBoxes = mDynamicBoxes;
+	PxU32* PX_RESTRICT posList = mPosList;
+
+#if PX_DEBUG
+	PxU32 verifyNbUpdated = 0;
+	for(PxU32 i=0;i<mMaxNbObjects;i++)
+	{
+		if(mObjects[i].mUpdated)
+			verifyNbUpdated++;
+	}
+	PX_ASSERT(verifyNbUpdated==_saved);
+#endif
+
+	// Build main list using the primary axis
+
+	PxU32 nbUpdated = 0;
+	PxU32 nbNonUpdated = 0;
+	{
+		nbUpdated = _saved;
+		nbNonUpdated = nb - _saved;
+		for(PxU32 i=0;i<nbUpdated;i++)
+		{
+#if PX_DEBUG
+			const PxU32 objectIndex = mInToOut_Dynamic[i];
+			PX_ASSERT(mObjects[objectIndex].mUpdated);
+			mObjects[objectIndex].mUpdated = false;
+#endif
+			posList[i] = dynamicBoxes[i].mMinX;
+		}
+		if(mNeedsSortingSleeping)
+		{
+			for(PxU32 i=0;i<nbNonUpdated;i++)
+			{
+#if PX_DEBUG
+				const PxU32 objectIndex = mInToOut_Dynamic[i];
+				PX_ASSERT(!mObjects[objectIndex].mUpdated);
+#endif
+				PxU32 j = i + nbUpdated;
+				posList[j] = dynamicBoxes[j].mMinX;
+			}
+		}
+#if PX_DEBUG
+		else
+		{
+			for(PxU32 i=0;i<nbNonUpdated;i++)
+			{
+				const PxU32 objectIndex = mInToOut_Dynamic[i];
+				PX_ASSERT(!mObjects[objectIndex].mUpdated);
+				PxU32 j = i + nbUpdated;
+				PX_ASSERT(posList[j] == dynamicBoxes[j].mMinX);
+			}
+		}
+#endif
+	}
+	PX_ASSERT(nbUpdated==verifyNbUpdated);
+	PX_ASSERT(nbUpdated+nbNonUpdated==nb);
+	mNbUpdatedBoxes = nbUpdated;
+	if(!nbUpdated)
+	{
+		mInput.mNeeded = false;
+		mPrevNbUpdatedBoxes = 0;
+		mNeedsSortingSleeping = true;
+		return;
+	}
+
+	mPrevNbUpdatedBoxes = mNbUpdatedBoxes;
+
+	///////
+
+	// ### TODO: no need to recreate those buffers each frame!
+	MBP_Index* PX_RESTRICT inToOut_Dynamic_Sleeping = NULL;
+	MBP_AABB* PX_RESTRICT sleepingDynamicBoxes = NULL;
+	if(nbNonUpdated)
+	{
+		if(mNeedsSortingSleeping)
+		{
+			const PxU32* PX_RESTRICT sorted = mRS.Sort(posList+nbUpdated, nbNonUpdated, RADIX_UNSIGNED).GetRanks();
+
+#ifdef MBP_USE_SENTINELS
+			const PxU32 nbSentinels = 2;
+#else
+			const PxU32 nbSentinels = 0;
+#endif
+			buffers.allocateSleeping(nbNonUpdated, nbSentinels);
+			sleepingDynamicBoxes = buffers.mSleepingDynamicBoxes;
+			inToOut_Dynamic_Sleeping = buffers.mInToOut_Dynamic_Sleeping;
+			for(PxU32 i=0;i<nbNonUpdated;i++)
+			{
+				const PxU32 sortedIndex = nbUpdated+sorted[i];
+				sleepingDynamicBoxes[i] = dynamicBoxes[sortedIndex];
+				inToOut_Dynamic_Sleeping[i] = mInToOut_Dynamic[sortedIndex];
+			}
+#ifdef MBP_USE_SENTINELS
+			initSentinel(sleepingDynamicBoxes[nbNonUpdated]);
+			initSentinel(sleepingDynamicBoxes[nbNonUpdated+1]);
+#endif
+			mNeedsSortingSleeping = false;
+		}
+		else
+		{
+			sleepingDynamicBoxes = buffers.mSleepingDynamicBoxes;
+			inToOut_Dynamic_Sleeping = buffers.mInToOut_Dynamic_Sleeping;
+#if PX_DEBUG
+			for(PxU32 i=0;i<nbNonUpdated-1;i++)
+				PX_ASSERT(sleepingDynamicBoxes[i].mMinX<=sleepingDynamicBoxes[i+1].mMinX);
+#endif
+		}
+	}
+	else
+	{
+		mNeedsSortingSleeping = true;
+	}
+
+	///////
+
+//	posList[nbUpdated] = MAX_PxU32;
+//	nb = nbUpdated;
+
+	// Sort the list
+//	const PxU32* PX_RESTRICT sorted = mRS.Sort(posList, nbUpdated+1, RADIX_UNSIGNED).GetRanks();
+	const PxU32* PX_RESTRICT sorted = mRS.Sort(posList, nbUpdated, RADIX_UNSIGNED).GetRanks();
+
+#ifdef MBP_USE_SENTINELS
+	const PxU32 nbSentinels = 2;
+#else
+	const PxU32 nbSentinels = 0;
+#endif
+	buffers.allocateUpdated(nbUpdated, nbSentinels);
+	MBP_AABB* PX_RESTRICT updatedDynamicBoxes = buffers.mUpdatedDynamicBoxes;
+	MBP_Index* PX_RESTRICT inToOut_Dynamic = reinterpret_cast<MBP_Index*>(mRS.GetRecyclable());
+	for(PxU32 i=0;i<nbUpdated;i++)
+	{
+		const PxU32 sortedIndex = sorted[i];
+		updatedDynamicBoxes[i] = dynamicBoxes[sortedIndex];
+		inToOut_Dynamic[i] = mInToOut_Dynamic[sortedIndex];
+	}
+#ifdef MBP_USE_SENTINELS
+	initSentinel(updatedDynamicBoxes[nbUpdated]);
+	initSentinel(updatedDynamicBoxes[nbUpdated+1]);
+#endif
+	dynamicBoxes = updatedDynamicBoxes;
+
+	mInput.mObjects						= mObjects;					// Can be shared (1)
+	mInput.mUpdatedDynamicBoxes			= updatedDynamicBoxes;		// Can be shared (2) => buffers.mUpdatedDynamicBoxes;
+	mInput.mSleepingDynamicBoxes		= sleepingDynamicBoxes;
+	mInput.mInToOut_Dynamic				= inToOut_Dynamic;			// Can be shared (3) => (MBP_Index*)mRS.GetRecyclable();
+	mInput.mInToOut_Dynamic_Sleeping	= inToOut_Dynamic_Sleeping;
+	mInput.mNbUpdated					= nbUpdated;				// Can be shared (4)
+	mInput.mNbNonUpdated				= nbNonUpdated;
+	mInput.mNeeded						= true;
+}
+
+void Region::prepareBIPPruning(const MBPOS_TmpBuffers& buffers)
+{
+	if(!mNbUpdatedBoxes || !mNbStaticBoxes)
+	{
+		mInput.mBIPInput.mNeeded = false;
+		return;
+	}
+
+	mInput.mBIPInput.mObjects			= mObjects;						// Can be shared (1)
+	mInput.mBIPInput.mNbUpdatedBoxes	= mNbUpdatedBoxes;				// Can be shared (4)
+	mInput.mBIPInput.mNbStaticBoxes		= mNbStaticBoxes;
+//	mInput.mBIPInput.mDynamicBoxes		= mDynamicBoxes;
+	mInput.mBIPInput.mDynamicBoxes		= buffers.mUpdatedDynamicBoxes;	// Can be shared (2)
+	mInput.mBIPInput.mStaticBoxes		= mStaticBoxes;
+	mInput.mBIPInput.mInToOut_Static	= mInToOut_Static;
+	mInput.mBIPInput.mInToOut_Dynamic	= reinterpret_cast<const MBP_Index*>(mRS.GetRecyclable());	// Can be shared (3)
+	mInput.mBIPInput.mNeeded			= true;
+}
+
+static void doCompleteBoxPruning(MBP_PairManager* PX_RESTRICT pairManager, const BoxPruning_Input& input, const BpHandle* PX_RESTRICT groups, const MBP_Object* mbpObjects)
+{
+	const MBPEntry* PX_RESTRICT objects						= input.mObjects;
+	const MBP_AABB* PX_RESTRICT updatedDynamicBoxes			= input.mUpdatedDynamicBoxes;
+	const MBP_AABB* PX_RESTRICT sleepingDynamicBoxes		= input.mSleepingDynamicBoxes;
+	const MBP_Index* PX_RESTRICT inToOut_Dynamic			= input.mInToOut_Dynamic;
+	const MBP_Index* PX_RESTRICT inToOut_Dynamic_Sleeping	= input.mInToOut_Dynamic_Sleeping;
+	const PxU32 nbUpdated 									= input.mNbUpdated;
+	const PxU32 nbNonUpdated								= input.mNbNonUpdated;
+
+	//
+
+	if(nbNonUpdated)
+	{
+		const PxU32 nb0 = nbUpdated;
+		const PxU32 nb1 = nbNonUpdated;
+
+		//
+		const PxU32 lastSortedIndex0 = nb0;
+		const PxU32 lastSortedIndex1 = nb1;
+		PxU32 sortedIndex0 = 0;
+		PxU32 runningIndex1 = 0;
+
+		while(runningIndex1<lastSortedIndex1 && sortedIndex0<lastSortedIndex0)
+		{
+			const PxU32 index0 = sortedIndex0++;
+			const MBP_AABB& box0 = updatedDynamicBoxes[index0];
+			const PxU32 limit = box0.mMaxX;
+	//const PxU32 id0 = mObjects[inToOut_Dynamic[index0]].mMBPHandle;
+			SIMD_OVERLAP_PRELOAD_BOX0
+
+			const PxU32 l = box0.mMinX;
+			while(
+#ifndef MBP_USE_SENTINELS
+				runningIndex1<lastSortedIndex1 &&
+#endif
+				sleepingDynamicBoxes[runningIndex1].mMinX<l)
+				runningIndex1++;
+
+			PxU32 index1 = runningIndex1;
+
+			while(
+#ifndef MBP_USE_SENTINELS
+				index1<lastSortedIndex1 &&
+#endif
+				sleepingDynamicBoxes[index1].mMinX<=limit)
+			{
+				MBP_OVERLAP_TEST(sleepingDynamicBoxes[index1])
+				{
+					outputPair_DynamicStatic(
+						*pairManager,
+						index0, index1, inToOut_Dynamic, inToOut_Dynamic_Sleeping, objects, groups, mbpObjects
+						);
+				}
+
+#ifdef TEST2
+				if(
+	#ifndef MBP_USE_SENTINELS
+					index1+1<lastSortedIndex1 &&
+	#endif
+					sleepingDynamicBoxes[index1+1].mMinX<=limit)
+				{
+					MBP_OVERLAP_TEST(sleepingDynamicBoxes[index1+1])
+					{
+						outputPair_DynamicStatic(
+							*pairManager,
+							index0, index1+1, inToOut_Dynamic, inToOut_Dynamic_Sleeping, objects, groups
+							);
+					}
+				}
+				index1+=2;
+#else
+				index1++;
+#endif
+			}
+		}
+
+		////
+
+		PxU32 sortedIndex1 = 0;
+		PxU32 runningIndex0 = 0;
+		while(runningIndex0<lastSortedIndex0 && sortedIndex1<lastSortedIndex1)
+		{
+			const PxU32 index0 = sortedIndex1++;
+			const MBP_AABB& box0 = sleepingDynamicBoxes[index0];
+			const PxU32 limit = box0.mMaxX;
+	//const PxU32 id1 = mObjects[mInToOut_Static[index0]].mMBPHandle;
+			SIMD_OVERLAP_PRELOAD_BOX0
+
+//			const PxU32 l = sleepingDynamicBoxes[index0].mMinX;
+			const PxU32 l = box0.mMinX;	// ### PhysX change
+			while(updatedDynamicBoxes[runningIndex0].mMinX<=l)
+				runningIndex0++;
+
+			PxU32 index1 = runningIndex0;
+
+			while(updatedDynamicBoxes[index1].mMinX<=limit)
+			{
+				MBP_OVERLAP_TEST(updatedDynamicBoxes[index1])
+				{
+					outputPair_DynamicStatic(
+						*pairManager,
+						index1, index0, inToOut_Dynamic, inToOut_Dynamic_Sleeping, objects, groups, mbpObjects
+						);
+				}
+
+#ifdef TEST2
+				if(updatedDynamicBoxes[index1+1].mMinX<=limit)
+				{
+					MBP_OVERLAP_TEST(updatedDynamicBoxes[index1+1])
+					{
+						outputPair_DynamicStatic(
+							*pairManager,
+							index1+1, index0, inToOut_Dynamic, inToOut_Dynamic_Sleeping, objects, groups
+							);
+					}
+				}
+				index1+=2;
+#else
+				index1++;
+#endif
+			}
+		}
+	}
+
+	///////
+
+	// Prune the list
+	const PxU32 lastSortedIndex = nbUpdated;
+	PxU32 sortedIndex = 0;
+	PxU32 runningIndex = 0;
+	while(runningIndex<lastSortedIndex && sortedIndex<lastSortedIndex)
+	{
+		const PxU32 index0 = sortedIndex++;
+		const MBP_AABB& box0 = updatedDynamicBoxes[index0];
+		const PxU32 limit = box0.mMaxX;
+
+		SIMD_OVERLAP_PRELOAD_BOX0
+
+//		const PxU32 l = updatedDynamicBoxes[index0].mMinX;
+		const PxU32 l = box0.mMinX;	// ### PhysX change
+		while(updatedDynamicBoxes[runningIndex++].mMinX<l);
+
+		if(runningIndex<lastSortedIndex)
+		{
+			PxU32 index1 = runningIndex;
+			while(updatedDynamicBoxes[index1].mMinX<=limit)
+			{
+				MBP_OVERLAP_TEST(updatedDynamicBoxes[index1])
+				{
+					outputPair_DynamicDynamic(
+						*pairManager,
+						index0, index1, inToOut_Dynamic, objects, groups, mbpObjects
+						);
+				}
+#ifdef TEST2
+				if(updatedDynamicBoxes[index1+1].mMinX<=limit)
+				{
+					MBP_OVERLAP_TEST(updatedDynamicBoxes[index1+1])
+					{
+						outputPair_DynamicDynamic(
+							*pairManager,
+							index0, index1+1, inToOut_Dynamic, objects, groups
+							);
+					}
+				}
+				index1+=2;
+#else
+				index1++;
+#endif
+			}
+		}
+	}
+}
+
+
+#define TWO_AT_A_TIME
+
+static void doBipartiteBoxPruning(MBP_PairManager* PX_RESTRICT pairManager, const BIP_Input& input, const BpHandle* PX_RESTRICT groups, const MBP_Object* mbpObjects)
+{
+	// ### crashes because the code expects the dynamic array to be sorted, but mDynamicBoxes is not
+	// ### we should instead modify mNbUpdatedBoxes so that mNbUpdatedBoxes == mNbDynamicBoxes, and
+	// ### then the proper sorting happens in CompleteBoxPruning (right?)
+
+	const PxU32 nb0 = input.mNbUpdatedBoxes;
+	const PxU32 nb1 = input.mNbStaticBoxes;
+
+	const MBPEntry* PX_RESTRICT mObjects			= input.mObjects;
+	const MBP_AABB* PX_RESTRICT dynamicBoxes		= input.mDynamicBoxes;
+	const MBP_AABB* PX_RESTRICT staticBoxes			= input.mStaticBoxes;
+	const MBP_Index* PX_RESTRICT inToOut_Static		= input.mInToOut_Static;
+	const MBP_Index* PX_RESTRICT inToOut_Dynamic	= input.mInToOut_Dynamic;
+
+#ifdef MBP_USE_SENTINELS
+	PX_ASSERT(isSentinel(staticBoxes[nb1]));
+	PX_ASSERT(isSentinel(staticBoxes[nb1+1]));
+//	const MBP_AABB Saved = staticBoxes[nb1];
+//	const MBP_AABB Saved1 = staticBoxes[nb1+1];
+//	initSentinel(((MBP_AABB* PX_RESTRICT)staticBoxes)[nb1]);
+//	initSentinel(((MBP_AABB* PX_RESTRICT)staticBoxes)[nb1+1]);
+#endif
+
+	//
+	const PxU32 lastSortedIndex0 = nb0;
+	const PxU32 lastSortedIndex1 = nb1;
+	PxU32 sortedIndex0 = 0;
+	PxU32 runningIndex1 = 0;
+
+	while(runningIndex1<lastSortedIndex1 && sortedIndex0<lastSortedIndex0)
+	{
+		const PxU32 index0 = sortedIndex0++;
+		const MBP_AABB& box0 = dynamicBoxes[index0];
+		const PxU32 limit = box0.mMaxX;
+//const PxU32 id0 = mObjects[inToOut_Dynamic[index0]].mMBPHandle;
+		SIMD_OVERLAP_PRELOAD_BOX0
+
+		const PxU32 l = box0.mMinX;
+		while(
+#ifndef MBP_USE_SENTINELS
+			runningIndex1<lastSortedIndex1 &&
+#endif
+			staticBoxes[runningIndex1].mMinX<l)
+			runningIndex1++;
+
+		PxU32 index1 = runningIndex1;
+
+		while(
+#ifndef MBP_USE_SENTINELS
+			index1<lastSortedIndex1 &&
+#endif
+			staticBoxes[index1].mMinX<=limit)
+		{
+			{
+			MBP_OVERLAP_TEST(staticBoxes[index1])
+			{
+				outputPair_DynamicStatic(
+					*pairManager,
+					index0, index1, inToOut_Dynamic, inToOut_Static, mObjects, groups, mbpObjects
+					);
+			}
+			}
+#ifdef TWO_AT_A_TIME
+			if(
+	#ifndef MBP_USE_SENTINELS
+				index1+1<lastSortedIndex1 &&
+	#endif
+				staticBoxes[index1+1].mMinX<=limit)
+			{
+				MBP_OVERLAP_TEST(staticBoxes[index1+1])
+				{
+					outputPair_DynamicStatic(
+						*pairManager,
+						index0, index1+1, inToOut_Dynamic, inToOut_Static, mObjects, groups, mbpObjects
+						);
+				}
+			}
+			index1+=2;
+#else
+			index1++;
+#endif
+		}
+	}
+
+	////
+
+	PxU32 sortedIndex1 = 0;
+	PxU32 runningIndex0 = 0;
+	while(runningIndex0<lastSortedIndex0 && sortedIndex1<lastSortedIndex1)
+	{
+		const PxU32 index0 = sortedIndex1++;
+		const MBP_AABB& box0 = staticBoxes[index0];
+		const PxU32 limit = box0.mMaxX;
+//const PxU32 id1 = mObjects[inToOut_Static[index0]].mMBPHandle;
+		SIMD_OVERLAP_PRELOAD_BOX0
+
+//		const PxU32 l = staticBoxes[index0].mMinX;
+		const PxU32 l = box0.mMinX;	// ### PhysX
+		while(dynamicBoxes[runningIndex0].mMinX<=l)
+			runningIndex0++;
+
+		PxU32 index1 = runningIndex0;
+
+		while(dynamicBoxes[index1].mMinX<=limit)
+		{
+			{
+			MBP_OVERLAP_TEST(dynamicBoxes[index1])
+			{
+				outputPair_DynamicStatic(
+					*pairManager,
+					index1, index0, inToOut_Dynamic, inToOut_Static, mObjects, groups, mbpObjects
+					);
+			}
+			}
+#ifdef TWO_AT_A_TIME
+			if(dynamicBoxes[index1+1].mMinX<=limit)
+			{
+				MBP_OVERLAP_TEST(dynamicBoxes[index1+1])
+				{
+					outputPair_DynamicStatic(
+						*pairManager,
+						index1+1, index0, inToOut_Dynamic, inToOut_Static, mObjects, groups, mbpObjects
+						);
+				}
+			}
+			index1+=2;
+#else
+			index1++;
+#endif
+		}
+	}
+
+//	MBP_FREE(inToOut_Dynamic);
+
+#ifdef MBP_USE_SENTINELS
+//	((MBP_AABB* PX_RESTRICT)staticBoxes)[nb1] = Saved;
+//	((MBP_AABB* PX_RESTRICT)staticBoxes)[nb1+1] = Saved1;
+#endif
+}
+
+#ifdef USE_SINGLE_THREADED_REFERENCE_CODE
+void Region::prepareOverlaps()
+{
+	if(!mNbUpdatedBoxes
+		&& !mNeedsSorting	//### bugfix added for PhysX integration
+		)
+		return;
+
+	if(mNeedsSorting)
+	{
+		staticSort();
+
+		// PT: when a static object is added/removed/updated we need to compute the overlaps again
+		// even if no dynamic box has been updated. The line below forces all dynamic boxes to be
+		// sorted in PreparePruning() and tested for overlaps in BipartiteBoxPruning(). It would be
+		// more efficient to:
+		// a) skip the actual pruning in PreparePruning() (we only need to re-sort)
+		// b) do BipartiteBoxPruning() with the new/modified boxes, not all of them
+		// Well, not done yet.
+		mNbUpdatedBoxes = mNbDynamicBoxes;	// ### PhysX
+#if PX_DEBUG
+		for(PxU32 i=0;i<mNbDynamicBoxes;i++)
+		{
+			const PxU32 objectIndex = mInToOut_Dynamic[i];
+			mObjects[objectIndex].mUpdated = true;
+		}
+#endif
+	}
+}
+#endif
+
+void Region::prepareOverlapsMT()
+{
+	if(!mNbUpdatedBoxes
+		&& !mNeedsSorting	//### bugfix added for PhysX integration
+		)
+		return;
+
+	if(mNeedsSorting)
+	{
+		staticSort();
+
+		// PT: when a static object is added/removed/updated we need to compute the overlaps again
+		// even if no dynamic box has been updated. The line below forces all dynamic boxes to be
+		// sorted in PreparePruning() and tested for overlaps in BipartiteBoxPruning(). It would be
+		// more efficient to:
+		// a) skip the actual pruning in PreparePruning() (we only need to re-sort)
+		// b) do BipartiteBoxPruning() with the new/modified boxes, not all of them
+		// Well, not done yet.
+		mNbUpdatedBoxes = mNbDynamicBoxes;	// ### PhysX
+		mPrevNbUpdatedBoxes = 0;
+		mNeedsSortingSleeping = true;
+#if PX_DEBUG
+		for(PxU32 i=0;i<mNbDynamicBoxes;i++)
+		{
+			const PxU32 objectIndex = mInToOut_Dynamic[i];
+			mObjects[objectIndex].mUpdated = true;
+		}
+#endif
+	}
+
+	preparePruning(mTmpBuffers);
+	prepareBIPPruning(mTmpBuffers);
+}
+
+#ifdef USE_SINGLE_THREADED_REFERENCE_CODE
+void Region::findOverlaps(MBP_PairManager& pairManager, MBPOS_TmpBuffers& buffers)
+{
+	PX_ASSERT(!mNeedsSorting);
+	if(!mNbUpdatedBoxes)
+		return;
+
+	preparePruning(buffers);
+	prepareBIPPruning(buffers);
+
+	if(mInput.mNeeded)
+		DoCompleteBoxPruning(&pairManager, mInput);
+
+	if(mInput.mBIPInput.mNeeded)
+		DoBipartiteBoxPruning(&pairManager, mInput.mBIPInput);
+
+	mNbUpdatedBoxes = 0;
+}
+#endif
+
+void Region::findOverlapsMT(MBP_PairManager& pairManager, const BpHandle* PX_RESTRICT groups, const MBP_Object* PX_RESTRICT mbpObjects)
+{
+	PX_ASSERT(!mNeedsSorting);
+	if(!mNbUpdatedBoxes)
+		return;
+
+	if(mInput.mNeeded)
+		doCompleteBoxPruning(&pairManager, mInput, groups, mbpObjects);
+
+	if(mInput.mBIPInput.mNeeded)
+		doBipartiteBoxPruning(&pairManager, mInput.mBIPInput, groups, mbpObjects);
+
+	mNbUpdatedBoxes = 0;
+}
+
+///////////////////////////////////////////////////////////////////////////
+
+MBP::MBP() :
+	mNbPairs			(0),
+	mNbRegions			(0),
+	mFirstFreeIndex		(INVALID_ID),
+	mFirstFreeIndexBP	(INVALID_ID)
+{
+
+	for(PxU32 i=0;i<MAX_NB_MBP+1;i++)
+		mFirstFree[i] = INVALID_ID;
+}
+
+MBP::~MBP()
+{
+/*	for(PxU32 i=1;i<MAX_NB_MBP;i++)
+	{
+		if(mHandles[i].GetNbEntries())
+		{
+			const PxU32 SizeOfBundle = sizeof(RegionHandle)*i;
+//			printf("Handles %d: %d\n", i, mHandles[i].GetNbEntries()*sizeof(PxU32)/SizeOfBundle);
+		}
+	}*/
+
+	reset();
+}
+
+void MBP::freeBuffers()
+{
+	mRemoved.empty();
+	mOutOfBoundsObjects.clear();
+}
+
+void MBP::preallocate(PxU32 nbRegions, PxU32 nbObjects, PxU32 maxNbOverlaps)
+{
+	if(nbRegions)
+	{
+		mRegions.clear();
+		mRegions.reserve(nbRegions);
+	}
+
+	if(nbObjects)
+	{
+		mMBP_Objects.clear();
+		mMBP_Objects.reserve(nbObjects);
+#ifdef USE_FULLY_INSIDE_FLAG
+		mFullyInsideBitmap.init(nbObjects);
+		mFullyInsideBitmap.clearAll();
+#endif
+	}
+
+	mPairManager.reserveMemory(maxNbOverlaps);
+}
+
+PX_COMPILE_TIME_ASSERT(sizeof(BpHandle)<=sizeof(PxU32));
+void MBP::addToOutOfBoundsArray(BpHandle id)
+{
+	PX_ASSERT(mOutOfBoundsObjects.find(PxU32(id)) == mOutOfBoundsObjects.end());
+	mOutOfBoundsObjects.pushBack(PxU32(id));
+}
+
+static void setupOverlapFlags(PxU32 nbRegions, RegionData* PX_RESTRICT regions)
+{
+	for(PxU32 i=0;i<nbRegions;i++)
+		regions[i].mOverlap = false;
+
+	for(PxU32 i=0;i<nbRegions;i++)
+	{
+		if(!regions[i].mBP)
+			continue;
+
+		for(PxU32 j=i+1;j<nbRegions;j++)
+		{
+			if(!regions[j].mBP)
+				continue;
+
+			if(regions[i].mBox.intersectNoTouch(regions[j].mBox))	
+			{
+				regions[i].mOverlap = true;
+				regions[j].mOverlap = true;
+			}
+		}
+	}
+}
+
+//#define PRINT_STATS
+#ifdef PRINT_STATS
+#include <stdio.h>
+#endif
+
+// PT: TODO:
+// - We could try to keep bounds around all objects (for each region), and then test the new region's bounds against these instead of
+//   testing all objects one by one. These new bounds (around all objects of a given region) would be delicate to maintain though.
+// - Just store these "fully inside flags" (i.e. objects) in a separate list? Or can we do MTF on objects? (probably not, else we
+//   wouldn't have holes in the array due to removed objects)
+
+// PT: automatically populate new region with overlapping objects.
+// Brute-force version checking all existing objects, potentially optimized using "fully inside" flags.
+//#define FIRST_VERSION
+#ifdef FIRST_VERSION
+void MBP::populateNewRegion(const MBP_AABB& box, Region* addedRegion, PxU32 regionIndex)
+{
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	const PxU32 nbObjects = mMBP_Objects.size();
+	MBP_Object* PX_RESTRICT objects = mMBP_Objects.begin();
+
+#ifdef PRINT_STATS
+	PxU32 nbObjectsFound = 0;
+	PxU32 nbObjectsTested = 0;
+#endif
+
+#ifdef USE_FULLY_INSIDE_FLAG
+	const PxU32* fullyInsideFlags = mFullyInsideBitmap.getBits();
+#endif
+
+	PxU32 j=0;
+	while(j<nbObjects)
+	{
+#ifdef USE_FULLY_INSIDE_FLAG
+		const PxU32 blockFlags = fullyInsideFlags[j>>5];
+	#ifdef HWSCAN
+		if(blockFlags==0)	//HWSCAN
+	#else
+		if(blockFlags==0xffffffff)
+	#endif
+		{
+			j+=32;
+			continue;
+		}
+		PxU32 nbToGo = PxMin(nbObjects - j, PxU32(32));
+		PxU32 mask = 1;
+		while(nbToGo--)
+		{
+
+		MBP_Object& currentObject = objects[j];
+		// PT: if an object A is fully contained inside all the regions S it overlaps, we don't need to test it against the new region R.
+		// The rationale is that even if R does overlap A, any new object B must touch S to overlap with A. So B would be added to S and
+		// the (A,B) overlap would be detected in S, even if it's not detected in R.
+		const PxU32 res = blockFlags & mask;
+		PX_ASSERT((mFullyInsideBitmap.isSet(j) && res) || (!mFullyInsideBitmap.isSet(j) && !res));
+		mask+=mask;
+		j++;
+	#ifdef HWSCAN
+		if(!res)	//HWSCAN
+	#else
+		if(res)
+	#endif
+			continue;
+		PX_ASSERT(!(currentObject.mFlags & MBP_REMOVED));
+#else
+		MBP_Object& currentObject = objects[j++];
+		if(currentObject.mFlags & MBP_REMOVED)
+			continue;	// PT: object is in the free list
+#endif
+
+#ifdef PRINT_STATS
+		nbObjectsTested++;
+#endif
+
+		MBP_AABB bounds;
+		MBP_Handle mbpHandle;
+
+		const PxU32 nbHandles = currentObject.mNbHandles;
+		if(nbHandles)
+		{
+			RegionHandle* PX_RESTRICT handles = getHandles(currentObject, nbHandles);
+			// PT: no need to test all regions since they should contain the same info. Just retrieve bounds from the first one.
+			PxU32 i=0;	//	for(PxU32 i=0;i<nbHandles;i++)
+			{
+				const RegionHandle& h = handles[i];
+				const RegionData& currentRegion = regions[h.mInternalBPHandle];
+				PX_ASSERT(currentRegion.mBP);
+
+				mbpHandle = currentRegion.mBP->retrieveBounds(bounds, h.mHandle);
+			}
+		}
+		else
+		{
+			PX_ASSERT(mManager);
+
+			// PT: if the object is out-of-bounds, we're out-of-luck. We don't have the object bounds, so we need to retrieve them
+			// from the AABB manager - and then re-encode them. This is not very elegant or efficient, but it should rarely happen
+			// so this is good enough for now.
+			const PxBounds3 decodedBounds = mManager->getBPBounds(currentObject.mUserID);
+
+			bounds.initFrom2(decodedBounds);
+
+			mbpHandle = currentObject.mHandlesIndex;
+		}
+
+		if(bounds.intersect(box))
+		{
+//			updateObject(mbpHandle, bounds);
+			updateObjectAfterNewRegionAdded(mbpHandle, bounds, addedRegion, regionIndex);
+#ifdef PRINT_STATS
+			nbObjectsFound++;
+#endif
+		}
+
+#ifdef USE_FULLY_INSIDE_FLAG
+		}
+#endif
+	}
+#ifdef PRINT_STATS
+	printf("Populating new region with %d objects (tested %d/%d object)\n", nbObjectsFound, nbObjectsTested, nbObjects);
+#endif
+}
+#endif
+
+// PT: version using lowestSetBit
+#define SECOND_VERSION
+#ifdef SECOND_VERSION
+
+/*	PX_FORCE_INLINE PxU32 lowestSetBitUnsafe64(PxU64 v)
+	{
+		unsigned long retval;
+		_BitScanForward64(&retval, v);
+		return retval;
+	}*/
+
+void MBP::populateNewRegion(const MBP_AABB& box, Region* addedRegion, PxU32 regionIndex)
+{
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	const PxU32 nbObjects = mMBP_Objects.size();
+	PX_UNUSED(nbObjects);
+	MBP_Object* PX_RESTRICT objects = mMBP_Objects.begin();
+	const PxU32* fullyInsideFlags = mFullyInsideBitmap.getBits();
+//	const PxU64* fullyInsideFlags = (const PxU64*)mFullyInsideBitmap.getBits();
+	if(!fullyInsideFlags)
+		return;
+
+	const PxU32 lastSetBit = mFullyInsideBitmap.findLast();
+//	const PxU64 lastSetBit = mFullyInsideBitmap.findLast();
+
+#ifdef PRINT_STATS
+	PxU32 nbObjectsFound = 0;
+	PxU32 nbObjectsTested = 0;
+#endif
+
+	for(PxU32 w = 0; w <= lastSetBit >> 5; ++w)
+//	for(PxU64 w = 0; w <= lastSetBit >> 6; ++w)
+	{
+		for(PxU32 b = fullyInsideFlags[w]; b; b &= b-1)
+//		for(PxU64 b = fullyInsideFlags[w]; b; b &= b-1)
+		{
+			const PxU32 index = PxU32(w<<5|Ps::lowestSetBit(b));
+//			const PxU64 index = (PxU64)(w<<6|::lowestSetBitUnsafe64(b));
+			PX_ASSERT(index<nbObjects);
+
+			MBP_Object& currentObject = objects[index];
+			// PT: if an object A is fully contained inside all the regions S it overlaps, we don't need to test it against the new region R.
+			// The rationale is that even if R does overlap A, any new object B must touch S to overlap with A. So B would be added to S and
+			// the (A,B) overlap would be detected in S, even if it's not detected in R.
+			PX_ASSERT(!(currentObject.mFlags & MBP_REMOVED));
+#ifdef HWSCAN
+			PX_ASSERT(mFullyInsideBitmap.isSet(index));
+#else
+			PX_ASSERT(!mFullyInsideBitmap.isSet(index));
+#endif
+
+#ifdef PRINT_STATS
+			nbObjectsTested++;
+#endif
+
+			MBP_AABB bounds;
+			MBP_Handle mbpHandle;
+
+			const PxU32 nbHandles = currentObject.mNbHandles;
+			if(nbHandles)
+			{
+				RegionHandle* PX_RESTRICT handles = getHandles(currentObject, nbHandles);
+				// PT: no need to test all regions since they should contain the same info. Just retrieve bounds from the first one.
+				PxU32 i=0;	//	for(PxU32 i=0;i<nbHandles;i++)
+				{
+					const RegionHandle& h = handles[i];
+					const RegionData& currentRegion = regions[h.mInternalBPHandle];
+					PX_ASSERT(currentRegion.mBP);
+
+					mbpHandle = currentRegion.mBP->retrieveBounds(bounds, h.mHandle);
+				}
+			}
+			else
+			{
+				// PT: if the object is out-of-bounds, we're out-of-luck. We don't have the object bounds, so we need to retrieve them
+				// from the AABB manager - and then re-encode them. This is not very elegant or efficient, but it should rarely happen
+				// so this is good enough for now.
+				const PxBounds3 rawBounds = mTransientBounds[currentObject.mUserID];
+				PxVec3 c(mTransientContactDistance[currentObject.mUserID]);
+				const PxBounds3 decodedBounds(rawBounds.minimum - c, rawBounds.maximum + c);
+				bounds.initFrom2(decodedBounds);
+
+				mbpHandle = currentObject.mHandlesIndex;
+			}
+
+			if(bounds.intersects(box))
+			{
+//				updateObject(mbpHandle, bounds);
+				updateObjectAfterNewRegionAdded(mbpHandle, bounds, addedRegion, regionIndex);
+#ifdef PRINT_STATS
+				nbObjectsFound++;
+#endif
+			}
+		}
+	}
+#ifdef PRINT_STATS
+	printf("Populating new region with %d objects (tested %d/%d object)\n", nbObjectsFound, nbObjectsTested, nbObjects);
+#endif
+}
+#endif
+
+//#define THIRD_VERSION
+#ifdef THIRD_VERSION
+void MBP::populateNewRegion(const MBP_AABB& box, Region* addedRegion, PxU32 regionIndex)
+{
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	const PxU32 nbObjects = mMBP_Objects.size();
+	PX_UNUSED(nbObjects);
+	MBP_Object* PX_RESTRICT objects = mMBP_Objects.begin();
+
+	const PxU32* fullyInsideFlags = mFullyInsideBitmap.getBits();
+	if(!fullyInsideFlags)
+		return;
+
+#ifdef PRINT_STATS
+	PxU32 nbObjectsFound = 0;
+	PxU32 nbObjectsTested = 0;
+#endif
+
+	Cm::BitMap bm;
+	bm.importData(mFullyInsideBitmap.getSize(), (PxU32*)fullyInsideFlags);
+
+	Cm::BitMap::Iterator it(bm);
+	PxU32 index = it.getNext();
+	while(index != Cm::BitMap::Iterator::DONE)
+	{
+		PX_ASSERT(index<nbObjects);
+
+		MBP_Object& currentObject = objects[index];
+		// PT: if an object A is fully contained inside all the regions S it overlaps, we don't need to test it against the new region R.
+		// The rationale is that even if R does overlap A, any new object B must touch S to overlap with A. So B would be added to S and
+		// the (A,B) overlap would be detected in S, even if it's not detected in R.
+		PX_ASSERT(!(currentObject.mFlags & MBP_REMOVED));
+
+#ifdef PRINT_STATS
+		nbObjectsTested++;
+#endif
+
+		MBP_AABB bounds;
+		MBP_Handle mbpHandle;
+
+		const PxU32 nbHandles = currentObject.mNbHandles;
+		if(nbHandles)
+		{
+			RegionHandle* PX_RESTRICT handles = getHandles(currentObject, nbHandles);
+			// PT: no need to test all regions since they should contain the same info. Just retrieve bounds from the first one.
+			PxU32 i=0;	//	for(PxU32 i=0;i<nbHandles;i++)
+			{
+				const RegionHandle& h = handles[i];
+				const RegionData& currentRegion = regions[h.mInternalBPHandle];
+				PX_ASSERT(currentRegion.mBP);
+
+				mbpHandle = currentRegion.mBP->retrieveBounds(bounds, h.mHandle);
+			}
+		}
+		else
+		{
+			PX_ASSERT(mManager);
+
+			// PT: if the object is out-of-bounds, we're out-of-luck. We don't have the object bounds, so we need to retrieve them
+			// from the AABB manager - and then re-encode them. This is not very elegant or efficient, but it should rarely happen
+			// so this is good enough for now.
+			const PxBounds3 decodedBounds = mManager->getBPBounds(currentObject.mUserID);
+
+			bounds.initFrom2(decodedBounds);
+
+			mbpHandle = currentObject.mHandlesIndex;
+		}
+
+		if(bounds.intersect(box))
+		{
+//			updateObject(mbpHandle, bounds);
+			updateObjectAfterNewRegionAdded(mbpHandle, bounds, addedRegion, regionIndex);
+#ifdef PRINT_STATS
+			nbObjectsFound++;
+#endif
+		}
+		index = it.getNext();
+	}
+#ifdef PRINT_STATS
+	printf("Populating new region with %d objects (tested %d/%d object)\n", nbObjectsFound, nbObjectsTested, nbObjects);
+#endif
+}
+#endif
+
+
+PxU32 MBP::addRegion(const PxBroadPhaseRegion& region, bool populateRegion)
+{
+	PxU32 regionHandle;
+	RegionData* PX_RESTRICT buffer;
+
+	if(mFirstFreeIndexBP!=INVALID_ID)
+	{
+		regionHandle = mFirstFreeIndexBP;
+
+		buffer = mRegions.begin();
+		buffer += regionHandle;
+
+		mFirstFreeIndexBP = PxU32(size_t(buffer->mUserData));	// PT: this is safe, we previously stored a PxU32 in there
+	}
+	else
+	{
+		if(mNbRegions>=MAX_NB_MBP)
+		{
+			Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, "MBP::addRegion: max number of regions reached.");
+			return INVALID_ID;
+		}
+
+		regionHandle = mNbRegions++;	
+		buffer = reserveContainerMemory<RegionData>(mRegions, 1);		
+	}
+
+	Region* newRegion = PX_NEW(Region);
+	buffer->mBox.initFrom2(region.bounds);
+	buffer->mBP			= newRegion;
+	buffer->mUserData	= region.userData;
+
+	setupOverlapFlags(mNbRegions, mRegions.begin());
+
+	// PT: automatically populate new region with overlapping objects
+	if(populateRegion)
+		populateNewRegion(buffer->mBox, newRegion, regionHandle);
+
+	return regionHandle;
+}
+
+// ### TODO: recycle regions, make sure objects are properly deleted/transferred, etc
+// ### TODO: what happens if we delete a zone then immediately add it back? Do objects get deleted?
+// ### TODO: in fact if we remove a zone but we keep the objects, what happens to their current overlaps? Are they kept or discarded?
+bool MBP::removeRegion(PxU32 handle)
+{
+	if(handle>=mNbRegions)
+	{
+		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "MBP::removeRegion: invalid handle.");
+		return false;
+	}
+
+	RegionData* PX_RESTRICT region = mRegions.begin();
+	region += handle;
+
+	Region* bp = region->mBP;
+	if(!bp)
+	{
+		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "MBP::removeRegion: invalid handle.");
+		return false;
+	}
+
+	PxBounds3 empty;
+	empty.setEmpty();
+	region->mBox.initFrom2(empty);
+
+	{
+		// We are going to remove the region but it can still contain objects. We need to update
+		// those objects so that their handles and out-of-bounds status are modified.
+		//
+		// Unfortunately there is no way to iterate over active objects in a region, so we need
+		// to iterate over the max amount of objects. ### TODO: optimize this
+		const PxU32 maxNbObjects = bp->mMaxNbObjects;
+		MBPEntry* PX_RESTRICT objects = bp->mObjects;
+		for(PxU32 j=0;j<maxNbObjects;j++)
+		{
+			// The handle is INVALID_ID for non-active entries
+			if(objects[j].mMBPHandle!=INVALID_ID)
+			{
+//				printf("Object to update!\n");
+				updateObjectAfterRegionRemoval(objects[j].mMBPHandle, bp);
+			}
+		}
+	}
+
+	PX_DELETE(bp);
+	region->mBP = NULL;
+	region->mUserData = reinterpret_cast<void*>(size_t(mFirstFreeIndexBP));
+	mFirstFreeIndexBP = handle;
+
+	// A region has been removed so we need to update the overlap flags for all remaining regions
+	// ### TODO: optimize this
+	setupOverlapFlags(mNbRegions, mRegions.begin());
+	return true;
+}
+
+const Region* MBP::getRegion(PxU32 i) const
+{
+	if(i>=mNbRegions)
+		return NULL;
+
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	return regions[i].mBP;
+}
+
+PX_FORCE_INLINE RegionHandle* MBP::getHandles(MBP_Object& currentObject, PxU32 nbHandles)
+{
+	RegionHandle* handles;
+	if(nbHandles==1)
+		handles = &currentObject.mHandle;
+	else
+	{
+		const PxU32 handlesIndex = currentObject.mHandlesIndex;
+		Ps::Array<PxU32>& c = mHandles[nbHandles];
+		handles = reinterpret_cast<RegionHandle*>(c.begin()+handlesIndex);
+	}
+	return handles;
+}
+
+void MBP::purgeHandles(MBP_Object* PX_RESTRICT object, PxU32 nbHandles)
+{
+	if(nbHandles>1)
+	{
+		const PxU32 handlesIndex = object->mHandlesIndex;
+		Ps::Array<PxU32>& c = mHandles[nbHandles];
+		PxU32* recycled = c.begin() + handlesIndex;
+		*recycled = mFirstFree[nbHandles];
+		mFirstFree[nbHandles] = handlesIndex;
+	}
+}
+
+void MBP::storeHandles(MBP_Object* PX_RESTRICT object, PxU32 nbHandles, const RegionHandle* PX_RESTRICT handles)
+{
+	if(nbHandles==1)
+	{
+		object->mHandle = handles[0];
+	}
+	else if(nbHandles)	// ### PhysX change
+	{
+		Ps::Array<PxU32>& c = mHandles[nbHandles];
+		const PxU32 firstFree = mFirstFree[nbHandles];
+		PxU32* handlesMemory;
+		if(firstFree!=INVALID_ID)
+		{
+			object->mHandlesIndex = firstFree;
+			handlesMemory = c.begin() + firstFree;
+			mFirstFree[nbHandles] = *handlesMemory;
+		}
+		else
+		{
+			const PxU32 handlesIndex = c.size();
+			object->mHandlesIndex = handlesIndex;
+			handlesMemory = reserveContainerMemory<PxU32>(c, sizeof(RegionHandle)*nbHandles/sizeof(PxU32));
+		}
+		PxMemCopy(handlesMemory, handles, sizeof(RegionHandle)*nbHandles);
+	}
+}
+
+MBP_Handle MBP::addObject(const MBP_AABB& box, BpHandle userID, bool isStatic)
+{
+	MBP_ObjectIndex objectIndex;
+	MBP_Object* objectMemory;
+	PxU32 flipFlop;
+	if(mFirstFreeIndex!=INVALID_ID)
+	{
+		objectIndex = mFirstFreeIndex;
+		MBP_Object* objects = mMBP_Objects.begin();
+		objectMemory = &objects[objectIndex];
+		PX_ASSERT(!objectMemory->mNbHandles);
+		mFirstFreeIndex = objectMemory->mHandlesIndex;
+		flipFlop = PxU32(objectMemory->getFlipFlop());
+	}
+	else
+	{
+		objectIndex = mMBP_Objects.size();
+		objectMemory = reserveContainerMemory<MBP_Object>(mMBP_Objects, 1);		
+		flipFlop = 0;
+	}
+	const MBP_Handle MBPObjectHandle = encodeHandle(objectIndex, flipFlop, isStatic);
+
+//	mMBP_Objects.Shrink();
+
+	PxU32 nbHandles = 0;
+#ifdef USE_FULLY_INSIDE_FLAG
+	bool newObjectIsFullyInsideRegions = true;
+#endif
+
+	const PxU32 nb = mNbRegions;
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+
+	RegionHandle tmpHandles[MAX_NB_MBP+1];
+	for(PxU32 i=0;i<nb;i++)
+	{
+#ifdef MBP_USE_NO_CMP_OVERLAP_3D
+		if(Intersect3D(regions[i].mBox, box))
+#else
+		if(regions[i].mBox.intersects(box))
+#endif
+		{
+#ifdef USE_FULLY_INSIDE_FLAG
+			if(!box.isInside(regions[i].mBox))
+				newObjectIsFullyInsideRegions = false;
+#endif
+			RegionHandle& h = tmpHandles[nbHandles++];
+
+			h.mHandle = regions[i].mBP->addObject(box, MBPObjectHandle, isStatic);
+			h.mInternalBPHandle = Ps::to16(i);
+		}
+	}
+	storeHandles(objectMemory, nbHandles, tmpHandles);
+
+	objectMemory->mNbHandles	= Ps::to16(nbHandles);
+	PxU16 flags = 0;
+	if(flipFlop)
+		flags |= MBP_FLIP_FLOP;
+#ifdef USE_FULLY_INSIDE_FLAG
+	if(nbHandles && newObjectIsFullyInsideRegions)
+		setBit(mFullyInsideBitmap, objectIndex);
+	else
+		clearBit(mFullyInsideBitmap, objectIndex);
+#endif
+	if(!nbHandles)
+	{
+		objectMemory->mHandlesIndex = MBPObjectHandle;
+		addToOutOfBoundsArray(userID);
+	}
+
+	if(!isStatic)
+		mUpdatedObjects.setBitChecked(objectIndex);
+
+//	objectMemory->mUpdated	= !isStatic;
+	objectMemory->mFlags	= flags;
+	objectMemory->mUserID	= userID;
+
+	return MBPObjectHandle;
+}
+
+bool MBP::removeObject(MBP_Handle handle)
+{
+	const MBP_ObjectIndex objectIndex = decodeHandle_Index(handle);
+
+	MBP_Object* PX_RESTRICT objects = mMBP_Objects.begin();
+	MBP_Object& currentObject = objects[objectIndex];
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	// Parse previously overlapping regions. If still overlapping, update object. Else remove from region.
+	const PxU32 nbHandles = currentObject.mNbHandles;
+	if(nbHandles)	// ### PhysX change
+	{
+		RegionHandle* handles = getHandles(currentObject, nbHandles);
+		for(PxU32 i=0;i<nbHandles;i++)
+		{
+			const RegionHandle& h = handles[i];
+			const RegionData& currentRegion = regions[h.mInternalBPHandle];
+//			if(currentRegion.mBP)
+			PX_ASSERT(currentRegion.mBP);
+			currentRegion.mBP->removeObject(h.mHandle);
+		}
+
+		purgeHandles(&currentObject, nbHandles);
+	}
+
+	currentObject.mNbHandles	= 0;
+	currentObject.mFlags		|= MBP_REMOVED;
+	currentObject.mHandlesIndex	= mFirstFreeIndex;
+//	if(!decodeHandle_IsStatic(handle))
+//	if(!currentObject.IsStatic())
+		mUpdatedObjects.setBitChecked(objectIndex);
+
+	mFirstFreeIndex				= objectIndex;
+
+	mRemoved.setBitChecked(objectIndex);	// PT: this is cleared each frame so it's not a replacement for the MBP_REMOVED flag
+
+#ifdef USE_FULLY_INSIDE_FLAG
+	// PT: when removing an object we mark it as "fully inside" so that it is automatically
+	// discarded in the "populateNewRegion" function, without the need for MBP_REMOVED.
+	setBit(mFullyInsideBitmap, objectIndex);
+#endif
+	return true;
+}
+
+static PX_FORCE_INLINE bool stillIntersects(PxU32 handle, PxU32& _nb, PxU32* PX_RESTRICT currentOverlaps)
+{
+	const PxU32 nb = _nb;
+	for(PxU32 i=0;i<nb;i++)
+	{
+		if(currentOverlaps[i]==handle)
+		{
+			_nb = nb-1;
+			currentOverlaps[i] = currentOverlaps[nb-1];
+			return true;
+		}
+	}
+	return false;
+}
+
+bool MBP::updateObject(MBP_Handle handle, const MBP_AABB& box)
+{
+	const MBP_ObjectIndex objectIndex = decodeHandle_Index(handle);
+	const PxU32 isStatic = decodeHandle_IsStatic(handle);
+
+	const PxU32 nbRegions = mNbRegions;
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	MBP_Object* PX_RESTRICT objects = mMBP_Objects.begin();
+	MBP_Object& currentObject = objects[objectIndex];
+
+//	if(!isStatic)	// ### removed for PhysX integration (bugfix)
+//	if(!currentObject.IsStatic())
+	{
+		mUpdatedObjects.setBitChecked(objectIndex);
+	}
+
+	// PT: fast path which should happen quite frequently. If:
+	// - the object was touching a single region
+	// - that region doesn't overlap other regions
+	// - the object's new bounds is fully inside the region
+	// then we know that the object can't touch another region, and we can use this fast-path that simply
+	// updates one region and avoids iterating over/testing all the other ones.
+	const PxU32 nbHandles = currentObject.mNbHandles;
+	if(nbHandles==1)
+	{
+		const RegionHandle& h = currentObject.mHandle;
+		const RegionData& currentRegion = regions[h.mInternalBPHandle];
+		if(!currentRegion.mOverlap && box.isInside(currentRegion.mBox))
+		{
+#ifdef USE_FULLY_INSIDE_FLAG
+			// PT: it is possible that this flag is not set already when reaching this place:
+			// - object touches 2 regions
+			// - then in one frame:
+			//    - object moves fully inside one region
+			//    - the other region is removed
+			// => nbHandles changes from 2 to 1 while MBP_FULLY_INSIDE is not set
+			setBit(mFullyInsideBitmap, objectIndex);
+#endif
+			currentRegion.mBP->updateObject(box, h.mHandle);
+			return true;
+		}
+	}
+
+	// Find regions overlapping object's new position
+#ifdef USE_FULLY_INSIDE_FLAG
+	bool objectIsFullyInsideRegions = true;
+#endif
+	PxU32 nbCurrentOverlaps = 0;
+	PxU32 currentOverlaps[MAX_NB_MBP+1];
+	// PT: here, we may still parse regions which have been removed. But their boxes have been set to empty,
+	// so nothing will happen.
+	for(PxU32 i=0;i<nbRegions;i++)
+	{
+#ifdef MBP_USE_NO_CMP_OVERLAP_3D
+		if(Intersect3D(regions[i].mBox, box))
+#else
+		if(regions[i].mBox.intersects(box))
+#endif
+		{
+#ifdef USE_FULLY_INSIDE_FLAG
+			if(!box.isInside(regions[i].mBox))
+				objectIsFullyInsideRegions = false;
+#endif
+			PX_ASSERT(nbCurrentOverlaps<MAX_NB_MBP);
+			currentOverlaps[nbCurrentOverlaps++] = i;
+		}
+	}
+
+	// New data for this frame
+	PxU32 nbNewHandles = 0;
+	RegionHandle newHandles[MAX_NB_MBP+1];
+
+	// Parse previously overlapping regions. If still overlapping, update object. Else remove from region.
+	RegionHandle* handles = getHandles(currentObject, nbHandles);
+	for(PxU32 i=0;i<nbHandles;i++)
+	{
+		const RegionHandle& h = handles[i];
+		PX_ASSERT(h.mInternalBPHandle<nbRegions);
+		const RegionData& currentRegion = regions[h.mInternalBPHandle];
+		// We need to update object even if it then gets removed, as the removal
+		// doesn't actually report lost pairs - and we need this.
+//		currentRegion.mBP->UpdateObject(box, h.mHandle);
+		if(stillIntersects(h.mInternalBPHandle, nbCurrentOverlaps, currentOverlaps))
+		{
+			currentRegion.mBP->updateObject(box, h.mHandle);
+			// Still collides => keep handle for this frame
+			newHandles[nbNewHandles++] = h;
+		}
+		else
+		{
+			PX_ASSERT(!currentRegion.mBox.intersects(box));
+//			if(currentRegion.mBP)
+			PX_ASSERT(currentRegion.mBP);
+			currentRegion.mBP->removeObject(h.mHandle);
+		}
+	}
+
+	// Add to new regions if needed
+	for(PxU32 i=0;i<nbCurrentOverlaps;i++)
+	{
+//		if(currentOverlaps[i]==INVALID_ID)
+//			continue;
+		const PxU32 regionIndex = currentOverlaps[i];
+		const MBP_Index BPHandle = regions[regionIndex].mBP->addObject(box, handle, isStatic!=0);
+		newHandles[nbNewHandles].mHandle = Ps::to16(BPHandle);
+		newHandles[nbNewHandles].mInternalBPHandle = Ps::to16(regionIndex);
+		nbNewHandles++;
+	}
+
+	if(nbHandles==nbNewHandles)
+	{
+		for(PxU32 i=0;i<nbNewHandles;i++)
+			handles[i] = newHandles[i];
+	}
+	else
+	{
+		purgeHandles(&currentObject, nbHandles);
+		storeHandles(&currentObject, nbNewHandles, newHandles);
+	}
+
+	currentObject.mNbHandles = Ps::to16(nbNewHandles);
+	if(!nbNewHandles && nbHandles)
+	{
+		currentObject.mHandlesIndex = handle;
+		addToOutOfBoundsArray(currentObject.mUserID);
+	}
+
+//	for(PxU32 i=0;i<nbNewHandles;i++)
+//		currentObject.mHandles[i] = newHandles[i];
+
+#ifdef USE_FULLY_INSIDE_FLAG
+	if(objectIsFullyInsideRegions && nbNewHandles)
+		setBit(mFullyInsideBitmap, objectIndex);
+	else
+		clearBit(mFullyInsideBitmap, objectIndex);
+#endif
+	return true;
+}
+
+bool MBP::updateObjectAfterRegionRemoval(MBP_Handle handle, Region* removedRegion)
+{
+	PX_ASSERT(removedRegion);
+
+	const MBP_ObjectIndex objectIndex = decodeHandle_Index(handle);
+
+	const PxU32 nbRegions = mNbRegions;
+	PX_UNUSED(nbRegions);
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	MBP_Object* PX_RESTRICT objects = mMBP_Objects.begin();
+	MBP_Object& currentObject = objects[objectIndex];
+
+	// Mark the object as updated so that its pairs are considered for removal. If we don't do this an out-of-bounds object
+	// resting on another non-out-of-bounds object still collides with that object and the memory associated with that pair
+	// is not released. If we mark it as updated the pair is lost, and the out-of-bounds object falls through.
+	//
+	// However if we do this any pair involving the object will be marked as lost, even the ones involving other regions.
+	// Typically the pair will then get lost one frame and get recreated the next frame.
+//	mUpdatedObjects.setBitChecked(objectIndex);
+
+	const PxU32 nbHandles = currentObject.mNbHandles;
+	PX_ASSERT(nbHandles);
+
+	// New handles
+	PxU32 nbNewHandles = 0;
+	RegionHandle newHandles[MAX_NB_MBP+1];
+
+	// Parse previously overlapping regions. Keep all of them except removed one.
+	RegionHandle* handles = getHandles(currentObject, nbHandles);
+	for(PxU32 i=0;i<nbHandles;i++)
+	{
+		const RegionHandle& h = handles[i];
+		PX_ASSERT(h.mInternalBPHandle<nbRegions);
+		if(regions[h.mInternalBPHandle].mBP!=removedRegion)
+			newHandles[nbNewHandles++] = h;
+	}
+#ifdef USE_FULLY_INSIDE_FLAG
+	// PT: in theory we should update the inside flag here but we don't do that for perf reasons.
+	// - If the flag is set, it means the object was fully inside all its regions. Removing one of them does not invalidate the flag.
+	// - If the flag is not set, removing one region might allow us to set the flag now. However not doing so simply makes the
+	//   populateNewRegion() function run a bit slower, it does not produce wrong results. This is only until concerned objects are
+	//   updated again anyway, so we live with this.
+#endif
+
+	PX_ASSERT(nbNewHandles==nbHandles-1);
+	purgeHandles(&currentObject, nbHandles);
+	storeHandles(&currentObject, nbNewHandles, newHandles);
+
+	currentObject.mNbHandles = Ps::to16(nbNewHandles);
+	if(!nbNewHandles)
+	{
+		currentObject.mHandlesIndex = handle;
+		addToOutOfBoundsArray(currentObject.mUserID);
+#ifdef USE_FULLY_INSIDE_FLAG
+		clearBit(mFullyInsideBitmap, objectIndex);
+#endif
+	}
+	return true;
+}
+
+bool MBP::updateObjectAfterNewRegionAdded(MBP_Handle handle, const MBP_AABB& box, Region* addedRegion, PxU32 regionIndex)
+{
+	PX_ASSERT(addedRegion);
+
+	const MBP_ObjectIndex objectIndex = decodeHandle_Index(handle);
+	const PxU32 isStatic = decodeHandle_IsStatic(handle);
+
+	MBP_Object* PX_RESTRICT objects = mMBP_Objects.begin();
+	MBP_Object& currentObject = objects[objectIndex];
+
+//	if(!isStatic)	// ### removed for PhysX integration (bugfix)
+//	if(!currentObject.IsStatic())
+	{
+		mUpdatedObjects.setBitChecked(objectIndex);
+	}
+
+	// PT: here we know that we're touching one more region than before and we'll need to update the handles.
+	// So there is no "fast path" in this case - well the whole function is a fast path if you want.
+	//
+	// We don't need to "find regions overlapping object's new position": we know it's going to be the
+	// same as before, plus the newly added region ("addedRegion").
+
+#ifdef USE_FULLY_INSIDE_FLAG
+	// PT: we know that the object is not marked as "fully inside", otherwise this function would not have been called.
+	#ifdef HWSCAN
+	PX_ASSERT(mFullyInsideBitmap.isSet(objectIndex));	//HWSCAN
+	#else
+	PX_ASSERT(!mFullyInsideBitmap.isSet(objectIndex));
+	#endif
+#endif
+
+	const PxU32 nbHandles = currentObject.mNbHandles;
+	PxU32 nbNewHandles = 0;
+	RegionHandle newHandles[MAX_NB_MBP+1];
+
+	// PT: get previously overlapping regions. We didn't actually move so we're still overlapping as before.
+	// We just need to get the handles here.
+	RegionHandle* handles = getHandles(currentObject, nbHandles);
+	for(PxU32 i=0;i<nbHandles;i++)
+		newHandles[nbNewHandles++] = handles[i];
+
+	// Add to new region
+	{
+#if PX_DEBUG
+		const RegionData* PX_RESTRICT regions = mRegions.begin();
+		const RegionData& currentRegion = regions[regionIndex];
+		PX_ASSERT(currentRegion.mBox.intersects(box));
+#endif
+		const MBP_Index BPHandle = addedRegion->addObject(box, handle, isStatic!=0);
+		newHandles[nbNewHandles].mHandle = Ps::to16(BPHandle);
+		newHandles[nbNewHandles].mInternalBPHandle = Ps::to16(regionIndex);
+		nbNewHandles++;
+	}
+
+	// PT: we know that we have one more handle than before, no need to test
+	purgeHandles(&currentObject, nbHandles);
+	storeHandles(&currentObject, nbNewHandles, newHandles);
+
+	currentObject.mNbHandles = Ps::to16(nbNewHandles);
+
+	// PT: we know that we have at least one handle (from the newly added region), so we can't be "out of bounds" here.
+	PX_ASSERT(nbNewHandles);
+
+#ifdef USE_FULLY_INSIDE_FLAG
+	// PT: we know that the object was not "fully inside" before, so even if it is fully inside the new region, it
+	// will not be fully inside all of them => no need to change its fully inside flag
+	// TODO: an exception to this would be the case where the object was out-of-bounds, and it's now fully inside the new region
+	// => we could set the flag in that case.
+#endif
+	return true;
+}
+
+bool MBP_PairManager::removeMarkedPairs(const MBP_Object* objects, BroadPhaseMBP* mbp, const BitArray& updated, const BitArray& removed)
+{
+	// PT: parse all currently active pairs. The goal here is to generate the found/lost pairs, compared to previous frame.
+	PxU32 i=0;
+	PxU32 nbActivePairs = mNbActivePairs;
+	while(i<nbActivePairs)
+	{
+		MBP_Pair& p = mActivePairs[i];
+
+		if(p.isNew)
+		{
+			// New pair
+
+			// PT: 'isNew' is set to true in the 'addPair' function. In this case the pair did not previously
+			// exist in the structure, and thus we must report the new pair to the client code.
+			//
+			// PT: group-based filtering is not needed here, since it has already been done in 'addPair'
+			const PxU32 id0 = p.id0;
+			const PxU32 id1 = p.id1;
+			PX_ASSERT(id0!=INVALID_ID);
+			PX_ASSERT(id1!=INVALID_ID);
+			const MBP_ObjectIndex index0 = decodeHandle_Index(id0);
+			const MBP_ObjectIndex index1 = decodeHandle_Index(id1);
+
+			const BpHandle object0 = objects[index0].mUserID;
+			const BpHandle object1 = objects[index1].mUserID;
+			mbp->mCreated.pushBack(BroadPhasePairReport(object0, object1, p.usrData, i));
+
+			p.isNew = false;
+			p.isUpdated = false;
+			i++;
+		}
+		else if(p.isUpdated)
+		{
+			// Persistent pair
+
+			// PT: this pair already existed in the structure, and has been found again this frame. Since
+			// MBP reports "all pairs" each frame (as opposed to SAP), this happens quite often, for each
+			// active persistent pair.
+			p.isUpdated = false;
+			i++;
+		}
+		else
+		{
+			// Lost pair
+
+			// PT: if the pair is not new and not 'updated', it might be a lost (separated) pair. But this
+			// is not always the case since we now handle "sleeping" objects directly within MBP. A pair
+			// of sleeping objects does not generate an 'addPair' call, so it ends up in this codepath.
+			// Nonetheless the sleeping pair should not be deleted. We can only delete pairs involving
+			// objects that have been actually moved during the frame. This is the only case in which
+			// a pair can indeed become 'lost'.
+			const PxU32 id0 = p.id0;
+			const PxU32 id1 = p.id1;
+			PX_ASSERT(id0!=INVALID_ID);
+			PX_ASSERT(id1!=INVALID_ID);
+
+			const MBP_ObjectIndex index0 = decodeHandle_Index(id0);
+			const MBP_ObjectIndex index1 = decodeHandle_Index(id1);
+			// PT: if none of the involved objects have been updated, the pair is just sleeping: keep it and skip it.
+			if(updated.isSetChecked(index0) || updated.isSetChecked(index1))
+			{
+				// PT: by design (for better or worse) we do not report pairs to the client when
+				// one of the involved objects has been deleted. The pair must still be deleted
+				// from the MBP structure though.
+				if(!removed.isSetChecked(index0) && !removed.isSetChecked(index1))
+				{
+					// PT: doing the group-based filtering here is useless. The pair should not have
+					// been added in the first place.
+					const BpHandle object0 = objects[index0].mUserID;
+					const BpHandle object1 = objects[index1].mUserID;
+					mbp->mDeleted.pushBack(BroadPhasePairReport(object0, object1, /*p.usrData*/NULL, i));
+				}
+
+				const PxU32 hashValue = hash(id0, id1) & mMask;
+				removePair(id0, id1, hashValue, i);
+				nbActivePairs--;
+			}
+			else i++;
+		}
+	}
+
+	shrinkMemory();
+	return true;
+}
+
+#ifdef USE_SINGLE_THREADED_REFERENCE_CODE
+void MBP::prepareOverlaps()
+{
+	PxU32 nb = mNbRegions;
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	for(PxU32 i=0;i<nb;i++)
+		if(regions[i].mBP)
+			regions[i].mBP->prepareOverlaps();
+}
+#endif
+
+void MBP::prepareOverlapsMT()
+{
+	const PxU32 nb = mNbRegions;
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	for(PxU32 i=0;i<nb;i++)
+	{
+		if(regions[i].mBP)
+			regions[i].mBP->prepareOverlapsMT();
+	}
+}
+
+#ifdef USE_SINGLE_THREADED_REFERENCE_CODE
+void MBP::findOverlaps()
+{
+	/*static*/ MBPOS_TmpBuffers TmpBuffers;	// ####
+
+	PxU32 nb = mNbRegions;
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	for(PxU32 i=0;i<nb;i++)
+	{
+		if(regions[i].mBP)
+			regions[i].mBP->findOverlaps(mPairManager, TmpBuffers);
+	}
+}
+#endif
+
+void MBP::findOverlapsMT(const BpHandle* PX_RESTRICT groups)
+{
+	PxU32 nb = mNbRegions;
+	const RegionData* PX_RESTRICT regions = mRegions.begin();
+	const MBP_Object* objects = mMBP_Objects.begin();
+	for(PxU32 i=0;i<nb;i++)
+	{
+		if(regions[i].mBP)
+			regions[i].mBP->findOverlapsMT(mPairManager, groups, objects);
+	}
+}
+
+PxU32 MBP::finalize(BroadPhaseMBP* mbp)
+{
+	const MBP_Object* objects = mMBP_Objects.begin();
+	mPairManager.removeMarkedPairs(objects, mbp, mUpdatedObjects, mRemoved);
+
+	mUpdatedObjects.clearAll();
+
+	return mPairManager.mNbActivePairs;
+}
+
+void MBP::reset()
+{
+	PxU32 nb = mNbRegions;
+	RegionData* PX_RESTRICT regions = mRegions.begin();
+	while(nb--)
+	{
+//		printf("%d objects in region\n", regions->mBP->mNbObjects);
+		DELETESINGLE(regions->mBP);
+		regions++;
+	}
+
+	mNbPairs			= 0;
+	mNbRegions			= 0;
+	mFirstFreeIndex		= INVALID_ID;
+	mFirstFreeIndexBP	= INVALID_ID;
+	for(PxU32 i=0;i<MAX_NB_MBP+1;i++)
+	{
+		mHandles[i].clear();
+		mFirstFree[i] = INVALID_ID;
+	}
+
+	mRegions.clear();
+	mMBP_Objects.clear();
+	mPairManager.purge();
+	mUpdatedObjects.empty();
+	mRemoved.empty();
+	mOutOfBoundsObjects.clear();
+#ifdef USE_FULLY_INSIDE_FLAG
+	mFullyInsideBitmap.empty();
+#endif
+}
+
+void MBP::shiftOrigin(const PxVec3& shift)
+{
+	const PxU32 size = mNbRegions;
+	RegionData* PX_RESTRICT regions = mRegions.begin();
+	//
+	// regions
+	//
+	for(PxU32 i=0; i < size; i++)
+	{
+		if(regions[i].mBP)
+		{
+			MBP_AABB& box = regions[i].mBox;
+			PxBounds3 bounds;
+			box.decode(bounds);
+
+			bounds.minimum -= shift;
+			bounds.maximum -= shift;
+
+			box.initFrom2(bounds);
+		}
+	}
+
+	//
+	// object bounds
+	//
+	const PxU32 nbObjects = mMBP_Objects.size();
+	MBP_Object* objects = mMBP_Objects.begin();
+
+	for(PxU32 i=0; i < nbObjects; i++)
+	{
+		MBP_Object& obj = objects[i];
+
+		const PxU32 nbHandles = obj.mNbHandles;
+		if(nbHandles)
+		{
+			MBP_AABB bounds;
+			const PxBounds3 rawBounds = mTransientBounds[obj.mUserID];
+			PxVec3 c(mTransientContactDistance[obj.mUserID]);
+			const PxBounds3 decodedBounds(rawBounds.minimum - c, rawBounds.maximum + c);
+			bounds.initFrom2(decodedBounds);
+
+			RegionHandle* PX_RESTRICT handles = getHandles(obj, nbHandles);
+			for(PxU32 j=0; j < nbHandles; j++)
+			{
+				const RegionHandle& h = handles[j];
+				const RegionData& currentRegion = regions[h.mInternalBPHandle];
+				PX_ASSERT(currentRegion.mBP);
+				currentRegion.mBP->setBounds(h.mHandle, bounds);
+			}
+		}
+	}
+}
+
+void MBP::setTransientBounds(const PxBounds3* bounds, const PxReal* contactDistance)
+{
+	mTransientBounds = bounds;
+	mTransientContactDistance = contactDistance;
+}
+///////////////////////////////////////////////////////////////////////////////
+
+// Below is the PhysX wrapper = link between AABBManager and MBP
+
+#define DEFAULT_CREATED_DELETED_PAIRS_CAPACITY 1024
+
+BroadPhaseMBP::BroadPhaseMBP(PxU32 maxNbRegions,
+								PxU32 maxNbBroadPhaseOverlaps,
+								PxU32 maxNbStaticShapes,
+								PxU32 maxNbDynamicShapes) :
+	mMapping				(NULL),
+	mCapacity				(0),
+	mGroups					(NULL)
+{
+
+	mMBP = PX_NEW(MBP)();
+
+	const PxU32 nbObjects = maxNbStaticShapes + maxNbDynamicShapes;
+	mMBP->preallocate(maxNbRegions, nbObjects, maxNbBroadPhaseOverlaps);
+
+	if(nbObjects)
+		allocateMappingArray(nbObjects);
+
+	mCreated.reserve(DEFAULT_CREATED_DELETED_PAIRS_CAPACITY);
+	mDeleted.reserve(DEFAULT_CREATED_DELETED_PAIRS_CAPACITY);
+}
+
+BroadPhaseMBP::~BroadPhaseMBP()
+{
+	DELETESINGLE(mMBP);
+	PX_FREE(mMapping);
+}
+
+void BroadPhaseMBP::allocateMappingArray(PxU32 newCapacity)
+{
+	PX_ASSERT(newCapacity>mCapacity);
+	MBP_Handle* newMapping = reinterpret_cast<MBP_Handle*>(PX_ALLOC(sizeof(MBP_Handle)*newCapacity, "MBP"));
+	if(mCapacity)
+		PxMemCopy(newMapping, mMapping, mCapacity*sizeof(MBP_Handle));
+	for(PxU32 i=mCapacity;i<newCapacity;i++)
+		newMapping[i] = PX_INVALID_U32;
+	PX_FREE(mMapping);
+	mMapping = newMapping;
+	mCapacity = newCapacity;
+}
+
+bool BroadPhaseMBP::getCaps(PxBroadPhaseCaps& caps) const
+{
+	caps.maxNbRegions			= 256;
+	caps.maxNbObjects			= 0;
+	caps.needsPredefinedBounds	= true;
+	return true;
+}
+
+PxU32 BroadPhaseMBP::getNbRegions() const
+{
+	// PT: we need to count active regions here, as we only keep track of the total number of
+	// allocated regions internally - and some of which might have been removed.
+	const PxU32 size = mMBP->mNbRegions;
+/*	const RegionData* PX_RESTRICT regions = (const RegionData*)mMBP->mRegions.GetEntries();
+	PxU32 nbActiveRegions = 0;
+	for(PxU32 i=0;i<size;i++)
+	{
+		if(regions[i].mBP)
+			nbActiveRegions++;
+	}
+	return nbActiveRegions;*/
+	return size;
+}
+
+PxU32 BroadPhaseMBP::getRegions(PxBroadPhaseRegionInfo* userBuffer, PxU32 bufferSize, PxU32 startIndex) const
+{
+	const PxU32 size = mMBP->mNbRegions;
+	const RegionData* PX_RESTRICT regions = mMBP->mRegions.begin();
+	regions += startIndex;
+
+	const PxU32 writeCount = PxMin(size, bufferSize);
+	for(PxU32 i=0;i<writeCount;i++)
+	{
+		const MBP_AABB& box = regions[i].mBox;
+		box.decode(userBuffer[i].region.bounds);
+		if(regions[i].mBP)
+		{
+			PX_ASSERT(userBuffer[i].region.bounds.isValid());
+			userBuffer[i].region.userData	= regions[i].mUserData;
+			userBuffer[i].active			= true;
+			userBuffer[i].overlap			= regions[i].mOverlap!=0;
+			userBuffer[i].nbStaticObjects	= regions[i].mBP->mNbStaticBoxes;
+			userBuffer[i].nbDynamicObjects	= regions[i].mBP->mNbDynamicBoxes;
+		}
+		else
+		{
+			userBuffer[i].region.bounds.setEmpty();
+			userBuffer[i].region.userData	= NULL;
+			userBuffer[i].active			= false;
+			userBuffer[i].overlap			= false;
+			userBuffer[i].nbStaticObjects	= 0;
+			userBuffer[i].nbDynamicObjects	= 0;
+		}
+	}
+	return writeCount;
+}
+
+PxU32 BroadPhaseMBP::addRegion(const PxBroadPhaseRegion& region, bool populateRegion)
+{
+	return mMBP->addRegion(region, populateRegion);
+}
+
+bool BroadPhaseMBP::removeRegion(PxU32 handle)
+{
+	return mMBP->removeRegion(handle);
+}
+
+void BroadPhaseMBP::destroy()
+{
+	delete this;
+}
+
+void BroadPhaseMBP::update(const PxU32 numCpuTasks, PxcScratchAllocator* scratchAllocator, const BroadPhaseUpdateData& updateData, physx::PxBaseTask* continuation, physx::PxBaseTask* narrowPhaseUnblockTask)
+{
+#if PX_CHECKED
+	PX_CHECK_AND_RETURN(scratchAllocator, "BroadPhaseMBP::update - scratchAllocator must be non-NULL \n");
+#endif
+
+	if(narrowPhaseUnblockTask)
+		narrowPhaseUnblockTask->removeReference();
+
+	setUpdateData(updateData);
+
+	mMBPPostUpdateWorkTask.setScratchAllocator(scratchAllocator);
+	mMBPUpdateWorkTask.setScratchAllocator(scratchAllocator);
+
+	mMBPPostUpdateWorkTask.setBroadphase(this);
+	mMBPUpdateWorkTask.setBroadphase(this);
+
+	mMBPPostUpdateWorkTask.setNumCpuTasks(numCpuTasks);
+	mMBPUpdateWorkTask.setNumCpuTasks(numCpuTasks);
+
+	mMBPPostUpdateWorkTask.setContinuation(continuation);
+	mMBPUpdateWorkTask.setContinuation(&mMBPPostUpdateWorkTask);
+
+	mMBPPostUpdateWorkTask.removeReference();
+	mMBPUpdateWorkTask.removeReference();
+}
+
+void BroadPhaseMBP::setUpdateData(const BroadPhaseUpdateData& updateData)
+{
+	mMBP->setTransientBounds(updateData.getAABBs(), updateData.getContactDistance());
+
+	const PxU32 newCapacity = updateData.getCapacity();
+	if(newCapacity>mCapacity)
+		allocateMappingArray(newCapacity);
+
+#if PX_CHECKED
+	if(!BroadPhaseUpdateData::isValid(updateData, *this))
+	{
+		PX_CHECK_MSG(false, "Illegal BroadPhaseUpdateData \n");
+		return;
+	}
+#endif
+
+	const PxBounds3* PX_RESTRICT boundsXYZ = updateData.getAABBs();
+	const BpHandle* PX_RESTRICT groups = updateData.getGroups();	// ### why are those 'handles'?
+	mGroups = groups;
+
+	// ### TODO: handle groups inside MBP
+	// ### TODO: get rid of AABB conversions
+
+	const BpHandle* removed = updateData.getRemovedHandles();
+	if(removed)
+	{
+		PxU32 nbToGo = updateData.getNumRemovedHandles();
+		while(nbToGo--)
+		{
+			const BpHandle index = *removed++;
+			PX_ASSERT(index<mCapacity);
+
+			const bool status = mMBP->removeObject(mMapping[index]);
+			PX_ASSERT(status);
+			PX_UNUSED(status);
+
+			mMapping[index] = PX_INVALID_U32;
+		}
+	}
+
+	const BpHandle* created = updateData.getCreatedHandles();
+	if(created)
+	{
+		PxU32 nbToGo = updateData.getNumCreatedHandles();
+		while(nbToGo--)
+		{
+			const BpHandle index = *created++;
+			PX_ASSERT(index<mCapacity);
+
+			const PxU32 group = groups[index];
+			const bool isStatic = group==FilterGroup::eSTATICS;
+
+			IntegerAABB bounds(boundsXYZ[index], updateData.getContactDistance()[index]);
+			MBP_AABB aabb;
+			aabb.mMinX	= bounds.mMinMax[IntegerAABB::MIN_X]>>1;
+			aabb.mMinY	= bounds.mMinMax[IntegerAABB::MIN_Y]>>1;
+			aabb.mMinZ	= bounds.mMinMax[IntegerAABB::MIN_Z]>>1;
+			aabb.mMaxX	= bounds.mMinMax[IntegerAABB::MAX_X]>>1;
+			aabb.mMaxY	= bounds.mMinMax[IntegerAABB::MAX_Y]>>1;
+			aabb.mMaxZ	= bounds.mMinMax[IntegerAABB::MAX_Z]>>1;
+
+/*			aabb.mMinX	&= ~1;
+			aabb.mMinY	&= ~1;
+			aabb.mMinZ	&= ~1;
+			aabb.mMaxX	|= 1;
+			aabb.mMaxY	|= 1;
+			aabb.mMaxZ	|= 1;*/
+
+#if PX_DEBUG
+			PxBounds3 decodedBox;
+			PxU32* bin = reinterpret_cast<PxU32*>(&decodedBox.minimum.x);
+			bin[0] = decodeFloat(bounds.mMinMax[IntegerAABB::MIN_X]);
+			bin[1] = decodeFloat(bounds.mMinMax[IntegerAABB::MIN_Y]);
+			bin[2] = decodeFloat(bounds.mMinMax[IntegerAABB::MIN_Z]);
+			bin[3] = decodeFloat(bounds.mMinMax[IntegerAABB::MAX_X]);
+			bin[4] = decodeFloat(bounds.mMinMax[IntegerAABB::MAX_Y]);
+			bin[5] = decodeFloat(bounds.mMinMax[IntegerAABB::MAX_Z]);
+
+			MBP_AABB PrunerBox;
+			PrunerBox.initFrom2(decodedBox);
+			PX_ASSERT(PrunerBox.mMinX==aabb.mMinX);
+			PX_ASSERT(PrunerBox.mMinY==aabb.mMinY);
+			PX_ASSERT(PrunerBox.mMinZ==aabb.mMinZ);
+			PX_ASSERT(PrunerBox.mMaxX==aabb.mMaxX);
+			PX_ASSERT(PrunerBox.mMaxY==aabb.mMaxY);
+			PX_ASSERT(PrunerBox.mMaxZ==aabb.mMaxZ);
+#endif
+
+			const MBP_Handle mbpHandle = mMBP->addObject(aabb, index, isStatic);
+			mMapping[index] = mbpHandle;
+		}
+	}
+
+	const BpHandle* updated = updateData.getUpdatedHandles();
+	if(updated)
+	{
+		PxU32 nbToGo = updateData.getNumUpdatedHandles();
+		while(nbToGo--)
+		{
+			const BpHandle index = *updated++;
+			PX_ASSERT(index<mCapacity);
+
+			MBP_AABB aabb;
+			IntegerAABB bounds(boundsXYZ[index], updateData.getContactDistance()[index]);
+
+			aabb.mMinX	= (bounds.mMinMax[IntegerAABB::MIN_X])>>1;
+			aabb.mMinY	= (bounds.mMinMax[IntegerAABB::MIN_Y])>>1;
+			aabb.mMinZ	= (bounds.mMinMax[IntegerAABB::MIN_Z])>>1;
+			aabb.mMaxX	= (bounds.mMinMax[IntegerAABB::MAX_X])>>1;
+			aabb.mMaxY	= (bounds.mMinMax[IntegerAABB::MAX_Y])>>1;
+			aabb.mMaxZ	= (bounds.mMinMax[IntegerAABB::MAX_Z])>>1;
+
+/*			aabb.mMinX	&= ~1;
+			aabb.mMinY	&= ~1;
+			aabb.mMinZ	&= ~1;
+			aabb.mMaxX	|= 1;
+			aabb.mMaxY	|= 1;
+			aabb.mMaxZ	|= 1;*/
+
+/*			PxBounds3 aabb;
+			PxU32* bin = (PxU32*)&aabb.minimum.x;
+			bin[0] = IntegerAABB::decodeFloat(boundsX[index*2]);
+			bin[1] = IntegerAABB::decodeFloat(boundsY[index*2]);
+			bin[2] = IntegerAABB::decodeFloat(boundsZ[index*2]);
+			bin[3] = IntegerAABB::decodeFloat(boundsX[index*2+1]);
+			bin[4] = IntegerAABB::decodeFloat(boundsY[index*2+1]);
+			bin[5] = IntegerAABB::decodeFloat(boundsZ[index*2+1]);*/
+
+			const bool status = mMBP->updateObject(mMapping[index], aabb);
+			PX_ASSERT(status);
+			PX_UNUSED(status);
+		}
+	}
+
+	PX_ASSERT(!mCreated.size());
+	PX_ASSERT(!mDeleted.size());
+
+#ifdef USE_SINGLE_THREADED_REFERENCE_CODE
+	mMBP->prepareOverlaps();
+	mMBP->findOverlaps();
+#else
+	mMBP->prepareOverlapsMT();
+#endif
+}
+
+void BroadPhaseMBP::update(physx::PxBaseTask* /*continuation*/)
+{
+#ifndef USE_SINGLE_THREADED_REFERENCE_CODE
+	#ifdef CHECK_NB_OVERLAPS
+	gNbOverlaps = 0;
+	#endif
+	mMBP->findOverlapsMT(mGroups);
+	#ifdef CHECK_NB_OVERLAPS
+	printf("PPU: %d overlaps\n", gNbOverlaps);
+	#endif
+#endif
+}
+
+void BroadPhaseMBP::postUpdate(physx::PxBaseTask* /*continuation*/)
+{
+#ifndef USE_SINGLE_THREADED_REFERENCE_CODE
+	{
+		PxU32 Nb = mMBP->mNbRegions;
+		const RegionData* PX_RESTRICT regions = mMBP->mRegions.begin();
+		for(PxU32 i=0;i<Nb;i++)
+		{
+			if(regions[i].mBP)
+				regions[i].mBP->mNbUpdatedBoxes = 0;
+		}
+	}
+
+	mMBP->finalize(this);
+#endif
+}
+
+PxU32 BroadPhaseMBP::getNbCreatedPairs() const
+{
+	return mCreated.size();
+}
+
+BroadPhasePairReport* BroadPhaseMBP::getCreatedPairs()
+{
+	return mCreated.begin();
+}
+
+PxU32 BroadPhaseMBP::getNbDeletedPairs() const
+{
+	return mDeleted.size();
+}
+
+BroadPhasePairReport* BroadPhaseMBP::getDeletedPairs()
+{
+	return mDeleted.begin();
+}
+
+PxU32 BroadPhaseMBP::getNbOutOfBoundsObjects() const
+{
+	return mMBP->mOutOfBoundsObjects.size();
+}
+
+const PxU32* BroadPhaseMBP::getOutOfBoundsObjects() const
+{
+	return mMBP->mOutOfBoundsObjects.begin();
+}
+
+static void freeBuffer(Ps::Array<BroadPhasePairReport>& buffer)
+{
+	const PxU32 size = buffer.size();
+	if(size>DEFAULT_CREATED_DELETED_PAIRS_CAPACITY)
+	{
+		buffer.reset();
+		buffer.reserve(DEFAULT_CREATED_DELETED_PAIRS_CAPACITY);
+	}
+	else
+	{
+		buffer.clear();
+	}
+}
+
+void BroadPhaseMBP::freeBuffers()
+{
+	mMBP->freeBuffers();
+	freeBuffer(mCreated);
+	freeBuffer(mDeleted);
+}
+
+#if PX_CHECKED
+bool BroadPhaseMBP::isValid(const BroadPhaseUpdateData& updateData) const
+{
+	const BpHandle* created = updateData.getCreatedHandles();
+	if(created)
+	{
+		PxU32 nbToGo = updateData.getNumCreatedHandles();
+		while(nbToGo--)
+		{
+			const BpHandle index = *created++;
+			PX_ASSERT(index<mCapacity);
+
+			PxU32 nbObjects = mMBP->mMBP_Objects.size();
+			const MBP_Object* PX_RESTRICT objects = mMBP->mMBP_Objects.begin();
+			while(nbObjects--)
+			{
+				if(!(objects->mFlags & MBP_REMOVED))
+				{
+					if(objects->mUserID==index)
+						return false;	// This object has been added already
+				}
+				objects++;
+			}
+		}
+	}
+
+	const BpHandle* updated = updateData.getUpdatedHandles();
+	if(updated)
+	{
+		PxU32 nbToGo = updateData.getNumUpdatedHandles();
+		while(nbToGo--)
+		{
+			const BpHandle index = *updated++;
+			PX_ASSERT(index<mCapacity);
+
+			if(mMapping[index]==PX_INVALID_U32)
+				return false;	// This object has been removed already, or never been added
+		}
+	}
+
+	const BpHandle* removed = updateData.getRemovedHandles();
+	if(removed)
+	{
+		PxU32 nbToGo = updateData.getNumRemovedHandles();
+		while(nbToGo--)
+		{
+			const BpHandle index = *removed++;
+			PX_ASSERT(index<mCapacity);
+
+			if(mMapping[index]==PX_INVALID_U32)
+				return false;	// This object has been removed already, or never been added
+		}
+	}
+	return true;
+}
+#endif
+
+
+void BroadPhaseMBP::shiftOrigin(const PxVec3& shift)
+{
+	mMBP->shiftOrigin(shift);
+}
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.h b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.h
new file mode 100644
index 00000000..f7122443
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBP.h
@@ -0,0 +1,118 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef BP_BROADPHASE_MBP_H
+#define BP_BROADPHASE_MBP_H
+
+#include "CmPhysXCommon.h"
+#include "BpBroadPhase.h"
+#include "BpBroadPhaseMBPCommon.h"
+#include "PsUserAllocated.h"
+#include "BpMBPTasks.h"
+
+	class MBP;
+	
+
+namespace physx
+{
+
+namespace Bp
+{
+	class BPDefaultMemoryAllocator;
+
+	class BroadPhaseMBP : public BroadPhase, public Ps::UserAllocated
+	{
+											PX_NOCOPY(BroadPhaseMBP)
+		public:
+											BroadPhaseMBP(PxU32 maxNbRegions,
+															PxU32 maxNbBroadPhaseOverlaps,
+															PxU32 maxNbStaticShapes,
+															PxU32 maxNbDynamicShapes);
+		virtual								~BroadPhaseMBP();
+
+	// BroadPhaseBase
+		virtual	bool						getCaps(PxBroadPhaseCaps& caps)														const;
+		virtual	PxU32						getNbRegions()																		const;
+		virtual	PxU32						getRegions(PxBroadPhaseRegionInfo* userBuffer, PxU32 bufferSize, PxU32 startIndex=0) const;
+		virtual	PxU32						addRegion(const PxBroadPhaseRegion& region, bool populateRegion);
+		virtual	bool						removeRegion(PxU32 handle);
+		virtual	PxU32						getNbOutOfBoundsObjects()	const;
+		virtual	const PxU32*				getOutOfBoundsObjects()		const;
+	//~BroadPhaseBase
+
+	// BroadPhase
+		virtual	PxBroadPhaseType::Enum			getType()					const	{ return PxBroadPhaseType::eMBP;	}
+
+		virtual	void							destroy();
+
+		virtual	void							update(const PxU32 numCpuTasks, PxcScratchAllocator* scratchAllocator, const BroadPhaseUpdateData& updateData, physx::PxBaseTask* continuation, physx::PxBaseTask* narrowPhaseUnblockTask);
+
+		virtual	PxU32							getNbCreatedPairs()		const;
+		virtual BroadPhasePairReport*			getCreatedPairs();
+		virtual PxU32							getNbDeletedPairs()		const;
+		virtual BroadPhasePairReport*			getDeletedPairs();
+
+		virtual void							freeBuffers();
+
+		virtual void							shiftOrigin(const PxVec3& shift);
+
+#if PX_CHECKED
+		virtual bool							isValid(const BroadPhaseUpdateData& updateData)	const;
+#endif
+
+		virtual BroadPhasePair*				getBroadPhasePairs() const  {return NULL;}  //KS - TODO - implement this!!!
+
+		virtual void						deletePairs(){}								//KS - TODO - implement this!!!
+
+		
+	//~BroadPhase
+
+				MBPUpdateWorkTask			mMBPUpdateWorkTask;
+				MBPPostUpdateWorkTask		mMBPPostUpdateWorkTask;
+
+				MBP*						mMBP;		// PT: TODO: aggregate
+
+				MBP_Handle*					mMapping;
+				PxU32						mCapacity;
+				Ps::Array<BroadPhasePairReport>	mCreated;
+				Ps::Array<BroadPhasePairReport>	mDeleted;
+
+				const BpHandle*						mGroups;	// ### why are those 'handles'?
+
+				void						setUpdateData(const BroadPhaseUpdateData& updateData);
+				void						update(physx::PxBaseTask* continuation);
+				void						postUpdate(physx::PxBaseTask* continuation);
+				void						allocateMappingArray(PxU32 newCapacity);
+	};
+
+} //namespace Bp
+
+} //namespace physx
+
+#endif // BP_BROADPHASE_MBP_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBPCommon.h b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBPCommon.h
new file mode 100644
index 00000000..cf5605b6
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseMBPCommon.h
@@ -0,0 +1,252 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef BP_BROADPHASE_MBP_COMMON_H
+#define BP_BROADPHASE_MBP_COMMON_H
+
+#include "PxPhysXConfig.h"
+#include "BpBroadPhaseUpdate.h"
+#include "PsUserAllocated.h"
+
+namespace physx
+{
+namespace Bp
+{
+
+#define MBP_USE_WORDS
+#define MBP_USE_SENTINELS		// Probably doesn't work with 16bits boxes
+#define MBP_USE_NO_CMP_OVERLAP
+//#define MBP_USE_NO_CMP_OVERLAP_3D	// Seems slower
+#if PX_INTEL_FAMILY
+	#define MBP_SIMD_OVERLAP
+#endif
+
+#ifdef MBP_USE_WORDS
+	typedef	PxU16	MBP_Index;
+#else
+	typedef	PxU32	MBP_Index;
+#endif
+	typedef	PxU32	MBP_ObjectIndex;	// PT: index in mMBP_Objects
+	typedef	PxU32	MBP_Handle;			// PT: returned to MBP users, combination of index/flip-flop/static-bit
+
+	PX_FORCE_INLINE MBP_Handle encodeHandle(MBP_ObjectIndex objectIndex, PxU32 flipFlop, bool isStatic)
+	{
+	/*	objectIndex += objectIndex;
+		objectIndex |= flipFlop;
+		return objectIndex;*/
+		return (objectIndex<<2)|(flipFlop<<1)|PxU32(isStatic);
+	}
+
+	PX_FORCE_INLINE MBP_ObjectIndex decodeHandle_Index(MBP_Handle handle)
+	{
+	//	return handle>>1;
+		return handle>>2;
+	}
+
+	PX_FORCE_INLINE PxU32 decodeHandle_IsStatic(MBP_Handle handle)
+	{
+		return handle&1;
+	}
+
+	struct MBPEntry_Data
+	{
+		#if PX_DEBUG
+			bool	mUpdated;
+		#endif
+
+		// ### mIndex could be PxU16 but beware, we store mFirstFree there
+		PxU32		mIndex;			// Out-to-in, maps user handle to internal array. mIndex indexes either the static or dynamic array.
+		MBP_Handle	mMBPHandle;		// MBP-level handle (the one returned to users)
+
+		PX_FORCE_INLINE		PxU32	isStatic()	const
+		{
+			return decodeHandle_IsStatic(mMBPHandle);
+		}
+	};
+
+//	#define MBP_MAX_NB_OVERLAPS	4096
+	#define MBP_MAX_NB_OVERLAPS	8192
+	struct MBP_Overlap
+	{
+		PxU16	mIndex0;
+		PxU16	mIndex1;
+	};
+
+//	#define MBP_BOX_CACHE_SIZE	(16)
+//	#define MBP_BOX_CACHE_SIZE	(16*2)
+//	#define MBP_BOX_CACHE_SIZE	(16*4)
+//	#define MBP_BOX_CACHE_SIZE	(16*8)
+//	#define MBP_BOX_CACHE_SIZE	(16*16)
+	#define MBP_BOX_CACHE_SIZE	(16*32)
+//	#define MBP_BOX_CACHE_SIZE	(16*64)
+
+	struct IAABB : public Ps::UserAllocated
+	{
+		PX_FORCE_INLINE bool isInside(const IAABB& box) const
+		{
+			if(box.mMinX>mMinX)	return false;
+			if(box.mMinY>mMinY)	return false;
+			if(box.mMinZ>mMinZ)	return false;
+			if(box.mMaxX<mMaxX)	return false;
+			if(box.mMaxY<mMaxY)	return false;
+			if(box.mMaxZ<mMaxZ)	return false;
+			return true;
+		}
+
+		PX_FORCE_INLINE			Ps::IntBool		intersects(const IAABB& a)	const
+		{
+			if(mMaxX < a.mMinX || a.mMaxX < mMinX
+			|| mMaxY < a.mMinY || a.mMaxY < mMinY
+			|| mMaxZ < a.mMinZ || a.mMaxZ < mMinZ
+			)
+				return Ps::IntFalse;
+			return Ps::IntTrue;
+		}
+
+		PX_FORCE_INLINE			Ps::IntBool		intersectNoTouch(const IAABB& a)	const
+		{
+			if(mMaxX <= a.mMinX || a.mMaxX <= mMinX
+			|| mMaxY <= a.mMinY || a.mMaxY <= mMinY
+			|| mMaxZ <= a.mMinZ || a.mMaxZ <= mMinZ
+			)
+				return Ps::IntFalse;
+			return Ps::IntTrue;
+		}
+
+		PX_FORCE_INLINE	void	initFrom2(const PxBounds3& box)
+		{
+			const PxU32* PX_RESTRICT binary = reinterpret_cast<const PxU32*>(&box.minimum.x);
+			mMinX = encodeFloat(binary[0])>>1;
+			mMinY = encodeFloat(binary[1])>>1;
+			mMinZ = encodeFloat(binary[2])>>1;
+			mMaxX = encodeFloat(binary[3])>>1;
+			mMaxY = encodeFloat(binary[4])>>1;
+			mMaxZ = encodeFloat(binary[5])>>1;
+		}
+
+		PX_FORCE_INLINE	void	decode(PxBounds3& box)	const
+		{
+			PxU32* PX_RESTRICT binary = reinterpret_cast<PxU32*>(&box.minimum.x);
+			binary[0] = decodeFloat(mMinX<<1);
+			binary[1] = decodeFloat(mMinY<<1);
+			binary[2] = decodeFloat(mMinZ<<1);
+			binary[3] = decodeFloat(mMaxX<<1);
+			binary[4] = decodeFloat(mMaxY<<1);
+			binary[5] = decodeFloat(mMaxZ<<1);
+		}
+
+		PX_FORCE_INLINE PxU32	getMin(PxU32 i)	const	{	return (&mMinX)[i];	}
+		PX_FORCE_INLINE PxU32	getMax(PxU32 i)	const	{	return (&mMaxX)[i];	}
+
+		PxU32 mMinX;
+		PxU32 mMinY;
+		PxU32 mMinZ;
+		PxU32 mMaxX;
+		PxU32 mMaxY;
+		PxU32 mMaxZ;
+	};
+
+	struct SIMD_AABB : public Ps::UserAllocated
+	{
+		PX_FORCE_INLINE	void	initFrom(const PxBounds3& box)
+		{
+			const PxU32* PX_RESTRICT binary = reinterpret_cast<const PxU32*>(&box.minimum.x);
+			mMinX = encodeFloat(binary[0]);
+			mMinY = encodeFloat(binary[1]);
+			mMinZ = encodeFloat(binary[2]);
+			mMaxX = encodeFloat(binary[3]);
+			mMaxY = encodeFloat(binary[4]);
+			mMaxZ = encodeFloat(binary[5]);
+		}
+
+		PX_FORCE_INLINE	void	initFrom2(const PxBounds3& box)
+		{
+			const PxU32* PX_RESTRICT binary = reinterpret_cast<const PxU32*>(&box.minimum.x);
+			mMinX = encodeFloat(binary[0])>>1;
+			mMinY = encodeFloat(binary[1])>>1;
+			mMinZ = encodeFloat(binary[2])>>1;
+			mMaxX = encodeFloat(binary[3])>>1;
+			mMaxY = encodeFloat(binary[4])>>1;
+			mMaxZ = encodeFloat(binary[5])>>1;
+		}
+
+		PX_FORCE_INLINE	void	decode(PxBounds3& box)	const
+		{
+			PxU32* PX_RESTRICT binary = reinterpret_cast<PxU32*>(&box.minimum.x);
+			binary[0] = decodeFloat(mMinX<<1);
+			binary[1] = decodeFloat(mMinY<<1);
+			binary[2] = decodeFloat(mMinZ<<1);
+			binary[3] = decodeFloat(mMaxX<<1);
+			binary[4] = decodeFloat(mMaxY<<1);
+			binary[5] = decodeFloat(mMaxZ<<1);
+		}
+
+		PX_FORCE_INLINE bool isInside(const SIMD_AABB& box) const
+		{
+			if(box.mMinX>mMinX)	return false;
+			if(box.mMinY>mMinY)	return false;
+			if(box.mMinZ>mMinZ)	return false;
+			if(box.mMaxX<mMaxX)	return false;
+			if(box.mMaxY<mMaxY)	return false;
+			if(box.mMaxZ<mMaxZ)	return false;
+			return true;
+		}
+
+		PX_FORCE_INLINE			Ps::IntBool		intersects(const SIMD_AABB& a)	const
+		{
+			if(mMaxX < a.mMinX || a.mMaxX < mMinX
+			|| mMaxY < a.mMinY || a.mMaxY < mMinY
+			|| mMaxZ < a.mMinZ || a.mMaxZ < mMinZ
+			)
+				return Ps::IntFalse;
+			return Ps::IntTrue;
+		}
+
+		PX_FORCE_INLINE			Ps::IntBool		intersectNoTouch(const SIMD_AABB& a)	const
+		{
+			if(mMaxX <= a.mMinX || a.mMaxX <= mMinX
+			|| mMaxY <= a.mMinY || a.mMaxY <= mMinY
+			|| mMaxZ <= a.mMinZ || a.mMaxZ <= mMinZ
+			)
+				return Ps::IntFalse;
+			return Ps::IntTrue;
+		}
+
+		PxU32 mMinX;
+		PxU32 mMaxX;
+		PxU32 mMinY;
+		PxU32 mMinZ;
+		PxU32 mMaxY;
+		PxU32 mMaxZ;
+	};
+
+}
+} // namespace physx
+
+#endif // BP_BROADPHASE_MBP_COMMON_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.cpp
new file mode 100644
index 00000000..69c49d8c
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.cpp
@@ -0,0 +1,2075 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "foundation/PxProfiler.h"
+#include "foundation/PxMath.h"
+#include "CmPhysXCommon.h"
+#include "CmTmpMem.h"
+#include "PxcScratchAllocator.h"
+#include "PxSceneDesc.h"
+#include "BpBroadPhaseSap.h"
+#include "BpBroadPhaseSapAux.h"
+#include "CmRadixSortBuffered.h"
+#include "PsFoundation.h"
+#include "PsAllocator.h"
+
+namespace physx
+{
+
+namespace Bp
+{
+
+#define DEFAULT_DATA_ARRAY_CAPACITY 1024
+#define DEFAULT_CREATEDDELETED_PAIR_ARRAY_CAPACITY 64
+#define DEFAULT_CREATEDDELETED1AXIS_CAPACITY 8192
+
+BroadPhaseSap::BroadPhaseSap(
+	const PxU32 maxNbBroadPhaseOverlaps,
+	const PxU32 maxNbStaticShapes,
+	const PxU32 maxNbDynamicShapes,
+	PxU64 contextID) :
+	mScratchAllocator	(NULL),
+	mContextID			(contextID)
+{
+	//Boxes
+	mBoxesSize=0;
+	mBoxesSizePrev=0;
+	mBoxesCapacity = (((maxNbStaticShapes + maxNbDynamicShapes) + 31) & ~31);
+	mBoxEndPts[0] = reinterpret_cast<SapBox1D*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(SapBox1D)*mBoxesCapacity)), "SapBox1D"));
+	mBoxEndPts[1] = reinterpret_cast<SapBox1D*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(SapBox1D)*mBoxesCapacity)), "SapBox1D"));
+	mBoxEndPts[2] = reinterpret_cast<SapBox1D*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(SapBox1D)*mBoxesCapacity)), "SapBox1D"));
+	for(PxU32 i=0; i<mBoxesCapacity;i++)
+	{
+		mBoxEndPts[0][i].mMinMax[0]=BP_INVALID_BP_HANDLE;
+		mBoxEndPts[0][i].mMinMax[1]=BP_INVALID_BP_HANDLE;
+		mBoxEndPts[1][i].mMinMax[0]=BP_INVALID_BP_HANDLE;
+		mBoxEndPts[1][i].mMinMax[1]=BP_INVALID_BP_HANDLE;
+		mBoxEndPts[2][i].mMinMax[0]=BP_INVALID_BP_HANDLE;
+		mBoxEndPts[2][i].mMinMax[1]=BP_INVALID_BP_HANDLE;
+	}
+		
+	//End points
+	mEndPointsCapacity = mBoxesCapacity*2 + NUM_SENTINELS;
+
+	mBoxesUpdated = reinterpret_cast<PxU8*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(PxU8)*mBoxesCapacity)), "BoxesUpdated"));
+	mSortedUpdateElements = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*mEndPointsCapacity)), "SortedUpdateElements"));
+	mActivityPockets = reinterpret_cast<BroadPhaseActivityPocket*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BroadPhaseActivityPocket)*mEndPointsCapacity)), "BroadPhaseActivityPocket"));
+
+	mEndPointValues[0] = reinterpret_cast<ValType*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(ValType)*(mEndPointsCapacity))), "ValType"));
+	mEndPointValues[1] = reinterpret_cast<ValType*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(ValType)*(mEndPointsCapacity))), "ValType"));
+	mEndPointValues[2] = reinterpret_cast<ValType*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(ValType)*(mEndPointsCapacity))), "ValType"));
+	mEndPointDatas[0] = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*(mEndPointsCapacity))), "BpHandle"));
+	mEndPointDatas[1] = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*(mEndPointsCapacity))), "BpHandle"));
+	mEndPointDatas[2]=  reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*(mEndPointsCapacity))), "BpHandle"));
+
+	// Initialize sentinels
+	setMinSentinel(mEndPointValues[0][0],mEndPointDatas[0][0]);
+	setMaxSentinel(mEndPointValues[0][1],mEndPointDatas[0][1]);
+	setMinSentinel(mEndPointValues[1][0],mEndPointDatas[1][0]);
+	setMaxSentinel(mEndPointValues[1][1],mEndPointDatas[1][1]);
+	setMinSentinel(mEndPointValues[2][0],mEndPointDatas[2][0]);
+	setMaxSentinel(mEndPointValues[2][1],mEndPointDatas[2][1]);
+
+	mListNext = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*mEndPointsCapacity)), "NextList"));
+	mListPrev = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*mEndPointsCapacity)), "PrevList"));
+
+	for(PxU32 a = 1; a < mEndPointsCapacity; ++a)
+	{
+		mListNext[a-1] = BpHandle(a);
+		mListPrev[a] = BpHandle(a-1);
+	}
+	mListNext[mEndPointsCapacity-1] = BpHandle(mEndPointsCapacity-1);
+	mListPrev[0] = 0;
+
+	mDefaultPairsCapacity = PxMax(maxNbBroadPhaseOverlaps, PxU32(DEFAULT_CREATEDDELETED_PAIR_ARRAY_CAPACITY));
+
+	mPairs.init(mDefaultPairsCapacity);
+
+	mBatchUpdateTasks[2].set(this,2);
+	mBatchUpdateTasks[1].set(this,1);
+	mBatchUpdateTasks[0].set(this,0);
+	mBatchUpdateTasks[2].setPairs(NULL, 0);
+	mBatchUpdateTasks[1].setPairs(NULL, 0);
+	mBatchUpdateTasks[0].setPairs(NULL, 0);
+
+	//Initialise data array.
+	mData = NULL;
+	mDataSize = 0;
+	mDataCapacity = 0;
+
+	//Initialise pairs arrays.
+	mCreatedPairsArray = NULL;
+	mCreatedPairsCapacity = 0;
+	mCreatedPairsSize = 0;
+	mDeletedPairsArray = NULL;
+	mDeletedPairsCapacity = 0;
+	mDeletedPairsSize = 0;
+	mActualDeletedPairSize = 0;
+}
+
+BroadPhaseSap::~BroadPhaseSap()
+{
+	PX_FREE(mBoxEndPts[0]);
+	PX_FREE(mBoxEndPts[1]);
+	PX_FREE(mBoxEndPts[2]);
+
+	PX_FREE(mEndPointValues[0]);
+	PX_FREE(mEndPointValues[1]);
+	PX_FREE(mEndPointValues[2]);
+	PX_FREE(mEndPointDatas[0]);
+	PX_FREE(mEndPointDatas[1]);
+	PX_FREE(mEndPointDatas[2]);
+
+	PX_FREE(mListNext);
+	PX_FREE(mListPrev);
+
+	PX_FREE(mSortedUpdateElements);
+	PX_FREE(mActivityPockets);
+	PX_FREE(mBoxesUpdated);
+
+	mPairs.release();
+
+	mBatchUpdateTasks[0].setPairs(NULL, 0);
+	mBatchUpdateTasks[1].setPairs(NULL, 0);
+	mBatchUpdateTasks[2].setPairs(NULL, 0);
+
+	mData = NULL;
+
+	mCreatedPairsArray = NULL;
+	mDeletedPairsArray = NULL;
+	
+}
+
+void BroadPhaseSap::destroy()
+{
+	this->~BroadPhaseSap();
+	PX_FREE(this);
+}
+
+void BroadPhaseSap::resizeBuffers()
+{
+	const PxU32 defaultPairsCapacity = mDefaultPairsCapacity;
+
+	mCreatedPairsArray = reinterpret_cast<BroadPhasePairReport*>(mScratchAllocator->alloc(sizeof(BroadPhasePairReport)*defaultPairsCapacity, true));
+	mCreatedPairsCapacity = defaultPairsCapacity;
+	mCreatedPairsSize = 0;
+
+	mDeletedPairsArray = reinterpret_cast<BroadPhasePairReport*>(mScratchAllocator->alloc(sizeof(BroadPhasePairReport)*defaultPairsCapacity, true));
+	mDeletedPairsCapacity = defaultPairsCapacity;
+	mDeletedPairsSize = 0;
+
+	mData = reinterpret_cast<BpHandle*>(mScratchAllocator->alloc(sizeof(BpHandle)*defaultPairsCapacity, true));
+	mDataCapacity = defaultPairsCapacity;
+	mDataSize = 0;
+
+	mBatchUpdateTasks[0].setPairs(reinterpret_cast<BroadPhasePair*>(mScratchAllocator->alloc(sizeof(BroadPhasePair)*defaultPairsCapacity, true)), defaultPairsCapacity);
+	mBatchUpdateTasks[0].setNumPairs(0);
+	mBatchUpdateTasks[1].setPairs(reinterpret_cast<BroadPhasePair*>(mScratchAllocator->alloc(sizeof(BroadPhasePair)*defaultPairsCapacity, true)), defaultPairsCapacity);
+	mBatchUpdateTasks[1].setNumPairs(0);
+	mBatchUpdateTasks[2].setPairs(reinterpret_cast<BroadPhasePair*>(mScratchAllocator->alloc(sizeof(BroadPhasePair)*defaultPairsCapacity, true)), defaultPairsCapacity);
+	mBatchUpdateTasks[2].setNumPairs(0);
+}
+
+void BroadPhaseSap::freeBuffers()
+{
+	if(mCreatedPairsArray) mScratchAllocator->free(mCreatedPairsArray);
+	mCreatedPairsArray = NULL;
+	mCreatedPairsSize = 0;
+	mCreatedPairsCapacity = 0;
+
+	if(mDeletedPairsArray) mScratchAllocator->free(mDeletedPairsArray);
+	mDeletedPairsArray = NULL;
+	mDeletedPairsSize = 0;
+	mDeletedPairsCapacity = 0;
+	mActualDeletedPairSize = 0;
+
+	if(mData) mScratchAllocator->free(mData);
+	mData = NULL;
+	mDataSize = 0;
+	mDataCapacity = 0;
+
+	if(mBatchUpdateTasks[0].getPairs()) mScratchAllocator->free(mBatchUpdateTasks[0].getPairs());
+	mBatchUpdateTasks[0].setPairs(NULL, 0);
+	mBatchUpdateTasks[0].setNumPairs(0);
+	if(mBatchUpdateTasks[1].getPairs()) mScratchAllocator->free(mBatchUpdateTasks[1].getPairs());
+	mBatchUpdateTasks[1].setPairs(NULL, 0);
+	mBatchUpdateTasks[1].setNumPairs(0);
+	if(mBatchUpdateTasks[2].getPairs()) mScratchAllocator->free(mBatchUpdateTasks[2].getPairs());
+	mBatchUpdateTasks[2].setPairs(NULL, 0);
+	mBatchUpdateTasks[2].setNumPairs(0);
+
+	//Shrink pair manager buffers it they are larger than needed but only let them shrink to a minimum size.
+	mPairs.shrinkMemory();
+}
+
+PX_FORCE_INLINE static void shiftCoord3(const ValType val0, const BpHandle handle0,
+										const ValType val1, const BpHandle handle1,
+										const ValType val2, const BpHandle handle2,
+										const PxF32* shift, ValType& oVal0, ValType& oVal1, ValType& oVal2)
+{
+	PX_ASSERT(!isSentinel(handle0));
+	PX_ASSERT(!isSentinel(handle1));
+	PX_ASSERT(!isSentinel(handle2));
+
+	PxF32 fl0, fl1, fl2;
+	ValType* PX_RESTRICT bpVal0 = PxUnionCast<ValType*, PxF32*>(&fl0);
+	ValType* PX_RESTRICT bpVal1 = PxUnionCast<ValType*, PxF32*>(&fl1);
+	ValType* PX_RESTRICT bpVal2 = PxUnionCast<ValType*, PxF32*>(&fl2);
+	*bpVal0 = decodeFloat(val0);
+	*bpVal1 = decodeFloat(val1);
+	*bpVal2 = decodeFloat(val2);
+	fl0 -= shift[0];
+	fl1 -= shift[1];
+	fl2 -= shift[2];
+	oVal0 = (isMax(handle0)) ? (IntegerAABB::encodeFloatMax(*bpVal0) | 1) : ((IntegerAABB::encodeFloatMin(*bpVal0) + 1) & ~1);
+	oVal1 = (isMax(handle1)) ? (IntegerAABB::encodeFloatMax(*bpVal1) | 1) : ((IntegerAABB::encodeFloatMin(*bpVal1) + 1) & ~1);
+	oVal2 = (isMax(handle2)) ? (IntegerAABB::encodeFloatMax(*bpVal2) | 1) : ((IntegerAABB::encodeFloatMin(*bpVal2) + 1) & ~1);
+}
+
+PX_FORCE_INLINE static void testPostShiftOrder(const ValType prevVal, ValType& currVal, const BpHandle prevIsMax, const BpHandle currIsMax)
+{
+	if(currVal < prevVal)
+	{
+		//The order has been broken by the lossy shift.
+		//Correct currVal so that it is greater than prevVal.
+		//If currVal is a box max then ensure that the box is of finite extent.
+		const ValType shiftCorrection = (prevIsMax==currIsMax) ? ValType(0) : ValType(1);
+		currVal = prevVal + shiftCorrection;
+	}
+}
+
+void BroadPhaseSap::shiftOrigin(const PxVec3& shift)
+{
+	//
+	// Note: shifting the bounds does not necessarily preserve the order of the broadphase interval endpoints. The encoding of the float bounds is a lossy
+	//       operation, thus it is not possible to get the original float values back and shift them. The only goal of this method is to shift the endpoints
+	//       such that the order is preserved. The new intervals might no reflect the correct bounds! Since all bounds have been marked dirty, they will get
+	//       recomputed in the next frame anyway. This method makes sure that the next frame update can start from a valid configuration that is close to
+	//       the correct one and does not require too many swaps.
+	//
+
+	if(0==mBoxesSize)
+	{
+		return;
+	}
+
+	//
+	// Note: processing all the axis at once improved performance on XBox 360 and PS3 because it allows to compensate for stalls
+	//
+
+	const PxF32 shiftAxis[3] = { shift.x, shift.y, shift.z };
+	const BpHandle* PX_RESTRICT epData0 = mEndPointDatas[0];
+	ValType* PX_RESTRICT epValues0 = mEndPointValues[0];
+	const BpHandle* PX_RESTRICT epData1 = mEndPointDatas[1];
+	ValType* PX_RESTRICT epValues1 = mEndPointValues[1];
+	const BpHandle* PX_RESTRICT epData2 = mEndPointDatas[2];
+	ValType* PX_RESTRICT epValues2 = mEndPointValues[2];
+
+	//Shift the first value in the array of sorted values.
+	{
+		//Shifted min (first element must be a min by definition).
+		shiftCoord3(epValues0[1], epData0[1], epValues1[1], epData1[1], epValues2[1], epData2[1], shiftAxis, epValues0[1], epValues1[1], epValues2[1]);
+		PX_ASSERT(!isMax(epData0[1]));
+		PX_ASSERT(!isMax(epData1[1]));
+		PX_ASSERT(!isMax(epData2[1]));
+	}
+
+	//Shift the remainder.
+	ValType prevVal0 = epValues0[1];
+	BpHandle prevIsMax0 = isMax(epData0[1]);
+	ValType prevVal1 = epValues1[1];
+	BpHandle prevIsMax1 = isMax(epData1[1]);
+	ValType prevVal2 = epValues2[1];
+	BpHandle prevIsMax2 = isMax(epData2[1]);
+	for(PxU32 i=2; i <= mBoxesSize*2; i++)
+	{
+		const BpHandle handle0 = epData0[i];
+		const BpHandle handle1 = epData1[i];
+		const BpHandle handle2 = epData2[i];
+		PX_ASSERT(!isSentinel(handle0));
+		PX_ASSERT(!isSentinel(handle1));
+		PX_ASSERT(!isSentinel(handle2));
+
+		//Get the relevant prev and curr values after the shift.
+		const BpHandle currIsMax0 = isMax(epData0[i]);
+		const BpHandle currIsMax1 = isMax(epData1[i]);
+		const BpHandle currIsMax2 = isMax(epData2[i]);
+		ValType currVal0, currVal1, currVal2;
+		shiftCoord3(epValues0[i], handle0, epValues1[i], handle1, epValues2[i], handle2, shiftAxis, currVal0, currVal1, currVal2);
+			
+		//Test if the order has been preserved by the lossy shift.
+		testPostShiftOrder(prevVal0, currVal0, prevIsMax0, currIsMax0);
+		testPostShiftOrder(prevVal1, currVal1, prevIsMax1, currIsMax1);
+		testPostShiftOrder(prevVal2, currVal2, prevIsMax2, currIsMax2);
+		
+		prevIsMax0 = currIsMax0;
+		prevVal0 = currVal0;
+		prevIsMax1 = currIsMax1;
+		prevVal1 = currVal1;
+		prevIsMax2 = currIsMax2;
+		prevVal2 = currVal2;
+
+		epValues0[i] = currVal0;
+		epValues1[i] = currVal1;
+		epValues2[i] = currVal2;
+	}
+
+	PX_ASSERT(isSelfOrdered());
+}
+
+#if PX_CHECKED
+bool BroadPhaseSap::isValid(const BroadPhaseUpdateData& updateData) const
+{
+	PX_UNUSED(updateData);
+#if 0
+	//Test that the created bounds haven't been added already (without first being removed).
+	const BpHandle* created=updateData.getCreatedHandles();
+	const PxU32 numCreated=updateData.getNumCreatedHandles();
+	for(PxU32 i=0;i<numCreated;i++)
+	{
+		const BpHandle id=created[i];
+
+		//If id >=mBoxesCapacity then we need to resize to add this id, meaning that the id must be new.
+		if(id<mBoxesCapacity)
+		{
+			for(PxU32 j=0;j<3;j++)
+			{
+				const SapBox1D& box1d=mBoxEndPts[j][id];
+				if(box1d.mMinMax[0] != BP_INVALID_BP_HANDLE && box1d.mMinMax[0] != PX_REMOVED_BP_HANDLE)
+				{
+					//This box has been added already but without being removed.
+					return false;
+				}
+				if(box1d.mMinMax[1] != BP_INVALID_BP_HANDLE && box1d.mMinMax[1] != PX_REMOVED_BP_HANDLE)
+				{
+					//This box has been added already but without being removed.
+					return false;
+				}
+			}
+		}
+	}
+
+	//Test that the updated bounds have valid ids.
+	const BpHandle* updated=updateData.getUpdatedHandles();
+	const PxU32 numUpdated=updateData.getNumUpdatedHandles();
+	for(PxU32 i=0;i<numUpdated;i++)
+	{
+		const BpHandle id = updated[i];
+		if(id >= mBoxesCapacity)
+		{
+			return false;
+		}
+	}
+
+	//Test that the updated bounds have been been added without being removed.
+	for(PxU32 i=0;i<numUpdated;i++)
+	{
+		const BpHandle id = updated[i];
+
+		for(PxU32 j=0;j<3;j++)
+		{
+			const SapBox1D& box1d=mBoxEndPts[j][id];
+
+			if(BP_INVALID_BP_HANDLE == box1d.mMinMax[0] || PX_REMOVED_BP_HANDLE == box1d.mMinMax[0])
+			{
+				//This box has either not been added or has been removed
+				return false;
+			}
+			if(BP_INVALID_BP_HANDLE == box1d.mMinMax[1] || PX_REMOVED_BP_HANDLE == box1d.mMinMax[1])
+			{
+				//This box has either not been added or has been removed
+				return false;
+			}
+		}
+	}
+
+	//Test that the removed bounds have valid ids.
+	const BpHandle* removed=updateData.getRemovedHandles();
+	const PxU32 numRemoved=updateData.getNumRemovedHandles();
+	for(PxU32 i=0;i<numRemoved;i++)
+	{
+		const BpHandle id = removed[i];
+		if(id >= mBoxesCapacity)
+		{
+			return false;
+		}
+	}
+
+	//Test that the removed bounds have already been added and haven't been removed.
+	for(PxU32 i=0;i<numRemoved;i++)
+	{
+		const BpHandle id = removed[i];
+
+		for(PxU32 j=0;j<3;j++)
+		{
+			const SapBox1D& box1d=mBoxEndPts[j][id];
+
+			if(BP_INVALID_BP_HANDLE == box1d.mMinMax[0] || PX_REMOVED_BP_HANDLE == box1d.mMinMax[0])
+			{
+				//This box has either not been added or has been removed
+				return false;
+			}
+			if(BP_INVALID_BP_HANDLE == box1d.mMinMax[1] || PX_REMOVED_BP_HANDLE == box1d.mMinMax[1])
+			{
+				//This box has either not been added or has been removed
+				return false;
+			}
+		}
+	}
+#endif
+	return true;
+}
+#endif
+
+void BroadPhaseSap::update(const PxU32 numCpuTasks, PxcScratchAllocator* scratchAllocator, const BroadPhaseUpdateData& updateData, PxBaseTask* continuation, PxBaseTask* narrowPhaseUnblockTask)
+{
+#if PX_CHECKED
+	PX_CHECK_AND_RETURN(scratchAllocator, "BroadPhaseSap::update - scratchAllocator must be non-NULL \n");
+#endif
+
+	if(narrowPhaseUnblockTask)
+		narrowPhaseUnblockTask->removeReference();
+
+	const bool success = setUpdateData(updateData);
+
+	if(success)
+	{
+		mScratchAllocator = scratchAllocator;
+
+		resizeBuffers();
+
+		mSapPostUpdateWorkTask.setBroadPhase(this);
+		mSapUpdateWorkTask.setBroadPhase(this);
+
+		mSapPostUpdateWorkTask.set(numCpuTasks);
+		mSapUpdateWorkTask.set(numCpuTasks);
+
+		mSapPostUpdateWorkTask.setContinuation(continuation);
+		mSapUpdateWorkTask.setContinuation(&mSapPostUpdateWorkTask);
+
+		mSapPostUpdateWorkTask.removeReference();
+		mSapUpdateWorkTask.removeReference();
+	}
+}
+
+bool BroadPhaseSap::setUpdateData(const BroadPhaseUpdateData& updateData) 
+{
+	PX_ASSERT(0==mCreatedPairsSize);
+	PX_ASSERT(0==mDeletedPairsSize);
+
+#if PX_CHECKED
+	if(!BroadPhaseUpdateData::isValid(updateData, *this))
+	{
+		PX_CHECK_MSG(false, "Illegal BroadPhaseUpdateData \n");
+		mCreated			= NULL;
+		mCreatedSize		= 0;
+		mUpdated			= NULL;
+		mUpdatedSize		= 0;
+		mRemoved			= NULL;
+		mRemovedSize		= 0;
+		mBoxBoundsMinMax	= updateData.getAABBs();
+		mBoxGroups			= updateData.getGroups();
+		return false;
+	}
+#endif
+
+	//Copy across the data ptrs and sizes.
+	mCreated			= updateData.getCreatedHandles();
+	mCreatedSize		= updateData.getNumCreatedHandles();
+	mUpdated			= updateData.getUpdatedHandles();
+	mUpdatedSize		= updateData.getNumUpdatedHandles();
+	mRemoved			= updateData.getRemovedHandles();
+	mRemovedSize		= updateData.getNumRemovedHandles();
+	mBoxBoundsMinMax	= updateData.getAABBs();
+	mBoxGroups			= updateData.getGroups();
+	mContactDistance	= updateData.getContactDistance();
+
+	//Do we need more memory to store the positions of each box min/max in the arrays of sorted boxes min/max?
+	if(updateData.getCapacity() > mBoxesCapacity)
+	{
+		const PxU32 oldBoxesCapacity=mBoxesCapacity;
+		const PxU32 newBoxesCapacity=updateData.getCapacity();
+		SapBox1D* newBoxEndPts0 = reinterpret_cast<SapBox1D*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(SapBox1D)*newBoxesCapacity)), "SapBox1D"));
+		SapBox1D* newBoxEndPts1 = reinterpret_cast<SapBox1D*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(SapBox1D)*newBoxesCapacity)), "SapBox1D"));
+		SapBox1D* newBoxEndPts2 = reinterpret_cast<SapBox1D*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(SapBox1D)*newBoxesCapacity)), "SapBox1D"));
+
+		PxMemCopy(newBoxEndPts0, mBoxEndPts[0], sizeof(SapBox1D)*oldBoxesCapacity);
+		PxMemCopy(newBoxEndPts1, mBoxEndPts[1], sizeof(SapBox1D)*oldBoxesCapacity);
+		PxMemCopy(newBoxEndPts2, mBoxEndPts[2], sizeof(SapBox1D)*oldBoxesCapacity);
+		for(PxU32 i=oldBoxesCapacity;i<newBoxesCapacity;i++)
+		{
+			newBoxEndPts0[i].mMinMax[0]=BP_INVALID_BP_HANDLE;
+			newBoxEndPts0[i].mMinMax[1]=BP_INVALID_BP_HANDLE;
+			newBoxEndPts1[i].mMinMax[0]=BP_INVALID_BP_HANDLE;
+			newBoxEndPts1[i].mMinMax[1]=BP_INVALID_BP_HANDLE;
+			newBoxEndPts2[i].mMinMax[0]=BP_INVALID_BP_HANDLE;
+			newBoxEndPts2[i].mMinMax[1]=BP_INVALID_BP_HANDLE;
+		}
+		PX_FREE(mBoxEndPts[0]);
+		PX_FREE(mBoxEndPts[1]);
+		PX_FREE(mBoxEndPts[2]);
+		mBoxEndPts[0] = newBoxEndPts0;
+		mBoxEndPts[1] = newBoxEndPts1;
+		mBoxEndPts[2] = newBoxEndPts2;
+		mBoxesCapacity = newBoxesCapacity;
+
+		
+
+		PX_FREE(mBoxesUpdated);
+		mBoxesUpdated = reinterpret_cast<PxU8*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(PxU8))*newBoxesCapacity), "Updated Boxes"));
+	}
+
+	//Do we need more memory for the array of sorted boxes?
+	if(2*(mBoxesSize + mCreatedSize) + NUM_SENTINELS > mEndPointsCapacity)
+	{
+		const PxU32 newEndPointsCapacity = 2*(mBoxesSize + mCreatedSize) + NUM_SENTINELS;
+
+		ValType* newEndPointValuesX = reinterpret_cast<ValType*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(ValType)*(newEndPointsCapacity))), "BPValType"));
+		ValType* newEndPointValuesY = reinterpret_cast<ValType*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(ValType)*(newEndPointsCapacity))), "BPValType"));
+		ValType* newEndPointValuesZ = reinterpret_cast<ValType*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(ValType)*(newEndPointsCapacity))), "BPValType"));
+		BpHandle* newEndPointDatasX = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*(newEndPointsCapacity))), "BpHandle"));
+		BpHandle* newEndPointDatasY = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*(newEndPointsCapacity))), "BpHandle"));
+		BpHandle* newEndPointDatasZ = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*(newEndPointsCapacity))), "BpHandle"));
+
+		PX_FREE(mListNext);
+		PX_FREE(mListPrev);
+
+		mListNext = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*newEndPointsCapacity)), "NextList"));
+		mListPrev = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*newEndPointsCapacity)), "Prev"));
+
+
+		for(PxU32 a = 1; a < newEndPointsCapacity; ++a)
+		{
+			mListNext[a-1] = BpHandle(a);
+			mListPrev[a] = BpHandle(a-1);
+		}
+		mListNext[newEndPointsCapacity-1] = BpHandle(newEndPointsCapacity-1);
+		mListPrev[0] = 0;
+
+		PxMemCopy(newEndPointValuesX, mEndPointValues[0], sizeof(ValType)*(mBoxesSize*2+NUM_SENTINELS));
+		PxMemCopy(newEndPointValuesY, mEndPointValues[1], sizeof(ValType)*(mBoxesSize*2+NUM_SENTINELS));
+		PxMemCopy(newEndPointValuesZ, mEndPointValues[2], sizeof(ValType)*(mBoxesSize*2+NUM_SENTINELS));
+		PxMemCopy(newEndPointDatasX, mEndPointDatas[0], sizeof(BpHandle)*(mBoxesSize*2+NUM_SENTINELS));
+		PxMemCopy(newEndPointDatasY, mEndPointDatas[1], sizeof(BpHandle)*(mBoxesSize*2+NUM_SENTINELS));
+		PxMemCopy(newEndPointDatasZ, mEndPointDatas[2], sizeof(BpHandle)*(mBoxesSize*2+NUM_SENTINELS));
+		PX_FREE(mEndPointValues[0]);
+		PX_FREE(mEndPointValues[1]);
+		PX_FREE(mEndPointValues[2]);
+		PX_FREE(mEndPointDatas[0]);
+		PX_FREE(mEndPointDatas[1]);
+		PX_FREE(mEndPointDatas[2]);
+		mEndPointValues[0] = newEndPointValuesX;
+		mEndPointValues[1] = newEndPointValuesY;
+		mEndPointValues[2] = newEndPointValuesZ;
+		mEndPointDatas[0] = newEndPointDatasX;
+		mEndPointDatas[1] = newEndPointDatasY;
+		mEndPointDatas[2] = newEndPointDatasZ;
+		mEndPointsCapacity = newEndPointsCapacity;
+
+		PX_FREE(mSortedUpdateElements);
+		PX_FREE(mActivityPockets);
+		mSortedUpdateElements = reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BpHandle)*newEndPointsCapacity)), "SortedUpdateElements"));
+		mActivityPockets = reinterpret_cast<BroadPhaseActivityPocket*>(PX_ALLOC(ALIGN_SIZE_16((sizeof(BroadPhaseActivityPocket)*newEndPointsCapacity)), "BroadPhaseActivityPocket"));
+	}
+
+	PxMemZero(mBoxesUpdated, sizeof(PxU8) * (mBoxesCapacity));	
+
+	for(PxU32 a=0;a<mUpdatedSize;a++)
+	{
+		const PxU32 handle=mUpdated[a];
+		mBoxesUpdated[handle] = 1;
+	}
+
+	//Update the size of the sorted boxes arrays.
+	PX_ASSERT(mBoxesSize==mBoxesSizePrev);
+	mBoxesSize += mCreatedSize;
+	PX_ASSERT(2*mBoxesSize+NUM_SENTINELS <= mEndPointsCapacity);
+
+	return true;
+}
+
+void BroadPhaseSap::postUpdate(PxBaseTask* /*continuation*/)
+{
+	PX_PROFILE_ZONE("BroadPhase.SapPostUpdate", mContextID);
+
+	for(PxU32 i=0;i<3;i++)
+	{
+		const PxU32 numPairs=mBatchUpdateTasks[i].getPairsSize();
+		const BroadPhasePair* PX_RESTRICT pairs=mBatchUpdateTasks[i].getPairs();
+		for(PxU32 j=0;j<numPairs;j++)
+		{
+			const BroadPhasePair& pair=pairs[j];
+			const BpHandle volA=pair.mVolA;
+			const BpHandle volB=pair.mVolB;
+			if(volA > volB)
+			{
+				AddPair(volA, volB, mScratchAllocator, mPairs, mData, mDataSize, mDataCapacity);
+			}
+			else
+			{
+				RemovePair(volA, volB, mScratchAllocator, mPairs, mData, mDataSize, mDataCapacity);
+			}
+		}
+	}
+
+	batchCreate();
+
+	//Compute the lists of created and deleted overlap pairs.
+
+	ComputeCreatedDeletedPairsLists(
+		mBoxGroups,
+		mData,mDataSize,
+		mScratchAllocator, 
+		mCreatedPairsArray,mCreatedPairsSize,mCreatedPairsCapacity,
+		mDeletedPairsArray,mDeletedPairsSize,mDeletedPairsCapacity,
+		mActualDeletedPairSize,
+		mPairs);
+
+	//DeletePairsLists(mActualDeletedPairSize, mDeletedPairsArray, mPairs);
+
+	PX_ASSERT(isSelfConsistent());
+	mBoxesSizePrev=mBoxesSize;
+}
+
+void BroadPhaseSap::deletePairs()
+{
+	DeletePairsLists(mActualDeletedPairSize, mDeletedPairsArray, mPairs);
+}
+
+void BroadPhaseBatchUpdateWorkTask::runInternal()
+{
+	mPairsSize=0;
+	mSap->batchUpdate(mAxis, mPairs, mPairsSize, mPairsCapacity);
+}
+
+void BroadPhaseSap::update(PxBaseTask* continuation)
+{
+	PX_UNUSED(continuation);
+
+	PX_PROFILE_ZONE("BroadPhase.SapUpdate", mContextID);
+
+	batchRemove();
+
+	//Check that the overlap pairs per axis have been reset.
+	PX_ASSERT(0==mBatchUpdateTasks[0].getPairsSize());
+	PX_ASSERT(0==mBatchUpdateTasks[1].getPairsSize());
+	PX_ASSERT(0==mBatchUpdateTasks[2].getPairsSize());
+
+	mBatchUpdateTasks[0].runInternal();
+	mBatchUpdateTasks[1].runInternal();
+	mBatchUpdateTasks[2].runInternal();
+}
+
+void BroadPhaseSap::batchCreate()
+{
+	if(!mCreatedSize)	return;	// Early-exit if no object has been created
+
+	//Number of newly-created boxes (still to be sorted) and number of old boxes (already sorted).
+	const PxU32 numNewBoxes=mCreatedSize;
+	//const PxU32 numOldBoxes = mBoxesSize - mCreatedSize;
+
+	//Array of newly-created box indices.
+	const BpHandle* PX_RESTRICT created = mCreated;
+
+	//Arrays of min and max coords for each box for each axis.
+	const PxBounds3* PX_RESTRICT minMax = mBoxBoundsMinMax;
+
+	//Insert new boxes into sorted endpoints lists.
+	{
+		const PxU32 numEndPoints = numNewBoxes*2;
+
+		Cm::TmpMem<ValType, 32> nepsv(numEndPoints), bv(numEndPoints);
+		Cm::TmpMem<BpHandle, 32> nepsd(numEndPoints), bd(numEndPoints);
+
+		ValType* newEPSortedValues = nepsv.getBase();
+		BpHandle* newEPSortedDatas = nepsd.getBase();
+		ValType* bufferValues = bv.getBase();
+		BpHandle* bufferDatas = bd.getBase();
+
+		Cm::RadixSortBuffered RS;
+
+		for(PxU32 Axis=0;Axis<3;Axis++)
+		{
+			for(PxU32 i=0;i<numNewBoxes;i++)
+			{
+				const PxU32 boxIndex = PxU32(created[i]);
+				PX_ASSERT(mBoxEndPts[Axis][boxIndex].mMinMax[0]==BP_INVALID_BP_HANDLE || mBoxEndPts[Axis][boxIndex].mMinMax[0]==PX_REMOVED_BP_HANDLE);
+				PX_ASSERT(mBoxEndPts[Axis][boxIndex].mMinMax[1]==BP_INVALID_BP_HANDLE || mBoxEndPts[Axis][boxIndex].mMinMax[1]==PX_REMOVED_BP_HANDLE);
+
+				//				const ValType minValue = minMax[boxIndex].getMin(Axis);
+				//				const ValType maxValue = minMax[boxIndex].getMax(Axis);
+
+				const ValType minValue = encodeMin(minMax[boxIndex], Axis, mContactDistance[boxIndex]);
+				const ValType maxValue = encodeMax(minMax[boxIndex], Axis, mContactDistance[boxIndex]);
+
+				newEPSortedValues[i*2+0]=minValue;
+				setData(newEPSortedDatas[i*2+0],boxIndex, false);
+				newEPSortedValues[i*2+1]=maxValue;
+				setData(newEPSortedDatas[i*2+1], boxIndex, true);
+			}
+
+			// Sort endpoints backwards
+			{
+				PxU32* keys = reinterpret_cast<PxU32*>(bufferValues);
+				for(PxU32 i=0;i<numEndPoints;i++)
+				{
+					keys[i] = newEPSortedValues[i];
+				}
+
+				const PxU32* Sorted = RS.Sort(keys, numEndPoints, Cm::RADIX_UNSIGNED).GetRanks();
+
+				for(PxU32 i=0;i<numEndPoints;i++)
+				{
+					bufferValues[i] = newEPSortedValues[Sorted[numEndPoints-1-i]];
+					bufferDatas[i] = newEPSortedDatas[Sorted[numEndPoints-1-i]];
+				}
+			}
+
+			InsertEndPoints(bufferValues, bufferDatas, numEndPoints, mEndPointValues[Axis], mEndPointDatas[Axis], 2*(mBoxesSize-mCreatedSize)+NUM_SENTINELS, mBoxEndPts[Axis]);
+		}
+	}
+
+	//Some debug tests.
+#if PX_DEBUG
+	{
+		for(PxU32 i=0;i<numNewBoxes;i++)
+		{
+			PxU32 BoxIndex = PxU32(created[i]);
+			PX_ASSERT(mBoxEndPts[0][BoxIndex].mMinMax[0]!=BP_INVALID_BP_HANDLE && mBoxEndPts[0][BoxIndex].mMinMax[0]!=PX_REMOVED_BP_HANDLE);
+			PX_ASSERT(mBoxEndPts[0][BoxIndex].mMinMax[1]!=BP_INVALID_BP_HANDLE && mBoxEndPts[0][BoxIndex].mMinMax[1]!=PX_REMOVED_BP_HANDLE);
+			PX_ASSERT(mBoxEndPts[1][BoxIndex].mMinMax[0]!=BP_INVALID_BP_HANDLE && mBoxEndPts[1][BoxIndex].mMinMax[0]!=PX_REMOVED_BP_HANDLE);
+			PX_ASSERT(mBoxEndPts[1][BoxIndex].mMinMax[1]!=BP_INVALID_BP_HANDLE && mBoxEndPts[1][BoxIndex].mMinMax[1]!=PX_REMOVED_BP_HANDLE);
+			PX_ASSERT(mBoxEndPts[2][BoxIndex].mMinMax[0]!=BP_INVALID_BP_HANDLE && mBoxEndPts[2][BoxIndex].mMinMax[0]!=PX_REMOVED_BP_HANDLE);
+			PX_ASSERT(mBoxEndPts[2][BoxIndex].mMinMax[1]!=BP_INVALID_BP_HANDLE && mBoxEndPts[2][BoxIndex].mMinMax[1]!=PX_REMOVED_BP_HANDLE);
+		}
+		for(PxU32 i=0;i<mBoxesSize*2+1;i++)
+		{
+			PX_ASSERT(mEndPointValues[0][i] <= mEndPointValues[0][i+1]);
+			PX_ASSERT(mEndPointValues[1][i] <= mEndPointValues[1][i+1]);
+			PX_ASSERT(mEndPointValues[2][i] <= mEndPointValues[2][i+1]);
+		}
+	}
+#endif
+
+	//Axes used to compute overlaps involving newly-created boxes.
+	const Gu::Axes axes(Gu::AXES_XYZ);
+	performBoxPruning(axes);
+}
+
+void BroadPhaseSap::performBoxPruning(const Gu::Axes axes)
+{
+	const PxU32 axis0=axes.mAxis0;
+
+	//Number of newly-created boxes (still to be sorted) and number of old boxes (already sorted).
+	const PxU32 numNewBoxes=mCreatedSize;
+	const PxU32 numOldBoxes = mBoxesSize - mCreatedSize;
+
+	//Gather two list of sorted boxes along the preferred axis direction: 
+	//one list for new boxes and one list for existing boxes.
+	//Only gather the existing boxes that overlap the bounding box of 
+	//all new boxes.
+	Cm::TmpMem<BpHandle, 8> oldBoxesIndicesSortedMem(numOldBoxes);
+	Cm::TmpMem<BpHandle, 8> newBoxesIndicesSortedMem(numNewBoxes);
+	BpHandle* oldBoxesIndicesSorted=oldBoxesIndicesSortedMem.getBase();
+	BpHandle* newBoxesIndicesSorted=newBoxesIndicesSortedMem.getBase();
+	PxU32 oldBoxCount=0;
+	PxU32 newBoxCount=0;
+
+	//To help us gather the two lists of sorted boxes we are going to use 
+	//a bitmap and our knowledge of the indices of the new boxes
+	const PxU32 bitmapWordCount = ((mBoxesCapacity*2 + 31) & ~31)/32;
+	Cm::TmpMem<PxU32, 8> bitMapMem(bitmapWordCount);
+	PxU32* bitMapWords=bitMapMem.getBase();
+	PxMemSet(bitMapWords, 0, sizeof(PxU32)*bitmapWordCount);
+	Cm::BitMap bitmap;
+	bitmap.setWords(bitMapWords, bitmapWordCount);
+
+	//Ready to gather the two lists now.
+	bool allNewBoxesStatics=false;
+	bool allOldBoxesStatics=false;
+	ComputeSortedLists
+		(&bitmap,
+		 0, mCreatedSize, mCreated,
+		 mBoxEndPts, mBoxGroups, 
+		 mEndPointDatas[axis0], mBoxesSize*2 + NUM_SENTINELS,
+		 axes,
+		 newBoxesIndicesSorted, newBoxCount, oldBoxesIndicesSorted, oldBoxCount, allNewBoxesStatics, allOldBoxesStatics);
+
+
+	//Intersect new boxes with new boxes and new boxes with existing boxes.
+	if(!allNewBoxesStatics || !allOldBoxesStatics)
+	{
+		Cm::TmpMem<BpHandle, 8> minPosListNewMem(numNewBoxes+1);
+		BpHandle* minPosListNew=minPosListNewMem.getBase();
+		performBoxPruningNewNew
+			(axes,
+			 newBoxesIndicesSorted, newBoxCount, allNewBoxesStatics,
+			 minPosListNew, mBoxEndPts, mBoxGroups,
+			 mScratchAllocator,
+			 mPairs, mData, mDataSize, mDataCapacity);
+
+		// the old boxes are not the first ones in the array
+		if(numOldBoxes)
+		{
+			Cm::TmpMem<BpHandle, 8> minPosListOldMem(numOldBoxes);
+			BpHandle* minPosListOld=minPosListOldMem.getBase();
+			performBoxPruningNewOld
+				(axes,
+				 newBoxesIndicesSorted, newBoxCount, oldBoxesIndicesSorted, oldBoxCount,
+				 minPosListNew, minPosListOld,
+				 mBoxEndPts, mBoxGroups,
+				 mScratchAllocator, 
+				 mPairs, mData, mDataSize, mDataCapacity);
+		}
+	}
+}
+
+void BroadPhaseSap::batchRemove()
+{
+	if(!mRemovedSize)	return;	// Early-exit if no object has been removed
+
+	//The box count is incremented when boxes are added to the create list but these boxes
+	//haven't yet been added to the pair manager or the sorted axis lists.  We need to 
+	//pretend that the box count is the value it was when the bp was last updated.
+	//Then, at the end, we need to set the box count to the number that includes the boxes
+	//in the create list and subtract off the boxes that have been removed.
+	PxU32 currBoxesSize=mBoxesSize;
+	mBoxesSize=mBoxesSizePrev;
+
+	for(PxU32 Axis=0;Axis<3;Axis++)
+	{
+		ValType* const BaseEPValue = mEndPointValues[Axis];
+		BpHandle* const BaseEPData = mEndPointDatas[Axis];
+		PxU32 MinMinIndex = PX_MAX_U32;
+		for(PxU32 i=0;i<mRemovedSize;i++)
+		{
+			PX_ASSERT(mRemoved[i]<mBoxesCapacity);
+
+			const PxU32 MinIndex = mBoxEndPts[Axis][mRemoved[i]].mMinMax[0];
+			PX_ASSERT(MinIndex<mBoxesCapacity*2+2);
+			PX_ASSERT(getOwner(BaseEPData[MinIndex])==mRemoved[i]);
+
+			const PxU32 MaxIndex = mBoxEndPts[Axis][mRemoved[i]].mMinMax[1];
+			PX_ASSERT(MaxIndex<mBoxesCapacity*2+2);
+			PX_ASSERT(getOwner(BaseEPData[MaxIndex])==mRemoved[i]);
+
+			PX_ASSERT(MinIndex<MaxIndex);
+
+			BaseEPData[MinIndex] = PX_REMOVED_BP_HANDLE;
+			BaseEPData[MaxIndex] = PX_REMOVED_BP_HANDLE;
+
+			if(MinIndex<MinMinIndex)	
+				MinMinIndex = MinIndex;
+		}
+
+
+		PxU32 ReadIndex = MinMinIndex;
+		PxU32 DestIndex = MinMinIndex;
+		const PxU32 Limit = mBoxesSize*2+NUM_SENTINELS;
+		while(ReadIndex!=Limit)
+		{
+			Ps::prefetchLine(&BaseEPData[ReadIndex],128);
+			while(ReadIndex!=Limit && BaseEPData[ReadIndex] == PX_REMOVED_BP_HANDLE)
+			{
+				Ps::prefetchLine(&BaseEPData[ReadIndex],128);
+				ReadIndex++;
+			}
+			if(ReadIndex!=Limit)
+			{
+				if(ReadIndex!=DestIndex)
+				{
+					BaseEPValue[DestIndex] = BaseEPValue[ReadIndex];
+					BaseEPData[DestIndex] = BaseEPData[ReadIndex];
+					PX_ASSERT(BaseEPData[DestIndex] != PX_REMOVED_BP_HANDLE);
+					if(!isSentinel(BaseEPData[DestIndex]))
+					{
+						BpHandle BoxOwner = getOwner(BaseEPData[DestIndex]);
+						PX_ASSERT(BoxOwner<mBoxesCapacity);
+						mBoxEndPts[Axis][BoxOwner].mMinMax[isMax(BaseEPData[DestIndex])] = BpHandle(DestIndex);
+					}
+				}
+				DestIndex++;
+				ReadIndex++;
+			}
+		}
+	}
+
+	for(PxU32 i=0;i<mRemovedSize;i++)
+	{
+		const PxU32 handle=mRemoved[i];
+		mBoxEndPts[0][handle].mMinMax[0]=PX_REMOVED_BP_HANDLE;
+		mBoxEndPts[0][handle].mMinMax[1]=PX_REMOVED_BP_HANDLE;
+		mBoxEndPts[1][handle].mMinMax[0]=PX_REMOVED_BP_HANDLE;
+		mBoxEndPts[1][handle].mMinMax[1]=PX_REMOVED_BP_HANDLE;
+		mBoxEndPts[2][handle].mMinMax[0]=PX_REMOVED_BP_HANDLE;
+		mBoxEndPts[2][handle].mMinMax[1]=PX_REMOVED_BP_HANDLE;
+	}
+
+	const PxU32 bitmapWordCount=1+(mBoxesCapacity>>5);
+	Cm::TmpMem<PxU32, 128> bitmapWords(bitmapWordCount);
+	PxMemZero(bitmapWords.getBase(),sizeof(PxU32)*bitmapWordCount);
+	Cm::BitMap bitmap;
+	bitmap.setWords(bitmapWords.getBase(),bitmapWordCount);
+	for(PxU32 i=0;i<mRemovedSize;i++)
+	{
+		PxU32 Index = mRemoved[i];
+		PX_ASSERT(Index<mBoxesCapacity);
+		PX_ASSERT(0==bitmap.test(Index));
+		bitmap.set(Index);
+	}
+	mPairs.RemovePairs(bitmap);
+
+	mBoxesSize=currBoxesSize;
+	mBoxesSize-=mRemovedSize;
+	mBoxesSizePrev=mBoxesSize-mCreatedSize;
+}
+
+PX_FORCE_INLINE bool intersect2D(	const SapBox1D*const* PX_RESTRICT c,
+									const SapBox1D*const* PX_RESTRICT boxEndPts,
+									PxU32 ownerId,
+									const PxU32 axis1, const PxU32 axis2)
+{
+	const SapBox1D* PX_RESTRICT b1 = boxEndPts[axis1] + ownerId;
+	const SapBox1D* PX_RESTRICT b2 = boxEndPts[axis2] + ownerId;
+
+	return (b1->mMinMax[1] >= c[axis1]->mMinMax[0] && c[axis1]->mMinMax[1] >= b1->mMinMax[0] &&
+			b2->mMinMax[1] >= c[axis2]->mMinMax[0] && c[axis2]->mMinMax[1] >= b2->mMinMax[0]);
+}
+
+static BroadPhasePair* resizeBroadPhasePairArray(const PxU32 oldMaxNb, const PxU32 newMaxNb, PxcScratchAllocator* scratchAllocator, BroadPhasePair* elements)
+{
+	PX_ASSERT(newMaxNb > oldMaxNb);
+	PX_ASSERT(newMaxNb > 0);
+	PX_ASSERT(0==((newMaxNb*sizeof(BroadPhasePair)) & 15)); 
+	BroadPhasePair* newElements = reinterpret_cast<BroadPhasePair*>(scratchAllocator->alloc(sizeof(BroadPhasePair)*newMaxNb, true));
+	PX_ASSERT(0==(uintptr_t(newElements) & 0x0f));
+	PxMemCopy(newElements, elements, oldMaxNb*sizeof(BroadPhasePair));
+	scratchAllocator->free(elements);
+	return newElements;
+}
+
+#define PERFORM_COMPARISONS 1
+
+
+void BroadPhaseSap::batchUpdate
+(const PxU32 Axis, BroadPhasePair*& pairs, PxU32& pairsSize, PxU32& pairsCapacity)
+{
+	//Nothin updated so don't do anything
+	if(mUpdatedSize == 0)
+		return;
+
+		//If number updated is sufficiently fewer than number of boxes (say less than 20%)
+	if((mUpdatedSize*5) < mBoxesSize)
+	{
+		batchUpdateFewUpdates(Axis, pairs, pairsSize, pairsCapacity);
+		return;
+	}
+
+	PxU32 numPairs=0;
+	PxU32 maxNumPairs=pairsCapacity;
+
+	const PxBounds3* PX_RESTRICT boxMinMax3D = mBoxBoundsMinMax;
+	SapBox1D* boxMinMax2D[6]={mBoxEndPts[1],mBoxEndPts[2],mBoxEndPts[2],mBoxEndPts[0],mBoxEndPts[0],mBoxEndPts[1]};
+
+	const SapBox1D* PX_RESTRICT boxMinMax0=boxMinMax2D[2*Axis+0];
+	const SapBox1D* PX_RESTRICT boxMinMax1=boxMinMax2D[2*Axis+1];
+
+
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE 
+	const BpHandle* PX_RESTRICT asapBoxGroupIds=mBoxGroups;
+#endif
+
+	SapBox1D* PX_RESTRICT asapBoxes=mBoxEndPts[Axis];
+
+	ValType* PX_RESTRICT asapEndPointValues=mEndPointValues[Axis];
+	BpHandle* PX_RESTRICT asapEndPointDatas=mEndPointDatas[Axis];
+
+	ValType* const PX_RESTRICT BaseEPValues = asapEndPointValues;
+	BpHandle* const PX_RESTRICT BaseEPDatas = asapEndPointDatas;			
+
+	PxU8* PX_RESTRICT updated = mBoxesUpdated;
+
+	//KS - can we lazy create these inside the loop? Might benefit us
+
+	//There are no extents, jus the sentinels, so exit early.
+	if(isSentinel(BaseEPDatas[1]))
+		return;
+
+	//We are going to skip the 1st element in the array (the sublist will be sorted)
+	//but we must first update its value if it has moved
+	//const PxU32 startIsMax = isMax(BaseEPDatas[1]);
+	PX_ASSERT(!isMax(BaseEPDatas[1]));
+	const BpHandle startHandle = getOwner(BaseEPDatas[1]);
+
+	//KS - in theory, we should just be able to grab the min element but there's some issue where a body's max < min (i.e. an invalid extents) that
+	//appears in a unit test
+	// ValType ThisValue_ = boxMinMax3D[startHandle].getMin(Axis);
+	ValType ThisValue_ = encodeMin(boxMinMax3D[startHandle], Axis, mContactDistance[startHandle]);
+
+	BaseEPValues[1] = ThisValue_;
+	
+	PxU32 updateCounter = mUpdatedSize*2;
+
+	updateCounter -= updated[startHandle];
+
+	//We'll never overlap with this sentinel but it just ensures that we don't need to branch to see if
+	//there's a pocket that we need to test against
+	
+	BroadPhaseActivityPocket* PX_RESTRICT currentPocket = mActivityPockets;
+
+	currentPocket->mEndIndex = 0;
+	currentPocket->mStartIndex = 0;
+
+
+	BpHandle ind = 2;
+	PxU8 wasUpdated = updated[startHandle];
+	for(; !isSentinel(BaseEPDatas[ind]); ++ind)
+	{
+		BpHandle ThisData = BaseEPDatas[ind];
+
+		const BpHandle handle = getOwner(ThisData);
+
+		if(updated[handle] || wasUpdated)
+		{
+			wasUpdated = updated[handle];
+			updateCounter -= wasUpdated;
+
+			BpHandle ThisIndex = ind;
+
+			const BpHandle startIsMax = isMax(ThisData);
+
+
+			//Access and write back the updated values. TODO - can we avoid this when we're walking through inactive nodes?
+			//BPValType ThisValue = boxMinMax1D[Axis][twoHandle+startIsMax];
+			//BPValType ThisValue = startIsMax ? boxMinMax3D[handle].getMax(Axis) : boxMinMax3D[handle].getMin(Axis);
+			//ValType ThisValue = boxMinMax3D[handle].getExtent(startIsMax, Axis);
+
+			ValType ThisValue = startIsMax ? encodeMax(boxMinMax3D[handle], Axis, mContactDistance[handle])
+										   : encodeMin(boxMinMax3D[handle], Axis, mContactDistance[handle]);
+
+			BaseEPValues[ThisIndex] = ThisValue;
+
+			PX_ASSERT(handle!=BP_INVALID_BP_HANDLE);
+
+			//We always iterate back through the list...
+
+			BpHandle CurrentIndex = mListPrev[ThisIndex];
+			ValType CurrentValue = BaseEPValues[CurrentIndex];
+			//PxBpHandle CurrentData = BaseEPDatas[CurrentIndex];
+
+			if(CurrentValue > ThisValue)
+			{
+				wasUpdated = 1;
+				//Get the bounds of the curr aabb.
+				//Get the box1d of the curr aabb.
+				/*const SapBox1D* PX_RESTRICT Object=&asapBoxes[handle];
+				PX_ASSERT(Object->mMinMax[0]!=BP_INVALID_BP_HANDLE);
+				PX_ASSERT(Object->mMinMax[1]!=BP_INVALID_BP_HANDLE);*/
+				
+				// const ValType boxMax=boxMinMax3D[handle].getMax(Axis);
+
+				const ValType boxMax=encodeMax(boxMinMax3D[handle], Axis, mContactDistance[handle]);
+
+				PxU32 endIndex = ind;
+				PxU32 startIndex = ind;
+
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+				ValType group = asapBoxGroupIds[handle];
+#endif
+
+				if(!isMax(ThisData))
+				{
+					do
+					{
+						BpHandle CurrentData = BaseEPDatas[CurrentIndex];
+						const BpHandle IsMax = isMax(CurrentData);
+						
+	#if PERFORM_COMPARISONS
+						if(IsMax)
+						{		
+							const BpHandle ownerId=getOwner(CurrentData);
+							SapBox1D* PX_RESTRICT id1 = asapBoxes + ownerId;
+							// Our min passed a max => start overlap
+
+							if(
+								BaseEPValues[id1->mMinMax[0]] < boxMax && 
+								//2D intersection test using up-to-date values
+								Intersect2D_Handle(boxMinMax0[handle].mMinMax[0], boxMinMax0[handle].mMinMax[1], boxMinMax1[handle].mMinMax[0], boxMinMax1[handle].mMinMax[1],
+								            boxMinMax0[ownerId].mMinMax[0],boxMinMax0[ownerId].mMinMax[1],boxMinMax1[ownerId].mMinMax[0],boxMinMax1[ownerId].mMinMax[1])
+
+	#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								&& (group!=asapBoxGroupIds[ownerId])
+	#else
+								&& handle!=ownerId
+	#endif
+								)
+							{
+								if(numPairs==maxNumPairs)
+								{
+									const PxU32 newMaxNumPairs=maxNumPairs*2;
+									pairs = reinterpret_cast<BroadPhasePair*>(resizeBroadPhasePairArray(maxNumPairs, newMaxNumPairs, mScratchAllocator, pairs));
+									maxNumPairs=newMaxNumPairs;
+								}
+								PX_ASSERT(numPairs<maxNumPairs);
+								pairs[numPairs].mVolA=BpHandle(PxMax(handle, ownerId));
+								pairs[numPairs].mVolB=BpHandle(PxMin(handle, ownerId));
+								numPairs++;
+								//AddPair(handle, getOwner(*CurrentMinData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+	#endif
+						startIndex--;
+						CurrentIndex = mListPrev[CurrentIndex];
+						CurrentValue = BaseEPValues[CurrentIndex];
+					}
+					while(ThisValue < CurrentValue);
+				}			
+				else 
+				{
+					// Max is moving left:
+					do
+					{
+						BpHandle CurrentData = BaseEPDatas[CurrentIndex];
+						const BpHandle IsMax = isMax(CurrentData);
+						
+	#if PERFORM_COMPARISONS
+						if(!IsMax)
+						{
+							// Our max passed a min => stop overlap
+							const BpHandle ownerId=getOwner(CurrentData);
+
+#if 1
+							if(
+#if BP_SAP_USE_OVERLAP_TEST_ON_REMOVES
+								Intersect2D_Handle(boxMinMax0[handle].mMinMax[0], boxMinMax0[handle].mMinMax[1], boxMinMax1[handle].mMinMax[0], boxMinMax1[handle].mMinMax[1],
+								       boxMinMax0[ownerId].mMinMax[0],boxMinMax0[ownerId].mMinMax[1],boxMinMax1[ownerId].mMinMax[0],boxMinMax1[ownerId].mMinMax[1])
+#endif
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								&& (group!=asapBoxGroupIds[ownerId])
+#else
+								&& handle!=ownerId
+#endif
+								)
+#endif
+							{
+								if(numPairs==maxNumPairs)
+								{
+									const PxU32 newMaxNumPairs=maxNumPairs*2;
+									pairs = reinterpret_cast<BroadPhasePair*>(resizeBroadPhasePairArray(maxNumPairs, newMaxNumPairs, mScratchAllocator, pairs));
+									maxNumPairs=newMaxNumPairs;
+								}
+								PX_ASSERT(numPairs<maxNumPairs);
+								pairs[numPairs].mVolA=BpHandle(PxMin(handle, ownerId));
+								pairs[numPairs].mVolB=BpHandle(PxMax(handle, ownerId));
+								numPairs++;
+								//RemovePair(handle, getOwner(*CurrentMaxData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+	#endif
+						startIndex--;
+						CurrentIndex = mListPrev[CurrentIndex];
+						CurrentValue = BaseEPValues[CurrentIndex];
+					}
+					while(ThisValue < CurrentValue);
+				}
+
+				//This test is unnecessary. If we entered the outer loop, we're doing the swap in here
+				{
+					//Unlink from old position and re-link to new position
+					BpHandle oldNextIndex = mListNext[ThisIndex];
+					BpHandle oldPrevIndex = mListPrev[ThisIndex];
+
+					BpHandle newNextIndex = mListNext[CurrentIndex];
+					BpHandle newPrevIndex = CurrentIndex;
+					
+					//Unlink this node
+					mListNext[oldPrevIndex] = oldNextIndex;
+					mListPrev[oldNextIndex] = oldPrevIndex;
+
+					//Link it to it's new place in the list
+					mListNext[ThisIndex] = newNextIndex;
+					mListPrev[ThisIndex] = newPrevIndex;
+					mListPrev[newNextIndex] = ThisIndex;
+					mListNext[newPrevIndex] = ThisIndex;
+				}
+
+				//There is a sentinel with 0 index, so we don't need
+				//to worry about walking off the array				
+				while(startIndex < currentPocket->mStartIndex)
+				{
+					currentPocket--;
+				}
+				//If our start index > currentPocket->mEndIndex, then we don't overlap so create a new pocket
+				if(currentPocket == mActivityPockets || startIndex > (currentPocket->mEndIndex+1))
+				{
+					currentPocket++;
+					currentPocket->mStartIndex = startIndex;
+				}
+				currentPocket->mEndIndex = endIndex;
+			}// update max
+			//ind++;
+		}
+		else if (updateCounter == 0) //We've updated all the bodies and neither this nor the previous body was updated, so we're done
+			break;
+
+	}// updated aabbs
+
+	pairsSize=numPairs;
+	pairsCapacity=maxNumPairs;
+
+
+	BroadPhaseActivityPocket* pocket = mActivityPockets+1;
+
+	while(pocket <= currentPocket)
+	{
+		for(PxU32 a = pocket->mStartIndex; a <= pocket->mEndIndex; ++a)
+		{
+			mListPrev[a] = BpHandle(a);
+		}
+
+		//Now copy all the data to the array, updating the remap table
+
+		PxU32 CurrIndex = pocket->mStartIndex-1;
+		for(PxU32 a = pocket->mStartIndex; a <= pocket->mEndIndex; ++a)
+		{
+			CurrIndex = mListNext[CurrIndex];
+			PxU32 origIndex =  CurrIndex;
+			BpHandle remappedIndex = mListPrev[origIndex];
+
+			if(origIndex != a)
+			{
+				const BpHandle ownerId=getOwner(BaseEPDatas[remappedIndex]);
+				const BpHandle IsMax = isMax(BaseEPDatas[remappedIndex]);
+				ValType tmp = BaseEPValues[a];
+				BpHandle tmpHandle = BaseEPDatas[a];
+
+				BaseEPValues[a] = BaseEPValues[remappedIndex];
+				BaseEPDatas[a] = BaseEPDatas[remappedIndex];
+
+				BaseEPValues[remappedIndex] = tmp;
+				BaseEPDatas[remappedIndex] = tmpHandle;
+
+				mListPrev[remappedIndex] = mListPrev[a];
+				//Write back remap index (should be an immediate jump to original index)
+				mListPrev[mListPrev[a]] = remappedIndex;
+				asapBoxes[ownerId].mMinMax[IsMax] = BpHandle(a);
+			}
+			
+		}
+
+		////Reset next and prev ptrs back
+		for(PxU32 a = pocket->mStartIndex-1; a <= pocket->mEndIndex; ++a)
+		{
+			mListPrev[a+1] = BpHandle(a);
+			mListNext[a] = BpHandle(a+1);
+		}
+
+		pocket++;
+	}
+	mListPrev[0] = 0;
+}
+
+
+void BroadPhaseSap::batchUpdateFewUpdates
+(const PxU32 Axis, BroadPhasePair*& pairs, PxU32& pairsSize, PxU32& pairsCapacity)
+{
+	PxU32 numPairs=0;
+	PxU32 maxNumPairs=pairsCapacity;
+
+	const PxBounds3* PX_RESTRICT boxMinMax3D = mBoxBoundsMinMax;
+	SapBox1D* boxMinMax2D[6]={mBoxEndPts[1],mBoxEndPts[2],mBoxEndPts[2],mBoxEndPts[0],mBoxEndPts[0],mBoxEndPts[1]};
+
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE 
+	const BpHandle* PX_RESTRICT asapBoxGroupIds=mBoxGroups;
+#endif
+
+	SapBox1D* PX_RESTRICT asapBoxes=mBoxEndPts[Axis];
+
+	/*const BPValType* PX_RESTRICT boxMinMax0=boxMinMax2D[2*Axis];
+	const BPValType* PX_RESTRICT boxMinMax1=boxMinMax2D[2*Axis+1];*/
+
+	ValType* PX_RESTRICT asapEndPointValues=mEndPointValues[Axis];
+	BpHandle* PX_RESTRICT asapEndPointDatas=mEndPointDatas[Axis];
+
+	ValType* const PX_RESTRICT BaseEPValues = asapEndPointValues;
+	BpHandle* const PX_RESTRICT BaseEPDatas = asapEndPointDatas;			
+
+	const SapBox1D* PX_RESTRICT boxMinMax0=boxMinMax2D[2*Axis+0];
+	const SapBox1D* PX_RESTRICT boxMinMax1=boxMinMax2D[2*Axis+1];
+
+	PxU8* PX_RESTRICT updated = mBoxesUpdated;
+
+	const PxU32 endPointSize = mBoxesSize*2 + 1;
+
+	//There are no extents, just the sentinels, so exit early.
+	if(isSentinel(BaseEPDatas[1]))
+		return;
+
+	PxU32 ind_ = 0;
+
+	PxU32 index = 1;
+
+	if(mUpdatedSize < 512)
+	{
+		//The array of updated elements is small, so use qsort to sort them
+		for(PxU32 a = 0; a < mUpdatedSize; ++a)
+		{
+			const PxU32 handle=mUpdated[a];
+
+			const SapBox1D* Object=&asapBoxes[handle];
+				
+			PX_ASSERT(Object->mMinMax[0]!=BP_INVALID_BP_HANDLE);
+			PX_ASSERT(Object->mMinMax[1]!=BP_INVALID_BP_HANDLE);
+
+			//Get the bounds of the curr aabb.
+
+//			const ValType boxMin=boxMinMax3D[handle].getMin(Axis);
+//			const ValType boxMax=boxMinMax3D[handle].getMax(Axis);
+
+			const ValType boxMin = encodeMin(boxMinMax3D[handle], Axis, mContactDistance[handle]);
+			const ValType boxMax = encodeMax(boxMinMax3D[handle], Axis, mContactDistance[handle]);
+
+			BaseEPValues[Object->mMinMax[0]] = boxMin;
+			BaseEPValues[Object->mMinMax[1]] = boxMax;
+
+			mSortedUpdateElements[ind_++] = Object->mMinMax[0];
+			mSortedUpdateElements[ind_++] = Object->mMinMax[1];
+		}
+		Ps::sort(mSortedUpdateElements, ind_);
+	}
+	else
+	{
+		//The array of updated elements is large so use a bucket sort to sort them
+		for(; index < endPointSize; ++index)
+		{
+			if(isSentinel( BaseEPDatas[index] ))
+				break;
+			BpHandle ThisData = BaseEPDatas[index];
+			BpHandle owner = BpHandle(getOwner(ThisData));
+			if(updated[owner])
+			{
+				//BPValType ThisValue = isMax(ThisData) ? boxMinMax3D[owner].getMax(Axis) : boxMinMax3D[owner].getMin(Axis);
+				ValType ThisValue = isMax(ThisData) ? encodeMax(boxMinMax3D[owner], Axis, mContactDistance[owner])
+													: encodeMin(boxMinMax3D[owner], Axis, mContactDistance[owner]);
+				BaseEPValues[index] = ThisValue;
+				mSortedUpdateElements[ind_++] = BpHandle(index);
+			}
+		}
+	}
+
+	const PxU32 updateCounter = ind_;
+	
+	//We'll never overlap with this sentinel but it just ensures that we don't need to branch to see if
+	//there's a pocket that we need to test against
+	BroadPhaseActivityPocket* PX_RESTRICT currentPocket = mActivityPockets;
+	currentPocket->mEndIndex = 0;
+	currentPocket->mStartIndex = 0;
+
+	for(PxU32 a = 0; a < updateCounter; ++a)
+	{
+		BpHandle ind = mSortedUpdateElements[a];
+
+		BpHandle NextData;
+		BpHandle PrevData;
+		do
+		{
+			BpHandle ThisData = BaseEPDatas[ind];
+
+			const BpHandle handle = getOwner(ThisData);
+
+			BpHandle ThisIndex = ind;
+			ValType ThisValue = BaseEPValues[ThisIndex];
+
+			//Get the box1d of the curr aabb.
+			const SapBox1D* PX_RESTRICT Object=&asapBoxes[handle];
+
+			PX_ASSERT(handle!=BP_INVALID_BP_HANDLE);
+
+			PX_ASSERT(Object->mMinMax[0]!=BP_INVALID_BP_HANDLE);
+			PX_ASSERT(Object->mMinMax[1]!=BP_INVALID_BP_HANDLE);
+			PX_UNUSED(Object);
+
+			//Get the bounds of the curr aabb.
+			//const PxU32 twoHandle = 2*handle;
+			
+			const ValType boxMax=encodeMax(boxMinMax3D[handle], Axis, mContactDistance[handle]);
+
+			//We always iterate back through the list...
+			BpHandle CurrentIndex = mListPrev[ThisIndex];
+			ValType CurrentValue = BaseEPValues[CurrentIndex];
+
+			if(CurrentValue > ThisValue)
+			{
+				//We're performing some swaps so we need an activity pocket here. This structure allows us to keep track of the range of 
+				//modifications in the sorted lists. Doesn't help when everything's moving but makes a really big difference to reconstituting the 
+				//list when only a small number of things are moving
+
+				PxU32 endIndex = ind;
+				PxU32 startIndex = ind;
+
+				//const BPValType* PX_RESTRICT box0MinMax0 = &boxMinMax0[twoHandle];
+				//const BPValType* PX_RESTRICT box0MinMax1 = &boxMinMax1[twoHandle];
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+				ValType group = asapBoxGroupIds[handle];
+#endif
+				if(!isMax(ThisData))
+				{
+					do
+					{
+						BpHandle CurrentData = BaseEPDatas[CurrentIndex];
+						const BpHandle IsMax = isMax(CurrentData);
+						
+	#if PERFORM_COMPARISONS
+						if(IsMax)
+						{		
+							const BpHandle ownerId=getOwner(CurrentData);
+							SapBox1D* PX_RESTRICT id1 = asapBoxes + ownerId;
+							// Our min passed a max => start overlap
+
+							if(
+								BaseEPValues[id1->mMinMax[0]] < boxMax && 
+								//2D intersection test using up-to-date values
+								Intersect2D_Handle(boxMinMax0[handle].mMinMax[0], boxMinMax0[handle].mMinMax[1], boxMinMax1[handle].mMinMax[0], boxMinMax1[handle].mMinMax[1],
+								       boxMinMax0[ownerId].mMinMax[0],boxMinMax0[ownerId].mMinMax[1],boxMinMax1[ownerId].mMinMax[0],boxMinMax1[ownerId].mMinMax[1])
+	#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								&& (group!=asapBoxGroupIds[ownerId])
+	#else
+								&& Object!=id1
+	#endif
+								)
+							{
+								if(numPairs==maxNumPairs)
+								{
+									const PxU32 newMaxNumPairs=maxNumPairs*2;
+									pairs = reinterpret_cast<BroadPhasePair*>(resizeBroadPhasePairArray(maxNumPairs, newMaxNumPairs, mScratchAllocator, pairs));
+									maxNumPairs=newMaxNumPairs;
+								}
+								PX_ASSERT(numPairs<maxNumPairs);
+								pairs[numPairs].mVolA=BpHandle(PxMax(handle, ownerId));
+								pairs[numPairs].mVolB=BpHandle(PxMin(handle, ownerId));
+								numPairs++;
+								//AddPair(handle, getOwner(*CurrentMinData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+	#endif
+						startIndex--;
+						CurrentIndex = mListPrev[CurrentIndex];
+						CurrentValue = BaseEPValues[CurrentIndex];
+					}
+					while(ThisValue < CurrentValue);
+				}			
+				else 
+				{
+					// Max is moving left:
+					do
+					{
+						BpHandle CurrentData = BaseEPDatas[CurrentIndex];
+						const BpHandle IsMax = isMax(CurrentData);
+						
+	#if PERFORM_COMPARISONS
+						if(!IsMax)
+						{
+							// Our max passed a min => stop overlap
+							const BpHandle ownerId=getOwner(CurrentData);
+
+#if 1
+							if(
+#if BP_SAP_USE_OVERLAP_TEST_ON_REMOVES
+								Intersect2D_Handle(boxMinMax0[handle].mMinMax[0], boxMinMax0[handle].mMinMax[1], boxMinMax1[handle].mMinMax[0], boxMinMax1[handle].mMinMax[1],
+								       boxMinMax0[ownerId].mMinMax[0],boxMinMax0[ownerId].mMinMax[1],boxMinMax1[ownerId].mMinMax[0],boxMinMax1[ownerId].mMinMax[1])
+#endif
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								&& (group!=asapBoxGroupIds[ownerId])
+#else
+								&& Object!=id1
+#endif
+								)
+#endif
+							{
+								if(numPairs==maxNumPairs)
+								{
+									const PxU32 newMaxNumPairs=maxNumPairs*2;
+									pairs = reinterpret_cast<BroadPhasePair*>(resizeBroadPhasePairArray(maxNumPairs, newMaxNumPairs, mScratchAllocator, pairs));
+									maxNumPairs=newMaxNumPairs;
+								}
+								PX_ASSERT(numPairs<maxNumPairs);
+								pairs[numPairs].mVolA=BpHandle(PxMin(handle, ownerId));
+								pairs[numPairs].mVolB=BpHandle(PxMax(handle, ownerId));
+								numPairs++;
+								//RemovePair(handle, getOwner(*CurrentMaxData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+	#endif
+						startIndex--;
+						CurrentIndex = mListPrev[CurrentIndex];
+						CurrentValue = BaseEPValues[CurrentIndex];
+					}
+					while(ThisValue < CurrentValue);
+				}
+
+				//This test is unnecessary. If we entered the outer loop, we're doing the swap in here
+				{
+					//Unlink from old position and re-link to new position
+					BpHandle oldNextIndex = mListNext[ThisIndex];
+					BpHandle oldPrevIndex = mListPrev[ThisIndex];
+
+					BpHandle newNextIndex = mListNext[CurrentIndex];
+					BpHandle newPrevIndex = CurrentIndex;
+					
+					//Unlink this node
+					mListNext[oldPrevIndex] = oldNextIndex;
+					mListPrev[oldNextIndex] = oldPrevIndex;
+
+					//Link it to it's new place in the list
+					mListNext[ThisIndex] = newNextIndex;
+					mListPrev[ThisIndex] = newPrevIndex;
+					mListPrev[newNextIndex] = ThisIndex;
+					mListNext[newPrevIndex] = ThisIndex;
+				}
+
+				//Loop over the activity pocket stack to make sure this set of shuffles didn't 
+				//interfere with the previous set. If it did, we roll this pocket into the previous
+				//pockets. If everything in the scene is moving, we should result in just 1 pocket
+				while(startIndex < currentPocket->mStartIndex)
+				{
+					currentPocket--;
+				}
+				//If our start index > currentPocket->mEndIndex, then we don't overlap so create a new pocket
+				if(currentPocket == mActivityPockets || startIndex > (currentPocket->mEndIndex+1))
+				{
+					currentPocket++;
+					currentPocket->mStartIndex = startIndex;
+				}
+				currentPocket->mEndIndex = endIndex;
+			}// update max
+			//Get prev and next ptr...
+
+			NextData = BaseEPDatas[++ind];
+			PrevData = BaseEPDatas[mListPrev[ind]];
+
+		}while(!isSentinel(NextData) && !updated[getOwner(NextData)] && updated[getOwner(PrevData)]);
+		
+	}// updated aabbs
+
+	pairsSize=numPairs;
+	pairsCapacity=maxNumPairs;
+
+
+	BroadPhaseActivityPocket* pocket = mActivityPockets+1;
+
+	while(pocket <= currentPocket)
+	{
+		//PxU32 CurrIndex = mListPrev[pocket->mStartIndex];
+		for(PxU32 a = pocket->mStartIndex; a <= pocket->mEndIndex; ++a)
+		{
+			mListPrev[a] = BpHandle(a);
+		}
+
+		//Now copy all the data to the array, updating the remap table
+		PxU32 CurrIndex = pocket->mStartIndex-1;
+		for(PxU32 a = pocket->mStartIndex; a <= pocket->mEndIndex; ++a)
+		{
+			CurrIndex = mListNext[CurrIndex];
+			PxU32 origIndex =  CurrIndex;
+			BpHandle remappedIndex = mListPrev[origIndex];
+
+			if(origIndex != a)
+			{
+				const BpHandle ownerId=getOwner(BaseEPDatas[remappedIndex]);
+				const BpHandle IsMax = isMax(BaseEPDatas[remappedIndex]);
+				ValType tmp = BaseEPValues[a];
+				BpHandle tmpHandle = BaseEPDatas[a];
+
+				BaseEPValues[a] = BaseEPValues[remappedIndex];
+				BaseEPDatas[a] = BaseEPDatas[remappedIndex];
+
+				BaseEPValues[remappedIndex] = tmp;
+				BaseEPDatas[remappedIndex] = tmpHandle;
+
+				mListPrev[remappedIndex] = mListPrev[a];
+				//Write back remap index (should be an immediate jump to original index)
+				mListPrev[mListPrev[a]] = remappedIndex;
+				asapBoxes[ownerId].mMinMax[IsMax] = BpHandle(a);
+			}
+			
+		}
+
+		for(PxU32 a = pocket->mStartIndex-1; a <= pocket->mEndIndex; ++a)
+		{
+			mListPrev[a+1] = BpHandle(a);
+			mListNext[a] = BpHandle(a+1);
+		}
+		pocket++;
+	}
+}
+
+#if PX_DEBUG
+
+bool BroadPhaseSap::isSelfOrdered() const 
+{
+	if(0==mBoxesSize)
+	{
+		return true;
+	}
+
+	for(PxU32 Axis=0;Axis<3;Axis++)
+	{
+		PxU32 it=1;
+		PX_ASSERT(mEndPointDatas[Axis]);
+		while(!isSentinel(mEndPointDatas[Axis][it]))
+		{
+			//Test the array is sorted.
+			const ValType prevVal=mEndPointValues[Axis][it-1];
+			const ValType currVal=mEndPointValues[Axis][it];
+			if(currVal<prevVal)
+			{
+				return false;
+			}
+
+			//Test the end point array is consistent.
+			const BpHandle ismax=isMax(mEndPointDatas[Axis][it]);
+			const BpHandle ownerId=getOwner(mEndPointDatas[Axis][it]);
+			if(mBoxEndPts[Axis][ownerId].mMinMax[ismax]!=it)
+			{
+				return false;
+			}
+
+			//Test the mins are even, the maxes are odd, and the extents are finite.
+			const ValType boxMin = mEndPointValues[Axis][mBoxEndPts[Axis][ownerId].mMinMax[0]];
+			const ValType boxMax = mEndPointValues[Axis][mBoxEndPts[Axis][ownerId].mMinMax[1]];
+			if(boxMin & 1)
+			{
+				return false;
+			}
+			if(0==(boxMax & 1))
+			{
+				return false;
+			}
+			if(boxMax<=boxMin)
+			{
+				return false;
+			}
+
+			it++;
+		}
+	}
+
+	return true;
+}
+
+bool BroadPhaseSap::isSelfConsistent() const 
+{
+	if(0==mBoxesSize)
+	{
+		return true;
+	}
+
+	for(PxU32 Axis=0;Axis<3;Axis++)
+	{
+		PxU32 it=1;
+		ValType prevVal=0;
+		const PxBounds3* PX_RESTRICT boxMinMax = mBoxBoundsMinMax;
+		const PxReal* PX_RESTRICT contactDistance = mContactDistance;
+		PX_ASSERT(mEndPointDatas[Axis]);
+		while(!isSentinel(mEndPointDatas[Axis][it]))
+		{
+			const BpHandle ownerId=getOwner(mEndPointDatas[Axis][it]);
+			const BpHandle ismax=isMax(mEndPointDatas[Axis][it]);
+			const ValType boxMinMaxs[2] = { encodeMin(boxMinMax[ownerId], Axis, contactDistance[ownerId]),
+											encodeMax(boxMinMax[ownerId], Axis, contactDistance[ownerId]) };
+//			const ValType boxMinMaxs[2] = { boxMinMax[ownerId].getMin(Axis), boxMinMax[ownerId].getMax(Axis) };
+			const ValType test1=boxMinMaxs[ismax];
+			const ValType test2=mEndPointValues[Axis][it];
+			if(test1!=test2)
+			{
+				return false;
+			}
+			if(test2<prevVal)
+			{
+				return false;
+			}
+			prevVal=test2;
+
+			if(mBoxEndPts[Axis][ownerId].mMinMax[ismax]!=it)
+			{
+				return false;
+			}
+
+			it++;
+		}
+	}
+
+	for(PxU32 i=0;i<mCreatedPairsSize;i++)
+	{
+		const PxU32 a=mCreatedPairsArray[i].mVolA;
+		const PxU32 b=mCreatedPairsArray[i].mVolB;
+		IntegerAABB aabb0(mBoxBoundsMinMax[a], mContactDistance[a]);
+		IntegerAABB aabb1(mBoxBoundsMinMax[b], mContactDistance[b]);
+		if(!aabb0.intersects(aabb1))
+		{
+			return false;
+		}
+	}
+
+	for(PxU32 i=0;i<mDeletedPairsSize;i++)
+	{
+		const PxU32 a=mDeletedPairsArray[i].mVolA;
+		const PxU32 b=mDeletedPairsArray[i].mVolB;
+
+		bool isDeleted=false;
+		for(PxU32 j=0;j<mRemovedSize;j++)
+		{
+			if(a==mRemoved[j] || b==mRemoved[j])
+			{
+				isDeleted=true;
+			}
+		}
+
+		if(!isDeleted)
+		{
+			IntegerAABB aabb0(mBoxBoundsMinMax[a], mContactDistance[a]);
+			IntegerAABB aabb1(mBoxBoundsMinMax[b], mContactDistance[b]);
+			if(aabb0.intersects(aabb1))
+			{
+				// with the past refactors this should have become illegal
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+#endif
+
+
+
+/*
+
+PX_FORCE_INLINE bool intersect1D_Max(	const SAP_AABB& a,
+									 const SapBox1D*const* PX_RESTRICT boxEndPts,
+									 PxU32 ownerId,
+									 const BPValType* const endPointValues, PxU32 axis)
+{
+	const SapBox1D* PX_RESTRICT b = boxEndPts[axis] + ownerId;
+
+	const BPValType& endPointValue = endPointValues[b->mMinMax[0]];
+	return (endPointValue < a.GetMax(axis));
+}
+
+PX_FORCE_INLINE bool intersect1D_Min(	const SAP_AABB& a,
+									 const SapBox1D*const* PX_RESTRICT boxEndPts,
+									 PxU32 ownerId,
+									 const BPValType* PX_RESTRICT endPointValues,
+									 PxU32 axis)
+{
+	const SapBox1D* PX_RESTRICT b = boxEndPts[axis] + ownerId;
+
+	const BPValType& endPointValue = endPointValues[b->mMinMax[1]];
+	return (endPointValue >= a.GetMin(axis));
+}
+
+void PxsBroadPhaseContextSap::batchUpdate()
+{
+	for(PxU32 i=0;i<mUpdatedSize;i++)
+	{
+		const PxU32 handle = mUpdated[i];
+		PX_ASSERT(handle!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(mBoxEndPts[0][handle].mMinMax[0]!=BP_INVALID_BP_HANDLE);
+
+		SapBox1D* Object[3] = {&mBoxEndPts[0][handle], &mBoxEndPts[1][handle], &mBoxEndPts[2][handle]};
+
+		PX_ASSERT(mBoxEndPts[0][handle].mMinMax[0]!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(mBoxEndPts[0][handle].mMinMax[1]!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(mBoxEndPts[1][handle].mMinMax[0]!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(mBoxEndPts[1][handle].mMinMax[1]!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(mBoxEndPts[2][handle].mMinMax[0]!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(mBoxEndPts[2][handle].mMinMax[1]!=BP_INVALID_BP_HANDLE);
+
+		IntegerAABB box;
+		box.mMinX = mBoxBoundsMinMaxX[2*handle+0];
+		box.mMaxX = mBoxBoundsMinMaxX[2*handle+1];
+		box.mMinY = mBoxBoundsMinMaxY[2*handle+0];
+		box.mMaxY = mBoxBoundsMinMaxY[2*handle+1];
+		box.mMinZ = mBoxBoundsMinMaxZ[2*handle+0];
+		box.mMaxZ = mBoxBoundsMinMaxZ[2*handle+1];
+
+		//	PxU32 Axis=0;
+		for(PxU32 Axis=0;Axis<3;Axis++)
+		{
+			const SapBox1D* Object_Axis = &mBoxEndPts[Axis][handle];
+
+			const PxU32 Axis1 = (1  << Axis) & 3;
+			const PxU32 Axis2 = (1  << Axis1) & 3;
+
+			BPValType* const BaseEPValue = mEndPointValues[Axis];
+			PxBpHandle* const BaseEPData = mEndPointDatas[Axis];
+
+			// Update min
+			{
+				const PxBpHandle MinMaxIndex = Object_Axis->mMinMax[0];
+				BPValType* CurrentMinValue = BaseEPValue + MinMaxIndex;
+				PxBpHandle* CurrentMinData = BaseEPData + MinMaxIndex;
+				PX_ASSERT(!isMax(*CurrentMinData));
+
+				const BPValType Limit = box.GetMin(Axis);
+				if(Limit < *CurrentMinValue)
+				{
+					*CurrentMinValue = Limit;
+
+					// Min is moving left:
+					BPValType SavedValue = *CurrentMinValue;
+					PxBpHandle SavedData = *CurrentMinData;
+					PxU32 EPIndex = PxU32(size_t(CurrentMinData) - size_t(BaseEPData))/sizeof(PxBpHandle);
+					const PxU32 SavedIndex = EPIndex;
+
+					CurrentMinData--;
+					CurrentMinValue--;
+					while(*CurrentMinValue > Limit)
+					{
+#if BP_SAP_USE_PREFETCH
+						Ps::prefetchLine(CurrentMinValue-1);
+						Ps::prefetchLine(CurrentMinData-1);
+#endif
+						const PxU32 ownerId = getOwner(*CurrentMinData);
+						SapBox1D* id1box = mBoxEndPts[Axis] + ownerId;
+
+						const PxU32 IsMax = isMax(*CurrentMinData);
+						if(IsMax)
+						{
+							// Our min passed a max => start overlap
+							if(
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								(mBoxGroups[handle]!=mBoxGroups[ownerId]) && 
+#endif								
+								intersect2D(Object, mBoxEndPts, ownerId, Axis1, Axis2) &&
+								intersect1D_Max(box, mBoxEndPts, ownerId, BaseEPValue, Axis) &&
+								Object_Axis != id1box)
+							{
+								AddPair(handle, getOwner(*CurrentMinData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+
+						id1box->mMinMax[IsMax] = EPIndex--;
+						*(CurrentMinValue+1) = *CurrentMinValue;
+						*(CurrentMinData+1) = *CurrentMinData;
+
+						CurrentMinValue--;
+						CurrentMinData--;
+					}
+
+					if(SavedIndex!=EPIndex)
+					{
+						mBoxEndPts[Axis][getOwner(SavedData)].mMinMax[isMax(SavedData)] = EPIndex;
+						BaseEPValue[EPIndex] = SavedValue;
+						BaseEPData[EPIndex] = SavedData;
+					}
+				}
+				else if(Limit > *CurrentMinValue)
+				{
+					*CurrentMinValue = Limit;
+
+					// Min is moving right:
+					BPValType SavedValue = *CurrentMinValue;
+					PxBpHandle SavedData = *CurrentMinData;
+
+					PxU32 EPIndex = PxU32(size_t(CurrentMinData) - size_t(BaseEPData))/sizeof(PxBpHandle);
+					const PxU32 SavedIndex = EPIndex;
+
+					CurrentMinValue++;
+					CurrentMinData++;
+					while(Limit > (*CurrentMinValue))
+					{
+#if BP_SAP_USE_PREFETCH
+						Ps::prefetchLine(CurrentMinValue+1);
+						Ps::prefetchLine(CurrentMinData+1);
+#endif
+						const PxU32 ownerId = getOwner(*CurrentMinData);
+						SapBox1D* id1box = mBoxEndPts[Axis] + ownerId;
+
+						const PxU32 IsMax = isMax(*CurrentMinData);
+						if(IsMax)
+						{
+							// Our min passed a max => stop overlap
+							if(
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								(mBoxGroups[handle]!=mBoxGroups[ownerId]) && 
+#endif								
+#if BP_SAP_USE_OVERLAP_TEST_ON_REMOVES
+								intersect2D(Object, mBoxEndPts, ownerId, Axis1, Axis2) &&
+#endif
+								Object_Axis != id1box)
+							{
+								RemovePair(handle, getOwner(*CurrentMinData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+
+						id1box->mMinMax[IsMax] = EPIndex++;
+						*(CurrentMinValue-1) = *CurrentMinValue;
+						*(CurrentMinData-1) = *CurrentMinData;
+
+						CurrentMinValue++;
+						CurrentMinData++;
+					}
+
+					if(SavedIndex!=EPIndex)
+					{
+						mBoxEndPts[Axis][getOwner(SavedData)].mMinMax[isMax(SavedData)] = EPIndex;
+						BaseEPValue[EPIndex] = SavedValue;
+						BaseEPData[EPIndex] = SavedData;
+					}
+				}
+			}
+
+			// Update max
+			{
+				const PxBpHandle MinMaxIndex = Object_Axis->mMinMax[1];
+				BPValType* CurrentMaxValue = BaseEPValue + MinMaxIndex;
+				PxBpHandle* CurrentMaxData = BaseEPData + MinMaxIndex;
+				PX_ASSERT(isMax(*CurrentMaxData));
+
+				const BPValType Limit = box.GetMax(Axis);
+				if(Limit > *CurrentMaxValue)
+				{
+					*CurrentMaxValue = Limit;
+
+					// Max is moving right:
+					BPValType SavedValue = *CurrentMaxValue;
+					PxBpHandle SavedData = *CurrentMaxData;
+
+					PxU32 EPIndex = PxU32(size_t(CurrentMaxData) - size_t(BaseEPData))/sizeof(PxBpHandle);
+					const PxU32 SavedIndex = EPIndex;
+
+					CurrentMaxValue++;
+					CurrentMaxData++;
+					while((*CurrentMaxValue) < Limit)
+					{
+#if BP_SAP_USE_PREFETCH
+						Ps::prefetchLine(CurrentMaxValue+1);
+						Ps::prefetchLine(CurrentMaxData+1);
+#endif
+						const PxU32 ownerId = getOwner(*CurrentMaxData);
+						SapBox1D* id1box = mBoxEndPts[Axis] + ownerId;
+
+						const PxU32 IsMax = isMax(*CurrentMaxData);
+						if(!IsMax)
+						{
+							// Our max passed a min => start overlap
+							if(
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								(mBoxGroups[handle]!=mBoxGroups[ownerId]) && 
+#endif								
+								intersect2D(Object, mBoxEndPts, ownerId, Axis1, Axis2) &&
+								intersect1D_Min(box, mBoxEndPts, ownerId, BaseEPValue, Axis) &&
+								Object_Axis != id1box)
+							{
+								AddPair(handle, getOwner(*CurrentMaxData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+
+						id1box->mMinMax[IsMax] = EPIndex++;
+						*(CurrentMaxValue-1) = *CurrentMaxValue;
+						*(CurrentMaxData-1) = *CurrentMaxData;
+
+						CurrentMaxValue++;
+						CurrentMaxData++;
+					}
+
+					if(SavedIndex!=EPIndex)
+					{
+						mBoxEndPts[Axis][getOwner(SavedData)].mMinMax[isMax(SavedData)] = EPIndex;
+						BaseEPValue[EPIndex] = SavedValue;
+						BaseEPData[EPIndex] = SavedData;
+					}
+				}
+				else if(Limit < *CurrentMaxValue)
+				{
+					*CurrentMaxValue = Limit;
+
+					// Max is moving left:
+					BPValType SavedValue = *CurrentMaxValue;
+					PxBpHandle SavedData = *CurrentMaxData;
+
+					PxU32 EPIndex = PxU32(size_t(CurrentMaxData) - size_t(BaseEPData))/sizeof(PxBpHandle);
+					const PxU32 SavedIndex = EPIndex;
+
+					CurrentMaxData--;
+					CurrentMaxValue--;
+					while(Limit < (*CurrentMaxValue))
+					{
+#if BP_SAP_USE_PREFETCH
+						Ps::prefetchLine(CurrentMaxValue-1);
+						Ps::prefetchLine(CurrentMaxData-1);
+#endif
+						const PxU32 ownerId = getOwner(*CurrentMaxData);
+						SapBox1D* id1box = mBoxEndPts[Axis] + ownerId;
+
+						const PxU32 IsMax = isMax(*CurrentMaxData);
+						if(!IsMax)
+						{
+							// Our max passed a min => stop overlap
+							if(
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+								(mBoxGroups[handle]!=mBoxGroups[ownerId]) && 
+#endif								
+#if BP_SAP_USE_OVERLAP_TEST_ON_REMOVES
+								intersect2D(Object, mBoxEndPts, ownerId, Axis1, Axis2) &&
+#endif
+								Object_Axis != id1box)
+							{
+								RemovePair(handle, getOwner(*CurrentMaxData), mPairs, mData, mDataSize, mDataCapacity);
+							}
+						}
+
+						id1box->mMinMax[IsMax] = EPIndex--;
+						*(CurrentMaxValue+1) = *CurrentMaxValue;
+						*(CurrentMaxData+1) = *CurrentMaxData;
+
+						CurrentMaxData--;
+						CurrentMaxValue--;
+					}
+
+					if(SavedIndex!=EPIndex)
+					{
+						mBoxEndPts[Axis][getOwner(SavedData)].mMinMax[isMax(SavedData)] = EPIndex;
+						BaseEPValue[EPIndex] = SavedValue;
+						BaseEPData[EPIndex] = SavedData;
+					}
+				}
+			}
+		}
+	}
+}
+*/
+
+} //namespace Bp
+
+} //namespace physx
+
+
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.h b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.h
new file mode 100644
index 00000000..83a9c54d
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSap.h
@@ -0,0 +1,226 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef BP_BROADPHASE_SAP_H
+#define BP_BROADPHASE_SAP_H
+
+#include "BpBroadPhase.h"
+#include "BpBroadPhaseSapAux.h"
+#include "CmPool.h"
+#include "CmPhysXCommon.h"
+#include "BpSAPTasks.h"
+#include "PsUserAllocated.h"
+
+namespace physx
+{
+
+// Forward declarations
+class PxcScratchAllocator;
+class PxcScratchAllocator;
+
+namespace Gu
+{
+	class Axes;
+}
+
+namespace Bp
+{
+
+class SapEndPoint;
+class IntegerAABB;
+
+class BroadPhaseBatchUpdateWorkTask: public Cm::Task
+{
+public:
+
+	BroadPhaseBatchUpdateWorkTask() 
+		: Cm::Task(), 
+		  mSap(NULL),
+   	      mAxis(0xffffffff),
+		  mPairs(NULL),
+		  mPairsSize(0),
+		  mPairsCapacity(0)
+	{
+	}
+
+	virtual void runInternal();
+
+	virtual const char* getName() const { return "BpBroadphaseSap.batchUpdate"; }
+
+	void set(class BroadPhaseSap* sap, const PxU32 axis) {mSap = sap; mAxis = axis;}
+
+	BroadPhasePair* getPairs() const {return mPairs;}
+	PxU32 getPairsSize() const {return mPairsSize;}
+	PxU32 getPairsCapacity() const {return mPairsCapacity;}
+
+	void setPairs(BroadPhasePair* pairs, const PxU32 pairsCapacity) {mPairs = pairs; mPairsCapacity = pairsCapacity;}
+
+	void setNumPairs(const PxU32 pairsSize) {mPairsSize=pairsSize;}
+
+private:
+
+	class BroadPhaseSap* mSap;
+	PxU32 mAxis;
+
+	BroadPhasePair* mPairs;
+	PxU32 mPairsSize;
+	PxU32 mPairsCapacity;
+};
+
+//KS - TODO, this could be reduced to U16 in smaller scenes
+struct BroadPhaseActivityPocket
+{
+	PxU32 mStartIndex;
+	PxU32 mEndIndex;
+};
+
+
+class BroadPhaseSap : public BroadPhase, public Ps::UserAllocated
+{
+	PX_NOCOPY(BroadPhaseSap)
+public:
+
+	friend class BroadPhaseBatchUpdateWorkTask;
+	friend class SapUpdateWorkTask;
+	friend class SapPostUpdateWorkTask;
+
+										BroadPhaseSap(const PxU32 maxNbBroadPhaseOverlaps, const PxU32 maxNbStaticShapes, const PxU32 maxNbDynamicShapes, PxU64 contextID);
+	virtual								~BroadPhaseSap();
+	virtual	void						destroy();
+
+	virtual	PxBroadPhaseType::Enum		getType()					const	{ return PxBroadPhaseType::eSAP;	}
+
+	virtual	void						update(const PxU32 numCpuTasks, PxcScratchAllocator* scratchAllocator, const BroadPhaseUpdateData& updateData, physx::PxBaseTask* continuation, physx::PxBaseTask* narrowPhaseUnblockTask);
+
+	virtual PxU32						getNbCreatedPairs()		const		{ return mCreatedPairsSize;		}
+	virtual BroadPhasePairReport*		getCreatedPairs()					{ return mCreatedPairsArray;	}
+	virtual PxU32						getNbDeletedPairs()		const		{ return mDeletedPairsSize;		}
+	virtual BroadPhasePairReport*		getDeletedPairs()					{ return mDeletedPairsArray;	}
+
+	virtual void						resizeBuffers();
+	virtual void						freeBuffers();
+
+	virtual void						shiftOrigin(const PxVec3& shift);
+	//~BroadPhase
+
+#if PX_CHECKED
+	virtual bool						isValid(const BroadPhaseUpdateData& updateData) const;
+#endif
+
+	virtual BroadPhasePair*				getBroadPhasePairs() const  {return mPairs.mActivePairs;}
+
+	virtual void						deletePairs();
+
+private:
+
+			PxcScratchAllocator*		mScratchAllocator;
+
+			SapUpdateWorkTask			mSapUpdateWorkTask;
+			SapPostUpdateWorkTask		mSapPostUpdateWorkTask;
+
+	//Data passed in from updateV.
+			const BpHandle*				mCreated;				
+			PxU32						mCreatedSize;			
+			const BpHandle*				mRemoved;				
+			PxU32						mRemovedSize;				
+			const BpHandle*				mUpdated;				
+			PxU32						mUpdatedSize;				
+			const PxBounds3*			mBoxBoundsMinMax;			
+			const BpHandle*				mBoxGroups;
+			const PxReal*				mContactDistance;
+			PxU32						mBoxesCapacity;
+
+
+	//Boxes.
+			SapBox1D*					mBoxEndPts[3];			//Position of box min/max in sorted arrays of end pts (needs to have mBoxesCapacity).
+
+	//End pts (endpts of boxes sorted along each axis).
+			ValType*					mEndPointValues[3];		//Sorted arrays of min and max box coords
+			BpHandle*					mEndPointDatas[3];		//Corresponding owner id and isMin/isMax for each entry in the sorted arrays of min and max box coords.
+
+			PxU8*						mBoxesUpdated;	
+			BpHandle*					mSortedUpdateElements;	
+			BroadPhaseActivityPocket*	mActivityPockets;
+			BpHandle*					mListNext;
+			BpHandle*					mListPrev;
+
+			PxU32						mBoxesSize;				//Number of sorted boxes + number of unsorted (new) boxes
+			PxU32						mBoxesSizePrev;			//Number of sorted boxes 
+			PxU32						mEndPointsCapacity;		//Capacity of sorted arrays. 
+
+	//Default maximum number of overlap pairs 
+			PxU32						mDefaultPairsCapacity;
+
+	//Box-box overlap pairs created or removed each update.
+			BpHandle*					mData;
+			PxU32						mDataSize;
+			PxU32						mDataCapacity;
+
+	//All current box-box overlap pairs.
+			SapPairManager				mPairs;
+
+	//Created and deleted overlap pairs reported back through api.
+			BroadPhasePairReport*		mCreatedPairsArray;
+			PxU32						mCreatedPairsSize;
+			PxU32						mCreatedPairsCapacity;
+			BroadPhasePairReport*		mDeletedPairsArray;
+			PxU32						mDeletedPairsSize;
+			PxU32						mDeletedPairsCapacity;
+			PxU32						mActualDeletedPairSize;
+
+			bool						setUpdateData(const BroadPhaseUpdateData& updateData);
+			void						update(physx::PxBaseTask* continuation);
+			void						postUpdate(physx::PxBaseTask* continuation);
+
+	//Batch create/remove/update.
+			void						batchCreate();
+			void						batchRemove();
+			void						batchUpdate();
+
+			void						batchUpdate(const PxU32 Axis, BroadPhasePair*& pairs, PxU32& pairsSize, PxU32& pairsCapacity);
+
+			void						batchUpdateFewUpdates(const PxU32 Axis, BroadPhasePair*& pairs, PxU32& pairsSize, PxU32& pairsCapacity);
+
+			void						performBoxPruning(const Gu::Axes axes);
+
+			BroadPhaseBatchUpdateWorkTask mBatchUpdateTasks[3];
+
+			PxU64						mContextID;
+#if PX_DEBUG
+			bool						isSelfOrdered() const;
+			bool						isSelfConsistent() const;
+#endif
+};
+
+} //namespace Bp
+
+} //namespace physx
+
+#endif //BP_BROADPHASE_SAP_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSapAux.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSapAux.cpp
new file mode 100644
index 00000000..98b8e482
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSapAux.cpp
@@ -0,0 +1,876 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "CmPhysXCommon.h"
+#include "BpBroadPhaseSapAux.h"
+#include "PsFoundation.h"
+
+namespace physx
+{
+
+namespace Bp
+{
+
+PX_FORCE_INLINE void PxBpHandleSwap(BpHandle& a, BpHandle& b)													
+{ 
+	const BpHandle c = a; a = b; b = c;		
+}
+
+PX_FORCE_INLINE void Sort(BpHandle& id0, BpHandle& id1)										
+{ 
+	if(id0>id1)	PxBpHandleSwap(id0, id1);						
+}
+
+PX_FORCE_INLINE bool DifferentPair(const BroadPhasePair& p, BpHandle id0, BpHandle id1)	
+{ 
+	return (id0!=p.mVolA) || (id1!=p.mVolB);						
+}
+
+PX_FORCE_INLINE int Hash32Bits_1(int key)
+{
+	key += ~(key << 15);
+	key ^=  (key >> 10);
+	key +=  (key << 3);
+	key ^=  (key >> 6);
+	key += ~(key << 11);
+	key ^=  (key >> 16);
+	return key;
+}
+
+PX_FORCE_INLINE PxU32 Hash(BpHandle id0, BpHandle id1)								
+{ 
+	return PxU32(Hash32Bits_1( int(PxU32(id0)|(PxU32(id1)<<16)) ));
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+SapPairManager::SapPairManager() :
+	mHashTable				(NULL),
+	mNext					(NULL),
+	mHashSize				(0),
+	mHashCapacity			(0),
+	mMinAllowedHashCapacity	(0),
+	mActivePairs			(NULL),
+	mActivePairStates		(NULL),
+	mNbActivePairs			(0),
+	mActivePairsCapacity	(0),
+	mMask					(0)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+SapPairManager::~SapPairManager()
+{
+	PX_ASSERT(NULL==mHashTable);
+	PX_ASSERT(NULL==mNext);
+	PX_ASSERT(NULL==mActivePairs);
+	PX_ASSERT(NULL==mActivePairStates);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+void SapPairManager::init(const PxU32 size)
+{
+	mHashTable=reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(BpHandle)*size), "BpHandle"));
+	mNext=reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(BpHandle)*size), "BpHandle"));
+	mActivePairs=reinterpret_cast<BroadPhasePair*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(BroadPhasePair)*size), "BroadPhasePair"));
+	mActivePairStates=reinterpret_cast<PxU8*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(PxU8)*size), "BroadPhaseContextSap ActivePairStates"));
+	mHashCapacity=size;
+	mMinAllowedHashCapacity = size;
+	mActivePairsCapacity=size;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void SapPairManager::release()
+{
+	PX_FREE(mHashTable);
+	PX_FREE(mNext);
+	PX_FREE(mActivePairs);
+	PX_FREE(mActivePairStates);
+	mHashTable				= NULL;
+	mNext					= NULL;
+	mActivePairs			= NULL;
+	mActivePairStates		= NULL;
+	mNext					= 0;
+	mHashSize				= 0;
+	mHashCapacity			= 0;
+	mMinAllowedHashCapacity	= 0;
+	mNbActivePairs			= 0;
+	mActivePairsCapacity	= 0;
+	mMask					= 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const BroadPhasePair* SapPairManager::FindPair(BpHandle id0, BpHandle id1) const
+{
+	if(0==mHashSize) return NULL;	// Nothing has been allocated yet
+
+	// Order the ids
+	Sort(id0, id1);
+
+	// Compute hash value for this pair
+	PxU32 HashValue = Hash(id0, id1) & mMask;
+	PX_ASSERT(HashValue<mHashCapacity);
+
+	// Look for it in the table
+	PX_ASSERT(HashValue<mHashCapacity);
+	PxU32 Offset = mHashTable[HashValue];
+	PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
+	while(Offset!=BP_INVALID_BP_HANDLE && DifferentPair(mActivePairs[Offset], id0, id1))
+	{
+		PX_ASSERT(mActivePairs[Offset].mVolA!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(Offset<mHashCapacity);
+		Offset = mNext[Offset];		// Better to have a separate array for this
+		PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
+	}
+	if(Offset==BP_INVALID_BP_HANDLE)	return NULL;
+	PX_ASSERT(Offset<mNbActivePairs);
+	// Match mActivePairs[Offset] => the pair is persistent
+	PX_ASSERT(Offset<mActivePairsCapacity);
+	return &mActivePairs[Offset];
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Internal version saving hash computation
+PX_FORCE_INLINE BroadPhasePair* SapPairManager::FindPair(BpHandle id0, BpHandle id1, PxU32 hash_value) const
+{
+	if(0==mHashSize) return NULL;	// Nothing has been allocated yet
+
+	// Look for it in the table
+	PX_ASSERT(hash_value<mHashCapacity);
+	PxU32 Offset = mHashTable[hash_value];
+	PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
+	while(Offset!=BP_INVALID_BP_HANDLE && DifferentPair(mActivePairs[Offset], id0, id1))
+	{
+		PX_ASSERT(mActivePairs[Offset].mVolA!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(Offset<mHashCapacity);
+		Offset = mNext[Offset];		// Better to have a separate array for this
+		PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
+	}
+	if(Offset==BP_INVALID_BP_HANDLE)	return NULL;
+	PX_ASSERT(Offset<mNbActivePairs);
+	// Match mActivePairs[Offset] => the pair is persistent
+	PX_ASSERT(Offset<mActivePairsCapacity);
+	return &mActivePairs[Offset];
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const BroadPhasePair* SapPairManager::AddPair(BpHandle id0, BpHandle id1, const PxU8 state)
+{
+	if(MAX_BP_HANDLE == mNbActivePairs)
+	{
+		PX_WARN_ONCE(MAX_BP_PAIRS_MESSAGE);
+		return NULL;
+	}
+
+	// Order the ids
+	Sort(id0, id1);
+
+	PxU32 HashValue = Hash(id0, id1) & mMask;
+
+	BroadPhasePair* P = FindPair(id0, id1, HashValue);
+	if(P)
+	{
+		return P;	// Persistent pair
+	}
+
+	// This is a new pair
+	if(mNbActivePairs >= mHashSize)
+	{
+		// Get more entries
+		mHashSize = Ps::nextPowerOfTwo(mNbActivePairs+1);
+		mMask = mHashSize-1;
+
+		reallocPairs(mHashSize>mHashCapacity);
+
+		// Recompute hash value with new hash size
+		HashValue = Hash(id0, id1) & mMask;
+	}
+
+	PX_ASSERT(mNbActivePairs<mActivePairsCapacity);
+	BroadPhasePair* p = &mActivePairs[mNbActivePairs];
+	p->mVolA		= id0;	// ### CMOVs would be nice here
+	p->mVolB		= id1;
+	mActivePairStates[mNbActivePairs]=state;
+
+	PX_ASSERT(mNbActivePairs<mHashSize);
+	PX_ASSERT(mNbActivePairs<mHashCapacity);
+	PX_ASSERT(HashValue<mHashCapacity);
+	mNext[mNbActivePairs] = mHashTable[HashValue];
+	mHashTable[HashValue] = BpHandle(mNbActivePairs++);
+	return p;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void SapPairManager::RemovePair(BpHandle /*id0*/, BpHandle /*id1*/, PxU32 hash_value, PxU32 pair_index)
+{
+	// Walk the hash table to fix mNext
+	{
+		PX_ASSERT(hash_value<mHashCapacity);
+		PxU32 Offset = mHashTable[hash_value];
+		PX_ASSERT(Offset!=BP_INVALID_BP_HANDLE);
+
+		PxU32 Previous=BP_INVALID_BP_HANDLE;
+		while(Offset!=pair_index)
+		{
+			Previous = Offset;
+			PX_ASSERT(Offset<mHashCapacity);
+			Offset = mNext[Offset];
+		}
+
+		// Let us go/jump us
+		if(Previous!=BP_INVALID_BP_HANDLE)
+		{
+			PX_ASSERT(Previous<mHashCapacity);
+			PX_ASSERT(pair_index<mHashCapacity);
+			PX_ASSERT(mNext[Previous]==pair_index);
+			mNext[Previous] = mNext[pair_index];
+		}
+		// else we were the first
+		else
+		{
+			PX_ASSERT(hash_value<mHashCapacity);
+			PX_ASSERT(pair_index<mHashCapacity);
+			mHashTable[hash_value] = mNext[pair_index];
+		}
+	}
+	// we're now free to reuse mNext[PairIndex] without breaking the list
+
+#if PX_DEBUG
+	PX_ASSERT(pair_index<mHashCapacity);
+	mNext[pair_index]=BP_INVALID_BP_HANDLE;
+#endif
+	// Invalidate entry
+
+	// Fill holes
+	{
+		// 1) Remove last pair
+		const PxU32 LastPairIndex = mNbActivePairs-1;
+		if(LastPairIndex==pair_index)
+		{
+			mNbActivePairs--;
+		}
+		else
+		{
+			PX_ASSERT(LastPairIndex<mActivePairsCapacity);
+			const BroadPhasePair* Last = &mActivePairs[LastPairIndex];
+			const PxU32 LastHashValue = Hash(Last->mVolA, Last->mVolB) & mMask;
+
+			// Walk the hash table to fix mNext
+			PX_ASSERT(LastHashValue<mHashCapacity);
+			PxU32 Offset = mHashTable[LastHashValue];
+			PX_ASSERT(Offset!=BP_INVALID_BP_HANDLE);
+
+			PxU32 Previous=BP_INVALID_BP_HANDLE;
+			while(Offset!=LastPairIndex)
+			{
+				Previous = Offset;
+				PX_ASSERT(Offset<mHashCapacity);
+				Offset = mNext[Offset];
+			}
+
+			// Let us go/jump us
+			if(Previous!=BP_INVALID_BP_HANDLE)
+			{
+				PX_ASSERT(Previous<mHashCapacity);
+				PX_ASSERT(LastPairIndex<mHashCapacity);
+				PX_ASSERT(mNext[Previous]==LastPairIndex);
+				mNext[Previous] = mNext[LastPairIndex];
+			}
+			// else we were the first
+			else
+			{
+				PX_ASSERT(LastHashValue<mHashCapacity);
+				PX_ASSERT(LastPairIndex<mHashCapacity);
+				mHashTable[LastHashValue] = mNext[LastPairIndex];
+			}
+			// we're now free to reuse mNext[LastPairIndex] without breaking the list
+
+#if PX_DEBUG
+			PX_ASSERT(LastPairIndex<mHashCapacity);
+			mNext[LastPairIndex]=BP_INVALID_BP_HANDLE;
+#endif
+
+			// Don't invalidate entry since we're going to shrink the array
+
+			// 2) Re-insert in free slot
+			PX_ASSERT(pair_index<mActivePairsCapacity);
+			PX_ASSERT(LastPairIndex<mActivePairsCapacity);
+			mActivePairs[pair_index] = mActivePairs[LastPairIndex];
+			mActivePairStates[pair_index] = mActivePairStates[LastPairIndex];
+#if PX_DEBUG
+			PX_ASSERT(pair_index<mHashCapacity);
+			PX_ASSERT(mNext[pair_index]==BP_INVALID_BP_HANDLE);
+#endif
+			PX_ASSERT(pair_index<mHashCapacity);
+			PX_ASSERT(LastHashValue<mHashCapacity);
+			mNext[pair_index] = mHashTable[LastHashValue];
+			mHashTable[LastHashValue] = BpHandle(pair_index);
+
+			mNbActivePairs--;
+		}
+	}
+}
+
+bool SapPairManager::RemovePair(BpHandle id0, BpHandle id1)
+{
+	// Order the ids
+	Sort(id0, id1);
+
+	const PxU32 HashValue = Hash(id0, id1) & mMask;
+	const BroadPhasePair* P = FindPair(id0, id1, HashValue);
+	if(!P)	return false;
+	PX_ASSERT(P->mVolA==id0);
+	PX_ASSERT(P->mVolB==id1);
+
+	RemovePair(id0, id1, HashValue, GetPairIndex(P));
+
+	shrinkMemory();
+
+	return true;
+}
+
+bool SapPairManager::RemovePairs(const Cm::BitMap& removedAABBs)
+{
+	PxU32 i=0;
+	while(i<mNbActivePairs)
+	{
+		const BpHandle id0 = mActivePairs[i].mVolA;
+		const BpHandle id1 = mActivePairs[i].mVolB;
+		if(removedAABBs.test(id0) || removedAABBs.test(id1))
+		{
+			const PxU32 HashValue = Hash(id0, id1) & mMask;
+			RemovePair(id0, id1, HashValue, i);
+		}
+		else i++;
+	}
+	return true;
+}
+
+void SapPairManager::shrinkMemory()
+{
+	//Compute the hash size given the current number of active pairs.
+	const PxU32 correctHashSize = Ps::nextPowerOfTwo(mNbActivePairs);
+
+	//If we have the correct hash size then no action required.
+	if(correctHashSize==mHashSize || (correctHashSize < mMinAllowedHashCapacity && mHashSize == mMinAllowedHashCapacity))	
+		return;
+
+	//The hash size can be reduced so take action.
+	//Don't let the hash size fall below a threshold value.
+	PxU32 newHashSize = correctHashSize;
+	if(newHashSize < mMinAllowedHashCapacity)
+	{
+		newHashSize = mMinAllowedHashCapacity;
+	}
+	mHashSize = newHashSize;
+	mMask = newHashSize-1;
+
+	reallocPairs( (newHashSize > mMinAllowedHashCapacity) || (mHashSize <= (mHashCapacity >> 2)) || (mHashSize <= (mActivePairsCapacity >> 2)));
+}
+
+void SapPairManager::reallocPairs(const bool allocRequired)
+{
+	if(allocRequired)
+	{
+		PX_FREE(mHashTable);
+		mHashCapacity=mHashSize;
+		mActivePairsCapacity=mHashSize;
+		mHashTable = reinterpret_cast<BpHandle*>(PX_ALLOC(mHashSize*sizeof(BpHandle), "BpHandle"));
+
+		for(PxU32 i=0;i<mHashSize;i++)	
+		{
+			mHashTable[i] = BP_INVALID_BP_HANDLE;
+		}
+
+		// Get some bytes for new entries
+		BroadPhasePair* NewPairs	= reinterpret_cast<BroadPhasePair*>(PX_ALLOC(mHashSize * sizeof(BroadPhasePair), "BroadPhasePair"));	PX_ASSERT(NewPairs);
+		BpHandle* NewNext			= reinterpret_cast<BpHandle*>(PX_ALLOC(mHashSize * sizeof(BpHandle), "BpHandle"));						PX_ASSERT(NewNext);
+		PxU8* NewPairStates			= reinterpret_cast<PxU8*>(PX_ALLOC(mHashSize * sizeof(PxU8), "SapPairStates"));							PX_ASSERT(NewPairStates);
+
+		// Copy old data if needed
+		if(mNbActivePairs) 
+		{
+			PxMemCopy(NewPairs, mActivePairs, mNbActivePairs*sizeof(BroadPhasePair));
+			PxMemCopy(NewPairStates, mActivePairStates, mNbActivePairs*sizeof(PxU8));
+		}
+
+		// ### check it's actually needed... probably only for pairs whose hash value was cut by the and
+		// yeah, since Hash(id0, id1) is a constant
+		// However it might not be needed to recompute them => only less efficient but still ok
+		for(PxU32 i=0;i<mNbActivePairs;i++)
+		{
+			const PxU32 HashValue = Hash(mActivePairs[i].mVolA, mActivePairs[i].mVolB) & mMask;	// New hash value with new mask
+			NewNext[i] = mHashTable[HashValue];
+			PX_ASSERT(HashValue<mHashCapacity);
+			mHashTable[HashValue] = BpHandle(i);
+		}
+
+		// Delete old data
+		PX_FREE(mNext);
+		PX_FREE(mActivePairs);
+		PX_FREE(mActivePairStates);
+
+		// Assign new pointer
+		mActivePairs = NewPairs;
+		mActivePairStates = NewPairStates;
+		mNext = NewNext;
+	}
+	else
+	{
+		for(PxU32 i=0;i<mHashSize;i++)	
+		{
+			mHashTable[i] = BP_INVALID_BP_HANDLE;
+		}
+
+		// ### check it's actually needed... probably only for pairs whose hash value was cut by the and
+		// yeah, since Hash(id0, id1) is a constant
+		// However it might not be needed to recompute them => only less efficient but still ok
+		for(PxU32 i=0;i<mNbActivePairs;i++)
+		{
+			const PxU32 HashValue = Hash(mActivePairs[i].mVolA, mActivePairs[i].mVolB) & mMask;	// New hash value with new mask
+			mNext[i] = mHashTable[HashValue];
+			PX_ASSERT(HashValue<mHashCapacity);
+			mHashTable[HashValue] = BpHandle(i);
+		}
+	}
+}
+
+void resizeCreatedDeleted(BroadPhasePair*& pairs, PxU32& maxNumPairs)
+{
+	PX_ASSERT(pairs);
+	PX_ASSERT(maxNumPairs>0);
+	const PxU32 newMaxNumPairs=2*maxNumPairs;
+	BroadPhasePair* newPairs=reinterpret_cast<BroadPhasePair*>(PX_ALLOC(sizeof(BroadPhasePair)*newMaxNumPairs, "BroadPhasePair"));
+	PxMemCopy(newPairs, pairs, sizeof(BroadPhasePair)*maxNumPairs);
+	PX_FREE(pairs);
+	pairs=newPairs;
+	maxNumPairs=newMaxNumPairs;
+}
+
+void ComputeCreatedDeletedPairsLists
+(const BpHandle* PX_RESTRICT boxGroups, 
+ const BpHandle* PX_RESTRICT dataArray, const PxU32 dataArraySize,
+ PxcScratchAllocator* scratchAllocator,
+ BroadPhasePairReport*& createdPairsList, PxU32& numCreatedPairs, PxU32& maxNumCreatedPairs,
+ BroadPhasePairReport*& deletedPairsList, PxU32& numDeletedPairs, PxU32& maxNumDeletedPairs,
+ PxU32& numActualDeletedPairs,
+ SapPairManager& pairManager)
+{
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+	PX_UNUSED(boxGroups);
+#endif
+
+	for(PxU32 i=0;i<dataArraySize;i++)
+	{
+		const PxU32 ID = dataArray[i];
+		PX_ASSERT(ID<pairManager.mNbActivePairs);
+
+		const BroadPhasePair* PX_RESTRICT UP = pairManager.mActivePairs + ID;
+		PX_ASSERT(pairManager.IsInArray(UP));
+
+		if(pairManager.IsRemoved(UP))
+		{
+			if(!pairManager.IsNew(UP))
+			{
+				// No need to call "ClearInArray" in this case, since the pair will get removed anyway
+				if(numDeletedPairs==maxNumDeletedPairs)
+				{
+					BroadPhasePairReport* newDeletedPairsList = reinterpret_cast<BroadPhasePairReport*>(scratchAllocator->alloc(sizeof(BroadPhasePairReport)*2*maxNumDeletedPairs, true));
+					PxMemCopy(newDeletedPairsList, deletedPairsList, sizeof(BroadPhasePairReport)*maxNumDeletedPairs);
+					scratchAllocator->free(deletedPairsList);
+					deletedPairsList = newDeletedPairsList;
+					maxNumDeletedPairs = 2*maxNumDeletedPairs;
+				}
+
+				PX_ASSERT(numDeletedPairs<maxNumDeletedPairs);
+				//PX_ASSERT((uintptr_t)UP->mUserData != 0xcdcdcdcd);
+				deletedPairsList[numDeletedPairs]=BroadPhasePairReport(UP->mVolA,UP->mVolB, UP->mUserData, ID);
+				numDeletedPairs++;
+			}
+		}
+		else
+		{
+			pairManager.ClearInArray(UP);
+			// Add => already there... Might want to create user data, though
+			if(pairManager.IsNew(UP))
+			{
+#if !BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+				if(boxGroups[UP->mVolA]!=boxGroups[UP->mVolB])
+#endif
+				{
+					if(numCreatedPairs==maxNumCreatedPairs)
+					{
+						BroadPhasePairReport* newCreatedPairsList = reinterpret_cast<BroadPhasePairReport*>(scratchAllocator->alloc(sizeof(BroadPhasePairReport)*2*maxNumCreatedPairs, true));
+						PxMemCopy(newCreatedPairsList, createdPairsList, sizeof(BroadPhasePairReport)*maxNumCreatedPairs);
+						scratchAllocator->free(createdPairsList);
+						createdPairsList = newCreatedPairsList;
+						maxNumCreatedPairs = 2*maxNumCreatedPairs;
+					}
+
+					PX_ASSERT(numCreatedPairs<maxNumCreatedPairs);
+					createdPairsList[numCreatedPairs]=BroadPhasePairReport(UP->mVolA,UP->mVolB, UP->mUserData, ID);
+					numCreatedPairs++;
+				}
+				pairManager.ClearNew(UP);
+			}
+		}
+	}
+
+	//Record pairs that are to be deleted because they were simultaneously created and removed 
+	//from different axis sorts.
+	numActualDeletedPairs=numDeletedPairs;
+	for(PxU32 i=0;i<dataArraySize;i++)
+	{
+		const PxU32 ID = dataArray[i];
+		PX_ASSERT(ID<pairManager.mNbActivePairs);
+		const BroadPhasePair* PX_RESTRICT UP = pairManager.mActivePairs + ID;
+		if(pairManager.IsRemoved(UP) && pairManager.IsNew(UP))
+		{
+			PX_ASSERT(pairManager.IsInArray(UP));
+
+			if(numActualDeletedPairs==maxNumDeletedPairs)
+			{
+				BroadPhasePairReport* newDeletedPairsList = reinterpret_cast<BroadPhasePairReport*>(scratchAllocator->alloc(sizeof(BroadPhasePairReport)*2*maxNumDeletedPairs, true));
+				PxMemCopy(newDeletedPairsList, deletedPairsList, sizeof(BroadPhasePairReport)*maxNumDeletedPairs);
+				scratchAllocator->free(deletedPairsList);
+				deletedPairsList = newDeletedPairsList;
+				maxNumDeletedPairs = 2*maxNumDeletedPairs;
+			}
+
+			PX_ASSERT(numActualDeletedPairs<=maxNumDeletedPairs);
+			deletedPairsList[numActualDeletedPairs]=BroadPhasePairReport(UP->mVolA,UP->mVolB, NULL, ID); //KS - should we even get here????
+			numActualDeletedPairs++;
+		}
+	}
+
+//	// #### try batch removal here
+//	for(PxU32 i=0;i<numActualDeletedPairs;i++)
+//	{
+//		const BpHandle id0 = deletedPairsList[i].mVolA;
+//		const BpHandle id1 = deletedPairsList[i].mVolB;
+//#if PX_DEBUG
+//		const bool Status = pairManager.RemovePair(id0, id1);
+//		PX_ASSERT(Status);
+//#else
+//		pairManager.RemovePair(id0, id1);
+//#endif
+//	}
+
+	//Only report deleted pairs from different groups.
+#if !BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+	for(PxU32 i=0;i<numDeletedPairs;i++)
+	{
+		const PxU32 id0 = deletedPairsList[i].mVolA;
+		const PxU32 id1 = deletedPairsList[i].mVolB;
+		if(boxGroups[id0]==boxGroups[id1])
+		{
+			while((numDeletedPairs-1) > i && boxGroups[deletedPairsList[numDeletedPairs-1].mVolA] == boxGroups[deletedPairsList[numDeletedPairs-1].mVolB])
+			{
+				numDeletedPairs--;
+			}
+			deletedPairsList[i]=deletedPairsList[numDeletedPairs-1];
+			numDeletedPairs--;
+		}
+	}
+#endif
+}
+
+void DeletePairsLists(const PxU32 numActualDeletedPairs, BroadPhasePairReport* deletedPairsList, SapPairManager& pairManager)
+{
+	// #### try batch removal here
+	for(PxU32 i=0;i<numActualDeletedPairs;i++)
+	{
+		const BpHandle id0 = deletedPairsList[i].mVolA;
+		const BpHandle id1 = deletedPairsList[i].mVolB;
+#if PX_DEBUG
+		const bool Status = pairManager.RemovePair(id0, id1);
+		PX_ASSERT(Status);
+#else
+		pairManager.RemovePair(id0, id1);
+#endif
+	}
+}
+
+#include "BpBroadPhase.h"
+PX_COMPILE_TIME_ASSERT(FilterGroup::eSTATICS==0);
+void ComputeSortedLists
+(Cm::BitMap* PX_RESTRICT bitmap, 
+ const PxU32 insertAABBStart, const PxU32 insertAABBEnd, const BpHandle* PX_RESTRICT createdAABBs,
+ SapBox1D** PX_RESTRICT asapBoxes, const BpHandle* PX_RESTRICT asapBoxGroupIds, 
+ BpHandle* PX_RESTRICT asapEndPointDatas, const PxU32 numSortedEndPoints, 
+ const Gu::Axes& axes,
+ BpHandle* PX_RESTRICT newBoxIndicesSorted, PxU32& newBoxIndicesCount, BpHandle* PX_RESTRICT oldBoxIndicesSorted, PxU32& oldBoxIndicesCount,
+ bool& allNewBoxesStatics, bool& allOldBoxesStatics)
+{
+	const PxU32 axis0=axes.mAxis0;
+	const PxU32 axis1=axes.mAxis1;
+	const PxU32 axis2=axes.mAxis2;
+
+	//Set the bitmap for new box ids and compute the aabb (of the sorted handles/indices and not of the values) that bounds all new boxes.
+	
+	PxU32 globalAABBMinX=PX_MAX_U32;
+	PxU32 globalAABBMinY=PX_MAX_U32;
+	PxU32 globalAABBMinZ=PX_MAX_U32;
+	PxU32 globalAABBMaxX=0;
+	PxU32 globalAABBMaxY=0;
+	PxU32 globalAABBMaxZ=0;
+	
+	for(PxU32 i=insertAABBStart;i<insertAABBEnd;i++)
+	{
+		const PxU32 boxId=createdAABBs[i];
+		bitmap->set(boxId);
+
+		globalAABBMinX = PxMin(globalAABBMinX, PxU32(asapBoxes[axis0][boxId].mMinMax[0]));
+		globalAABBMinY = PxMin(globalAABBMinY, PxU32(asapBoxes[axis1][boxId].mMinMax[0]));
+		globalAABBMinZ = PxMin(globalAABBMinZ, PxU32(asapBoxes[axis2][boxId].mMinMax[0]));
+		globalAABBMaxX = PxMax(globalAABBMaxX, PxU32(asapBoxes[axis0][boxId].mMinMax[1]));
+		globalAABBMaxY = PxMax(globalAABBMaxY, PxU32(asapBoxes[axis1][boxId].mMinMax[1]));
+		globalAABBMaxZ = PxMax(globalAABBMaxZ, PxU32(asapBoxes[axis2][boxId].mMinMax[1]));
+	}
+
+	PxU32 oldStaticCount=0;
+	PxU32 newStaticCount=0;
+
+	//Assign the sorted end pts to the appropriate arrays.
+	for(PxU32 i=1;i<numSortedEndPoints-1;i++)
+	{
+		//Make sure we haven't encountered a sentinel - 
+		//they should only be at each end of the array.
+		PX_ASSERT(!isSentinel(asapEndPointDatas[i]));
+		PX_ASSERT(!isSentinel(asapEndPointDatas[i]));
+		PX_ASSERT(!isSentinel(asapEndPointDatas[i]));
+
+		if(!isMax(asapEndPointDatas[i]))
+		{
+			const BpHandle boxId=BpHandle(getOwner(asapEndPointDatas[i]));
+			if(!bitmap->test(boxId))
+			{
+				if(Intersect3D(
+					globalAABBMinX, globalAABBMaxX, globalAABBMinY, globalAABBMaxY, globalAABBMinZ, globalAABBMaxZ,
+					asapBoxes[axis0][boxId].mMinMax[0],asapBoxes[axis0][boxId].mMinMax[1],asapBoxes[axis1][boxId].mMinMax[0],asapBoxes[axis1][boxId].mMinMax[1],asapBoxes[axis2][boxId].mMinMax[0],asapBoxes[axis2][boxId].mMinMax[1]))
+				{
+					oldBoxIndicesSorted[oldBoxIndicesCount]=boxId;
+					oldBoxIndicesCount++;
+					oldStaticCount+=asapBoxGroupIds[boxId];
+				}
+			}
+			else 
+			{
+				newBoxIndicesSorted[newBoxIndicesCount]=boxId;
+				newBoxIndicesCount++;
+				newStaticCount+=asapBoxGroupIds[boxId];
+			}
+		}
+	}
+
+	allOldBoxesStatics = oldStaticCount ? false : true;
+	allNewBoxesStatics = newStaticCount ? false : true;
+
+	//Make sure that we've found the correct number of boxes.
+	PX_ASSERT(newBoxIndicesCount==(insertAABBEnd-insertAABBStart));
+	PX_ASSERT(oldBoxIndicesCount<=((numSortedEndPoints-NUM_SENTINELS)/2));
+}
+
+void performBoxPruningNewNew
+(const Gu::Axes& axes,
+ const BpHandle* PX_RESTRICT newBoxIndicesSorted, const PxU32 newBoxIndicesCount, const bool allNewBoxesStatics,
+ BpHandle* PX_RESTRICT minPosList0,
+ SapBox1D** PX_RESTRICT asapBoxes, const BpHandle* PX_RESTRICT asapBoxGroupIds, 
+ PxcScratchAllocator* scratchAllocator,
+ SapPairManager& pairManager, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity)
+{
+	// Checkings
+	if(!newBoxIndicesCount)	return;
+
+	// Catch axes
+	const PxU32 Axis0 = axes.mAxis0;
+	const PxU32 Axis1 = axes.mAxis1;
+	const PxU32 Axis2 = axes.mAxis2;
+
+	// 1) Build main list using the primary axis
+	for(PxU32 i=0;i<newBoxIndicesCount;i++)	
+	{
+		const BpHandle boxId = newBoxIndicesSorted[i];
+		minPosList0[i] = asapBoxes[Axis0][boxId].mMinMax[0];
+	}
+
+	if(allNewBoxesStatics) return;
+
+	// 2) Prune the list
+
+	const PxU32 LastSortedIndex = newBoxIndicesCount;
+	PxU32 RunningIndex = 0;
+	PxU32 SortedIndex = 0;
+
+	while(RunningIndex<LastSortedIndex && SortedIndex<LastSortedIndex)
+	{
+		const PxU32 Index0 = SortedIndex++;
+		const BpHandle boxId0 = newBoxIndicesSorted[Index0];
+		const BpHandle Limit = asapBoxes[Axis0][boxId0].mMinMax[1];
+
+		while(RunningIndex<LastSortedIndex && minPosList0[RunningIndex++]<minPosList0[Index0]);
+
+		if(RunningIndex<LastSortedIndex)
+		{
+			PxU32 RunningIndex2 = RunningIndex;
+
+			PxU32 Index1;
+			while(RunningIndex2<LastSortedIndex && minPosList0[Index1 = RunningIndex2++] <= Limit)
+			{
+				const BpHandle boxId1 = newBoxIndicesSorted[Index1];
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+				if(asapBoxGroupIds[boxId0]!=asapBoxGroupIds[boxId1])
+#endif
+				{
+					if(Intersect2D(
+						asapBoxes[Axis1][boxId0].mMinMax[0],asapBoxes[Axis1][boxId0].mMinMax[1],asapBoxes[Axis2][boxId0].mMinMax[0],asapBoxes[Axis2][boxId0].mMinMax[1],
+						asapBoxes[Axis1][boxId1].mMinMax[0],asapBoxes[Axis1][boxId1].mMinMax[1],asapBoxes[Axis2][boxId1].mMinMax[0],asapBoxes[Axis2][boxId1].mMinMax[1]))
+					{
+						AddPair(boxId0, boxId1, scratchAllocator, pairManager, dataArray, dataArraySize, dataArrayCapacity);
+					}
+				}
+			}
+		}
+	}
+}
+
+void performBoxPruningNewOld
+(const Gu::Axes& axes,
+ const BpHandle* PX_RESTRICT newBoxIndicesSorted, const PxU32 newBoxIndicesCount, const BpHandle* PX_RESTRICT oldBoxIndicesSorted, const PxU32 oldBoxIndicesCount,
+ BpHandle* PX_RESTRICT minPosListNew,  BpHandle* PX_RESTRICT minPosListOld,
+ SapBox1D** PX_RESTRICT asapBoxes, const BpHandle* PX_RESTRICT asapBoxGroupIds,
+ PxcScratchAllocator* scratchAllocator, 
+ SapPairManager& pairManager, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity)
+{
+	// Checkings
+	if(!newBoxIndicesCount || !oldBoxIndicesCount)	return;
+
+	// Catch axes
+	const PxU32 Axis0 = axes.mAxis0;
+	const PxU32 Axis1 = axes.mAxis1;
+	const PxU32 Axis2 = axes.mAxis2;
+
+	BpHandle* PX_RESTRICT minPosList0=minPosListNew;
+	BpHandle* PX_RESTRICT minPosList1=minPosListOld;
+
+	// 1) Build main lists using the primary axis
+	for(PxU32 i=0;i<newBoxIndicesCount;i++)	
+	{
+		const BpHandle boxId=newBoxIndicesSorted[i];
+		minPosList0[i] = asapBoxes[Axis0][boxId].mMinMax[0];
+	}
+	for(PxU32 i=0;i<oldBoxIndicesCount;i++)	
+	{
+		const BpHandle boxId=oldBoxIndicesSorted[i];
+		minPosList1[i] = asapBoxes[Axis0][boxId].mMinMax[0];
+	}
+
+	// 3) Prune the lists
+	const PxU32 LastSortedIndex0 = newBoxIndicesCount;
+	const PxU32 LastSortedIndex1 = oldBoxIndicesCount;
+	PxU32 RunningIndex0 = 0;
+	PxU32 RunningIndex1 = 0;
+	PxU32 SortedIndex1 = 0;
+	PxU32 SortedIndex0 = 0;
+
+
+	while(RunningIndex1<LastSortedIndex1 && SortedIndex0<LastSortedIndex0)
+	{
+		const PxU32 Index0 = SortedIndex0++;
+		const BpHandle boxId0 = newBoxIndicesSorted[Index0];
+		const BpHandle Limit = asapBoxes[Axis0][boxId0].mMinMax[1];//Box0.mMaxIndex[Axis0];
+
+		while(RunningIndex1<LastSortedIndex1 && minPosList1[RunningIndex1]<minPosList0[Index0])
+			RunningIndex1++;
+
+		PxU32 RunningIndex2_1 = RunningIndex1;
+
+		PxU32 Index1;
+		while(RunningIndex2_1<LastSortedIndex1 && minPosList1[Index1 = RunningIndex2_1++] <= Limit)
+		{
+			const BpHandle boxId1 = oldBoxIndicesSorted[Index1];
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+			if(asapBoxGroupIds[boxId0]!=asapBoxGroupIds[boxId1])
+#endif
+			{
+				if(Intersect2D(
+					asapBoxes[Axis1][boxId0].mMinMax[0],asapBoxes[Axis1][boxId0].mMinMax[1],asapBoxes[Axis2][boxId0].mMinMax[0],asapBoxes[Axis2][boxId0].mMinMax[1],
+					asapBoxes[Axis1][boxId1].mMinMax[0],asapBoxes[Axis1][boxId1].mMinMax[1],asapBoxes[Axis2][boxId1].mMinMax[0],asapBoxes[Axis2][boxId1].mMinMax[1]))
+				{
+					AddPair(boxId0, boxId1, scratchAllocator, pairManager, dataArray, dataArraySize, dataArrayCapacity);
+				}
+			}
+		}
+	}
+
+	////
+
+	while(RunningIndex0<LastSortedIndex0 && SortedIndex1<LastSortedIndex1)
+	{
+		const PxU32 Index0 = SortedIndex1++;
+		const BpHandle boxId0 = oldBoxIndicesSorted[Index0];
+		const BpHandle Limit = asapBoxes[Axis0][boxId0].mMinMax[1];
+
+		while(RunningIndex0<LastSortedIndex0 && minPosList0[RunningIndex0]<=minPosList1[Index0])
+			RunningIndex0++;
+
+		PxU32 RunningIndex2_0 = RunningIndex0;
+
+		PxU32 Index1;
+		while(RunningIndex2_0<LastSortedIndex0 && minPosList0[Index1 = RunningIndex2_0++] <= Limit)
+		{
+			const BpHandle boxId1 = newBoxIndicesSorted[Index1];
+#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
+			if(asapBoxGroupIds[boxId0]!=asapBoxGroupIds[boxId1])
+#endif
+			{
+				if(Intersect2D(
+					asapBoxes[Axis1][boxId0].mMinMax[0],asapBoxes[Axis1][boxId0].mMinMax[1],asapBoxes[Axis2][boxId0].mMinMax[0],asapBoxes[Axis2][boxId0].mMinMax[1],
+					asapBoxes[Axis1][boxId1].mMinMax[0],asapBoxes[Axis1][boxId1].mMinMax[1],asapBoxes[Axis2][boxId1].mMinMax[0],asapBoxes[Axis2][boxId1].mMinMax[1]))
+				{
+					AddPair(boxId0, boxId1, scratchAllocator, pairManager, dataArray, dataArraySize, dataArrayCapacity);
+				}
+			}
+		}
+	}
+}
+
+} //namespace Bp
+
+} //namespace physx
+
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSapAux.h b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSapAux.h
new file mode 100644
index 00000000..5cc9b062
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpBroadPhaseSapAux.h
@@ -0,0 +1,411 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef BP_BROADPHASE_SAP_AUX_H
+#define BP_BROADPHASE_SAP_AUX_H
+
+#include "foundation/PxAssert.h"
+#include "CmPhysXCommon.h"
+#include "PsIntrinsics.h"
+#include "PsUserAllocated.h"
+#include "BpBroadPhase.h"
+#include "BpBroadPhaseUpdate.h"
+#include "CmBitMap.h"
+#include "GuAxes.h"
+#include "PxcScratchAllocator.h"
+
+namespace physx
+{
+
+namespace Bp
+{
+
+#define NUM_SENTINELS 2
+
+#define BP_SAP_USE_PREFETCH 1//prefetch in batchUpdate
+
+#define BP_SAP_USE_OVERLAP_TEST_ON_REMOVES	1// "Useless" but faster overall because seriously reduces number of calls (from ~10000 to ~3 sometimes!)
+
+//Set 1 to test for group ids in batchCreate/batchUpdate so we can avoid group id test in ComputeCreatedDeletedPairsLists
+//Set 0 to neglect group id test in batchCreate/batchUpdate and delay test until ComputeCreatedDeletedPairsLists
+#define BP_SAP_TEST_GROUP_ID_CREATEUPDATE 1
+
+#if PX_USE_16_BIT_HANDLES
+#define MAX_BP_HANDLE			0xffff
+#define PX_REMOVED_BP_HANDLE	0xfffd
+#define MAX_BP_PAIRS_MESSAGE "Only 65536 broadphase pairs are supported.  This limit has been exceeded and some pairs will be dropped \n"
+#else
+#define MAX_BP_HANDLE			0x3fffffff
+#define PX_REMOVED_BP_HANDLE	0x3ffffffd
+#define MAX_BP_PAIRS_MESSAGE "Only 4294967296 broadphase pairs are supported.  This limit has been exceeded and some pairs will be dropped \n"
+#endif
+
+PX_FORCE_INLINE	void setMinSentinel(ValType& v, BpHandle& d)
+{
+	v = 0x00000000;//0x00800000;  //0x00800000 is -FLT_MAX but setting it to 0 means we don't crash when we get a value outside the float range.
+	d = (BP_INVALID_BP_HANDLE & ~1);
+}
+
+PX_FORCE_INLINE	void setMaxSentinel(ValType& v, BpHandle& d)
+{										
+	v = 0xffffffff;//0xff7fffff;  //0xff7fffff is +FLT_MAX but setting it to 0xffffffff means we don't crash when we get a value outside the float range.
+	d = BP_INVALID_BP_HANDLE;
+}
+
+PX_FORCE_INLINE	void setData(BpHandle& d, PxU32 owner_box_id, const bool is_max)
+{
+	//v=v;
+	d = BpHandle(owner_box_id<<1);
+	if(is_max)	d |= 1;
+}
+
+PX_FORCE_INLINE	bool isSentinel(const BpHandle& d)	
+{ 
+	return (d&~1)==(BP_INVALID_BP_HANDLE & ~1);	
+}
+
+PX_FORCE_INLINE	BpHandle isMax(const BpHandle& d) 	
+{
+	return BpHandle(d & 1);		
+}
+
+PX_FORCE_INLINE	BpHandle getOwner(const BpHandle& d) 	
+{ 
+	return BpHandle(d>>1);		
+}
+
+class SapBox1D
+{
+public:
+
+	PX_FORCE_INLINE					SapBox1D()	{}
+	PX_FORCE_INLINE					~SapBox1D()	{}
+
+	BpHandle		mMinMax[2];//mMinMax[0]=min, mMinMax[1]=max
+};
+
+class SapPairManager
+{
+public:
+	SapPairManager();
+	~SapPairManager();
+
+	void						init(const PxU32 size);
+	void						release();
+
+	void						shrinkMemory();
+
+	const BroadPhasePair*	AddPair		(BpHandle id0, BpHandle id1, const PxU8 state);
+	bool						RemovePair	(BpHandle id0, BpHandle id1);
+	bool						RemovePairs	(const Cm::BitMap& removedAABBs);
+	const BroadPhasePair*	FindPair	(BpHandle id0, BpHandle id1)	const;
+
+	PX_FORCE_INLINE	PxU32		GetPairIndex(const BroadPhasePair* PX_RESTRICT pair)	const
+	{
+		return (PxU32((size_t(pair) - size_t(mActivePairs)))/sizeof(BroadPhasePair));
+	}
+
+	BpHandle*			mHashTable;
+	BpHandle*			mNext;
+	PxU32				mHashSize;
+	PxU32				mHashCapacity;
+	PxU32				mMinAllowedHashCapacity;
+	BroadPhasePair*		mActivePairs;
+	PxU8*				mActivePairStates;
+	PxU32				mNbActivePairs;
+	PxU32				mActivePairsCapacity;
+	PxU32				mMask;
+
+	BroadPhasePair*	FindPair	(BpHandle id0, BpHandle id1, PxU32 hash_value) const;
+	void				RemovePair	(BpHandle id0, BpHandle id1, PxU32 hash_value, PxU32 pair_index);
+	void				reallocPairs(const bool allocRequired);
+
+	enum
+	{
+		PAIR_INARRAY=1,
+		PAIR_REMOVED=2,
+		PAIR_NEW=4,
+		PAIR_UNKNOWN=8
+	};
+
+	PX_FORCE_INLINE bool IsInArray(const BroadPhasePair* PX_RESTRICT pair) const 
+	{
+		const PxU8 state=mActivePairStates[pair-mActivePairs];
+		return state & PAIR_INARRAY ? true : false;
+	}
+	PX_FORCE_INLINE bool IsRemoved(const BroadPhasePair* PX_RESTRICT pair) const 
+	{
+		const PxU8 state=mActivePairStates[pair-mActivePairs];
+		return state & PAIR_REMOVED ? true : false;
+	}
+	PX_FORCE_INLINE bool IsNew(const BroadPhasePair* PX_RESTRICT pair) const
+	{
+		const PxU8 state=mActivePairStates[pair-mActivePairs];
+		return state & PAIR_NEW ? true : false;
+	}
+	PX_FORCE_INLINE bool IsUnknown(const BroadPhasePair* PX_RESTRICT pair) const
+	{
+		const PxU8 state=mActivePairStates[pair-mActivePairs];
+		return state & PAIR_UNKNOWN ? true : false;
+	}
+
+	PX_FORCE_INLINE void ClearState(const BroadPhasePair* PX_RESTRICT pair)
+	{
+		mActivePairStates[pair-mActivePairs]=0;
+	}
+
+	PX_FORCE_INLINE void SetInArray(const BroadPhasePair* PX_RESTRICT pair)
+	{
+		mActivePairStates[pair-mActivePairs] |= PAIR_INARRAY;
+	}
+	PX_FORCE_INLINE void SetRemoved(const BroadPhasePair* PX_RESTRICT pair)
+	{
+		mActivePairStates[pair-mActivePairs] |= PAIR_REMOVED;
+	}
+	PX_FORCE_INLINE void SetNew(const BroadPhasePair* PX_RESTRICT pair)
+	{
+		mActivePairStates[pair-mActivePairs] |= PAIR_NEW;
+	}
+	PX_FORCE_INLINE void ClearInArray(const BroadPhasePair* PX_RESTRICT pair)
+	{		
+		mActivePairStates[pair-mActivePairs] &= ~PAIR_INARRAY;
+	}
+	PX_FORCE_INLINE void ClearRemoved(const BroadPhasePair* PX_RESTRICT pair)
+	{
+		mActivePairStates[pair-mActivePairs] &= ~PAIR_REMOVED;
+	}
+	PX_FORCE_INLINE void ClearNew(const BroadPhasePair* PX_RESTRICT pair)
+	{
+		mActivePairStates[pair-mActivePairs] &= ~PAIR_NEW;
+	}
+};
+
+PX_FORCE_INLINE void AddData(const PxU32 data, PxcScratchAllocator* scratchAllocator, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity)
+{
+	if(dataArraySize==dataArrayCapacity)
+	{
+		BpHandle* newDataArray = reinterpret_cast<BpHandle*>(scratchAllocator->alloc(sizeof(BpHandle)*dataArrayCapacity*2, true));
+		PxMemCopy(newDataArray, dataArray, dataArrayCapacity*sizeof(BpHandle));
+		scratchAllocator->free(dataArray);
+		dataArray = newDataArray;
+		dataArrayCapacity = dataArrayCapacity*2;
+	}
+	PX_UNUSED(scratchAllocator);
+	PX_UNUSED(dataArrayCapacity);
+	PX_ASSERT(dataArraySize<dataArrayCapacity);
+	dataArray[dataArraySize]=BpHandle(data);
+	dataArraySize++;
+}
+
+PX_FORCE_INLINE	bool AddPair
+(const BpHandle id0, const BpHandle id1, 
+ PxcScratchAllocator* scratchAllocator,
+ SapPairManager& pairManager, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity)
+{
+	const BroadPhasePair* UP = reinterpret_cast<const BroadPhasePair*>(pairManager.AddPair(id0, id1, SapPairManager::PAIR_UNKNOWN));
+
+	//If the hash table has reached its limit then we're unable to add a new pair.
+	if(NULL==UP)
+	{
+		return false;
+	}
+
+	PX_ASSERT(UP);
+	if(pairManager.IsUnknown(UP))
+	{
+		pairManager.ClearState(UP);
+		pairManager.SetInArray(UP);
+		AddData(pairManager.GetPairIndex(UP), scratchAllocator, dataArray, dataArraySize, dataArrayCapacity);
+		pairManager.SetNew(UP);
+	}
+	pairManager.ClearRemoved(UP);
+
+	return true;
+}
+
+PX_FORCE_INLINE	void RemovePair
+(BpHandle id0, BpHandle id1, 
+ PxcScratchAllocator* scratchAllocator,
+ SapPairManager& pairManager,  BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity)
+{
+	const BroadPhasePair* UP = reinterpret_cast<const BroadPhasePair*>(pairManager.FindPair(id0, id1));
+	if(UP)
+	{
+		if(!pairManager.IsInArray(UP))
+		{
+			pairManager.SetInArray(UP);
+			AddData(pairManager.GetPairIndex(UP), scratchAllocator, dataArray, dataArraySize, dataArrayCapacity);
+		}
+		pairManager.SetRemoved(UP);
+	}
+}
+
+PX_FORCE_INLINE void InsertEndPoints
+(const ValType* PX_RESTRICT newEndPointValues, const BpHandle* PX_RESTRICT newEndPointDatas, PxU32 numNewEndPoints,
+ ValType* PX_RESTRICT endPointValues, BpHandle* PX_RESTRICT endPointDatas, const PxU32 numEndPoints, 
+ SapBox1D* PX_RESTRICT boxes)
+{
+	ValType* const BaseEPValue = endPointValues;
+	BpHandle* const BaseEPData = endPointDatas;
+
+	const PxU32 OldSize = numEndPoints-NUM_SENTINELS;
+	const PxU32 NewSize = numEndPoints-NUM_SENTINELS+numNewEndPoints;
+
+	BaseEPValue[NewSize + 1] = BaseEPValue[OldSize + 1];
+	BaseEPData[NewSize + 1] = BaseEPData[OldSize + 1];
+
+	PxI32 WriteIdx = PxI32(NewSize);
+	PxU32 CurrInsIdx = 0;
+
+	//const SapValType* FirstValue = &BaseEPValue[0];
+	const BpHandle* FirstData = &BaseEPData[0];
+	const ValType* CurrentValue = &BaseEPValue[OldSize];
+	const BpHandle* CurrentData = &BaseEPData[OldSize];
+	while(CurrentData>=FirstData)
+	{
+		const ValType& SrcValue = *CurrentValue;
+		const BpHandle& SrcData = *CurrentData;
+		const ValType& InsValue = newEndPointValues[CurrInsIdx];
+		const BpHandle& InsData = newEndPointDatas[CurrInsIdx];
+
+		// We need to make sure we insert maxs before mins to handle exactly equal endpoints correctly
+		const bool ShouldInsert = isMax(InsData) ? (SrcValue <= InsValue) : (SrcValue < InsValue);
+
+		const ValType& MovedValue = ShouldInsert ? InsValue : SrcValue;
+		const BpHandle& MovedData = ShouldInsert ? InsData : SrcData;
+		BaseEPValue[WriteIdx] = MovedValue;
+		BaseEPData[WriteIdx] = MovedData;
+		boxes[getOwner(MovedData)].mMinMax[isMax(MovedData)] = BpHandle(WriteIdx--);
+
+		if(ShouldInsert)
+		{
+			CurrInsIdx++;
+			if(CurrInsIdx >= numNewEndPoints)
+				break;//we just inserted the last endpoint
+		}
+		else
+		{
+			CurrentValue--;
+			CurrentData--;
+		}
+	}
+}
+
+void ComputeCreatedDeletedPairsLists
+(const BpHandle* PX_RESTRICT boxGroups, 
+ const BpHandle* PX_RESTRICT dataArray, const PxU32 dataArraySize,
+ PxcScratchAllocator* scratchAllocator,
+ BroadPhasePairReport* & createdPairsList, PxU32& numCreatedPairs, PxU32& maxNumCreatdPairs,
+ BroadPhasePairReport* & deletedPairsList, PxU32& numDeletedPairs, PxU32& maxNumDeletedPairs,
+ PxU32&numActualDeletedPairs,
+ SapPairManager& pairManager);
+
+void DeletePairsLists(const PxU32 numActualDeletedPairs, BroadPhasePairReport* deletedPairsList, SapPairManager& pairManager);
+
+void ComputeSortedLists
+(Cm::BitMap* PX_RESTRICT bitmap, 
+ const PxU32 insertAABBStart, const PxU32 insertAABBEnd, const BpHandle* PX_RESTRICT createdAABBs,
+ SapBox1D** PX_RESTRICT asapBoxes, const BpHandle* PX_RESTRICT asapBoxGroupIds, 
+ BpHandle* PX_RESTRICT asapEndPointDatas, const PxU32 numSortedEndPoints, 
+ const Gu::Axes& axes,
+ BpHandle* PX_RESTRICT newBoxIndicesSorted, PxU32& newBoxIndicesCount, BpHandle* PX_RESTRICT oldBoxIndicesSorted, PxU32& oldBoxIndicesCount,
+ bool& allNewBoxesStatics, bool& allOldBoxesStatics);
+
+void performBoxPruningNewNew
+(const Gu::Axes& axes,
+ const BpHandle* PX_RESTRICT newBoxIndicesSorted, const PxU32 newBoxIndicesCount,  const bool allNewBoxesStatics,
+ BpHandle* PX_RESTRICT minPosList0,
+ SapBox1D** PX_RESTRICT asapBoxes, const BpHandle* PX_RESTRICT asapBoxGroupIds, 
+ PxcScratchAllocator* scratchAllocator,
+ SapPairManager& pairManager, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity);
+
+void performBoxPruningNewOld
+(const Gu::Axes& axes,
+ const BpHandle* PX_RESTRICT newBoxIndicesSorted, const PxU32 newBoxIndicesCount, const BpHandle* PX_RESTRICT oldBoxIndicesSorted, const PxU32 oldBoxIndicesCount,
+ BpHandle* PX_RESTRICT minPosListNew,  BpHandle* PX_RESTRICT minPosListOld,
+ SapBox1D** PX_RESTRICT asapBoxes, const BpHandle* PX_RESTRICT asapBoxGroupIds,
+ PxcScratchAllocator* scratchAllocator,
+ SapPairManager& pairManager, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity);
+
+PX_FORCE_INLINE bool Intersect2D(SapBox1D** PX_RESTRICT c, SapBox1D** PX_RESTRICT b, const PxU32 axis1, const PxU32 axis2)
+{
+	return (b[axis1]->mMinMax[1] >= c[axis1]->mMinMax[0] && c[axis1]->mMinMax[1] >= b[axis1]->mMinMax[0] &&
+		    b[axis2]->mMinMax[1] >= c[axis2]->mMinMax[0] && c[axis2]->mMinMax[1] >= b[axis2]->mMinMax[0]);
+}
+
+PX_FORCE_INLINE bool Intersect3D(SapBox1D** PX_RESTRICT c, SapBox1D** PX_RESTRICT b)
+{
+	return (b[0]->mMinMax[1] >= c[0]->mMinMax[0] && c[0]->mMinMax[1] >= b[0]->mMinMax[0] &&
+		    b[1]->mMinMax[1] >= c[1]->mMinMax[0] && c[1]->mMinMax[1] >= b[1]->mMinMax[0] &&
+			b[2]->mMinMax[1] >= c[2]->mMinMax[0] && c[2]->mMinMax[1] >= b[2]->mMinMax[0]);       
+}
+
+PX_FORCE_INLINE bool Intersect1D_Min(const ValType aMin, const ValType /*aMax*/, SapBox1D* PX_RESTRICT b, const ValType* PX_RESTRICT endPointValues)
+{
+	const ValType& endPointValue=endPointValues[b->mMinMax[1]];
+	return (endPointValue >= aMin);
+}
+
+PX_FORCE_INLINE bool Intersect1D_Max(const ValType /*aMin*/, const ValType aMax, SapBox1D* PX_RESTRICT b, const ValType* PX_RESTRICT endPointValues)
+{
+	const ValType& endPointValue=endPointValues[b->mMinMax[0]];
+	return (endPointValue < aMax);
+}
+
+PX_FORCE_INLINE bool Intersect2D
+(const ValType bDir1Min, const ValType bDir1Max, const ValType bDir2Min, const ValType bDir2Max,
+ const ValType cDir1Min, const ValType cDir1Max, const ValType cDir2Min, const ValType cDir2Max)
+{
+	return (bDir1Max >= cDir1Min && cDir1Max >= bDir1Min && 
+			bDir2Max >= cDir2Min && cDir2Max >= bDir2Min);        
+}
+
+PX_FORCE_INLINE bool Intersect2D_Handle
+(const BpHandle bDir1Min, const BpHandle bDir1Max, const BpHandle bDir2Min, const BpHandle bDir2Max,
+ const BpHandle cDir1Min, const BpHandle cDir1Max, const BpHandle cDir2Min, const BpHandle cDir2Max)
+{
+	return (bDir1Max > cDir1Min && cDir1Max > bDir1Min && 
+			bDir2Max > cDir2Min && cDir2Max > bDir2Min);        
+}
+
+PX_FORCE_INLINE bool Intersect3D
+(const ValType bDir1Min, const ValType bDir1Max, const ValType bDir2Min, const ValType bDir2Max, const ValType bDir3Min, const ValType bDir3Max,
+ const ValType cDir1Min, const ValType cDir1Max, const ValType cDir2Min, const ValType cDir2Max, const ValType cDir3Min, const ValType cDir3Max)
+{
+	return (bDir1Max >= cDir1Min && cDir1Max >= bDir1Min && 
+			bDir2Max >= cDir2Min && cDir2Max >= bDir2Min &&
+			bDir3Max >= cDir3Min && cDir3Max >= bDir3Min);       
+}
+
+} //namespace Bp
+
+} //namespace physx
+
+#endif //BP_BROADPHASE_SAP_AUX_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.cpp
new file mode 100644
index 00000000..0c9f0b99
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.cpp
@@ -0,0 +1,63 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "BpMBPTasks.h"
+#include "BpBroadPhaseMBP.h"
+#include "PsAllocator.h"
+
+using namespace physx;
+using namespace Bp;
+
+///////////////////////////////////////////////////////////////////////////////
+
+MBPUpdateWorkTask::MBPUpdateWorkTask() 
+{
+}
+
+MBPUpdateWorkTask::~MBPUpdateWorkTask()
+{
+}
+
+MBPPostUpdateWorkTask::MBPPostUpdateWorkTask() 
+{
+}
+
+void MBPUpdateWorkTask::runInternal()
+{
+	mMBP->update(getContinuation());
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBPPostUpdateWorkTask::runInternal()
+{
+	mMBP->postUpdate(getContinuation());
+}
+
+///////////////////////////////////////////////////////////////////////////////
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.h b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.h
new file mode 100644
index 00000000..702e3e38
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpMBPTasks.h
@@ -0,0 +1,108 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef BP_MBP_TASKS_H
+#define BP_MBP_TASKS_H
+
+#include "PsUserAllocated.h"
+#include "CmTask.h"
+
+namespace physx
+{
+	class PxcScratchAllocator;
+
+	namespace Bp
+	{
+		class BroadPhaseMBP;
+	}
+
+#define MBP_USE_SCRATCHPAD
+
+	class MBPTask : public Cm::Task, public shdfnd::UserAllocated
+	{
+		public:
+												MBPTask() :
+												mMBP				(NULL),
+												mNumCpuTasks		(0)
+												{}
+
+		PX_FORCE_INLINE	void					setBroadphase(Bp::BroadPhaseMBP* mbp)			{ mMBP = mbp;					}
+		PX_FORCE_INLINE	void					setScratchAllocator(PxcScratchAllocator* sa)	{ mScratchAllocator = sa;		}
+		PX_FORCE_INLINE void					setNumCpuTasks(const PxU32 numCpuTasks)			{ mNumCpuTasks = numCpuTasks;	}
+
+		protected:
+						Bp::BroadPhaseMBP*		mMBP;
+						PxU32					mNumCpuTasks;
+
+						PxcScratchAllocator*	mScratchAllocator;
+
+		private:
+		MBPTask& operator=(const MBPTask&);
+	};
+
+	// PT: this is the main 'update' task doing the actual box pruning work.
+	class MBPUpdateWorkTask : public MBPTask
+	{
+	public:							
+								MBPUpdateWorkTask();
+								~MBPUpdateWorkTask();
+		// PxBaseTask
+		virtual const char*		getName() const { return "BpMBP.updateWork"; }
+		//~PxBaseTask
+
+		// Cm::Task
+		virtual void			runInternal();
+		//~Cm::Task
+
+	private:
+		MBPUpdateWorkTask& operator=(const MBPUpdateWorkTask&);
+	};
+
+	// PT: this task runs after MBPUpdateWorkTask. This is where MBP_PairManager::removeMarkedPairs is called, to finalize
+	// the work and come up with created/removed lists. This is single-threaded.
+	class MBPPostUpdateWorkTask : public MBPTask
+	{
+	public:
+								MBPPostUpdateWorkTask();					
+
+		// PxBaseTask
+		virtual const char*		getName() const { return "BpMBP.postUpdateWork"; }
+		//~PxBaseTask
+
+		// Cm::Task
+		virtual void			runInternal();
+		//~Cm::Task
+
+	private:
+		MBPPostUpdateWorkTask& operator=(const MBPPostUpdateWorkTask&);
+	};
+
+} //namespace physx
+
+#endif // BP_MBP_TASKS_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.cpp
new file mode 100644
index 00000000..c452581a
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.cpp
@@ -0,0 +1,69 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "BpSAPTasks.h"
+#include "BpBroadPhaseSap.h"
+#include "PsTime.h"
+
+namespace physx
+{
+
+namespace Bp
+{
+
+///////////////////////////////////////////////////////////////////////////////
+
+
+//	#define DUMP_TOTAL_SAP_TIME
+// 256 convex stacks: from ~13000 down to ~2000
+// pot pourri box: from ~4000 to ~700
+// boxes: ~3400 to ~4000
+
+#ifdef DUMP_TOTAL_SAP_TIME
+	static PxU64 gStartTime = shdfnd::Time::getCurrentCounterValue();
+#endif
+
+void SapUpdateWorkTask::runInternal()
+{
+	mSAP->update(getContinuation());
+}
+
+void SapPostUpdateWorkTask::runInternal()
+{
+	mSAP->postUpdate(getContinuation());
+#ifdef DUMP_TOTAL_SAP_TIME
+	PxU64 endTime = shdfnd::Time::getCurrentCounterValue();
+	printf("SAP Time: %" PX_PRIu64 "\n", endTime - gStartTime);
+#endif
+}
+
+} //namespace Bp
+
+} //namespace physx
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.h b/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.h
new file mode 100644
index 00000000..89a3a412
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpSAPTasks.h
@@ -0,0 +1,103 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef BP_SAP_TASKS_H
+#define BP_SAP_TASKS_H
+
+#include "CmTask.h"
+
+namespace physx
+{
+
+namespace Bp
+{
+
+	class BroadPhaseSap;
+
+	class SapUpdateWorkTask: public Cm::Task
+	{
+	public:
+
+		SapUpdateWorkTask()
+		{
+		}
+
+		void setBroadPhase(BroadPhaseSap* sap) 
+		{
+			mSAP = sap;
+		}
+
+		void set(const PxU32 numCpuTasks) 
+		{
+			mNumCpuTasks = numCpuTasks; 
+		}
+
+		virtual void runInternal();
+
+		virtual const char* getName() const { return "BpSAP.updateWork"; }
+
+	private:
+
+		BroadPhaseSap* mSAP;
+		PxU32 mNumCpuTasks;
+	};
+
+	class SapPostUpdateWorkTask: public Cm::Task
+	{
+	public:
+
+		SapPostUpdateWorkTask()
+		{
+		
+		}
+		void setBroadPhase(BroadPhaseSap* sap) 
+		{
+			mSAP = sap;
+		}
+
+		void set(const PxU32 numCpuTasks) 
+		{
+			mNumCpuTasks=numCpuTasks; 
+		}
+
+		virtual void runInternal();
+
+		virtual const char* getName() const { return "BpSAP.postUpdateWork"; }
+
+	private:
+
+		BroadPhaseSap* mSAP;
+		PxU32 mNumCpuTasks;
+	};
+
+} //namespace Bp
+
+} //namespace physx
+
+#endif // BP_SAP_TASKS_H
diff --git a/PhysX_3.4/Source/LowLevelAABB/src/BpSimpleAABBManager.cpp b/PhysX_3.4/Source/LowLevelAABB/src/BpSimpleAABBManager.cpp
new file mode 100644
index 00000000..37b5027b
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelAABB/src/BpSimpleAABBManager.cpp
@@ -0,0 +1,2604 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "CmRenderOutput.h"
+#include "CmFlushPool.h"
+#include "BpBroadPhaseMBPCommon.h"
+#include "BpSimpleAABBManager.h"
+#include "BpBroadPhase.h"
+#include "PsFoundation.h"
+#include "PsHash.h"
+#include "PsSort.h"
+#include "PsHashSet.h"
+#include "PsVecMath.h"
+#include "GuInternal.h"
+//#include <stdio.h>
+
+using namespace physx;
+using namespace Cm;
+using namespace Ps::aos;
+
+
+// PT: TODO: use PX_INTEL_FAMILY for overlap macros
+// PT: TODO: port MBP_Pair optim to MBP
+// PT: TODO: benchmark for aggregate creation/deletion
+// PT: TODO: "mixed" benchmark where objects are added/removed/updated from aggregates (not just benchmarking one specific part)
+// PT: TODO: cleanup code, add/fix comments, fix AABBManager UTs
+// PT: TODO: revisit bool getStateChanged() const { return mStateChanged; }
+// => it triggers various DMAs in the Cuda code, some of them not always necessary
+// PT: TODO: PEEL
+// PT: TODO: remove random lines and check that at least one UT fails => e.g. outputDeletedOverlaps
+// PT: TODO: UTs for all combinations of add/remove aggregated/aggregate and dirty lists and stuff
+// PT: TODO: origin shift
+// PT: TODO: eventually revisit choices of data structures/etc (LLs, arrays, hashmaps, arays of pointers, arrays of objects...)
+// PT: TODO: consider dropping actor-agg pairs and replace with actorS-agg pairs
+// PT: TODO: different kinds of dirty?
+// PT: TODO: changed/deleted/updated bitmaps are inconsistent: changed/deleted maps are owned by the manager, always as large as the max amount of objects,
+// and use unbounded tests. Updated map is owned by the scene, only as large as the largest updated object, and uses bounded tests. What's the story here?
+// PT: TODO: remove the sort of updated objects. Best way is to move the updated map inside the manager, and intercept the scene-calls accessing it. That
+// way when an aggregated is marked dirty we can mark its aggregate dirty at the same time, and we can also reuse that map for dirty aggregates, removing
+// the need for the dirty aggregates array, the dirty index, etc
+// PT: TODO: the whole hashmap things may not be needed if the BP can provide a pair pointer or a pair handle....
+// PT: TODO: multi-thread, MBP, micro optims
+// PT: TODO: consider storing all pairs in the same hashmap
+// PT: TODO: when creating an object with a new id, add asserts that no interaction exists with this id
+// PT: TODO: investigate how it's done in Sc with interactions. It should be the same kind of problems. Could one part benefit from better ideas from the other part? could we share the code?
+// PT: TODO: optimize redundant hash computations for pairs (inserted in hashmap & hashset)
+// PT: TODO: static aggs => add a benchmark for that
+// PT: TODO: revisit SIMD overlap (move first part out of the test)
+// PT: TODO: revisit pair deltas (10% slower than trunk). Options are a PM instead of the two arrays+hashset, or try to see if we can output pairs in sorted order (in which case
+// we can simply go over the previous and current arrays at the same time, no need for a hashset). Maybe the best option would be to reuse the MBP region code, because it would handle
+// "sleeping" objects directly.... Or just use bitmaps?
+// PT: TODO: idea: store the hash in the pair structure somehow, to avoid recomputing it each frame. Useful?
+// PT: TODO: use MBP hash instead of AggPair hash, consider using a shared PM to get rid of the last hashset thing (last part of post-BP)
+// PT: TODO: consider optims when objects are marked as updated but bounds didn't actually change
+// PT: TODO: use shared MBP PM between aggs? ==> not sure it would work, would have to put back the bitmap test for updated objects in the create/delete final loop (same as in MBP)
+// PT: TODO: optimize memory allocations
+// PT: TODO: try to refactor/share pruning code => seems to have a significant impact on perf?
+// PT: TODO: revisit forceinline for addPair. Seems to help but is it worth it?
+// PT: TODO: handle pair deltas differently in each case? For example for actor-vs-aggregate a "bitmap" makes more sense => the bit can be hidden in the aggregated index, removing the need
+// for extra memory and bitmap promotion => doesn't work easily since the aggregated array is shared between all actor-aggregate pairs. So the bits have to be in the persistent pairs structure,
+// which creates an issue when we remove an object and the bitmap must be reorganized as well. Unless we leave a hole in it I guess. Or can we fix it in "computeBounds" or something?
+
+static const bool gSingleThreaded = false;
+static const bool gUseBruteForce = false;
+//#define EXPERIMENT
+#define USE_MBP_PAIR_MANAGER
+#define STORE_SORTED_BOUNDS
+#if PX_INTEL_FAMILY
+	#define USE_SIMD_BOUNDS
+#endif
+
+static PX_FORCE_INLINE bool groupFiltering(PxU32 group0, PxU32 group1)
+{
+	return group0!=group1;
+}
+
+namespace physx
+{
+
+namespace Bp
+{
+static PX_FORCE_INLINE uint32_t hash(const Pair& p)
+{
+	return PxU32(Ps::hash( (p.mID0&0xffff)|(p.mID1<<16)) );
+}
+
+static PX_FORCE_INLINE uint32_t hash(const AggPair& p)
+{
+	return PxU32(Ps::hash( (p.mIndex0&0xffff)|(p.mIndex1<<16)) );
+}
+
+static PX_FORCE_INLINE bool shouldPairBeDeleted(const Ps::Array<PxU32, Ps::VirtualAllocator>& groups, ShapeHandle h0, ShapeHandle h1)
+{
+	PX_ASSERT(h0<groups.size());
+	PX_ASSERT(h1<groups.size());
+	return (groups[h0]==PX_INVALID_U32) || (groups[h1]==PX_INVALID_U32);
+}
+
+// PT: TODO: refactor with CCT version
+template <class T> 
+static void resetOrClear(T& a)
+{
+	const PxU32 c = a.capacity();
+	const PxU32 s = a.size();
+	if(s>=c/2)
+		a.clear();
+	else
+		a.reset();
+}
+
+///
+
+#ifdef USE_SIMD_BOUNDS
+	typedef SIMD_AABB	InflatedAABB;
+	typedef PxU32		InflatedType;
+#else
+	typedef PxBounds3	InflatedAABB;
+	typedef float		InflatedType;
+#endif
+
+	// PT: TODO: revisit/optimize all that stuff once it works
+	class Aggregate : public Ps::UserAllocated
+	{
+		public:
+														Aggregate(BoundsIndex index, bool selfCollisions);
+														~Aggregate();
+
+						BoundsIndex						mIndex;
+		private:
+						Ps::Array<BoundsIndex>			mAggregated;	// PT: TODO: replace with linked list?
+		public:
+						PersistentSelfCollisionPairs*	mSelfCollisionPairs;
+						PxU32							mDirtyIndex;	// PT: index in mDirtyAggregates
+						InflatedAABB*					mInflatedBounds;
+						PxU32							mAllocatedSize;
+
+		PX_FORCE_INLINE	PxU32							getNbAggregated()		const	{ return mAggregated.size();					}
+		PX_FORCE_INLINE	BoundsIndex						getAggregated(PxU32 i)	const	{ return mAggregated[i];						}
+		PX_FORCE_INLINE	const BoundsIndex*				getIndices()			const	{ return mAggregated.begin();					}
+		PX_FORCE_INLINE	void							addAggregated(BoundsIndex i)	{ mAggregated.pushBack(i);						}
+		PX_FORCE_INLINE	bool							removeAggregated(BoundsIndex i)	{ return mAggregated.findAndReplaceWithLast(i);	}	// PT: TODO: optimize?
+
+		PX_FORCE_INLINE	void							resetDirtyState()				{ mDirtyIndex = PX_INVALID_U32;				}
+		PX_FORCE_INLINE	bool							isDirty()				const	{ return mDirtyIndex != PX_INVALID_U32;		}
+		PX_FORCE_INLINE void							markAsDirty(Ps::Array<Aggregate*>& dirtyAggregates)
+														{
+															if(!isDirty())
+															{
+																mDirtyIndex = dirtyAggregates.size();
+																dirtyAggregates.pushBack(this);
+															}
+														}
+
+						void							allocateBounds();
+						void							computeBounds(const BoundsArray& boundsArray, const float* contactDistances)	/*PX_RESTRICT*/;
+
+#ifdef STORE_SORTED_BOUNDS
+		PX_FORCE_INLINE	void							getSortedMinBounds()
+														{
+															if(mDirtySort)
+																sortBounds();
+														}
+#else
+		PX_FORCE_INLINE	const PxU32*					getSortedMinBounds()
+														{
+															if(mDirtySort)
+																sortBounds();
+															return mRS.GetRanks();
+														}
+#endif
+						PxBounds3						mBounds;
+		private:
+#ifndef STORE_SORTED_BOUNDS
+						Cm::RadixSortBuffered			mRS;
+#endif
+						bool							mDirtySort;
+
+						void							sortBounds();
+						PX_NOCOPY(Aggregate)
+	};
+
+///
+
+#ifdef USE_MBP_PAIR_MANAGER
+namespace
+{
+#define MBP_ALLOC(x)		PX_ALLOC(x, "MBP")
+#define MBP_ALLOC_TMP(x)	PX_ALLOC_TEMP(x, "MBP_TMP")
+#define MBP_FREE(x)			if(x)	PX_FREE_AND_RESET(x)
+#define	INVALID_ID	0xffffffff
+#define INVALID_USER_ID	0xffffffff
+
+	// PT: TODO: consider sharing this with the MBP code but we cannot atm because of the extra "usrData" in the other structure
+/*	struct MBP_Pair : public Ps::UserAllocated
+	{
+		PxU32		id0;
+		PxU32		id1;
+		// TODO: optimize memory here
+		bool		isNew;
+		bool		isUpdated;
+
+		PX_FORCE_INLINE	PxU32	getId0()	const	{ return id0;		}
+		PX_FORCE_INLINE	PxU32	getId1()	const	{ return id1;		}
+
+		PX_FORCE_INLINE	bool	isNew()		const	{ return isNew;		}
+		PX_FORCE_INLINE	bool	isUpdated()	const	{ return isUpdated;	}
+
+		PX_FORCE_INLINE	void	setNewPair(PxU32 id0_, PxU32 id1_)
+		{
+			p->id0		= id0_;	// ### CMOVs would be nice here
+			p->id1		= id1_;
+			p->isNew	= true;
+			p->isUpdated= false;
+		}
+
+		PX_FORCE_INLINE	void	setUpdated()		{ isUpdated = true;		}
+		PX_FORCE_INLINE	void	clearUpdated()		{ isUpdated = false;	}
+		PX_FORCE_INLINE	void	clearNew()			{ isNew = false;		}
+	};
+	static PX_FORCE_INLINE bool differentPair(const MBP_Pair& p, PxU32 id0, PxU32 id1)	{ return (id0!=p.id0) || (id1!=p.id1);	}*/
+
+	struct MBP_Pair : public Ps::UserAllocated
+	{
+		PX_FORCE_INLINE	PxU32	getId0()	const	{ return id0_isNew & ~PX_SIGN_BITMASK;		}
+		PX_FORCE_INLINE	PxU32	getId1()	const	{ return id1_isUpdated & ~PX_SIGN_BITMASK;	}
+
+		PX_FORCE_INLINE	PxU32	isNew()		const	{ return id0_isNew & PX_SIGN_BITMASK;		}
+		PX_FORCE_INLINE	PxU32	isUpdated()	const	{ return id1_isUpdated & PX_SIGN_BITMASK;	}
+
+		PX_FORCE_INLINE	void	setNewPair(PxU32 id0, PxU32 id1)
+		{
+			PX_ASSERT(!(id0 & PX_SIGN_BITMASK));
+			PX_ASSERT(!(id1 & PX_SIGN_BITMASK));
+			id0_isNew = id0 | PX_SIGN_BITMASK;
+			id1_isUpdated = id1;
+		}
+		PX_FORCE_INLINE	void	setUpdated()		{ id1_isUpdated |= PX_SIGN_BITMASK;		}
+		PX_FORCE_INLINE	void	clearUpdated()		{ id1_isUpdated &= ~PX_SIGN_BITMASK;	}
+		PX_FORCE_INLINE	void	clearNew()			{ id0_isNew &= ~PX_SIGN_BITMASK;		}
+
+		protected:
+		PxU32		id0_isNew;
+		PxU32		id1_isUpdated;
+	};
+	static PX_FORCE_INLINE bool differentPair(const MBP_Pair& p, PxU32 id0, PxU32 id1)	{ return (id0!=p.getId0()) || (id1!=p.getId1());	}
+
+	struct MBPEntry;
+	struct RegionHandle;
+	struct MBP_Object;
+
+	class MBP_PairManager : public Ps::UserAllocated
+	{
+		public:
+											MBP_PairManager();
+											~MBP_PairManager();
+
+						void				purge();
+						void				shrinkMemory();
+
+		PX_FORCE_INLINE	MBP_Pair*			addPair				(PxU32 id0, PxU32 id1/*, const BpHandle* PX_RESTRICT groups = NULL, const MBP_Object* objects = NULL*/);
+
+						PxU32				mHashSize;
+						PxU32				mMask;
+						PxU32				mNbActivePairs;
+						PxU32*				mHashTable;
+						PxU32*				mNext;
+						MBP_Pair*			mActivePairs;
+						PxU32				mReservedMemory;
+						bool				mStuff;
+
+		PX_FORCE_INLINE	MBP_Pair*			findPair(PxU32 id0, PxU32 id1, PxU32 hashValue) const;
+						void				removePair(PxU32 id0, PxU32 id1, PxU32 hashValue, PxU32 pairIndex);
+						void				reallocPairs();
+		PX_NOINLINE		PxU32				growPairs(PxU32 fullHashValue);
+	};
+
+
+static PX_FORCE_INLINE void sort(PxU32& id0, PxU32& id1)							{ if(id0>id1)	Ps::swap(id0, id1);		}
+
+///////////////////////////////////////////////////////////////////////////////
+
+MBP_PairManager::MBP_PairManager() :
+	mHashSize		(0),
+	mMask			(0),
+	mNbActivePairs	(0),
+	mHashTable		(NULL),
+	mNext			(NULL),
+	mActivePairs	(NULL),
+	mReservedMemory (0)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+MBP_PairManager::~MBP_PairManager()
+{
+	purge();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::purge()
+{
+	MBP_FREE(mNext);
+	MBP_FREE(mActivePairs);
+	MBP_FREE(mHashTable);
+	mHashSize		= 0;
+	mMask			= 0;
+	mNbActivePairs	= 0;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+	static PX_FORCE_INLINE PxU32 hash(PxU32 id0, PxU32 id1)
+	{
+		return PxU32(Ps::hash( (id0&0xffff)|(id1<<16)) );
+	}
+
+	static PX_FORCE_INLINE void storeDwords(PxU32* dest, PxU32 nb, PxU32 value)
+	{
+		while(nb--)
+			*dest++ = value;
+	}
+
+///////////////////////////////////////////////////////////////////////////////
+
+// Internal version saving hash computation
+PX_FORCE_INLINE MBP_Pair* MBP_PairManager::findPair(PxU32 id0, PxU32 id1, PxU32 hashValue) const
+{
+	if(!mHashTable)
+		return NULL;	// Nothing has been allocated yet
+
+	MBP_Pair* PX_RESTRICT activePairs = mActivePairs;
+	const PxU32* PX_RESTRICT next = mNext;
+
+	// Look for it in the table
+	PxU32 offset = mHashTable[hashValue];
+	while(offset!=INVALID_ID && differentPair(activePairs[offset], id0, id1))
+	{
+		PX_ASSERT(activePairs[offset].getId0()!=INVALID_USER_ID);	// PT: TODO: this one is not possible with the new encoding of id0
+		offset = next[offset];		// Better to have a separate array for this
+	}
+	if(offset==INVALID_ID)
+		return NULL;
+	PX_ASSERT(offset<mNbActivePairs);
+	// Match mActivePairs[offset] => the pair is persistent
+	return &activePairs[offset];
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+PX_NOINLINE PxU32 MBP_PairManager::growPairs(PxU32 fullHashValue)
+{
+	// Get more entries
+	mHashSize = Ps::nextPowerOfTwo(mNbActivePairs+1);
+	mMask = mHashSize-1;
+
+	reallocPairs();
+
+	// Recompute hash value with new hash size
+	return fullHashValue & mMask;
+}
+
+PX_FORCE_INLINE MBP_Pair* MBP_PairManager::addPair(PxU32 id0, PxU32 id1/*, const BpHandle* PX_RESTRICT groups, const MBP_Object* objects*/)
+{
+	PX_ASSERT(id0!=INVALID_ID);
+	PX_ASSERT(id1!=INVALID_ID);
+
+/*	if(groups)
+	{
+		const MBP_ObjectIndex index0 = decodeHandle_Index(id0);
+		const MBP_ObjectIndex index1 = decodeHandle_Index(id1);
+
+		const BpHandle object0 = objects[index0].mUserID;
+		const BpHandle object1 = objects[index1].mUserID;
+
+		if(groups[object0] == groups[object1])
+			return NULL;
+	}*/
+
+	// Order the ids
+	sort(id0, id1);
+
+	const PxU32 fullHashValue = hash(id0, id1);
+	PxU32 hashValue = fullHashValue & mMask;
+
+	{
+		MBP_Pair* PX_RESTRICT p = findPair(id0, id1, hashValue);
+		if(p)
+		{
+			p->setUpdated();
+			return p;	// Persistent pair
+		}
+	}
+
+	// This is a new pair
+	if(mNbActivePairs >= mHashSize)
+		hashValue = growPairs(fullHashValue);
+
+	const PxU32 index = mNbActivePairs++;
+	MBP_Pair* PX_RESTRICT p = &mActivePairs[index];
+	p->setNewPair(id0, id1);
+	mNext[index] = mHashTable[hashValue];
+	mHashTable[hashValue] = index;
+	return p;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::removePair(PxU32 /*id0*/, PxU32 /*id1*/, PxU32 hashValue, PxU32 pairIndex)
+{
+	// Walk the hash table to fix mNext
+	{
+		PxU32 offset = mHashTable[hashValue];
+		PX_ASSERT(offset!=INVALID_ID);
+
+		PxU32 previous=INVALID_ID;
+		while(offset!=pairIndex)
+		{
+			previous = offset;
+			offset = mNext[offset];
+		}
+
+		// Let us go/jump us
+		if(previous!=INVALID_ID)
+		{
+			PX_ASSERT(mNext[previous]==pairIndex);
+			mNext[previous] = mNext[pairIndex];
+		}
+		// else we were the first
+		else mHashTable[hashValue] = mNext[pairIndex];
+		// we're now free to reuse mNext[pairIndex] without breaking the list
+	}
+#if PX_DEBUG
+	mNext[pairIndex]=INVALID_ID;
+#endif
+	// Invalidate entry
+
+	// Fill holes
+	{
+		// 1) Remove last pair
+		const PxU32 lastPairIndex = mNbActivePairs-1;
+		if(lastPairIndex==pairIndex)
+		{
+			mNbActivePairs--;
+		}
+		else
+		{
+			const MBP_Pair* last = &mActivePairs[lastPairIndex];
+			const PxU32 lastHashValue = hash(last->getId0(), last->getId1()) & mMask;
+
+			// Walk the hash table to fix mNext
+			PxU32 offset = mHashTable[lastHashValue];
+			PX_ASSERT(offset!=INVALID_ID);
+
+			PxU32 previous=INVALID_ID;
+			while(offset!=lastPairIndex)
+			{
+				previous = offset;
+				offset = mNext[offset];
+			}
+
+			// Let us go/jump us
+			if(previous!=INVALID_ID)
+			{
+				PX_ASSERT(mNext[previous]==lastPairIndex);
+				mNext[previous] = mNext[lastPairIndex];
+			}
+			// else we were the first
+			else mHashTable[lastHashValue] = mNext[lastPairIndex];
+			// we're now free to reuse mNext[lastPairIndex] without breaking the list
+
+#if PX_DEBUG
+			mNext[lastPairIndex]=INVALID_ID;
+#endif
+
+			// Don't invalidate entry since we're going to shrink the array
+
+			// 2) Re-insert in free slot
+			mActivePairs[pairIndex] = mActivePairs[lastPairIndex];
+#if PX_DEBUG
+			PX_ASSERT(mNext[pairIndex]==INVALID_ID);
+#endif
+			mNext[pairIndex] = mHashTable[lastHashValue];
+			mHashTable[lastHashValue] = pairIndex;
+
+			mNbActivePairs--;
+		}
+	}
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::shrinkMemory()
+{
+	// Check correct memory against actually used memory
+	const PxU32 correctHashSize = Ps::nextPowerOfTwo(mNbActivePairs);
+	if(mHashSize==correctHashSize)
+		return;
+
+	if(mReservedMemory && correctHashSize < mReservedMemory)
+		return;
+
+	// Reduce memory used
+	mHashSize = correctHashSize;
+	mMask = mHashSize-1;
+
+	reallocPairs();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void MBP_PairManager::reallocPairs()
+{
+	MBP_FREE(mHashTable);
+	mHashTable = reinterpret_cast<PxU32*>(MBP_ALLOC(mHashSize*sizeof(PxU32)));
+	storeDwords(mHashTable, mHashSize, INVALID_ID);
+
+	// Get some bytes for new entries
+	MBP_Pair* newPairs	= reinterpret_cast<MBP_Pair*>(MBP_ALLOC(mHashSize * sizeof(MBP_Pair)));	PX_ASSERT(newPairs);
+	PxU32* newNext		= reinterpret_cast<PxU32*>(MBP_ALLOC(mHashSize * sizeof(PxU32)));		PX_ASSERT(newNext);
+
+	// Copy old data if needed
+	if(mNbActivePairs)
+		PxMemCopy(newPairs, mActivePairs, mNbActivePairs*sizeof(MBP_Pair));
+	// ### check it's actually needed... probably only for pairs whose hash value was cut by the and
+	// yeah, since hash(id0, id1) is a constant
+	// However it might not be needed to recompute them => only less efficient but still ok
+	for(PxU32 i=0;i<mNbActivePairs;i++)
+	{
+		const PxU32 hashValue = hash(mActivePairs[i].getId0(), mActivePairs[i].getId1()) & mMask;	// New hash value with new mask
+		newNext[i] = mHashTable[hashValue];
+		mHashTable[hashValue] = i;
+	}
+
+	// Delete old data
+	MBP_FREE(mNext);
+	MBP_FREE(mActivePairs);
+
+	// Assign new pointer
+	mActivePairs = newPairs;
+	mNext = newNext;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+}
+#endif
+
+#ifdef USE_MBP_PAIR_MANAGER
+	typedef MBP_PairManager	PairArray;
+#else
+	typedef Ps::Array<Pair>	PairArray;
+#endif
+
+static PX_FORCE_INLINE void outputPair(PairArray& pairs, ShapeHandle index0, ShapeHandle index1)
+{
+#ifdef USE_MBP_PAIR_MANAGER
+	pairs.addPair(index0, index1);
+#else
+	if(index1<index0)
+		Ps::swap(index0, index1);
+	pairs.pushBack(Pair(index0, index1));
+#endif
+}
+
+///
+
+class PersistentPairs : public Ps::UserAllocated
+{
+	public:
+									PersistentPairs() : mTimestamp(PX_INVALID_U32),
+#ifndef USE_MBP_PAIR_MANAGER
+										mCurrentPairArray(0),
+#endif
+										mShouldBeDeleted(false)					{}
+	virtual							~PersistentPairs()							{}
+
+	virtual			bool			update(SimpleAABBManager& /*manager*/)		{ return false; }
+
+	PX_FORCE_INLINE	void			updatePairs(PxU32 timestamp, const PxBounds3* bounds, const float* contactDistances, const PxU32* groups,
+												Ps::Array<VolumeData>& volumeData, Ps::Array<AABBOverlap>* createdOverlaps, Ps::Array<AABBOverlap>* destroyedOverlaps);
+					void			outputDeletedOverlaps(Ps::Array<AABBOverlap>* overlaps, const Ps::Array<VolumeData>& volumeData);
+	private:
+	virtual			void			findOverlaps(PairArray& pairs, const PxBounds3* PX_RESTRICT bounds, const float* PX_RESTRICT contactDistances, const PxU32* PX_RESTRICT groups)	= 0;
+	protected:
+					PxU32			mTimestamp;
+#ifdef USE_MBP_PAIR_MANAGER
+					MBP_PairManager	mPM;
+#else
+					PxU32			mCurrentPairArray;
+					Ps::Array<Pair>	mCurrentPairs[2];
+#endif
+	public:
+					bool			mShouldBeDeleted;
+};
+
+/////
+#ifdef USE_SIMD_BOUNDS
+#define SIMD_OVERLAP_INIT(box)	\
+	const __m128i b = _mm_shuffle_epi32(_mm_loadu_si128(reinterpret_cast<const __m128i*>(&box.mMinY)), 78);
+#define SIMD_OVERLAP_TEST(box)	\
+	const __m128i a = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&box.mMinY));	\
+	const __m128i d = _mm_cmpgt_epi32(a, b);											\
+	if(_mm_movemask_epi8(d)==0x0000ff00)
+
+static PX_FORCE_INLINE int intersects2D(const InflatedAABB& box0, const InflatedAABB& box1)
+{
+#if PX_INTEL_FAMILY
+	// PT: TODO: move this out of the test
+	const __m128i b = _mm_shuffle_epi32(_mm_loadu_si128(reinterpret_cast<const __m128i*>(&box0.mMinY)), 78);
+
+	const __m128i a = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&box1.mMinY));
+	const __m128i d = _mm_cmpgt_epi32(a, b);
+	if(_mm_movemask_epi8(d)==0x0000ff00)
+		return 1;
+	return 0;
+#else
+	if(	(box1.mMaxY < box0.mMinY) || (box0.mMaxY < box1.mMinY)
+	||	(box1.mMaxZ < box0.mMinZ) || (box0.mMaxZ < box1.mMinZ))
+		return 0;
+	return 1;
+#endif
+}
+#else
+static PX_FORCE_INLINE int intersects2D(const PxBounds3& a, const PxBounds3& b)
+{
+	if(	(b.maximum.y < a.minimum.y) || (a.maximum.y < b.minimum.y)
+	||	(b.maximum.z < a.minimum.z) || (a.maximum.z < b.minimum.z))
+		return 0;
+	return 1;
+}
+#endif
+
+static PX_FORCE_INLINE InflatedType getMinX(const InflatedAABB& box)
+{
+#ifdef USE_SIMD_BOUNDS
+	return box.mMinX;
+#else
+	return box.minimum.x;
+#endif
+}
+
+static PX_FORCE_INLINE InflatedType getMaxX(const InflatedAABB& box)
+{
+#ifdef USE_SIMD_BOUNDS
+	return box.mMaxX;
+#else
+	return box.maximum.x;
+#endif
+}
+
+static PX_FORCE_INLINE void testPair(PairArray& pairs, const InflatedAABB* bounds0, const InflatedAABB* bounds1, const PxU32* PX_RESTRICT groups, const PxU32 aggIndex0, const PxU32 aggIndex1, const PxU32 index0, const PxU32 index1)
+{
+	if(groupFiltering(groups[aggIndex0], groups[aggIndex1]))
+		if(intersects2D(bounds0[index0], bounds1[index1]))
+			outputPair(pairs, aggIndex0, aggIndex1);
+}
+
+#ifdef REMOVED_FAILED_EXPERIMENT
+static PX_NOINLINE void processCandidates(PxU32 nbCandidates, const Pair* PX_RESTRICT candidates,
+										PairArray& pairs, const InflatedAABB* bounds0, const InflatedAABB* bounds1, const PxU32* PX_RESTRICT groups, const PxU32* aggIndices0, const PxU32* aggIndices1)
+{
+	while(nbCandidates--)
+	{
+		const PxU32 index0 = candidates->mID0;
+		const PxU32 index1 = candidates->mID1;
+		candidates++;
+
+		const PxU32 aggIndex0 = aggIndices0[index0];
+		const PxU32 aggIndex1 = aggIndices1[index1];
+
+		testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index0, index1);
+	}
+}
+
+static /*PX_FORCE_INLINE*/ void boxPruning(PairArray& pairs, const InflatedAABB* bounds0, const InflatedAABB* bounds1, const PxU32* PX_RESTRICT groups, PxU32 size0, PxU32 size1, const PxU32* aggIndices0, const PxU32* aggIndices1)
+{
+//	PxU32 nbCandidates = 0;
+//	Pair candidates[16384];
+
+	PxU32 index0 = 0;
+	PxU32 runningAddress1 = 0;
+	while(runningAddress1<size1 && index0<size0)
+	{
+		const PxU32 aggIndex0 = aggIndices0[index0];
+		const InflatedAABB& box0 = bounds0[index0];
+
+		const InflatedType minLimit = getMinX(box0);
+		while(getMinX(bounds1[runningAddress1])<minLimit)
+			runningAddress1++;
+
+		PxU32 index1 = runningAddress1;
+		const InflatedType maxLimit = getMaxX(box0);
+		while(getMinX(bounds1[index1])<=maxLimit)
+		{
+			const PxU32 aggIndex1 = aggIndices1[index1];
+			testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index0, index1);
+			index1++;
+//			if(nbCandidates==1024)
+//				processCandidates(nbCandidates, candidates, pairs, bounds0, bounds1, groups, aggIndices0, aggIndices1);
+//			candidates[nbCandidates++] = Pair(index0, index1);
+//			index1++;
+		}
+		index0++;
+	}
+//	processCandidates(nbCandidates, candidates, pairs, bounds0, bounds1, groups, aggIndices0, aggIndices1);
+}
+#endif
+
+#ifdef REMOVED
+static /*PX_FORCE_INLINE*/ void boxPruning(PairArray& pairs, const InflatedAABB* bounds0, const InflatedAABB* bounds1, const PxU32* PX_RESTRICT groups, PxU32 size0, PxU32 size1, const PxU32* aggIndices0, const PxU32* aggIndices1)
+{
+	PxU32 index0 = 0;
+	PxU32 runningAddress1 = 0;
+	while(runningAddress1<size1 && index0<size0)
+	{
+		const PxU32 aggIndex0 = aggIndices0[index0];
+		const InflatedAABB& box0 = bounds0[index0];
+		index0++;
+
+		const InflatedType minLimit = getMinX(box0);
+		while(getMinX(bounds1[runningAddress1])<minLimit)
+			runningAddress1++;
+
+		PxU32 index1 = runningAddress1;
+		const InflatedType maxLimit = getMaxX(box0);
+		while(getMinX(bounds1[index1])<=maxLimit)
+		{
+			const PxU32 aggIndex1 = aggIndices1[index1];
+//			testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index0, index1);
+				if(groupFiltering(groups[aggIndex0], groups[aggIndex1]))
+					if(intersects2D(box0, bounds1[index1]))
+						outputPair(pairs, aggIndex0, aggIndex1);
+
+			index1++;
+		}
+//		index0++;
+	}
+}
+#endif
+
+#ifdef STORE_SORTED_BOUNDS
+static void boxPruning(	PairArray& pairs, const InflatedAABB* PX_RESTRICT bounds0, const InflatedAABB* PX_RESTRICT bounds1, const PxU32* PX_RESTRICT groups,
+						PxU32 size0, PxU32 size1, const BoundsIndex* PX_RESTRICT aggIndices0, const BoundsIndex* PX_RESTRICT aggIndices1)
+{
+	{
+		PxU32 index0 = 0;
+		PxU32 runningAddress1 = 0;
+		while(runningAddress1<size1 && index0<size0)
+		{
+			const PxU32 aggIndex0 = aggIndices0[index0];
+			const InflatedAABB& box0 = bounds0[index0];
+			index0++;
+#ifdef USE_SIMD_BOUNDS
+			SIMD_OVERLAP_INIT(box0)
+#endif
+			const InflatedType minLimit = getMinX(box0);
+			while(getMinX(bounds1[runningAddress1])<minLimit)
+				runningAddress1++;
+
+			PxU32 index1 = runningAddress1;
+			const InflatedType maxLimit = getMaxX(box0);
+			while(getMinX(bounds1[index1])<=maxLimit)
+			{
+				const PxU32 aggIndex1 = aggIndices1[index1];
+//				testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index0, index1);
+					if(groupFiltering(groups[aggIndex0], groups[aggIndex1]))
+					{
+#ifdef USE_SIMD_BOUNDS
+						SIMD_OVERLAP_TEST(bounds1[index1])
+#else
+						if(intersects2D(box0, bounds1[index1]))
+#endif
+						{
+							outputPair(pairs, aggIndex0, aggIndex1);
+						}
+					}
+				index1++;
+			}
+//			index0++;
+		}
+//		boxPruning(pairs, bounds0, bounds1, groups, size0, size1, mAggregate0->mAggregated.begin(), mAggregate1->mAggregated.begin());
+	}
+
+	{
+		PxU32 index0 = 0;
+		PxU32 runningAddress0 = 0;
+		while(runningAddress0<size0 && index0<size1)
+		{
+			const PxU32 aggIndex0 = aggIndices1[index0];
+			const InflatedAABB& box1 = bounds1[index0];
+			index0++;
+#ifdef USE_SIMD_BOUNDS
+			SIMD_OVERLAP_INIT(box1)
+#endif
+			const InflatedType minLimit = getMinX(box1);
+			while(getMinX(bounds0[runningAddress0])<=minLimit)
+				runningAddress0++;
+
+			PxU32 index1 = runningAddress0;
+			const InflatedType maxLimit = getMaxX(box1);
+			while(getMinX(bounds0[index1])<=maxLimit)
+			{
+				const PxU32 aggIndex1 = aggIndices0[index1];
+//				testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index1, index0);
+					if(groupFiltering(groups[aggIndex0], groups[aggIndex1]))
+					{
+#ifdef USE_SIMD_BOUNDS
+						SIMD_OVERLAP_TEST(bounds0[index1])
+#else
+						if(intersects2D(bounds0[index1], box1))
+#endif
+						{
+							outputPair(pairs, aggIndex0, aggIndex1);
+						}
+					}
+				index1++;
+			}
+//			index0++;
+		}
+//		boxPruning(pairs, bounds1, bounds0, groups, size1, size0, mAggregate1->mAggregated.begin(), mAggregate0->mAggregated.begin());
+	}
+}
+#endif
+
+/////
+
+class PersistentActorAggregatePair : public PersistentPairs
+{
+	public:
+								PersistentActorAggregatePair(Aggregate* aggregate, ShapeHandle actorHandle);
+	virtual						~PersistentActorAggregatePair()	{}
+
+	virtual			void		findOverlaps(PairArray& pairs, const PxBounds3* PX_RESTRICT bounds, const float* PX_RESTRICT contactDistances, const PxU32* PX_RESTRICT groups);
+	virtual			bool		update(SimpleAABBManager& manager);
+
+					ShapeHandle	mAggregateHandle;
+					ShapeHandle	mActorHandle;
+					Aggregate*	mAggregate;
+};
+
+PersistentActorAggregatePair::PersistentActorAggregatePair(Aggregate* aggregate, ShapeHandle actorHandle) :
+	mAggregateHandle	(aggregate->mIndex),
+	mActorHandle		(actorHandle),
+	mAggregate			(aggregate)
+{
+}
+
+void PersistentActorAggregatePair::findOverlaps(PairArray& pairs, const PxBounds3* PX_RESTRICT bounds, const float* PX_RESTRICT contactDistances, const PxU32* PX_RESTRICT groups)
+{
+	const ShapeHandle actorHandle = mActorHandle;
+
+	// PT: TODO: don't recompute inflated bounds all the time - worth it?
+	const PxBounds3& actorSrcBounds = bounds[actorHandle];
+	const PxVec3 offset(contactDistances[actorHandle]);
+	const PxBounds3 actorInflatedBounds(actorSrcBounds.minimum - offset, actorSrcBounds.maximum + offset);
+
+	InflatedAABB actorBounds[2];
+#ifdef USE_SIMD_BOUNDS
+	actorBounds[0].initFrom2(actorInflatedBounds);
+#else
+	actorBounds[0] = actorInflatedBounds;
+#endif
+
+	const PxU32 size = mAggregate->getNbAggregated();
+	const InflatedAABB* inflatedBounds = mAggregate->mInflatedBounds;
+	if(gUseBruteForce)
+	{
+		const PxU32 actorGroup = groups[actorHandle];
+		for(PxU32 i=0;i<size;i++)
+		{
+			const BoundsIndex index = mAggregate->getAggregated(i);
+			if(groupFiltering(actorGroup, groups[index]))
+			{
+				if(actorBounds[0].intersects(inflatedBounds[i]))
+					outputPair(pairs, index, actorHandle);
+			}
+		}
+	}
+	else
+	{
+#ifdef USE_SIMD_BOUNDS
+		actorBounds[1].mMinX = 0xffffffff;
+#else
+		actorBounds[1].minimum.x = PX_MAX_F32;
+#endif
+
+#ifdef STORE_SORTED_BOUNDS
+		const InflatedAABB* PX_RESTRICT bounds0 = actorBounds;
+		const InflatedAABB* PX_RESTRICT bounds1 = inflatedBounds;
+		mAggregate->getSortedMinBounds();
+		boxPruning(pairs, bounds0, bounds1, groups, 1, size, &actorHandle, mAggregate->getIndices());
+#else
+		const InflatedAABB* PX_RESTRICT bounds0 = actorBounds;
+		const InflatedAABB* PX_RESTRICT bounds1 = inflatedBounds;
+		const PxU32 size0 = 1;
+		const PxU32 size1 = size;
+
+		const PxU32 sortedIndices[2] = { 0, 1 };
+		const PxU32* sorted0 = sortedIndices;
+		const PxU32* sorted1 = mAggregate->getSortedMinBounds();
+
+		const PxU32* const lastSorted0 = &sorted0[size0];
+		const PxU32* const lastSorted1 = &sorted1[size1];
+		const PxU32* runningAddress0 = sorted0;
+		const PxU32* runningAddress1 = sorted1;
+
+		while(runningAddress1<lastSorted1 && sorted0<lastSorted0)
+		{
+			const PxU32 index0 = *sorted0++;
+//			const PxU32 aggIndex0 = mAggregate0->getAggregated(index0);
+			const PxU32 aggIndex0 = actorHandle;
+
+			const InflatedType minLimit = getMinX(bounds0[index0]);
+			while(getMinX(bounds1[*runningAddress1])<minLimit)
+				runningAddress1++;
+
+			const PxU32* runningAddress2_1 = runningAddress1;
+			const InflatedType maxLimit = getMaxX(bounds0[index0]);
+			PxU32 index1;
+			while(getMinX(bounds1[index1 = *runningAddress2_1++])<=maxLimit)
+			{
+				const PxU32 aggIndex1 = mAggregate->getAggregated(index1);
+				testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index0, index1);
+			}
+		}
+
+		////
+
+		while(runningAddress0<lastSorted0 && sorted1<lastSorted1)
+		{
+			const PxU32 index0 = *sorted1++;
+			const PxU32 aggIndex0 = mAggregate->getAggregated(index0);
+
+			const InflatedType minLimit = getMinX(bounds1[index0]);
+			while(getMinX(bounds0[*runningAddress0])<=minLimit)
+				runningAddress0++;
+
+			const PxU32* runningAddress2_0 = runningAddress0;
+			const InflatedType maxLimit = getMaxX(bounds1[index0]);
+			PxU32 index1;
+			while(getMinX(bounds0[index1 = *runningAddress2_0++])<=maxLimit)
+			{
+//				const PxU32 aggIndex1 = mAggregate0->getAggregated(index1);
+				const PxU32 aggIndex1 = actorHandle;
+				testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index1, index0);
+			}
+		}
+#endif
+	}
+}
+
+bool PersistentActorAggregatePair::update(SimpleAABBManager& manager)
+{
+	if(mShouldBeDeleted || shouldPairBeDeleted(manager.mGroups, mAggregateHandle, mActorHandle))
+		return true;
+
+	if(!mAggregate->getNbAggregated())	// PT: needed with lazy empty actors
+		return true;
+
+	if(mAggregate->isDirty() || manager.mChangedHandleMap.boundedTest(mActorHandle))
+		manager.updatePairs(*this);
+
+	return false;
+}
+
+/////
+
+class PersistentAggregateAggregatePair : public PersistentPairs
+{
+	public:
+								PersistentAggregateAggregatePair(Aggregate* aggregate0, Aggregate* aggregate1);
+	virtual						~PersistentAggregateAggregatePair()	{}
+
+	virtual			void		findOverlaps(PairArray& pairs, const PxBounds3* PX_RESTRICT bounds, const float* PX_RESTRICT contactDistances, const PxU32* PX_RESTRICT groups);
+	virtual			bool		update(SimpleAABBManager& manager);
+
+					ShapeHandle	mAggregateHandle0;
+					ShapeHandle	mAggregateHandle1;
+					Aggregate*	mAggregate0;
+					Aggregate*	mAggregate1;
+};
+
+PersistentAggregateAggregatePair::PersistentAggregateAggregatePair(Aggregate* aggregate0, Aggregate* aggregate1) :
+	mAggregateHandle0	(aggregate0->mIndex),
+	mAggregateHandle1	(aggregate1->mIndex),
+	mAggregate0			(aggregate0),
+	mAggregate1			(aggregate1)
+{
+}
+
+void PersistentAggregateAggregatePair::findOverlaps(PairArray& pairs, const PxBounds3* PX_RESTRICT /*bounds*/, const float* PX_RESTRICT /*contactDistances*/, const PxU32* PX_RESTRICT groups)
+{
+	const PxU32 size0 = mAggregate0->getNbAggregated();
+	const PxU32 size1 = mAggregate1->getNbAggregated();
+	if(gUseBruteForce)
+	{
+		const InflatedAABB* inflated0 = mAggregate0->mInflatedBounds;
+		const InflatedAABB* inflated1 = mAggregate1->mInflatedBounds;
+
+		// Brute-force nb0*nb1 overlap tests
+		for(PxU32 i=0;i<size0;i++)
+		{
+			const BoundsIndex index0 = mAggregate0->getAggregated(i);
+
+			const InflatedAABB& inflatedBounds0 = inflated0[i];
+
+			for(PxU32 j=0;j<size1;j++)
+			{
+				const BoundsIndex index1 = mAggregate1->getAggregated(j);
+
+				if(groupFiltering(groups[index0], groups[index1]))
+				{
+					if(inflatedBounds0.intersects(inflated1[j]))
+						outputPair(pairs, index0, index1);
+				}
+			}
+		}
+	}
+	else
+	{
+#ifdef STORE_SORTED_BOUNDS
+		mAggregate0->getSortedMinBounds();
+		mAggregate1->getSortedMinBounds();
+		const InflatedAABB* PX_RESTRICT bounds0 = mAggregate0->mInflatedBounds;
+		const InflatedAABB* PX_RESTRICT bounds1 = mAggregate1->mInflatedBounds;
+		boxPruning(pairs, bounds0, bounds1, groups, size0, size1, mAggregate0->getIndices(), mAggregate1->getIndices());
+#else
+		const InflatedAABB* PX_RESTRICT bounds0 = mAggregate0->mInflatedBounds;
+		const InflatedAABB* PX_RESTRICT bounds1 = mAggregate1->mInflatedBounds;
+
+		const PxU32* sorted0 = mAggregate0->getSortedMinBounds();
+		const PxU32* sorted1 = mAggregate1->getSortedMinBounds();
+
+		const PxU32* const lastSorted0 = &sorted0[size0];
+		const PxU32* const lastSorted1 = &sorted1[size1];
+		const PxU32* runningAddress0 = sorted0;
+		const PxU32* runningAddress1 = sorted1;
+
+		while(runningAddress1<lastSorted1 && sorted0<lastSorted0)
+		{
+			const PxU32 index0 = *sorted0++;
+			const PxU32 aggIndex0 = mAggregate0->getAggregated(index0);
+
+//			const __m128i b = _mm_shuffle_epi32(_mm_loadu_si128(reinterpret_cast<const __m128i*>(&bounds0[index0].mMinY)), 78);
+
+			const InflatedType minLimit = getMinX(bounds0[index0]);
+			while(getMinX(bounds1[*runningAddress1])<minLimit)
+				runningAddress1++;
+
+			const PxU32* runningAddress2_1 = runningAddress1;
+			const InflatedType maxLimit = getMaxX(bounds0[index0]);
+			PxU32 index1;
+			while(getMinX(bounds1[index1 = *runningAddress2_1++])<=maxLimit)
+			{
+				const PxU32 aggIndex1 = mAggregate1->getAggregated(index1);
+				testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index0, index1);
+/*					if(groupFiltering(groups[aggIndex0], groups[aggIndex1]))
+					{
+						const __m128i a = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&bounds1[index1].mMinY));
+						const __m128i d = _mm_cmpgt_epi32(a, b);
+						if(_mm_movemask_epi8(d)==0x0000ff00)
+//						if(intersects2D(bounds0[index0], bounds1[index1]))
+						{
+							outputPair(pairs, aggIndex0, aggIndex1);
+						}
+					}*/
+			}
+		}
+
+		////
+
+		while(runningAddress0<lastSorted0 && sorted1<lastSorted1)
+		{
+			const PxU32 index0 = *sorted1++;
+			const PxU32 aggIndex0 = mAggregate1->getAggregated(index0);
+
+//			const __m128i b = _mm_shuffle_epi32(_mm_loadu_si128(reinterpret_cast<const __m128i*>(&bounds1[index0].mMinY)), 78);
+
+			const InflatedType minLimit = getMinX(bounds1[index0]);
+			while(getMinX(bounds0[*runningAddress0])<=minLimit)
+				runningAddress0++;
+
+			const PxU32* runningAddress2_0 = runningAddress0;
+			const InflatedType maxLimit = getMaxX(bounds1[index0]);
+			PxU32 index1;
+			while(getMinX(bounds0[index1 = *runningAddress2_0++])<=maxLimit)
+			{
+				const PxU32 aggIndex1 = mAggregate0->getAggregated(index1);
+				testPair(pairs, bounds0, bounds1, groups, aggIndex0, aggIndex1, index1, index0);
+/*					if(groupFiltering(groups[aggIndex0], groups[aggIndex1]))
+					{
+						const __m128i a = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&bounds0[index1].mMinY));
+						const __m128i d = _mm_cmpgt_epi32(a, b);
+						if(_mm_movemask_epi8(d)==0x0000ff00)
+//						if(intersects2D(bounds0[index1], bounds1[index0]))
+						{
+							outputPair(pairs, aggIndex0, aggIndex1);
+						}
+					}*/
+			}
+		}
+#endif
+	}
+}
+
+bool PersistentAggregateAggregatePair::update(SimpleAABBManager& manager)
+{
+	if(mShouldBeDeleted || shouldPairBeDeleted(manager.mGroups, mAggregateHandle0, mAggregateHandle1))
+		return true;
+
+	if(!mAggregate0->getNbAggregated() || !mAggregate1->getNbAggregated())	// PT: needed with lazy empty actors
+		return true;
+
+	if(mAggregate0->isDirty() || mAggregate1->isDirty())
+		manager.updatePairs(*this);
+
+	return false;
+}
+
+/////
+
+class PersistentSelfCollisionPairs : public PersistentPairs
+{
+	public:
+						PersistentSelfCollisionPairs(Aggregate* aggregate);
+	virtual				~PersistentSelfCollisionPairs()	{}
+
+	virtual	void		findOverlaps(PairArray& pairs, const PxBounds3* PX_RESTRICT bounds, const float* PX_RESTRICT contactDistances, const PxU32* PX_RESTRICT groups);
+
+			Aggregate*	mAggregate;
+};
+
+PersistentSelfCollisionPairs::PersistentSelfCollisionPairs(Aggregate* aggregate) :
+	mAggregate	(aggregate)
+{
+}
+
+void PersistentSelfCollisionPairs::findOverlaps(PairArray& pairs, const PxBounds3* PX_RESTRICT/*bounds*/, const float* PX_RESTRICT/*contactDistances*/, const PxU32* PX_RESTRICT groups)
+{
+	const PxU32 size0 = mAggregate->getNbAggregated();
+
+	if(gUseBruteForce)
+	{
+		const InflatedAABB* inflatedBounds = mAggregate->mInflatedBounds;
+
+		// Brute-force n(n-1)/2 overlap tests
+		for(PxU32 i=0;i<size0;i++)
+		{
+			const BoundsIndex index0 = mAggregate->getAggregated(i);
+
+			const InflatedAABB& inflatedBounds0 = inflatedBounds[i];
+
+			for(PxU32 j=i+1;j<size0;j++)
+			{
+				const BoundsIndex index1 = mAggregate->getAggregated(j);
+				if(groupFiltering(groups[index0], groups[index1]))
+				{
+					if(inflatedBounds0.intersects(inflatedBounds[j]))
+						outputPair(pairs, index0, index1);
+				}
+			}
+		}
+	}
+	else
+	{
+#ifdef STORE_SORTED_BOUNDS
+		mAggregate->getSortedMinBounds();
+		const InflatedAABB* PX_RESTRICT bounds = mAggregate->mInflatedBounds;
+
+		PxU32 index0 = 0;
+		PxU32 runningAddress = 0;
+		while(runningAddress<size0 && index0<size0)
+		{
+			const PxU32 aggIndex0 = mAggregate->getAggregated(index0);
+			const InflatedAABB& box0 = bounds[index0];
+
+			const InflatedType minLimit = getMinX(box0);
+			while(getMinX(bounds[runningAddress++])<minLimit);
+
+			PxU32 index1 = runningAddress;
+			const InflatedType maxLimit = getMaxX(box0);
+			while(getMinX(bounds[index1])<=maxLimit)
+			{
+//				if(index0!=index1)
+				{
+					const PxU32 aggIndex1 = mAggregate->getAggregated(index1);
+					testPair(pairs, bounds, bounds, groups, aggIndex0, aggIndex1, index0, index1);
+				}
+				index1++;
+			}
+			index0++;
+		}
+#else
+		const InflatedAABB* bounds = mAggregate->mInflatedBounds;
+		const PxU32* sorted = mAggregate->getSortedMinBounds();
+
+		const PxU32* const lastSorted = &sorted[size0];
+		const PxU32* runningAddress = sorted;
+		while(runningAddress<lastSorted && sorted<lastSorted)
+		{
+			const PxU32 index0 = *sorted++;
+			const PxU32 aggIndex0 = mAggregate->getAggregated(index0);
+
+			const InflatedType minLimit = getMinX(bounds[index0]);
+			while(getMinX(bounds[*runningAddress++])<minLimit);
+
+			const PxU32* runningAddress2 = runningAddress;
+			const InflatedType maxLimit = getMaxX(bounds[index0]);
+			PxU32 index1;
+			while(getMinX(bounds[index1 = *runningAddress2++])<=maxLimit)
+			{
+//				if(index0!=index1)
+				{
+					const PxU32 aggIndex1 = mAggregate->getAggregated(index1);
+					testPair(pairs, bounds, bounds, groups, aggIndex0, aggIndex1, index0, index1);
+				}
+			}
+		}
+#endif
+	}
+}
+
+/////
+
+Aggregate::Aggregate(BoundsIndex index, bool selfCollisions) :
+	mIndex			(index),
+	mInflatedBounds	(NULL),
+	mAllocatedSize	(0),
+	mDirtySort		(false)
+{
+	resetDirtyState();
+	mSelfCollisionPairs = selfCollisions ? PX_NEW(PersistentSelfCollisionPairs)(this) : NULL;
+}
+
+Aggregate::~Aggregate()
+{
+	PX_FREE_AND_RESET(mInflatedBounds);
+
+	if(mSelfCollisionPairs)
+		PX_DELETE_AND_RESET(mSelfCollisionPairs);
+}
+
+void Aggregate::sortBounds()
+{
+	mDirtySort = false;
+	const PxU32 nbObjects = getNbAggregated();
+
+//	if(nbObjects>128)
+	{
+		PX_ALLOCA(minPosBounds, InflatedType, nbObjects+1);
+		bool alreadySorted =  true;
+		InflatedType previousB = getMinX(mInflatedBounds[0]);
+		minPosBounds[0] = previousB;
+		for(PxU32 i=1;i<nbObjects;i++)
+		{
+			const InflatedType minB = getMinX(mInflatedBounds[i]);
+			if(minB<previousB)
+				alreadySorted = false;
+			previousB = minB;
+			minPosBounds[i] = minB;
+		}
+		if(alreadySorted)
+			return;
+
+		{
+#ifdef STORE_SORTED_BOUNDS
+		Cm::RadixSortBuffered mRS;
+#endif
+
+#ifdef USE_SIMD_BOUNDS
+		minPosBounds[nbObjects] = 0xffffffff;
+		mRS.Sort(minPosBounds, nbObjects+1, /*RadixHint::*/RADIX_UNSIGNED);
+#else
+		minPosBounds[nbObjects] = PX_MAX_F32;
+		mRS.Sort(minPosBounds, nbObjects+1);
+#endif
+
+#ifdef STORE_SORTED_BOUNDS
+		if(1)
+		{
+			Ps::Array<PxU32> copy = mAggregated;
+			InflatedAABB* boundsCopy = reinterpret_cast<InflatedAABB*>(PX_ALLOC(sizeof(InflatedAABB)*(nbObjects+1), "mInflatedBounds"));
+			memcpy(boundsCopy, mInflatedBounds, (nbObjects+1)*sizeof(InflatedAABB));
+
+			const PxU32* Sorted = mRS.GetRanks();
+			for(PxU32 i=0;i<nbObjects;i++)
+			{
+				const PxU32 sortedIndex = Sorted[i];
+				mAggregated[i] = copy[sortedIndex];
+				mInflatedBounds[i] = boundsCopy[sortedIndex];
+			}
+			PX_FREE(boundsCopy);
+		}
+#endif
+		}
+	}
+/*	else
+	{
+		struct Keys
+		{
+			InflatedType	mMinX;
+			PxU32			mIndex;
+			PX_FORCE_INLINE bool operator < (const Keys& data) const
+			{
+				return mMinX < data.mMinX;
+			}		
+		};
+
+		PX_ALLOCA(minPosBounds, Keys, nbObjects+1);
+		for(PxU32 i=0;i<nbObjects;i++)
+		{
+			minPosBounds[i].mMinX = getMinX(mInflatedBounds[i]);
+			minPosBounds[i].mIndex = i;
+		}
+
+#ifdef USE_SIMD_BOUNDS
+		minPosBounds[nbObjects].mMinX = 0xffffffff;
+		minPosBounds[nbObjects].mIndex = nbObjects;
+//		mRS.Sort(minPosBounds, nbObjects+1, Cm::RadixHint::RADIX_UNSIGNED);
+		Ps::sort<Keys>(minPosBounds, nbObjects+1);
+#else
+		minPosBounds[nbObjects].mMinX = PX_MAX_F32;
+		minPosBounds[nbObjects].mIndex = nbObjects;
+		mRS.Sort(minPosBounds, nbObjects+1);
+#endif
+	}*/
+}
+
+#if PX_INTEL_FAMILY
+	#define SSE_CONST4(name, val) static const PX_ALIGN(16, PxU32 name[4]) = { (val), (val), (val), (val) } 
+	#define SSE_CONST(name) *(const __m128i *)&name
+	#define SSE_CONSTF(name) *(const __m128 *)&name
+
+SSE_CONST4(gMaskAll, 0xffffffff);
+SSE_CONST4(gMaskSign, PX_SIGN_BITMASK);
+
+static PX_FORCE_INLINE __m128 encode(const Vec4V src)
+{
+	const __m128i MinV = _mm_castps_si128(src);
+#if PX_PS4 || PX_OSX
+	// PT: this doesn't compile on PC
+	const __m128i BranchAValueV = _mm_andnot_si128(MinV, _mm_load_ps(reinterpret_cast<const float*>(&gMaskAll)));
+	const __m128i BranchBValueV = _mm_or_si128(MinV, _mm_load_ps(reinterpret_cast<const float*>(&gMaskSign)));
+#else
+	// PT: this doesn't compile on PS4
+	const __m128i BranchAValueV = _mm_andnot_si128(MinV, SSE_CONST(gMaskAll));
+	const __m128i BranchBValueV = _mm_or_si128(MinV, SSE_CONST(gMaskSign));
+#endif
+	const __m128i SelMaskV = _mm_srai_epi32(MinV, 31);
+	__m128i EncodedV = _mm_or_si128(_mm_andnot_si128(SelMaskV, BranchBValueV), _mm_and_si128(SelMaskV, BranchAValueV));
+	EncodedV = _mm_srli_epi32(EncodedV, 1);
+	return _mm_castsi128_ps(EncodedV);
+}
+#endif
+
+#ifdef USE_SIMD_BOUNDS
+static PX_FORCE_INLINE	void encodeBounds(SIMD_AABB* PX_RESTRICT dst, Vec4V minV, Vec4V maxV)
+{
+#if PX_INTEL_FAMILY
+	const Vec4V EncodedMinV = encode(minV);
+	PX_ALIGN(16, PxVec4) TmpMin4;
+	V4StoreA(EncodedMinV, &TmpMin4.x);
+
+	const Vec4V EncodedMaxV = encode(maxV);
+	PX_ALIGN(16, PxVec4) TmpMax4;
+	V4StoreA(EncodedMaxV, &TmpMax4.x);
+
+	const PxU32* BinaryMin = reinterpret_cast<const PxU32*>(&TmpMin4.x);
+	dst->mMinX = BinaryMin[0];
+	dst->mMinY = BinaryMin[1];
+	dst->mMinZ = BinaryMin[2];
+
+	const PxU32* BinaryMax = reinterpret_cast<const PxU32*>(&TmpMax4.x);
+	dst->mMaxX = BinaryMax[0];
+	dst->mMaxY = BinaryMax[1];
+	dst->mMaxZ = BinaryMax[2];
+#else
+	PxBounds3 tmp;
+	StoreBounds(tmp, minV, maxV);
+	dst->initFrom2(tmp);
+#endif
+}
+#endif //USE_SIMD_BOUNDS
+
+void Aggregate::allocateBounds()
+{
+	const PxU32 size = getNbAggregated();
+	if(size!=mAllocatedSize)
+	{
+		mAllocatedSize = size;
+		PX_FREE(mInflatedBounds);
+		mInflatedBounds = reinterpret_cast<InflatedAABB*>(PX_ALLOC(sizeof(InflatedAABB)*(size+1), "mInflatedBounds"));
+	}
+}
+
+void Aggregate::computeBounds(const BoundsArray& boundsArray, const float* contactDistances) /*PX_RESTRICT*/
+{
+//	PX_PROFILE_ZONE("Aggregate::computeBounds",0);
+
+	const PxU32 size = getNbAggregated();
+	PX_ASSERT(size);
+
+	// PT: TODO: delay the conversion to integers until we sort (i.e. really need) the aggregated bounds?
+
+	const PxU32 lookAhead = 4;
+	const PxBounds3* PX_RESTRICT bounds = boundsArray.begin();
+	Vec4V minimumV;
+	Vec4V maximumV;
+	{
+		const BoundsIndex index0 = getAggregated(0);
+		const PxU32 last = PxMin(lookAhead, size-1);
+		for(PxU32 i=1;i<=last;i++)
+		{
+			const BoundsIndex index = getAggregated(i);
+			Ps::prefetchLine(bounds + index, 0);
+			Ps::prefetchLine(contactDistances + index, 0);
+		}
+		const PxBounds3& b = bounds[index0];
+		const Vec4V offsetV = V4Load(contactDistances[index0]);
+		minimumV = V4Sub(V4LoadU(&b.minimum.x), offsetV);
+		maximumV = V4Add(V4LoadU(&b.maximum.x), offsetV);
+#ifdef USE_SIMD_BOUNDS
+		encodeBounds(&mInflatedBounds[0], minimumV, maximumV);
+#else
+		StoreBounds(mInflatedBounds[0], minimumV, maximumV);
+#endif
+	}
+
+	for(PxU32 i=1;i<size;i++)
+	{
+		const BoundsIndex index = getAggregated(i);
+		if(i+lookAhead<size)
+		{
+			const BoundsIndex nextIndex = getAggregated(i+lookAhead);
+			Ps::prefetchLine(bounds + nextIndex, 0);
+			Ps::prefetchLine(contactDistances + nextIndex, 0);
+		}
+		const PxBounds3& b = bounds[index];
+		const Vec4V offsetV = V4Load(contactDistances[index]);
+		const Vec4V aggregatedBoundsMinV = V4Sub(V4LoadU(&b.minimum.x), offsetV);
+		const Vec4V aggregatedBoundsMaxV = V4Add(V4LoadU(&b.maximum.x), offsetV);
+		minimumV = V4Min(minimumV, aggregatedBoundsMinV);
+		maximumV = V4Max(maximumV, aggregatedBoundsMaxV);
+#ifdef USE_SIMD_BOUNDS
+		encodeBounds(&mInflatedBounds[i], aggregatedBoundsMinV, aggregatedBoundsMaxV);
+#else
+		StoreBounds(mInflatedBounds[i], aggregatedBoundsMinV, aggregatedBoundsMaxV);
+#endif
+	}
+
+	StoreBounds(mBounds, minimumV, maximumV);
+//	StoreBounds(boundsArray.begin()[mIndex], minimumV, maximumV);
+//	boundsArray.setChangedState();
+
+/*	if(0)
+	{
+		const PxBounds3& previousBounds = boundsArray.getBounds(mIndex);
+		if(previousBounds.minimum==aggregateBounds.minimum && previousBounds.maximum==aggregateBounds.maximum)
+		{
+			// PT: same bounds as before
+			printf("SAME BOUNDS\n");
+		}
+	}*/
+#ifdef USE_SIMD_BOUNDS
+	mInflatedBounds[size].mMinX = 0xffffffff;
+#else
+	mInflatedBounds[size].minimum.x = PX_MAX_F32;
+#endif
+	mDirtySort = true;
+}
+
+/////
+
+void SimpleAABBManager::reserveShapeSpace(PxU32 nbTotalBounds)
+{
+	nbTotalBounds = Ps::nextPowerOfTwo(nbTotalBounds);
+	mGroups.resize(nbTotalBounds, PX_INVALID_U32);
+	mVolumeData.resize(nbTotalBounds);					//KS - must be initialized so that userData is NULL for SQ-only shapes
+	mContactDistance.resizeUninitialized(nbTotalBounds);
+	mAddedHandleMap.resize(nbTotalBounds);
+	mRemovedHandleMap.resize(nbTotalBounds);
+}
+
+#if PX_VC
+#pragma warning(disable: 4355 )	// "this" used in base member initializer list
+#endif
+
+SimpleAABBManager::SimpleAABBManager(BroadPhase& bp, BoundsArray& boundsArray, Ps::Array<PxReal, Ps::VirtualAllocator>& contactDistance, PxU32 maxNbAggregates, PxU32 maxNbShapes, Ps::VirtualAllocator& allocator, PxU64 contextID) :
+	mPostBroadPhase				(this, "postBroadPhase"),
+	mChangedHandleMap			(allocator),
+	mGroups						(allocator),
+	mContactDistance			(contactDistance),
+	mVolumeData					(PX_DEBUG_EXP("SimpleAABBManager::mVolumeData")),
+	mAddedHandles				(allocator),
+	mUpdatedHandles				(allocator),
+	mRemovedHandles				(allocator),
+	mBroadPhase					(bp),
+	mBoundsArray				(boundsArray),
+	mOutOfBoundsObjects		(PX_DEBUG_EXP("SimpleAABBManager::mOutOfBoundsObjects")),
+	mOutOfBoundsAggregates	(PX_DEBUG_EXP("SimpleAABBManager::mOutOfBoundsAggregates")),
+	//mCreatedOverlaps			{ Ps::Array<Bp::AABBOverlap>(PX_DEBUG_EXP("SimpleAABBManager::mCreatedOverlaps")) },
+	//mDestroyedOverlaps			{ Ps::Array<Bp::AABBOverlap>(PX_DEBUG_EXP("SimpleAABBManager::mDestroyedOverlaps")) },
+	mScratchAllocator			(NULL),
+	mNarrowPhaseUnblockTask		(NULL),
+	mUsedSize					(0),
+	mOriginShifted				(false),
+	mPersistentStateChanged		(true),
+	mNbAggregates				(0),
+	mFirstFreeAggregate			(PX_INVALID_U32),
+	mTimestamp					(0),
+	mAggregateGroupTide			(BP_INVALID_BP_HANDLE-1),
+	mContextID					(contextID)
+{
+	PX_UNUSED(maxNbAggregates);	// PT: TODO: use it or remove it
+	reserveShapeSpace(PxMax(maxNbShapes, 1u));
+
+//	mCreatedOverlaps.reserve(16000);
+}
+
+static void releasePairs(AggPairMap& map)
+{
+	for(AggPairMap::Iterator iter = map.getIterator(); !iter.done(); ++iter)
+		PX_DELETE(iter->second);
+}
+
+void SimpleAABBManager::destroy()
+{
+	releasePairs(mActorAggregatePairs);
+	releasePairs(mAggregateAggregatePairs);
+
+	const PxU32 nb = mAggregates.size();
+	for(PxU32 i=0;i<nb;i++)
+	{
+		bool found = false;
+		PxU32 currentFree = mFirstFreeAggregate;
+		while(currentFree!=PX_INVALID_U32)
+		{
+			if(currentFree==i)
+			{
+				found = true;
+				break;
+			}
+			currentFree = PxU32(reinterpret_cast<size_t>(mAggregates[currentFree]));
+		}
+
+		if(!found)
+		{
+			Aggregate* a = mAggregates[i];
+			PX_DELETE(a);
+		}
+	}
+
+	PX_DELETE(this);
+}
+
+/*bool SimpleAABBManager::checkID(ShapeHandle id)
+{
+	for(AggPairMap::Iterator iter = mActorAggregatePairs.getIterator(); !iter.done(); ++iter)
+	{
+		PersistentActorAggregatePair* p = static_cast<PersistentActorAggregatePair*>(iter->second);
+		if(p->mActorHandle==id || p->mAggregateHandle==id)
+			return false;
+	}
+
+	for(AggPairMap::Iterator iter = mAggregateAggregatePairs.getIterator(); !iter.done(); ++iter)
+	{
+		PersistentAggregateAggregatePair* p = static_cast<PersistentAggregateAggregatePair*>(iter->second);
+		if(p->mAggregateHandle0==id || p->mAggregateHandle1==id)
+			return false;
+	}
+	return true;
+}*/
+
+static void removeAggregateFromDirtyArray(Aggregate* aggregate, Ps::Array<Aggregate*>& dirtyAggregates)
+{
+	// PT: TODO: do this lazily like for interactions?
+	if(aggregate->isDirty())
+	{
+		const PxU32 dirtyIndex = aggregate->mDirtyIndex;
+		PX_ASSERT(dirtyAggregates[dirtyIndex]==aggregate);
+		dirtyAggregates.replaceWithLast(dirtyIndex);
+		if(dirtyIndex<dirtyAggregates.size())
+			dirtyAggregates[dirtyIndex]->mDirtyIndex = dirtyIndex;
+		aggregate->resetDirtyState();
+	}
+	else
+	{
+		PX_ASSERT(!dirtyAggregates.findAndReplaceWithLast(aggregate));
+	}
+}
+
+// PT: TODO: what is the "userData" here?
+bool SimpleAABBManager::addBounds(BoundsIndex index, PxReal contactDistance, PxU32 group, void* userData, AggregateHandle aggregateHandle, PxU8 volumeType)
+{
+//	PX_ASSERT(checkID(index));
+
+	initEntry(index, contactDistance, group, userData);
+	mVolumeData[index].setVolumeType(volumeType);
+
+	if(aggregateHandle==PX_INVALID_U32)
+	{
+		mVolumeData[index].setSingleActor();
+
+		mAddedHandleMap.set(index);
+
+		mPersistentStateChanged = true;
+	}
+	else
+	{
+#if PX_CHECKED
+		if(aggregateHandle>=mAggregates.size())
+		{
+			Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "AABBManager::addBounds - aggregateId out of bounds\n");
+			return false;
+		}
+
+/*		{
+			PxU32 firstFreeAggregate = mFirstFreeAggregate;
+			while(firstFreeAggregate!=PX_INVALID_U32)
+			{
+				if(firstFreeAggregate==aggregateHandle)
+				{
+					Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "AABBManager::destroyAggregate - aggregate has already been removed\n");
+					return BP_INVALID_BP_HANDLE;
+				}
+				firstFreeAggregate = PxU32(reinterpret_cast<size_t>(mAggregates[firstFreeAggregate]));
+			}
+		}*/
+#endif
+		mVolumeData[index].setAggregated(aggregateHandle);
+
+		mPersistentStateChanged = true;		// PT: TODO: do we need this here?
+
+		Aggregate* aggregate = getAggregateFromHandle(aggregateHandle);
+
+		if(aggregate->getNbAggregated()<BP_MAX_AGGREGATE_BOUND_SIZE)
+		{
+			// PT: schedule the aggregate for BP insertion here, if we just added its first shape
+			if(!aggregate->getNbAggregated())
+			{
+				if(mRemovedHandleMap.test(aggregate->mIndex))
+					mRemovedHandleMap.reset(aggregate->mIndex);
+				else
+					mAddedHandleMap.set(aggregate->mIndex);
+			}
+			aggregate->addAggregated(index);
+
+			// PT: new actor added to aggregate => mark dirty to recompute bounds later
+			aggregate->markAsDirty(mDirtyAggregates);
+		}
+#if PX_CHECKED
+		else
+		{
+			// PT: TODO: change "AABBManager::createVolume", revisit limit & what happens to these shapes
+			PX_CHECK_AND_RETURN_VAL(false, "AABBManager::createVolume - An aggregate has exceeded the limit of 128 PxShapes, not all shapes of the aggregate will be added to the broapdhase \n", true);
+		}
+#endif
+	}
+
+	// PT: TODO: remove or use this return value. Currently useless since always true. Gives birth to unreachable code in callers.
+	return true;
+}
+
+void SimpleAABBManager::removeBounds(BoundsIndex index)
+{
+	// PT: TODO: shouldn't it be compared to mUsedSize?
+	PX_ASSERT(index < mVolumeData.size());
+
+	if(mVolumeData[index].isSingleActor())
+	{
+		removeBPEntry(index);
+
+		mPersistentStateChanged = true;
+	}
+	else
+	{
+		PX_ASSERT(mVolumeData[index].isAggregated());
+
+		const AggregateHandle aggregateHandle = mVolumeData[index].getAggregateOwner();
+		Aggregate* aggregate = getAggregateFromHandle(aggregateHandle);
+		bool status = aggregate->removeAggregated(index);
+		(void)status;
+//		PX_ASSERT(status);	// PT: can be false when >128 shapes
+
+		// PT: remove empty aggregates, otherwise the BP will crash with empty bounds
+		if(!aggregate->getNbAggregated())
+		{
+			removeBPEntry(aggregate->mIndex);
+			removeAggregateFromDirtyArray(aggregate, mDirtyAggregates);
+		}
+		else
+			aggregate->markAsDirty(mDirtyAggregates);	// PT: actor removed from aggregate => mark dirty to recompute bounds later
+
+		mPersistentStateChanged = true;	// PT: TODO: do we need this here?
+	}
+
+	resetEntry(index);
+}
+
+// PT: TODO: the userData is actually a PxAggregate pointer. Maybe we could expose/use that.
+AggregateHandle SimpleAABBManager::createAggregate(BoundsIndex index, void* userData, const bool selfCollisions)
+{
+//	PX_ASSERT(checkID(index));
+
+	Aggregate* aggregate = PX_NEW(Aggregate)(index, selfCollisions);
+
+	AggregateHandle handle;
+	if(mFirstFreeAggregate==PX_INVALID_U32)
+	{
+		handle = mAggregates.size();
+		mAggregates.pushBack(aggregate);
+	}
+	else
+	{
+		handle = mFirstFreeAggregate;
+		mFirstFreeAggregate = PxU32(reinterpret_cast<size_t>(mAggregates[mFirstFreeAggregate]));
+		mAggregates[handle] = aggregate;
+	}
+
+	initEntry(index, 0.0f, getAggregateGroup(), userData);
+
+	mVolumeData[index].setAggregate(handle);
+
+	mBoundsArray.setBounds(PxBounds3::empty(), index);	// PT: no need to set mPersistentStateChanged since "setBounds" already does something similar
+
+	mNbAggregates++;
+
+	// PT: we don't add empty aggregates to mAddedHandleMap yet, since they make the BP crash.
+	return handle;
+}
+
+BoundsIndex SimpleAABBManager::destroyAggregate(AggregateHandle aggregateHandle)
+{
+#if PX_CHECKED
+	if(aggregateHandle>=mAggregates.size())
+	{
+		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "AABBManager::destroyAggregate - aggregateId out of bounds\n");
+		return BP_INVALID_BP_HANDLE;
+	}
+
+	{
+		PxU32 firstFreeAggregate = mFirstFreeAggregate;
+		while(firstFreeAggregate!=PX_INVALID_U32)
+		{
+			if(firstFreeAggregate==aggregateHandle)
+			{
+				Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "AABBManager::destroyAggregate - aggregate has already been removed\n");
+				return BP_INVALID_BP_HANDLE;
+			}
+			firstFreeAggregate = PxU32(reinterpret_cast<size_t>(mAggregates[firstFreeAggregate]));
+		}
+	}
+#endif
+
+	Aggregate* aggregate = getAggregateFromHandle(aggregateHandle);
+
+#if PX_CHECKED
+	if(aggregate->getNbAggregated())
+	{
+		Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "AABBManager::destroyAggregate - aggregate still has bounds that needs removed\n");
+		return BP_INVALID_BP_HANDLE;
+	}
+#endif
+
+	const BoundsIndex index = aggregate->mIndex;
+	removeAggregateFromDirtyArray(aggregate, mDirtyAggregates);
+
+	if(mAddedHandleMap.test(index))			// PT: if object had been added this frame...
+		mAddedHandleMap.reset(index);		// PT: ...then simply revert the previous operation locally (it hasn't been passed to the BP yet).
+	else if(aggregate->getNbAggregated())	// PT: else we need to remove it from the BP if it has been added there. If there's no aggregated
+		mRemovedHandleMap.set(index);		// PT: shapes then the aggregate has never been added, or already removed.
+
+	PX_DELETE(aggregate);
+	mAggregates[aggregateHandle] = reinterpret_cast<Aggregate*>(size_t(mFirstFreeAggregate));
+	mFirstFreeAggregate = aggregateHandle;
+
+	// PT: TODO: shouldn't it be compared to mUsedSize?
+	PX_ASSERT(index < mVolumeData.size());
+
+	releaseAggregateGroup(mGroups[index]);
+	resetEntry(index);
+
+	mPersistentStateChanged = true;
+
+	PX_ASSERT(mNbAggregates);
+	mNbAggregates--;
+
+	return index;
+}
+
+void SimpleAABBManager::handleOriginShift()
+{
+	mOriginShifted = false;
+	mPersistentStateChanged = true;
+	// PT: TODO: isn't the following loop potentially updating removed objects?
+	// PT: TODO: check that aggregates code is correct here
+	for(PxU32 i=0; i<mUsedSize; i++)
+	{
+		if(!mAddedHandleMap.test(i) && mGroups[i] != PX_INVALID_U32)
+		{
+			if(mVolumeData[i].isSingleActor())
+				mUpdatedHandles.pushBack(i);	// PT: TODO: BoundsIndex-to-ShapeHandle confusion here
+			else if(mVolumeData[i].isAggregate())
+			{
+				const AggregateHandle aggregateHandle = mVolumeData[i].getAggregate();
+				Aggregate* aggregate = getAggregateFromHandle(aggregateHandle);
+				aggregate->markAsDirty(mDirtyAggregates);
+				aggregate->allocateBounds();
+				aggregate->computeBounds(mBoundsArray, mContactDistance.begin());
+				mBoundsArray.begin()[aggregate->mIndex] = aggregate->mBounds;
+				mUpdatedHandles.pushBack(i);	// PT: TODO: BoundsIndex-to-ShapeHandle confusion here
+			}
+		}
+	}
+}
+
+void AggregateBoundsComputationTask::runInternal()
+{
+	const BoundsArray& boundArray = mManager->getBoundsArray();
+	const float* contactDistances = mManager->getContactDistances();
+
+	PxU32 size = mNbToGo;
+	Aggregate** currentAggregate = mAggregates + mStart;
+	while(size--)
+	{
+		if(size)
+		{
+			Aggregate* nextAggregate = *(currentAggregate+1);
+			Ps::prefetchLine(nextAggregate, 0);
+			Ps::prefetchLine(nextAggregate, 64);
+		}
+
+		(*currentAggregate)->computeBounds(boundArray, contactDistances);
+		currentAggregate++;
+	}
+}
+
+void FinalizeUpdateTask::runInternal()
+{
+	mManager->finalizeUpdate(mNumCpuTasks, mScratchAllocator, getContinuation(), mNarrowPhaseUnlockTask);
+}
+
+void SimpleAABBManager::startAggregateBoundsComputationTasks(PxU32 nbToGo, PxU32 numCpuTasks, Cm::FlushPool& flushPool)
+{
+	const PxU32 nbAggregatesPerTask = nbToGo > numCpuTasks ? nbToGo / numCpuTasks : nbToGo;
+
+	// PT: TODO: better load balancing
+
+	PxU32 start = 0;
+	while(nbToGo)
+	{
+		AggregateBoundsComputationTask* T = PX_PLACEMENT_NEW(flushPool.allocate(sizeof(AggregateBoundsComputationTask)), AggregateBoundsComputationTask());
+
+		const PxU32 nb = nbToGo < nbAggregatesPerTask ? nbToGo : nbAggregatesPerTask;
+		T->Init(this, start, nb, mDirtyAggregates.begin());
+		start += nb;
+		nbToGo -= nb;
+
+		T->setContinuation(&mFinalizeUpdateTask);
+		T->removeReference();
+	}
+}
+
+void SimpleAABBManager::updateAABBsAndBP(PxU32 numCpuTasks, Cm::FlushPool& flushPool, PxcScratchAllocator* scratchAllocator, bool hasContactDistanceUpdated, PxBaseTask* continuation, PxBaseTask* narrowPhaseUnlockTask)
+{
+	PX_PROFILE_ZONE("SimpleAABBManager::updateAABBsAndBP", getContextId());
+
+	mPersistentStateChanged = mPersistentStateChanged || hasContactDistanceUpdated;
+
+	mScratchAllocator = scratchAllocator;
+	mNarrowPhaseUnblockTask = narrowPhaseUnlockTask;
+
+	const bool singleThreaded = gSingleThreaded || numCpuTasks<2;
+	if(!singleThreaded)
+	{
+		PX_ASSERT(numCpuTasks);
+		mFinalizeUpdateTask.Init(this, numCpuTasks, scratchAllocator, narrowPhaseUnlockTask);
+		mFinalizeUpdateTask.setContinuation(continuation);
+	}
+
+	// Add
+	{
+		PX_PROFILE_ZONE("SimpleAABBManager::updateAABBsAndBP - add", getContextId());
+
+		resetOrClear(mAddedHandles);
+
+		const PxU32* bits = mAddedHandleMap.getWords();
+		if(bits)
+		{
+			const PxU32 lastSetBit = mAddedHandleMap.findLast();
+			for(PxU32 w = 0; w <= lastSetBit >> 5; ++w)
+			{
+				for(PxU32 b = bits[w]; b; b &= b-1)
+				{
+					const BoundsIndex handle = PxU32(w<<5|Ps::lowestSetBit(b));
+					PX_ASSERT(!mVolumeData[handle].isAggregated());
+					mAddedHandles.pushBack(handle);		// PT: TODO: BoundsIndex-to-ShapeHandle confusion here
+				}
+			}
+		}
+	}
+
+	// Update
+	{
+		PX_PROFILE_ZONE("SimpleAABBManager::updateAABBsAndBP - update", getContextId());
+
+		resetOrClear(mUpdatedHandles);
+		if(!mOriginShifted)
+		{
+			// PT: TODO:
+			// - intercept calls marking aggregateD shapes dirty, in order to mark their aggregates dirty at the same time. That way we don't discover
+			// them while parsing the map, i.e. the map is already fully complete when the parsing begins (no need to parse twice etc).
+			// - once this is done, aggregateD shapes can be ignored during parsing (since we only needed them to go to their aggregates)
+			// - we can then handle aggregates while parsing the map, i.e. there's no need for sorting anymore.
+			// - there will be some thoughts to do about the dirty aggregates coming from the added map parsing: we still need to compute their bounds,
+			// but we don't want to add these to mUpdatedHandles (since they're already in  mAddedHandles)
+			// - we still need the set of dirty aggregates post broadphase, but we don't want to re-parse the full map for self-collisions. So we may still
+			// need to put dirty aggregates in an array, but that might be simplified now
+			// - the 'isDirty' checks to updatePairs can use the update map though - but the boundedTest is probably more expensive than the current test
+
+			// PT: TODO: another idea: just output all aggregate handles by default then have a pass on mUpdatedHandles to remove them if that wasn't actually necessary
+			// ...or just drop the artificial requirement for aggregates...
+
+			{
+				PX_PROFILE_ZONE("SimpleAABBManager::updateAABBsAndBP - update - bitmap iteration", getContextId());
+
+				const PxU32* bits = mChangedHandleMap.getWords();
+				if(bits)
+				{
+					const PxU32 lastSetBit = mChangedHandleMap.findLast();
+					for(PxU32 w = 0; w <= lastSetBit >> 5; ++w)
+					{
+						for(PxU32 b = bits[w]; b; b &= b-1)
+						{
+							const BoundsIndex handle = PxU32(w<<5|Ps::lowestSetBit(b));
+							PX_ASSERT(!mRemovedHandleMap.test(handle));		// a handle may only be updated and deleted if it was just added.
+							PX_ASSERT(!mVolumeData[handle].isAggregate());	// PT: make sure changedShapes doesn't contain aggregates
+
+							if(mAddedHandleMap.test(handle))					// just-inserted handles may also be marked updated, so skip them
+								continue;
+
+							if(mVolumeData[handle].isSingleActor())
+							{
+								PX_ASSERT(mGroups[handle] != PX_INVALID_U32);
+								mUpdatedHandles.pushBack(handle);	// PT: TODO: BoundsIndex-to-ShapeHandle confusion here
+							}
+							else
+							{
+								PX_ASSERT(mVolumeData[handle].isAggregated());
+								const AggregateHandle aggregateHandle = mVolumeData[handle].getAggregateOwner();
+								Aggregate* aggregate = getAggregateFromHandle(aggregateHandle);
+								// PT: an actor from the aggregate has been updated => mark dirty to recompute bounds later
+								// PT: we don't recompute the bounds right away since multiple actors from the same aggregate may have changed.
+								aggregate->markAsDirty(mDirtyAggregates);
+							}
+						}
+					}
+				}
+			}
+
+			const PxU32 size = mDirtyAggregates.size();
+			if(size)
+			{
+				PX_PROFILE_ZONE("SimpleAABBManager::updateAABBsAndBP - update - dirty iteration", getContextId());
+				for(PxU32 i=0;i<size;i++)
+				{
+					Aggregate* aggregate = mDirtyAggregates[i];
+					if(i!=size-1)
+					{
+						Aggregate* nextAggregate = mDirtyAggregates[i];
+						Ps::prefetchLine(nextAggregate, 0);
+					}
+
+					aggregate->allocateBounds();
+					if(singleThreaded)
+					{
+						aggregate->computeBounds(mBoundsArray, mContactDistance.begin());
+						mBoundsArray.begin()[aggregate->mIndex] = aggregate->mBounds;
+					}
+
+					// PT: Can happen when an aggregate has been created and then its actors have been changed (with e.g. setLocalPose)
+					// before a BP call.
+					if(!mAddedHandleMap.test(aggregate->mIndex))
+						mUpdatedHandles.pushBack(aggregate->mIndex);	// PT: TODO: BoundsIndex-to-ShapeHandle confusion here
+				}
+
+				if(!singleThreaded)
+					startAggregateBoundsComputationTasks(size, numCpuTasks, flushPool);
+
+				// PT: we're already sorted if no dirty-aggregates are involved
+				{
+					PX_PROFILE_ZONE("SimpleAABBManager::updateAABBsAndBP - update - sort", getContextId());
+
+					mPersistentStateChanged = true;	// PT: was previously set multiple times within 'computeBounds'
+					// PT: TODO: remove this
+					Ps::sort(mUpdatedHandles.begin(), mUpdatedHandles.size());
+				}
+			}
+		}
+		else
+		{
+			handleOriginShift();
+		}
+	}
+
+	// Remove
+	{
+		PX_PROFILE_ZONE("SimpleAABBManager::updateAABBsAndBP - remove", getContextId());
+
+		resetOrClear(mRemovedHandles);
+
+		const PxU32* bits = mRemovedHandleMap.getWords();
+		if(bits)
+		{
+			const PxU32 lastSetBit = mRemovedHandleMap.findLast();
+			for(PxU32 w = 0; w <= lastSetBit >> 5; ++w)
+			{
+				for(PxU32 b = bits[w]; b; b &= b-1)
+				{
+					const BoundsIndex handle = PxU32(w<<5|Ps::lowestSetBit(b));
+					PX_ASSERT(!mVolumeData[handle].isAggregated());
+					mRemovedHandles.pushBack(handle);	// PT: TODO: BoundsIndex-to-ShapeHandle confusion here
+				}
+			}
+		}
+	}
+
+	/////
+
+	// PT: TODO: do we need to run these threads when we origin-shifted everything before?
+	if(singleThreaded)
+		finalizeUpdate(numCpuTasks, scratchAllocator, continuation, narrowPhaseUnlockTask);
+	else
+		mFinalizeUpdateTask.removeReference();
+}
+
+void SimpleAABBManager::finalizeUpdate(PxU32 numCpuTasks, PxcScratchAllocator* scratchAllocator, PxBaseTask* continuation, PxBaseTask* narrowPhaseUnlockTask)
+{
+	PX_PROFILE_ZONE("SimpleAABBManager::finalizeUpdate", getContextId());
+
+	const bool singleThreaded = gSingleThreaded || numCpuTasks<2;
+	if(!singleThreaded)
+	{
+		const PxU32 size = mDirtyAggregates.size();
+		for(PxU32 i=0;i<size;i++)
+		{
+			Aggregate* aggregate = mDirtyAggregates[i];
+			mBoundsArray.begin()[aggregate->mIndex] = aggregate->mBounds;
+		}
+	}
+
+	const BroadPhaseUpdateData updateData(	mAddedHandles.begin(), mAddedHandles.size(),
+											mUpdatedHandles.begin(), mUpdatedHandles.size(),
+											mRemovedHandles.begin(), mRemovedHandles.size(),
+											mBoundsArray.begin(), mGroups.begin(), mContactDistance.begin(), mBoundsArray.getCapacity(),
+											mPersistentStateChanged || mBoundsArray.hasChanged());
+	mPersistentStateChanged = false;
+
+	PX_ASSERT(updateData.isValid());
+	mPostBroadPhase.setContinuation(continuation);
+
+	//KS - skip broad phase if there are no updated shapes.
+	if (updateData.getNumCreatedHandles() != 0 || updateData.getNumRemovedHandles() != 0 || updateData.getNumUpdatedHandles() != 0)
+		mBroadPhase.update(numCpuTasks, scratchAllocator, updateData, &mPostBroadPhase, narrowPhaseUnlockTask);
+	else if (narrowPhaseUnlockTask)
+		narrowPhaseUnlockTask->removeReference();
+	mPostBroadPhase.removeReference();
+}
+
+static PX_FORCE_INLINE void createOverlap(Ps::Array<AABBOverlap>* overlaps, const Ps::Array<VolumeData>& volumeData, PxU32 id0, PxU32 id1, ActorHandle handle)
+{
+//	overlaps.pushBack(AABBOverlap(volumeData[id0].userData, volumeData[id1].userData, handle));
+	PxU8 volumeType = PxMax(volumeData[id0].getVolumeType(), volumeData[id1].getVolumeType());
+	overlaps[volumeType].pushBack(AABBOverlap(reinterpret_cast<void*>(size_t(id0)), reinterpret_cast<void*>(size_t(id1)), handle));
+}
+
+static PX_FORCE_INLINE void deleteOverlap(Ps::Array<AABBOverlap>* overlaps, const Ps::Array<VolumeData>& volumeData, PxU32 id0, PxU32 id1, ActorHandle handle)
+{
+//	PX_ASSERT(volumeData[id0].userData);
+//	PX_ASSERT(volumeData[id1].userData);
+	if (volumeData[id0].getUserData() && volumeData[id1].getUserData())	// PT: TODO: no idea if this is the right thing to do or if it's normal to get null ptrs here
+	{
+		PxU8 volumeType = PxMax(volumeData[id0].getVolumeType(), volumeData[id1].getVolumeType());
+		//		overlaps.pushBack(AABBOverlap(volumeData[id0].userData, volumeData[id1].userData, handle));
+		overlaps[volumeType].pushBack(AABBOverlap(reinterpret_cast<void*>(size_t(id0)), reinterpret_cast<void*>(size_t(id1)), handle));
+	}
+}
+
+void PersistentPairs::outputDeletedOverlaps(Ps::Array<AABBOverlap>* overlaps, const Ps::Array<VolumeData>& volumeData)
+{
+#ifdef USE_MBP_PAIR_MANAGER
+	const PxU32 nbActivePairs = mPM.mNbActivePairs;
+	for(PxU32 i=0;i<nbActivePairs;i++)
+	{
+		const MBP_Pair& p = mPM.mActivePairs[i];
+		deleteOverlap(overlaps, volumeData, p.getId0(), p.getId1(), BP_INVALID_BP_HANDLE);
+	}
+#else
+	const PxU32 size = mCurrentPairs[mCurrentPairArray].size();
+	const Pair* pairs = mCurrentPairs[mCurrentPairArray].begin();
+	for(PxU32 i=0;i<size;i++)
+		deleteOverlap(overlaps, volumeData, pairs[i].mID0, pairs[i].mID1, BP_INVALID_BP_HANDLE);
+#endif
+}
+
+#ifndef USE_MBP_PAIR_MANAGER
+static void computePairDeltas(/*const*/ Ps::Array<Pair>& newPairs, const Ps::Array<Pair>& previousPairs,
+							  Ps::Array<VolumeData>& volumeData, Ps::Array<AABBOverlap>& createdOverlaps, Ps::Array<AABBOverlap>& destroyedOverlaps)
+{
+	if(0)
+	{
+		Ps::sort(newPairs.begin(), newPairs.size());
+		(void)previousPairs;
+		(void)volumeData;
+		(void)createdOverlaps;
+		(void)destroyedOverlaps;
+/*		Ps::sort(newPairs.begin(), newPairs.size());
+		const Pair* prevPairs = previousPairs.begin();
+		const Pair* currPairs = newPairs.begin();
+		const Pair& p = *currPairs;
+		const Pair& o = *prevPairs;
+		if(p==o)
+		{
+		}*/
+
+	}
+	else
+	{
+
+	// PT: TODO: optimize
+	const PxU32 nbPreviousPairs = previousPairs.size();
+/*	if(!nbPreviousPairs)
+	{
+		// PT: "HashSet::erase" still computes the hash when the table is empty
+		const PxU32 size = newPairs.size();
+		for(PxU32 i=0;i<size;i++)
+			createOverlap(createdOverlaps, volumeData, newPairs[i].mID0, newPairs[i].mID1, BP_INVALID_BP_HANDLE);	// pairs[i] is new => add to mCreatedOverlaps
+	}
+	else*/
+	{
+		Ps::HashSet<Pair> existingPairs;
+		{
+			for(PxU32 i=0;i<nbPreviousPairs;i++)
+				existingPairs.insert(previousPairs[i]);
+		}
+
+		const PxU32 size = newPairs.size();
+		for(PxU32 i=0;i<size;i++)
+		{
+			if(!existingPairs.erase(newPairs[i]))	// PT: pairs[i] existed before => persistent pair => nothing to do. Just remove from current pairs so that we don't delete it later
+				createOverlap(createdOverlaps, volumeData, newPairs[i].mID0, newPairs[i].mID1, BP_INVALID_BP_HANDLE);	// pairs[i] is new => add to mCreatedOverlaps
+		}
+
+		// PT: pairs remaining in previousPairs did not appear in pairs => add to mDestroyedOverlaps
+		for(Ps::HashSet<Pair>::Iterator iter = existingPairs.getIterator(); !iter.done(); ++iter)
+			deleteOverlap(destroyedOverlaps, volumeData, iter->mID0, iter->mID1, BP_INVALID_BP_HANDLE);
+	}
+
+	}
+}
+#endif
+
+PX_FORCE_INLINE void PersistentPairs::updatePairs(	PxU32 timestamp, const PxBounds3* bounds, const float* contactDistances, const PxU32* groups,
+									Ps::Array<VolumeData>& volumeData, Ps::Array<AABBOverlap>* createdOverlaps, Ps::Array<AABBOverlap>* destroyedOverlaps)
+{
+	if(mTimestamp==timestamp)
+		return;
+
+	mTimestamp = timestamp;
+
+#ifdef USE_MBP_PAIR_MANAGER
+	findOverlaps(mPM, bounds, contactDistances, groups);
+
+	PxU32 i=0;
+	PxU32 nbActivePairs = mPM.mNbActivePairs;
+	while(i<nbActivePairs)
+	{
+		MBP_Pair& p = mPM.mActivePairs[i];
+		const PxU32 id0 = p.getId0();
+		const PxU32 id1 = p.getId1();
+
+		if(p.isNew())
+		{
+			createOverlap(createdOverlaps, volumeData, id0, id1, BP_INVALID_BP_HANDLE);
+
+			p.clearNew();
+			p.clearUpdated();
+
+			i++;
+		}
+		else if(p.isUpdated())
+		{
+			p.clearUpdated();
+			i++;
+		}
+		else
+		{
+			deleteOverlap(destroyedOverlaps, volumeData, id0, id1, BP_INVALID_BP_HANDLE);
+
+			const PxU32 hashValue = hash(id0, id1) & mPM.mMask;
+			mPM.removePair(id0, id1, hashValue, i);
+			nbActivePairs--;
+		}
+	}
+	mPM.shrinkMemory();
+#else
+	const PxU32 otherArray = 1 - mCurrentPairArray;
+	Ps::Array<Pair>& mTempPairs = mCurrentPairs[otherArray];
+
+//		mTempPairs.clear();
+	mTempPairs.forceSize_Unsafe(0);
+
+	findOverlaps(mTempPairs, bounds, contactDistances, groups);
+
+	computePairDeltas(mTempPairs, mCurrentPairs[mCurrentPairArray], volumeData, createdOverlaps, destroyedOverlaps);
+	mCurrentPairArray = otherArray;
+#endif
+}
+
+PersistentActorAggregatePair* SimpleAABBManager::createPersistentActorAggregatePair(ShapeHandle volA, ShapeHandle volB)
+{
+	ShapeHandle	actorHandle;
+	ShapeHandle	aggregateHandle;
+	if(mVolumeData[volA].isAggregate())
+	{
+		aggregateHandle = volA;
+		actorHandle = volB;
+	}
+	else
+	{
+		PX_ASSERT(mVolumeData[volB].isAggregate());
+		aggregateHandle = volB;
+		actorHandle = volA;
+	}
+	const AggregateHandle h = mVolumeData[aggregateHandle].getAggregate();
+	Aggregate* aggregate = getAggregateFromHandle(h);
+	PX_ASSERT(aggregate->mIndex==aggregateHandle);
+	return PX_NEW(PersistentActorAggregatePair)(aggregate, actorHandle);	// PT: TODO: use a pool or something
+}
+
+PersistentAggregateAggregatePair* SimpleAABBManager::createPersistentAggregateAggregatePair(ShapeHandle volA, ShapeHandle volB)
+{
+	PX_ASSERT(mVolumeData[volA].isAggregate());
+	PX_ASSERT(mVolumeData[volB].isAggregate());
+	const AggregateHandle h0 = mVolumeData[volA].getAggregate();
+	const AggregateHandle h1 = mVolumeData[volB].getAggregate();
+	Aggregate* aggregate0 = getAggregateFromHandle(h0);
+	Aggregate* aggregate1 = getAggregateFromHandle(h1);
+	PX_ASSERT(aggregate0->mIndex==volA);
+	PX_ASSERT(aggregate1->mIndex==volB);
+	return PX_NEW(PersistentAggregateAggregatePair)(aggregate0, aggregate1);	// PT: TODO: use a pool or something
+}
+
+void SimpleAABBManager::updatePairs(PersistentPairs& p)
+{
+	p.updatePairs(mTimestamp, mBoundsArray.begin(), mContactDistance.begin(), mGroups.begin(), mVolumeData, mCreatedOverlaps, mDestroyedOverlaps);
+}
+
+void SimpleAABBManager::processBPCreatedPair(const BroadPhasePairReport& pair)
+{
+	PX_ASSERT(!mVolumeData[pair.mVolA].isAggregated());
+	PX_ASSERT(!mVolumeData[pair.mVolB].isAggregated());
+	const bool isSingleActorA = mVolumeData[pair.mVolA].isSingleActor();
+	const bool isSingleActorB = mVolumeData[pair.mVolB].isSingleActor();
+
+	if(isSingleActorA && isSingleActorB)
+	{
+		createOverlap(mCreatedOverlaps, mVolumeData, pair.mVolA, pair.mVolB, pair.mHandle);	// PT: regular actor-actor pair
+		return;
+	}
+
+	// PT: TODO: check if this is needed
+	ShapeHandle volA = pair.mVolA;
+	ShapeHandle volB = pair.mVolB;
+	if(volB<volA)
+		Ps::swap(volA, volB);
+
+	PersistentPairs* newPair;
+	AggPairMap* pairMap;
+	if(!isSingleActorA && !isSingleActorB)
+	{
+		pairMap = &mAggregateAggregatePairs;	// PT: aggregate-aggregate pair
+		newPair = createPersistentAggregateAggregatePair(volA, volB);
+	}
+	else
+	{
+		pairMap = &mActorAggregatePairs;	// PT: actor-aggregate pair
+		newPair = createPersistentActorAggregatePair(volA, volB);
+	}
+	bool status = pairMap->insert(AggPair(volA, volB), newPair);
+	PX_UNUSED(status);
+	PX_ASSERT(status);
+	updatePairs(*newPair);
+
+#ifdef EXPERIMENT
+	BroadPhasePair* bpPairs = mBroadPhase.getBroadPhasePairs();
+	if(bpPairs)
+		bpPairs[pair.mHandle].mUserData = newPair;
+#endif
+}
+
+void SimpleAABBManager::processBPDeletedPair(const BroadPhasePairReport& pair)
+{
+	PX_ASSERT(!mVolumeData[pair.mVolA].isAggregated());
+	PX_ASSERT(!mVolumeData[pair.mVolB].isAggregated());
+	const bool isSingleActorA = mVolumeData[pair.mVolA].isSingleActor();
+	const bool isSingleActorB = mVolumeData[pair.mVolB].isSingleActor();
+
+	if(isSingleActorA && isSingleActorB)
+	{
+		deleteOverlap(mDestroyedOverlaps, mVolumeData, pair.mVolA, pair.mVolB, pair.mHandle);	// PT: regular actor-actor pair
+		return;
+	}
+
+	// PT: TODO: check if this is needed
+	ShapeHandle volA = pair.mVolA;
+	ShapeHandle volB = pair.mVolB;
+	if(volB<volA)
+		Ps::swap(volA, volB);
+
+	AggPairMap* pairMap;
+	if(!isSingleActorA && !isSingleActorB)
+		pairMap = &mAggregateAggregatePairs;	// PT: aggregate-aggregate pair
+	else
+		pairMap = &mActorAggregatePairs;		// PT: actor-aggregate pair
+
+	PersistentPairs* p;
+#ifdef EXPERIMENT
+	BroadPhasePair* bpPairs = mBroadPhase.getBroadPhasePairs();
+	if(bpPairs)
+	{
+		p = reinterpret_cast<PersistentPairs*>(bpPairs[pair.mHandle].mUserData);
+	}
+	else
+#endif
+	{
+		const AggPairMap::Entry* e = pairMap->find(AggPair(volA, volB));
+		PX_ASSERT(e);
+		p = e->second;
+	}
+
+	p->outputDeletedOverlaps(mDestroyedOverlaps, mVolumeData);
+	p->mShouldBeDeleted = true;
+}
+
+struct CreatedPairHandler
+{
+	static PX_FORCE_INLINE void processPair(SimpleAABBManager& manager, const BroadPhasePairReport& pair) { manager.processBPCreatedPair(pair); }
+};
+
+struct DeletedPairHandler
+{
+	static PX_FORCE_INLINE void processPair(SimpleAABBManager& manager, const BroadPhasePairReport& pair) { manager.processBPDeletedPair(pair); }
+};
+
+template<class FunctionT>
+static void processBPPairs(PxU32 nbPairs, const BroadPhasePairReport* pairs, SimpleAABBManager& manager)
+{
+	// PT: TODO: figure out this ShapeHandle/BpHandle thing. Is it ok to use "BP_INVALID_BP_HANDLE" for a "ShapeHandle"?
+	ShapeHandle previousA = BP_INVALID_BP_HANDLE;
+	ShapeHandle previousB = BP_INVALID_BP_HANDLE;
+
+	while(nbPairs--)
+	{
+		PX_ASSERT(pairs->mVolA!=BP_INVALID_BP_HANDLE);
+		PX_ASSERT(pairs->mVolB!=BP_INVALID_BP_HANDLE);
+		// PT: TODO: why is that test needed now? GPU broadphase?
+		if(pairs->mVolA != previousA || pairs->mVolB != previousB)
+		{
+			previousA = pairs->mVolA;
+			previousB = pairs->mVolB;
+			FunctionT::processPair(manager, *pairs);
+		}
+		pairs++;
+	}
+}
+
+static void processAggregatePairs(AggPairMap& map, SimpleAABBManager& manager)
+{
+	// PT: TODO: hmmm we have a list of dirty aggregates but we don't have a list of dirty pairs.
+	// PT: not sure how the 3.4 trunk solves this but let's just iterate all pairs for now
+	// PT: atm we reuse this loop to delete removed interactions
+	// PT: TODO: in fact we could handle all the "lost pairs" stuff right there with extra aabb-abb tests
+
+	// PT: TODO: replace with decent hash map - or remove the hashmap entirely and use a linear array
+	Ps::Array<AggPair> removedEntries;
+	for(AggPairMap::Iterator iter = map.getIterator(); !iter.done(); ++iter)
+	{
+		PersistentPairs* p = iter->second;
+		if(p->update(manager))
+		{
+			removedEntries.pushBack(iter->first);
+			PX_DELETE(p);
+		}
+	}
+	for(PxU32 i=0;i<removedEntries.size();i++)
+	{
+		bool status = map.erase(removedEntries[i]);
+		PX_ASSERT(status);
+		PX_UNUSED(status);
+	}
+}
+
+void SimpleAABBManager::postBroadPhase(PxBaseTask*)
+{
+	PX_PROFILE_ZONE("SimpleAABBManager::postBroadPhase", getContextId());
+
+	mTimestamp++;
+
+	// PT: TODO: consider merging mCreatedOverlaps & mDestroyedOverlaps
+	// PT: TODO: revisit memory management of mCreatedOverlaps & mDestroyedOverlaps
+
+	for (PxU32 i = 0; i < VolumeBuckets::eCOUNT; ++i)
+	{
+		resetOrClear(mCreatedOverlaps[i]);
+		resetOrClear(mDestroyedOverlaps[i]);
+	}
+
+//	processBPPairs<CreatedPairHandler>(mBroadPhase.getNbCreatedPairs(), mBroadPhase.getCreatedPairs(), *this);
+	processBPPairs<DeletedPairHandler>(mBroadPhase.getNbDeletedPairs(), mBroadPhase.getDeletedPairs(), *this);
+
+	{
+		processAggregatePairs(mActorAggregatePairs, *this);
+		processAggregatePairs(mAggregateAggregatePairs, *this);
+
+		const PxU32 size = mDirtyAggregates.size();
+		for(PxU32 i=0;i<size;i++)
+		{
+			Aggregate* aggregate = mDirtyAggregates[i];
+			aggregate->resetDirtyState();
+
+			if(aggregate->mSelfCollisionPairs)
+				updatePairs(*aggregate->mSelfCollisionPairs);
+		}
+		resetOrClear(mDirtyAggregates);
+	}
+
+	processBPPairs<CreatedPairHandler>(mBroadPhase.getNbCreatedPairs(), mBroadPhase.getCreatedPairs(), *this);
+//	processBPPairs<DeletedPairHandler>(mBroadPhase.getNbDeletedPairs(), mBroadPhase.getDeletedPairs(), *this);
+
+	// PT: TODO: revisit this
+	// Filter out pairs in mDestroyedOverlaps that already exist in mCreatedOverlaps. This should be done better using bitmaps
+	// and some imposed ordering on previous operations. Later.
+	// We could also have a dedicated function "reinsertBroadPhase()", which would preserve the existing interactions at Sc-level.
+	if(1)
+	{
+		PxU32 totalCreatedOverlaps = 0;
+		for (PxU32 idx = 0; idx < VolumeBuckets::eCOUNT; ++idx)
+			totalCreatedOverlaps += mCreatedOverlaps[idx].size();
+
+		mCreatedPairs.clear();
+		mCreatedPairs.reserve(totalCreatedOverlaps);
+		
+		for (PxU32 idx = 0; idx < VolumeBuckets::eCOUNT; ++idx)
+		{
+			const PxU32 nbDestroyedOverlaps = mDestroyedOverlaps[idx].size();
+			{
+				const PxU32 size = mCreatedOverlaps[idx].size();
+				for (PxU32 i = 0; i < size; i++)
+				{
+					const PxU32 id0 = PxU32(size_t(mCreatedOverlaps[idx][i].mUserData0));
+					const PxU32 id1 = PxU32(size_t(mCreatedOverlaps[idx][i].mUserData1));
+					mCreatedOverlaps[idx][i].mUserData0 = mVolumeData[id0].getUserData();
+					mCreatedOverlaps[idx][i].mUserData1 = mVolumeData[id1].getUserData();
+					if (nbDestroyedOverlaps)
+						mCreatedPairs.insert(Pair(id0, id1));
+				}
+			}
+			PxU32 newSize = 0;
+			for (PxU32 i = 0; i < nbDestroyedOverlaps; i++)
+			{
+				const PxU32 id0 = PxU32(size_t(mDestroyedOverlaps[idx][i].mUserData0));
+				const PxU32 id1 = PxU32(size_t(mDestroyedOverlaps[idx][i].mUserData1));
+				if (!mCreatedPairs.contains(Pair(id0, id1)))
+				{
+					mDestroyedOverlaps[idx][newSize].mUserData0 = mVolumeData[id0].getUserData();
+					mDestroyedOverlaps[idx][newSize].mUserData1 = mVolumeData[id1].getUserData();
+					newSize++;
+				}
+			}
+			mDestroyedOverlaps[idx].forceSize_Unsafe(newSize);
+		}
+	}
+
+	// Handle out-of-bounds objects
+	{
+		PxU32 nbObjects = mBroadPhase.getNbOutOfBoundsObjects();
+		const PxU32* objects = mBroadPhase.getOutOfBoundsObjects();
+		while(nbObjects--)
+		{
+			const PxU32 index = *objects++;
+			if(!mRemovedHandleMap.test(index))
+			{
+				if(mVolumeData[index].isSingleActor())
+					mOutOfBoundsObjects.pushBack(mVolumeData[index].getUserData());
+				else
+				{
+					PX_ASSERT(mVolumeData[index].isAggregate());
+					mOutOfBoundsAggregates.pushBack(mVolumeData[index].getUserData());
+				}
+			}
+		}
+	}
+
+	mAddedHandleMap.clear();
+	mRemovedHandleMap.clear();
+}
+
+void SimpleAABBManager::freeBuffers()
+{
+	// PT: TODO: investigate if we need more stuff here
+	mBroadPhase.freeBuffers();
+}
+
+void SimpleAABBManager::shiftOrigin(const PxVec3& shift)
+{
+	mBroadPhase.shiftOrigin(shift);
+	mOriginShifted = true;
+}
+
+// PT: disabled this by default, since it bypasses all per-shape/per-actor visualization flags
+//static const bool gVisualizeAggregateElems = false;
+
+void SimpleAABBManager::visualize(Cm::RenderOutput& out)
+{
+	const PxTransform idt = PxTransform(PxIdentity);
+	out << idt;
+
+	const PxU32 N = mAggregates.size();
+	for(PxU32 i=0;i<N;i++)
+	{
+		Aggregate* aggregate = mAggregates[i];
+		if(aggregate->getNbAggregated())
+		{
+			out << PxU32(PxDebugColor::eARGB_GREEN);
+			const PxBounds3& b = mBoundsArray.getBounds(aggregate->mIndex);
+			out << Cm::DebugBox(b, true);
+		}
+	}
+
+/*	const PxU32 N = mAggregateManager.getAggregatesCapacity();
+	for(PxU32 i=0;i<N;i++)
+	{
+		const Aggregate* aggregate = mAggregateManager.getAggregate(i);
+		if(aggregate->nbElems)
+		{
+			if(!mAggregatesUpdated.isInList(BpHandle(i)))
+				out << PxU32(PxDebugColor::eARGB_GREEN);
+			else
+				out << PxU32(PxDebugColor::eARGB_RED);
+
+			PxBounds3 decoded;
+			const IntegerAABB& iaabb = mBPElems.getAABB(aggregate->bpElemId);
+			iaabb.decode(decoded);
+
+			out << Cm::DebugBox(decoded, true);
+
+			if(gVisualizeAggregateElems)
+			{
+				PxU32 elem = aggregate->elemHeadID;
+				while(BP_INVALID_BP_HANDLE!=elem)
+				{
+					out << PxU32(PxDebugColor::eARGB_CYAN);
+					const IntegerAABB elemBounds = mAggregateElems.getAABB(elem);
+					elemBounds.decode(decoded);
+					out << Cm::DebugBox(decoded, true);
+					elem = mAggregateElems.getNextId(elem);
+				}
+			}
+		}
+	}*/
+}
+
+} //namespace Bp
+
+} //namespace physx