public unsafe void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) { float num = sortKey(proxy.UserData); if (num >= 0f) { fixed (float* ptr = this._querySortKeys) { float* ptr2 = ptr; while (*ptr2 < num && ptr2 < ptr + this._queryResultCount) { ptr2 += 4 / 4; } //int num2 = ((int)ptr2 - ((int)ptr) / 4) / 4; int num2 = (int)((ulong)ptr2 - (ulong)ptr) / 4; // STEVE if (maxCount != this._queryResultCount || num2 != this._queryResultCount) { if (maxCount == this._queryResultCount) { this._queryResultCount--; } for (int i = this._queryResultCount + 1; i > num2; i--) { this._querySortKeys[i] = this._querySortKeys[i - 1]; this._queryResults[i] = this._queryResults[i - 1]; } this._querySortKeys[num2] = num; this._queryResults[num2] = proxyId; this._queryResultCount++; //ptr = null; } } } }
public void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) { float key = sortKey(proxy.UserData); //Filter proxies on positive keys if (key < 0) { return; } //Merge the new key into the sorted list. //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); float[] querySortKeysPtr = _querySortKeys; int ip = 0; float p = querySortKeysPtr[ip]; while (p < key && ip < _queryResultCount) { p = querySortKeysPtr[ip]; ip++; } int i = ip; if (maxCount == _queryResultCount && i == _queryResultCount) { return; } if (maxCount == _queryResultCount) _queryResultCount--; }
public unsafe void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) { float key = sortKey(proxy.UserData); //Filter proxies on positive keys if (key < 0) { return; //Merge the new key into the sorted list. //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); fixed(float *querySortKeysPtr = _querySortKeys) { float *p = querySortKeysPtr; while (*p < key && p < &querySortKeysPtr[_queryResultCount]) { p++; } int i = (int)(p - &querySortKeysPtr[0]); if (maxCount == _queryResultCount && i == _queryResultCount) { return; } if (maxCount == _queryResultCount) { _queryResultCount--; } //std::copy_backward for (int j = _queryResultCount + 1; j > i; --j) { _querySortKeys[j] = _querySortKeys[j - 1]; _queryResults[j] = _queryResults[j - 1]; } _querySortKeys[i] = key; _queryResults[i] = proxyId; _queryResultCount++; } }
public unsafe void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) { float num = sortKey(proxy.UserData); if (num >= 0f) { fixed(float *ptr = this._querySortKeys) { float *ptr2 = ptr; while (*ptr2 < num && ptr2 < ptr + this._queryResultCount) { ptr2 += 4 / 4; } //int num2 = ((int)ptr2 - ((int)ptr) / 4) / 4; int num2 = (int)((ulong)ptr2 - (ulong)ptr) / 4; // STEVE if (maxCount != this._queryResultCount || num2 != this._queryResultCount) { if (maxCount == this._queryResultCount) { this._queryResultCount--; } for (int i = this._queryResultCount + 1; i > num2; i--) { this._querySortKeys[i] = this._querySortKeys[i - 1]; this._queryResults[i] = this._queryResults[i - 1]; } this._querySortKeys[num2] = num; this._queryResults[num2] = proxyId; this._queryResultCount++; //ptr = null; } } } }
int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey) { float maxLambda = 1; float dx = (segment.P2.X - segment.P1.X) * QuantizationFactor.X; float dy = (segment.P2.Y - segment.P1.Y) * QuantizationFactor.Y; int sx = dx <-Settings.FltEpsilon ? -1 : dx> Settings.FltEpsilon ? 1 : 0; int sy = dy <-Settings.FltEpsilon ? -1 : dy> Settings.FltEpsilon ? 1 : 0; Box2DxDebug.Assert(sx != 0 || sy != 0); float p1X = (segment.P1.X - WorldAabb.LowerBound.X) * QuantizationFactor.X; float p1Y = (segment.P1.Y - WorldAabb.LowerBound.Y) * QuantizationFactor.Y; #if ALLOWUNSAFE ushort *startValues = stackalloc ushort[2]; ushort *startValues2 = stackalloc ushort[2]; #else ushort[] startValues = new ushort[2]; ushort[] startValues2 = new ushort[2]; #endif int xIndex; int yIndex; ushort proxyId; // TODO_ERIN implement fast float to ushort conversion. startValues[0] = (ushort)((ushort)p1X & (BroadphaseMax - 1)); startValues2[0] = (ushort)((ushort)p1X | 1); startValues[1] = (ushort)((ushort)p1Y & (BroadphaseMax - 1)); startValues2[1] = (ushort)((ushort)p1Y | 1); //First deal with all the proxies that contain segment.p1 int lowerIndex; int upperIndex; Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], Bounds[0], 2 * ProxyCount, 0); if (sx >= 0) { xIndex = upperIndex - 1; } else { xIndex = lowerIndex; } Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], Bounds[1], 2 * ProxyCount, 1); if (sy >= 0) { yIndex = upperIndex - 1; } else { yIndex = lowerIndex; } //If we are using sortKey, then sort what we have so far, filtering negative keys if (sortKey != null) { //Fill keys for (int j = 0; j < QueryResultCount; j++) { QuerySortKeys[j] = sortKey(ProxyPool[QueryResults[j]].UserData); } //Bubble sort keys //Sorting negative values to the top, so we can easily remove them int i = 0; while (i < QueryResultCount - 1) { float a = QuerySortKeys[i]; float b = QuerySortKeys[i + 1]; if (a < 0 ? b >= 0 : a > b && b >= 0) { QuerySortKeys[i + 1] = a; QuerySortKeys[i] = b; ushort tempValue = QueryResults[i + 1]; QueryResults[i + 1] = QueryResults[i]; QueryResults[i] = tempValue; i--; if (i == -1) { i = 1; } } else { i++; } } //Skim off negative values while (QueryResultCount > 0 && QuerySortKeys[QueryResultCount - 1] < 0) { QueryResultCount--; } } //Now work through the rest of the segment for (;;) { float xProgress = 0; float yProgress = 0; if (xIndex < 0 || xIndex >= ProxyCount * 2) { break; } if (yIndex < 0 || yIndex >= ProxyCount * 2) { break; } if (sx != 0) { //Move on to the next bound if (sx > 0) { xIndex++; if (xIndex == ProxyCount * 2) { break; } } else { xIndex--; if (xIndex < 0) { break; } } xProgress = (Bounds[0][xIndex].Value - p1X) / dx; } if (sy != 0) { //Move on to the next bound if (sy > 0) { yIndex++; if (yIndex == ProxyCount * 2) { break; } } else { yIndex--; if (yIndex < 0) { break; } } yProgress = (Bounds[1][yIndex].Value - p1Y) / dy; } for (;;) { Proxy proxy; if (sy == 0 || (sx != 0 && xProgress < yProgress)) { if (xProgress > maxLambda) { break; } //Check that we are entering a proxy, not leaving if (sx > 0 ? Bounds[0][xIndex].IsLower : Bounds[0][xIndex].IsUpper) { //Check the other axis of the proxy proxyId = Bounds[0][xIndex].ProxyId; proxy = ProxyPool[proxyId]; if (sy >= 0) { if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { QueryResults[QueryResultCount] = proxyId; ++QueryResultCount; } } } else { if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { QueryResults[QueryResultCount] = proxyId; ++QueryResultCount; } } } } //Early out if (sortKey != null && QueryResultCount == maxCount && QueryResultCount > 0 && xProgress > QuerySortKeys[QueryResultCount - 1]) { break; } //Move on to the next bound if (sx > 0) { xIndex++; if (xIndex == ProxyCount * 2) { break; } } else { xIndex--; if (xIndex < 0) { break; } } xProgress = (Bounds[0][xIndex].Value - p1X) / dx; } else { if (yProgress > maxLambda) { break; } //Check that we are entering a proxy, not leaving if (sy > 0 ? Bounds[1][yIndex].IsLower : Bounds[1][yIndex].IsUpper) { //Check the other axis of the proxy proxyId = Bounds[1][yIndex].ProxyId; proxy = ProxyPool[proxyId]; if (sx >= 0) { if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { QueryResults[QueryResultCount] = proxyId; ++QueryResultCount; } } } else { if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { QueryResults[QueryResultCount] = proxyId; ++QueryResultCount; } } } } //Early out if (sortKey != null && QueryResultCount == maxCount && QueryResultCount > 0 && yProgress > QuerySortKeys[QueryResultCount - 1]) { break; } //Move on to the next bound if (sy > 0) { yIndex++; if (yIndex == ProxyCount * 2) { break; } } else { yIndex--; if (yIndex < 0) { break; } } yProgress = (Bounds[1][yIndex].Value - p1Y) / dy; } } break; } int count = 0; for (int i = 0; i < QueryResultCount && count < maxCount; ++i, ++count) { Box2DxDebug.Assert(QueryResults[i] < Settings.MaxProxies); Proxy proxy = ProxyPool[QueryResults[i]]; Box2DxDebug.Assert(proxy.IsValid); userData[i] = proxy.UserData; } // Prepare for next query. QueryResultCount = 0; IncrementTimeStamp(); return(count); }
public unsafe int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey) { float num = 1f; float num2 = (segment.P2.X - segment.P1.X) * this._quantizationFactor.X; float num3 = (segment.P2.Y - segment.P1.Y) * this._quantizationFactor.Y; int num4 = (num2 < -Settings.FLT_EPSILON) ? -1 : ((num2 > Settings.FLT_EPSILON) ? 1 : 0); int num5 = (num3 < -Settings.FLT_EPSILON) ? -1 : ((num3 > Settings.FLT_EPSILON) ? 1 : 0); Box2DXDebug.Assert(num4 != 0 || num5 != 0); float num6 = (segment.P1.X - this._worldAABB.LowerBound.X) * this._quantizationFactor.X; float num7 = (segment.P1.Y - this._worldAABB.LowerBound.Y) * this._quantizationFactor.Y; ushort *ptr = stackalloc ushort[2]; ushort *ptr2 = stackalloc ushort[2]; *ptr = (ushort)((ushort)num6 & BroadPhase.BROADPHASE_MAX - 1); *ptr2 = (ushort)((ushort)num6 | 1); ptr[2 / 2] = (ushort)((ushort)num7 & BroadPhase.BROADPHASE_MAX - 1); ptr2[2 / 2] = (ushort)((ushort)num7 | 1); int num8; int num9; this.Query(out num8, out num9, *ptr, *ptr2, this._bounds[0], 2 * this._proxyCount, 0); int num10; if (num4 >= 0) { num10 = num9 - 1; } else { num10 = num8; } this.Query(out num8, out num9, ptr[2 / 2], *(ptr2 + 2 / 2), this._bounds[1], 2 * this._proxyCount, 1); int num11; if (num5 >= 0) { num11 = num9 - 1; } else { num11 = num8; } int j; if (sortKey != null) { for (int i = 0; i < this._queryResultCount; i++) { this._querySortKeys[i] = sortKey(this._proxyPool[(int)this._queryResults[i]].UserData); } j = 0; while (j < this._queryResultCount - 1) { float num12 = this._querySortKeys[j]; float num13 = this._querySortKeys[j + 1]; if (((num12 < 0f) ? ((num13 >= 0f) ? 1 : 0) : ((num12 <= num13) ? 0 : ((num13 >= 0f) ? 1 : 0))) != 0) { this._querySortKeys[j + 1] = num12; this._querySortKeys[j] = num13; ushort num14 = this._queryResults[j + 1]; this._queryResults[j + 1] = this._queryResults[j]; this._queryResults[j] = num14; j--; if (j == -1) { j = 1; } } else { j++; } } while (this._queryResultCount > 0 && this._querySortKeys[this._queryResultCount - 1] < 0f) { this._queryResultCount--; } } float num15 = 0f; float num16 = 0f; if (num10 >= 0 && num10 < this._proxyCount * 2) { if (num11 >= 0 && num11 < this._proxyCount * 2) { if (num4 != 0) { if (num4 > 0) { num10++; if (num10 == this._proxyCount * 2) { goto IL_829; } } else { num10--; if (num10 < 0) { goto IL_829; } } num15 = ((float)this._bounds[0][num10].Value - num6) / num2; } if (num5 != 0) { if (num5 > 0) { num11++; if (num11 == this._proxyCount * 2) { goto IL_829; } } else { num11--; if (num11 < 0) { goto IL_829; } } num16 = ((float)this._bounds[1][num11].Value - num7) / num3; } while (true) { if (num5 == 0 || (num4 != 0 && num15 < num16)) { if (num15 > num) { break; } if ((num4 > 0) ? this._bounds[0][num10].IsLower : this._bounds[0][num10].IsUpper) { ushort proxyId = this._bounds[0][num10].ProxyId; Proxy proxy = this._proxyPool[(int)proxyId]; if (num5 >= 0) { if ((int)proxy.LowerBounds[1] <= num11 - 1 && (int)proxy.UpperBounds[1] >= num11) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } else { if ((int)proxy.LowerBounds[1] <= num11 && (int)proxy.UpperBounds[1] >= num11 + 1) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } } if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num15 > this._querySortKeys[this._queryResultCount - 1]) { break; } if (num4 > 0) { num10++; if (num10 == this._proxyCount * 2) { break; } } else { num10--; if (num10 < 0) { break; } } num15 = ((float)this._bounds[0][num10].Value - num6) / num2; } else { if (num16 > num) { break; } if ((num5 > 0) ? this._bounds[1][num11].IsLower : this._bounds[1][num11].IsUpper) { ushort proxyId = this._bounds[1][num11].ProxyId; Proxy proxy = this._proxyPool[(int)proxyId]; if (num4 >= 0) { if ((int)proxy.LowerBounds[0] <= num10 - 1 && (int)proxy.UpperBounds[0] >= num10) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } else { if ((int)proxy.LowerBounds[0] <= num10 && (int)proxy.UpperBounds[0] >= num10 + 1) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } } if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num16 > this._querySortKeys[this._queryResultCount - 1]) { break; } if (num5 > 0) { num11++; if (num11 == this._proxyCount * 2) { break; } } else { num11--; if (num11 < 0) { break; } } num16 = ((float)this._bounds[1][num11].Value - num7) / num3; } } } } IL_829: int num17 = 0; j = 0; while (j < this._queryResultCount && num17 < maxCount) { Box2DXDebug.Assert((int)this._queryResults[j] < Settings.MaxProxies); Proxy proxy2 = this._proxyPool[(int)this._queryResults[j]]; Box2DXDebug.Assert(proxy2.IsValid); userData[j] = proxy2.UserData; j++; num17++; } this._queryResultCount = 0; this.IncrementTimeStamp(); return(num17); }
public unsafe int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey) { float num = 1f; float num2 = (segment.P2.X - segment.P1.X) * this._quantizationFactor.X; float num3 = (segment.P2.Y - segment.P1.Y) * this._quantizationFactor.Y; int num4 = (num2 < -Settings.FLT_EPSILON) ? -1 : ((num2 > Settings.FLT_EPSILON) ? 1 : 0); int num5 = (num3 < -Settings.FLT_EPSILON) ? -1 : ((num3 > Settings.FLT_EPSILON) ? 1 : 0); Box2DXDebug.Assert(num4 != 0 || num5 != 0); float num6 = (segment.P1.X - this._worldAABB.LowerBound.X) * this._quantizationFactor.X; float num7 = (segment.P1.Y - this._worldAABB.LowerBound.Y) * this._quantizationFactor.Y; ushort* ptr = stackalloc ushort[2]; ushort* ptr2 = stackalloc ushort[2]; *ptr = (ushort)((ushort)num6 & BroadPhase.BROADPHASE_MAX - 1); *ptr2 = (ushort)((ushort)num6 | 1); ptr[2 / 2] = (ushort)((ushort)num7 & BroadPhase.BROADPHASE_MAX - 1); ptr2[2 / 2] = (ushort)((ushort)num7 | 1); int num8; int num9; this.Query(out num8, out num9, *ptr, *ptr2, this._bounds[0], 2 * this._proxyCount, 0); int num10; if (num4 >= 0) { num10 = num9 - 1; } else { num10 = num8; } this.Query(out num8, out num9, ptr[2 / 2], *(ptr2 + 2 / 2), this._bounds[1], 2 * this._proxyCount, 1); int num11; if (num5 >= 0) { num11 = num9 - 1; } else { num11 = num8; } int j; if (sortKey != null) { for (int i = 0; i < this._queryResultCount; i++) { this._querySortKeys[i] = sortKey(this._proxyPool[(int)this._queryResults[i]].UserData); } j = 0; while (j < this._queryResultCount - 1) { float num12 = this._querySortKeys[j]; float num13 = this._querySortKeys[j + 1]; if (((num12 < 0f) ? ((num13 >= 0f) ? 1 : 0) : ((num12 <= num13) ? 0 : ((num13 >= 0f) ? 1 : 0))) != 0) { this._querySortKeys[j + 1] = num12; this._querySortKeys[j] = num13; ushort num14 = this._queryResults[j + 1]; this._queryResults[j + 1] = this._queryResults[j]; this._queryResults[j] = num14; j--; if (j == -1) { j = 1; } } else { j++; } } while (this._queryResultCount > 0 && this._querySortKeys[this._queryResultCount - 1] < 0f) { this._queryResultCount--; } } float num15 = 0f; float num16 = 0f; if (num10 >= 0 && num10 < this._proxyCount * 2) { if (num11 >= 0 && num11 < this._proxyCount * 2) { if (num4 != 0) { if (num4 > 0) { num10++; if (num10 == this._proxyCount * 2) { goto IL_829; } } else { num10--; if (num10 < 0) { goto IL_829; } } num15 = ((float)this._bounds[0][num10].Value - num6) / num2; } if (num5 != 0) { if (num5 > 0) { num11++; if (num11 == this._proxyCount * 2) { goto IL_829; } } else { num11--; if (num11 < 0) { goto IL_829; } } num16 = ((float)this._bounds[1][num11].Value - num7) / num3; } while (true) { if (num5 == 0 || (num4 != 0 && num15 < num16)) { if (num15 > num) { break; } if ((num4 > 0) ? this._bounds[0][num10].IsLower : this._bounds[0][num10].IsUpper) { ushort proxyId = this._bounds[0][num10].ProxyId; Proxy proxy = this._proxyPool[(int)proxyId]; if (num5 >= 0) { if ((int)proxy.LowerBounds[1] <= num11 - 1 && (int)proxy.UpperBounds[1] >= num11) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } else { if ((int)proxy.LowerBounds[1] <= num11 && (int)proxy.UpperBounds[1] >= num11 + 1) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } } if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num15 > this._querySortKeys[this._queryResultCount - 1]) { break; } if (num4 > 0) { num10++; if (num10 == this._proxyCount * 2) { break; } } else { num10--; if (num10 < 0) { break; } } num15 = ((float)this._bounds[0][num10].Value - num6) / num2; } else { if (num16 > num) { break; } if ((num5 > 0) ? this._bounds[1][num11].IsLower : this._bounds[1][num11].IsUpper) { ushort proxyId = this._bounds[1][num11].ProxyId; Proxy proxy = this._proxyPool[(int)proxyId]; if (num4 >= 0) { if ((int)proxy.LowerBounds[0] <= num10 - 1 && (int)proxy.UpperBounds[0] >= num10) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } else { if ((int)proxy.LowerBounds[0] <= num10 && (int)proxy.UpperBounds[0] >= num10 + 1) { if (sortKey != null) { this.AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { this._queryResults[this._queryResultCount] = proxyId; this._queryResultCount++; } } } } if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num16 > this._querySortKeys[this._queryResultCount - 1]) { break; } if (num5 > 0) { num11++; if (num11 == this._proxyCount * 2) { break; } } else { num11--; if (num11 < 0) { break; } } num16 = ((float)this._bounds[1][num11].Value - num7) / num3; } } } } IL_829: int num17 = 0; j = 0; while (j < this._queryResultCount && num17 < maxCount) { Box2DXDebug.Assert((int)this._queryResults[j] < Settings.MaxProxies); Proxy proxy2 = this._proxyPool[(int)this._queryResults[j]]; Box2DXDebug.Assert(proxy2.IsValid); userData[j] = proxy2.UserData; j++; num17++; } this._queryResultCount = 0; this.IncrementTimeStamp(); return num17; }
int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey) { float maxLambda = 1; float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X; float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y; int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0); int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0); Box2DXDebug.Assert(sx != 0 || sy != 0); float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X; float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y; #if ALLOWUNSAFE ushort* startValues = stackalloc ushort[2]; ushort* startValues2 = stackalloc ushort[2]; #else ushort[] startValues = new ushort[2]; ushort[] startValues2 = new ushort[2]; #endif int xIndex; int yIndex; ushort proxyId; Proxy proxy; // TODO_ERIN implement fast float to ushort conversion. startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1)); startValues2[0] = (ushort)((ushort)(p1x) | 1); startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1)); startValues2[1] = (ushort)((ushort)(p1y) | 1); //First deal with all the proxies that contain segment.p1 int lowerIndex; int upperIndex; Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0); if (sx >= 0) xIndex = upperIndex - 1; else xIndex = lowerIndex; Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1); if (sy >= 0) yIndex = upperIndex - 1; else yIndex = lowerIndex; //If we are using sortKey, then sort what we have so far, filtering negative keys if (sortKey != null) { //Fill keys for (int j = 0; j < _queryResultCount; j++) { _querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData); } //Bubble sort keys //Sorting negative values to the top, so we can easily remove them int i = 0; while (i < _queryResultCount - 1) { float a = _querySortKeys[i]; float b = _querySortKeys[i + 1]; if ((a < 0) ? (b >= 0) : (a > b && b >= 0)) { _querySortKeys[i + 1] = a; _querySortKeys[i] = b; ushort tempValue = _queryResults[i + 1]; _queryResults[i + 1] = _queryResults[i]; _queryResults[i] = tempValue; i--; if (i == -1) i = 1; } else { i++; } } //Skim off negative values while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0) _queryResultCount--; } //Now work through the rest of the segment for (; ; ) { float xProgress = 0; float yProgress = 0; if (xIndex < 0 || xIndex >= _proxyCount * 2) break; if (yIndex < 0 || yIndex >= _proxyCount * 2) break; if (sx != 0) { //Move on to the next bound if (sx > 0) { xIndex++; if (xIndex == _proxyCount * 2) break; } else { xIndex--; if (xIndex < 0) break; } xProgress = (_bounds[0][xIndex].Value - p1x) / dx; } if (sy != 0) { //Move on to the next bound if (sy > 0) { yIndex++; if (yIndex == _proxyCount * 2) break; } else { yIndex--; if (yIndex < 0) break; } yProgress = (_bounds[1][yIndex].Value - p1y) / dy; } for (; ; ) { if (sy == 0 || (sx != 0 && xProgress < yProgress)) { if (xProgress > maxLambda) break; //Check that we are entering a proxy, not leaving if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper) { //Check the other axis of the proxy proxyId = _bounds[0][xIndex].ProxyId; proxy = _proxyPool[proxyId]; if (sy >= 0) { if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } else { if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } } //Early out if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1]) break; //Move on to the next bound if (sx > 0) { xIndex++; if (xIndex == _proxyCount * 2) break; } else { xIndex--; if (xIndex < 0) break; } xProgress = (_bounds[0][xIndex].Value - p1x) / dx; } else { if (yProgress > maxLambda) break; //Check that we are entering a proxy, not leaving if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper) { //Check the other axis of the proxy proxyId = _bounds[1][yIndex].ProxyId; proxy = _proxyPool[proxyId]; if (sx >= 0) { if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } else { if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } } //Early out if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1]) break; //Move on to the next bound if (sy > 0) { yIndex++; if (yIndex == _proxyCount * 2) break; } else { yIndex--; if (yIndex < 0) break; } yProgress = (_bounds[1][yIndex].Value - p1y) / dy; } } break; } int count = 0; for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count) { Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies); Proxy proxy_ = _proxyPool[_queryResults[i]]; Box2DXDebug.Assert(proxy_.IsValid); userData[i] = proxy_.UserData; } // Prepare for next query. _queryResultCount = 0; IncrementTimeStamp(); return count; }
public void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) { float key = sortKey(proxy.UserData); //Filter proxies on positive keys if (key < 0) return; //Merge the new key into the sorted list. //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); float[] querySortKeysPtr = _querySortKeys; int ip = 0; float p = querySortKeysPtr[ip]; while (p < key && ip < _queryResultCount) { p = querySortKeysPtr[ip]; ip++; } int i = ip; if (maxCount == _queryResultCount && i == _queryResultCount) return; if (maxCount == _queryResultCount) _queryResultCount--; //std::copy_backward for (int j = _queryResultCount + 1; j > i; --j) { _querySortKeys[j] = _querySortKeys[j - 1]; _queryResults[j] = _queryResults[j - 1]; } _querySortKeys[i] = key; _queryResults[i] = proxyId; _queryResultCount++; }
public unsafe void AddProxyResult(ushort proxyId, Proxy proxy, int maxCount, SortKeyFunc sortKey) { float key = sortKey(proxy.UserData); //Filter proxies on positive keys if (key < 0) return; //Merge the new key into the sorted list. //float32* p = std::lower_bound(m_querySortKeys,m_querySortKeys+m_queryResultCount,key); fixed (float* querySortKeysPtr = _querySortKeys) { float* p = querySortKeysPtr; while (*p < key && p < &querySortKeysPtr[_queryResultCount]) p++; int i = (int)(p - &querySortKeysPtr[0]); if (maxCount == _queryResultCount && i == _queryResultCount) return; if (maxCount == _queryResultCount) _queryResultCount--; //std::copy_backward for (int j = _queryResultCount + 1; j > i; --j) { _querySortKeys[j] = _querySortKeys[j - 1]; _queryResults[j] = _queryResults[j - 1]; } _querySortKeys[i] = key; _queryResults[i] = proxyId; _queryResultCount++; } }
int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey) { float maxLambda = 1; float dx = (segment.P2.x - segment.P1.x) * _quantizationFactor.x; float dy = (segment.P2.y - segment.P1.y) * _quantizationFactor.y; int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0); int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0); Box2DXDebug.Assert(sx != 0 || sy != 0); float p1x = (segment.P1.x - _worldAABB.LowerBound.x) * _quantizationFactor.x; float p1y = (segment.P1.y - _worldAABB.LowerBound.y) * _quantizationFactor.y; #if ALLOWUNSAFE ushort *startValues = stackalloc ushort[2]; ushort *startValues2 = stackalloc ushort[2]; #else ushort[] startValues = new ushort[2]; ushort[] startValues2 = new ushort[2]; #endif int xIndex; int yIndex; ushort proxyId; Proxy proxy; // TODO_ERIN implement fast float to ushort conversion. startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1)); startValues2[0] = (ushort)((ushort)(p1x) | 1); startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1)); startValues2[1] = (ushort)((ushort)(p1y) | 1); //First deal with all the proxies that contain segment.p1 int lowerIndex; int upperIndex; Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0); if (sx >= 0) { xIndex = upperIndex - 1; } else { xIndex = lowerIndex; } Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1); if (sy >= 0) { yIndex = upperIndex - 1; } else { yIndex = lowerIndex; } //If we are using sortKey, then sort what we have so far, filtering negative keys if (sortKey != null) { //Fill keys for (int j = 0; j < _queryResultCount; j++) { _querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData); } //Bubble sort keys //Sorting negative values to the top, so we can easily remove them int i = 0; while (i < _queryResultCount - 1) { float a = _querySortKeys[i]; float b = _querySortKeys[i + 1]; if ((a < 0) ? (b >= 0) : (a > b && b >= 0)) { _querySortKeys[i + 1] = a; _querySortKeys[i] = b; ushort tempValue = _queryResults[i + 1]; _queryResults[i + 1] = _queryResults[i]; _queryResults[i] = tempValue; i--; if (i == -1) { i = 1; } } else { i++; } } //Skim off negative values while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0) { _queryResultCount--; } } //Now work through the rest of the segment for (; ;) { float xProgress = 0; float yProgress = 0; if (xIndex < 0 || xIndex >= _proxyCount * 2) { break; } if (yIndex < 0 || yIndex >= _proxyCount * 2) { break; } if (sx != 0) { //Move on to the next bound if (sx > 0) { xIndex++; if (xIndex == _proxyCount * 2) { break; } } else { xIndex--; if (xIndex < 0) { break; } } xProgress = (_bounds[0][xIndex].Value - p1x) / dx; } if (sy != 0) { //Move on to the next bound if (sy > 0) { yIndex++; if (yIndex == _proxyCount * 2) { break; } } else { yIndex--; if (yIndex < 0) { break; } } yProgress = (_bounds[1][yIndex].Value - p1y) / dy; } for (; ;) { if (sy == 0 || (sx != 0 && xProgress < yProgress)) { if (xProgress > maxLambda) { break; } //Check that we are entering a proxy, not leaving if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper) { //Check the other axis of the proxy proxyId = _bounds[0][xIndex].ProxyId; proxy = _proxyPool[proxyId]; if (sy >= 0) { if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } else { if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } } //Early out if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1]) { break; } //Move on to the next bound if (sx > 0) { xIndex++; if (xIndex == _proxyCount * 2) { break; } } else { xIndex--; if (xIndex < 0) { break; } } xProgress = (_bounds[0][xIndex].Value - p1x) / dx; } else { if (yProgress > maxLambda) { break; } //Check that we are entering a proxy, not leaving if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper) { //Check the other axis of the proxy proxyId = _bounds[1][yIndex].ProxyId; proxy = _proxyPool[proxyId]; if (sx >= 0) { if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } else { if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1) { //Add the proxy if (sortKey != null) { AddProxyResult(proxyId, proxy, maxCount, sortKey); } else { _queryResults[_queryResultCount] = proxyId; ++_queryResultCount; } } } } //Early out if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1]) { break; } //Move on to the next bound if (sy > 0) { yIndex++; if (yIndex == _proxyCount * 2) { break; } } else { yIndex--; if (yIndex < 0) { break; } } yProgress = (_bounds[1][yIndex].Value - p1y) / dy; } } break; } int count = 0; for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count) { Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies); Proxy proxy_ = _proxyPool[_queryResults[i]]; Box2DXDebug.Assert(proxy_.IsValid); userData[i] = proxy_.UserData; } // Prepare for next query. _queryResultCount = 0; IncrementTimeStamp(); return(count); }