public void debugPrintAxis(int axis, bool checkCardinality) { int numEdges = m_pHandles[0].m_maxEdges[axis]; if (numEdges == 5) { int ibreak = 0; } System.Console.Out.WriteLine("SAP Axis {0}, numEdges={1}", axis, numEdges); if (numEdges == 3) { int ibreak = 0; } for (int i = 0; i < numEdges + 1; i++) { Edge pEdge = m_pEdges[axis, i]; Handle pHandlePrev = getHandle(pEdge.m_handle); int handleIndex = pEdge.IsMax()? pHandlePrev.m_maxEdges[axis] : pHandlePrev.m_minEdges[axis]; char beginOrEnd; beginOrEnd = pEdge.IsMax()?'E':'B'; System.Console.Out.WriteLine("[{0},h={1},p={2:x},i={3}]", beginOrEnd, pEdge.m_handle, pEdge.m_pos, handleIndex); if (pEdge.m_pos == 0x7fd4) { int ibreak = 0; } } if (checkCardinality) { Debug.Assert(numEdges == m_numHandles * 2 + 1); } }
public void SortMinUp(int axis, ushort edge, IDispatcher dispatcher, bool updateOverlaps) { int edgeIndex = edge; int nextIndex = edgeIndex + 1; Edge pEdge = m_pEdges[axis, edgeIndex]; Edge pNext = m_pEdges[axis, nextIndex]; Handle pHandleEdge = GetHandle(pEdge.m_handle); while (pNext.m_handle != 0 && (pEdge.m_pos >= pNext.m_pos)) { Handle pHandleNext = GetHandle(pNext.m_handle); if (pNext.IsMax()) { Handle handle0 = GetHandle(pEdge.m_handle); Handle handle1 = GetHandle(pNext.m_handle); int axis1 = (1 << axis) & 3; int axis2 = (1 << axis1) & 3; // if next edge is maximum remove any overlap between the two handles if (updateOverlaps #if USE_OVERLAP_TEST_ON_REMOVES && TestOverlap2D(handle0, handle1, axis1, axis2) #endif //USE_OVERLAP_TEST_ON_REMOVES ) { m_pairCache.RemoveOverlappingPair(handle0, handle1, dispatcher); if (m_userPairCallback != null) { m_userPairCallback.RemoveOverlappingPair(handle0, handle1, dispatcher); } } // update edge reference in other handle pHandleNext.m_maxEdges[axis]--; } else { pHandleNext.m_minEdges[axis]--; } pHandleEdge.m_minEdges[axis]++; SanityCheckHandle(pHandleEdge, axis); Edge.Swap(pEdge, pNext); // increment edgeIndex++; nextIndex++; pEdge = m_pEdges[axis, edgeIndex]; pNext = m_pEdges[axis, nextIndex]; } }
//Overlap* AddOverlap(int handleA, int handleB); //void RemoveOverlap(int handleA, int handleB); public void SortMinDown(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 is a maximum check the bounds and add an overlap if necessary int axis1 = (1 << axis) & 3; int axis2 = (1 << axis1) & 3; if (updateOverlaps && TestOverlap2D(pHandleEdge, pHandlePrev, axis1, axis2)) { m_pairCache.AddOverlappingPair(pHandleEdge, pHandlePrev); if (m_userPairCallback != null) { m_userPairCallback.AddOverlappingPair(pHandleEdge, pHandlePrev); } //AddOverlap(pEdge.m_handle, pPrev.m_handle); } // update edge reference in other handle pHandlePrev.m_maxEdges[axis]++; } else { pHandlePrev.m_minEdges[axis]++; } pHandleEdge.m_minEdges[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 }
public void SortMaxUp(int axis, ushort edge, IDispatcher dispatcher, bool updateOverlaps) { int edgeIndex = edge; int nextIndex = edgeIndex + 1; Edge pEdge = m_pEdges[axis, edgeIndex]; Edge pNext = m_pEdges[axis, nextIndex]; Handle pHandleEdge = GetHandle(pEdge.m_handle); while (pNext.m_handle != 0 && (pEdge.m_pos >= pNext.m_pos)) { Handle pHandleNext = GetHandle(pNext.m_handle); int axis1 = (1 << axis) & 3; int axis2 = (1 << axis1) & 3; if (!pNext.IsMax()) { // if next edge is a minimum check the bounds and add an overlap if necessary if (updateOverlaps && TestOverlap2D(pHandleEdge, pHandleNext, axis1, axis2)) { Handle handle0 = GetHandle(pEdge.m_handle); Handle handle1 = GetHandle(pNext.m_handle); m_pairCache.AddOverlappingPair(handle0, handle1); if (m_userPairCallback != null) { m_userPairCallback.AddOverlappingPair(handle0, handle1); } } // update edge reference in other handle pHandleNext.m_minEdges[axis]--; } else { pHandleNext.m_maxEdges[axis]--; } pHandleEdge.m_maxEdges[axis]++; SanityCheckHandle(pHandleEdge, axis); Edge.Swap(pEdge, pNext); // increment edgeIndex++; nextIndex++; pEdge = m_pEdges[axis, edgeIndex]; pNext = m_pEdges[axis, nextIndex]; } }
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 }