public static void Swap(Edge a, Edge b)
{
swapEdge.Copy(a);
a.Copy(b);
b.Copy(swapEdge);
}
public void Copy(Edge edge)
{
m_pos = edge.m_pos;
m_handle = edge.m_handle;
}
//public AxisSweep3Internal(ref IndexedVector3 worldAabbMin,ref IndexedVector3 worldAabbMax, int handleMask, int handleSentinel, int maxHandles = 16384, OverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false);
public AxisSweep3Internal(ref IndexedVector3 worldAabbMin, ref IndexedVector3 worldAabbMax, int handleMask, ushort handleSentinel, ushort userMaxHandles, IOverlappingPairCache pairCache, bool disableRaycastAccelerator)
{
m_bpHandleMask = (handleMask);
m_handleSentinel = (handleSentinel);
m_pairCache = (pairCache);
m_userPairCallback = null;
m_ownsPairCache = (false);
m_invalidPair = 0;
m_raycastAccelerator = null;
ushort maxHandles = (ushort)(userMaxHandles + 1);//need to add one sentinel handle
if (m_pairCache == null)
{
m_pairCache = new HashedOverlappingPairCache();
m_ownsPairCache = true;
}
if (!disableRaycastAccelerator)
{
m_nullPairCache = new NullPairCache();
m_raycastAccelerator = new DbvtBroadphase(m_nullPairCache); //m_pairCache);
m_raycastAccelerator.m_deferedcollide = true; //don't add/remove pairs
}
//btAssert(bounds.HasVolume());
// init bounds
m_worldAabbMin = worldAabbMin;
m_worldAabbMax = worldAabbMax;
IndexedVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
int maxInt = m_handleSentinel;
m_quantize = new IndexedVector3((float)maxInt) / aabbSize;
// allocate handles buffer, using btAlignedAlloc, and put all handles on free list
m_pHandles = new Handle[maxHandles];
for (int i = 0; i < m_pHandles.Length; ++i)
{
m_pHandles[i] = new Handle();
}
m_maxHandles = maxHandles;
m_numHandles = 0;
// handle 0 is reserved as the null index, and is also used as the sentinel
m_firstFreeHandle = 1;
{
for (ushort i = m_firstFreeHandle; i < maxHandles; i++)
{
ushort nextFree = (ushort)(i + (ushort)1);
m_pHandles[i].SetNextFree(nextFree);
}
m_pHandles[maxHandles - 1].SetNextFree(0);
}
{
m_pEdges = new Edge[3, (maxHandles * 2)];
// allocate edge buffers
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < maxHandles * 2; ++j)
{
m_pEdges[i, j] = new Edge();
}
}
}
//removed overlap management
// make boundary sentinels
m_pHandles[0].SetClientObject(null);
for (int axis = 0; axis < 3; axis++)
{
m_pHandles[0].m_minEdges[axis] = 0;
m_pHandles[0].m_maxEdges[axis] = 1;
m_pEdges[axis, 0].m_pos = 0;
m_pEdges[axis, 0].m_handle = 0;
m_pEdges[axis, 1].m_pos = m_handleSentinel;
m_pEdges[axis, 1].m_handle = 0;
#if DEBUG_BROADPHASE
debugPrintAxis(axis);
#endif //DEBUG_BROADPHASE
}
}
public void SortMaxDown(int axis, ushort edge, IDispatcher dispatcher, bool updateOverlaps)
{
int edgeIndex = edge;
int prevIndex = edgeIndex - 1;
Edge pEdge = m_pEdges[axis, edgeIndex];
Edge pPrev = m_pEdges[axis, prevIndex];
Handle pHandleEdge = GetHandle(pEdge.m_handle);
while (pEdge.m_pos < pPrev.m_pos)
{
Handle pHandlePrev = GetHandle(pPrev.m_handle);
if (!pPrev.IsMax())
{
// if previous edge was a minimum remove any overlap between the two handles
Handle handle0 = GetHandle(pEdge.m_handle);
Handle handle1 = GetHandle(pPrev.m_handle);
int axis1 = (1 << axis) & 3;
int axis2 = (1 << axis1) & 3;
if (updateOverlaps
#if USE_OVERLAP_TEST_ON_REMOVES
&& TestOverlap2D(handle0, handle1, axis1, axis2)
#endif //USE_OVERLAP_TEST_ON_REMOVES
)
{
//this is done during the overlappingpairarray iteration/narrowphase collision
m_pairCache.RemoveOverlappingPair(handle0, handle1, dispatcher);
if (m_userPairCallback != null)
{
m_userPairCallback.RemoveOverlappingPair(handle0, handle1, dispatcher);
}
}
// update edge reference in other handle
pHandlePrev.m_minEdges[axis]++;;
}
else
{
pHandlePrev.m_maxEdges[axis]++;
}
pHandleEdge.m_maxEdges[axis]--;
SanityCheckHandle(pHandleEdge, axis);
Edge.Swap(pEdge, pPrev);
// decrement
edgeIndex--;
prevIndex--;
pEdge = m_pEdges[axis, edgeIndex];
pPrev = m_pEdges[axis, prevIndex];
}
#if DEBUG_BROADPHASE
debugPrintAxis(axis);
#endif //DEBUG_BROADPHASE
}