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);
}
