Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 2075 insertions, 0 deletions

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
+
+