public Bound Clone()
{
Bound newBound = new Bound();
newBound.Value = this.Value;
newBound.ProxyId = this.ProxyId;
newBound.StabbingCount = this.StabbingCount;
return newBound;
}
public BroadPhase(AABB worldAABB, PairCallback callback)
{
_pairManager = new PairManager();
_pairManager.Initialize(this, callback);
Box2DXDebug.Assert(worldAABB.IsValid);
_worldAABB = worldAABB;
_proxyCount = 0;
Vector2 d = worldAABB.UpperBound - worldAABB.LowerBound;
_quantizationFactor.x = (float)BROADPHASE_MAX / d.x;
_quantizationFactor.y = (float)BROADPHASE_MAX / d.y;
for (ushort i = 0; i < Settings.MaxProxies - 1; ++i)
{
_proxyPool[i] = new Proxy();
_proxyPool[i].Next = (ushort)(i + 1);
_proxyPool[i].TimeStamp = 0;
_proxyPool[i].OverlapCount = BroadPhase.Invalid;
_proxyPool[i].UserData = null;
}
_proxyPool[Settings.MaxProxies - 1] = new Proxy();
_proxyPool[Settings.MaxProxies - 1].Next = PairManager.NullProxy;
_proxyPool[Settings.MaxProxies - 1].TimeStamp = 0;
_proxyPool[Settings.MaxProxies - 1].OverlapCount = BroadPhase.Invalid;
_proxyPool[Settings.MaxProxies - 1].UserData = null;
_freeProxy = 0;
_timeStamp = 1;
_queryResultCount = 0;
for (int i = 0; i < 2; i++)
{
_bounds[i] = new Bound[(2 * Settings.MaxProxies)];
}
int bCount = 2 * Settings.MaxProxies;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < bCount; k++)
{
_bounds[j][k] = new Bound();
}
}
}
public void DestroyProxy(int proxyId)
{
Box2DXDebug.Assert(0 < this._proxyCount && this._proxyCount <= Settings.MaxProxies);
Proxy proxy = this._proxyPool[proxyId];
Box2DXDebug.Assert(proxy.IsValid);
int num = 2 * this._proxyCount;
for (int i = 0; i < 2; i++)
{
Bound[] array = this._bounds[i];
int num2 = (int)proxy.LowerBounds[i];
int num3 = (int)proxy.UpperBounds[i];
ushort value = array[num2].Value;
ushort value2 = array[num3].Value;
Bound[] array2 = new Bound[num3 - num2 - 1];
for (int j = 0; j < num3 - num2 - 1; j++)
{
array2[j] = array[num2 + 1 + j].Clone();
}
for (int j = 0; j < num3 - num2 - 1; j++)
{
array[num2 + j] = array2[j];
}
array2 = new Bound[num - num3 - 1];
for (int j = 0; j < num - num3 - 1; j++)
{
array2[j] = array[num3 + 1 + j].Clone();
}
for (int j = 0; j < num - num3 - 1; j++)
{
array[num3 - 1 + j] = array2[j];
}
for (int k = num2; k < num - 2; k++)
{
Proxy proxy2 = this._proxyPool[(int)array[k].ProxyId];
if (array[k].IsLower)
{
proxy2.LowerBounds[i] = (ushort)k;
}
else
{
proxy2.UpperBounds[i] = (ushort)k;
}
}
for (int k = num2; k < num3 - 1; k++)
{
Bound expr_1B5 = array[k];
expr_1B5.StabbingCount -= 1;
}
this.Query(out num2, out num3, value, value2, array, num - 2, i);
}
Box2DXDebug.Assert(this._queryResultCount < Settings.MaxProxies);
for (int j = 0; j < this._queryResultCount; j++)
{
Box2DXDebug.Assert(this._proxyPool[(int)this._queryResults[j]].IsValid);
this._pairManager.RemoveBufferedPair(proxyId, (int)this._queryResults[j]);
}
this._pairManager.Commit();
this._queryResultCount = 0;
this.IncrementTimeStamp();
proxy.UserData = null;
proxy.OverlapCount = BroadPhase.Invalid;
proxy.LowerBounds[0] = BroadPhase.Invalid;
proxy.LowerBounds[1] = BroadPhase.Invalid;
proxy.UpperBounds[0] = BroadPhase.Invalid;
proxy.UpperBounds[1] = BroadPhase.Invalid;
proxy.Next = this._freeProxy;
this._freeProxy = (ushort)proxyId;
this._proxyCount--;
if (BroadPhase.IsValidate)
{
this.Validate();
}
}
private void Query(out int lowerQueryOut, out int upperQueryOut, ushort lowerValue, ushort upperValue, Bound[] bounds, int boundCount, int axis)
{
int num = BroadPhase.BinarySearch(bounds, boundCount, lowerValue);
int num2 = BroadPhase.BinarySearch(bounds, boundCount, upperValue);
for (int i = num; i < num2; i++)
{
if (bounds[i].IsLower)
{
this.IncrementOverlapCount((int)bounds[i].ProxyId);
}
}
if (num > 0)
{
int i = num - 1;
int num3 = (int)bounds[i].StabbingCount;
while (num3 != 0)
{
Box2DXDebug.Assert(i >= 0);
if (bounds[i].IsLower)
{
Proxy proxy = this._proxyPool[(int)bounds[i].ProxyId];
if (num <= (int)proxy.UpperBounds[axis])
{
this.IncrementOverlapCount((int)bounds[i].ProxyId);
num3--;
}
}
i--;
}
}
lowerQueryOut = num;
upperQueryOut = num2;
}
private static int BinarySearch(Bound[] bounds, int count, ushort value)
{
int num = 0;
int num2 = count - 1;
int result;
while (num <= num2)
{
int num3 = num + num2 >> 1;
if (bounds[num3].Value > value)
{
num2 = num3 - 1;
}
else
{
if (bounds[num3].Value >= value)
{
result = (int)((ushort)num3);
return result;
}
num = num3 + 1;
}
}
result = num;
return result;
}
public void DestroyProxy(int proxyId)
{
Box2DXDebug.Assert(0 < this._proxyCount && this._proxyCount <= Settings.MaxProxies);
Proxy proxy = this._proxyPool[proxyId];
Box2DXDebug.Assert(proxy.IsValid);
int num = 2 * this._proxyCount;
for (int i = 0; i < 2; i++)
{
Bound[] array = this._bounds[i];
int num2 = (int)proxy.LowerBounds[i];
int num3 = (int)proxy.UpperBounds[i];
ushort value = array[num2].Value;
ushort value2 = array[num3].Value;
Bound[] array2 = new Bound[num3 - num2 - 1];
for (int j = 0; j < num3 - num2 - 1; j++)
{
array2[j] = array[num2 + 1 + j].Clone();
}
for (int j = 0; j < num3 - num2 - 1; j++)
{
array[num2 + j] = array2[j];
}
array2 = new Bound[num - num3 - 1];
for (int j = 0; j < num - num3 - 1; j++)
{
array2[j] = array[num3 + 1 + j].Clone();
}
for (int j = 0; j < num - num3 - 1; j++)
{
array[num3 - 1 + j] = array2[j];
}
for (int k = num2; k < num - 2; k++)
{
Proxy proxy2 = this._proxyPool[(int)array[k].ProxyId];
if (array[k].IsLower)
{
proxy2.LowerBounds[i] = (ushort)k;
}
else
{
proxy2.UpperBounds[i] = (ushort)k;
}
}
for (int k = num2; k < num3 - 1; k++)
{
Bound expr_1B5 = array[k];
expr_1B5.StabbingCount -= 1;
}
this.Query(out num2, out num3, value, value2, array, num - 2, i);
}
Box2DXDebug.Assert(this._queryResultCount < Settings.MaxProxies);
for (int j = 0; j < this._queryResultCount; j++)
{
Box2DXDebug.Assert(this._proxyPool[(int)this._queryResults[j]].IsValid);
this._pairManager.RemoveBufferedPair(proxyId, (int)this._queryResults[j]);
}
this._pairManager.Commit();
this._queryResultCount = 0;
this.IncrementTimeStamp();
proxy.UserData = null;
proxy.OverlapCount = BroadPhase.Invalid;
proxy.LowerBounds[0] = BroadPhase.Invalid;
proxy.LowerBounds[1] = BroadPhase.Invalid;
proxy.UpperBounds[0] = BroadPhase.Invalid;
proxy.UpperBounds[1] = BroadPhase.Invalid;
proxy.Next = this._freeProxy;
this._freeProxy = (ushort)proxyId;
this._proxyCount--;
if (BroadPhase.IsValidate)
{
this.Validate();
}
}
public ushort CreateProxy(AABB aabb, object userData)
{
Box2DXDebug.Assert(this._proxyCount < Settings.MaxProxies);
Box2DXDebug.Assert(this._freeProxy != PairManager.NullProxy);
ushort freeProxy = this._freeProxy;
Proxy proxy = this._proxyPool[(int)freeProxy];
this._freeProxy = proxy.Next;
proxy.OverlapCount = 0;
proxy.UserData = userData;
int num = 2 * this._proxyCount;
ushort[] array = new ushort[2];
ushort[] array2 = new ushort[2];
this.ComputeBounds(out array, out array2, aabb);
for (int i = 0; i < 2; i++)
{
Bound[] array3 = this._bounds[i];
int num2;
int num3;
this.Query(out num2, out num3, array[i], array2[i], array3, num, i);
Bound[] array4 = new Bound[num - num3];
for (int j = 0; j < num - num3; j++)
{
array4[j] = array3[num3 + j].Clone();
}
for (int j = 0; j < num - num3; j++)
{
array3[num3 + 2 + j] = array4[j];
}
array4 = new Bound[num3 - num2];
for (int j = 0; j < num3 - num2; j++)
{
array4[j] = array3[num2 + j].Clone();
}
for (int j = 0; j < num3 - num2; j++)
{
array3[num2 + 1 + j] = array4[j];
}
num3++;
array3[num2].Value = array[i];
array3[num2].ProxyId = freeProxy;
array3[num3].Value = array2[i];
array3[num3].ProxyId = freeProxy;
array3[num2].StabbingCount = ((num2 == 0) ? (ushort)0 : array3[num2 - 1].StabbingCount);
array3[num3].StabbingCount = array3[num3 - 1].StabbingCount;
for (int k = num2; k < num3; k++)
{
Bound expr_1E4 = array3[k];
expr_1E4.StabbingCount += 1;
}
for (int k = num2; k < num + 2; k++)
{
Proxy proxy2 = this._proxyPool[(int)array3[k].ProxyId];
if (array3[k].IsLower)
{
proxy2.LowerBounds[i] = (ushort)k;
}
else
{
proxy2.UpperBounds[i] = (ushort)k;
}
}
}
this._proxyCount++;
Box2DXDebug.Assert(this._queryResultCount < Settings.MaxProxies);
for (int j = 0; j < this._queryResultCount; j++)
{
Box2DXDebug.Assert((int)this._queryResults[j] < Settings.MaxProxies);
Box2DXDebug.Assert(this._proxyPool[(int)this._queryResults[j]].IsValid);
this._pairManager.AddBufferedPair((int)freeProxy, (int)this._queryResults[j]);
}
this._pairManager.Commit();
if (BroadPhase.IsValidate)
{
this.Validate();
}
this._queryResultCount = 0;
this.IncrementTimeStamp();
return freeProxy;
}
// Create and destroy proxies. These call Flush first.
public ushort CreateProxy(AABB aabb, object userData)
{
Box2DXDebug.Assert(_proxyCount < Settings.MaxProxies);
Box2DXDebug.Assert(_freeProxy != PairManager.NullProxy);
ushort proxyId = _freeProxy;
Proxy proxy = _proxyPool[proxyId];
_freeProxy = proxy.Next;
proxy.OverlapCount = 0;
proxy.UserData = userData;
int boundCount = 2 * _proxyCount;
ushort[] lowerValues = new ushort[2], upperValues = new ushort[2];
ComputeBounds(out lowerValues, out upperValues, aabb);
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex, upperIndex;
Query(out lowerIndex, out upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
#warning "Check this"
//memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
Bound[] tmp = new Bound[boundCount - upperIndex];
for (int i = 0; i < (boundCount - upperIndex); i++)
{
tmp[i] = bounds[upperIndex + i].Clone();
}
for (int i = 0; i < (boundCount - upperIndex); i++)
{
bounds[upperIndex + 2 + i] = tmp[i];
}
//memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));
tmp = new Bound[upperIndex - lowerIndex];
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
tmp[i] = bounds[lowerIndex + i].Clone();
}
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
bounds[lowerIndex + 1 + i] = tmp[i];
}
// The upper index has increased because of the lower bound insertion.
++upperIndex;
// Copy in the new bounds.
bounds[lowerIndex].Value = lowerValues[axis];
bounds[lowerIndex].ProxyId = proxyId;
bounds[upperIndex].Value = upperValues[axis];
bounds[upperIndex].ProxyId = proxyId;
bounds[lowerIndex].StabbingCount = lowerIndex == 0 ? (ushort)0 : bounds[lowerIndex - 1].StabbingCount;
bounds[upperIndex].StabbingCount = bounds[upperIndex - 1].StabbingCount;
// Adjust the stabbing count between the new bounds.
for (int index = lowerIndex; index < upperIndex; ++index)
{
++bounds[index].StabbingCount;
}
// Adjust the all the affected bound indices.
for (int index = lowerIndex; index < boundCount + 2; ++index)
{
Proxy proxy_ = _proxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy_.LowerBounds[axis] = (ushort)index;
}
else
{
proxy_.UpperBounds[axis] = (ushort)index;
}
}
}
++_proxyCount;
Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies);
// Create pairs if the AABB is in range.
for (int i = 0; i < _queryResultCount; ++i)
{
Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid);
_pairManager.AddBufferedPair(proxyId, _queryResults[i]);
}
_pairManager.Commit();
if (IsValidate)
{
Validate();
}
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
return(proxyId);
}
// Create and destroy proxies. These call Flush first.
public ushort CreateProxy(AABB aabb, object userData)
{
Box2DXDebug.Assert(_proxyCount < Settings.MaxProxies);
Box2DXDebug.Assert(_freeProxy != PairManager.NullProxy);
ushort proxyId = _freeProxy;
Proxy proxy = _proxyPool[proxyId];
_freeProxy = proxy.Next;
proxy.OverlapCount = 0;
proxy.UserData = userData;
int boundCount = 2 * _proxyCount;
ushort[] lowerValues = new ushort[2], upperValues = new ushort[2];
ComputeBounds(out lowerValues, out upperValues, aabb);
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex, upperIndex;
Query(out lowerIndex, out upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
#warning "Check this"
//memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
Bound[] tmp = new Bound[boundCount - upperIndex];
for (int i = 0; i < (boundCount - upperIndex); i++)
{
tmp[i] = bounds[upperIndex + i].Clone();
}
for (int i = 0; i < (boundCount - upperIndex); i++)
{
bounds[upperIndex + 2 + i] = tmp[i];
}
//memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));
tmp = new Bound[upperIndex - lowerIndex];
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
tmp[i] = bounds[lowerIndex + i].Clone();
}
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
bounds[lowerIndex + 1 + i] = tmp[i];
}
// The upper index has increased because of the lower bound insertion.
++upperIndex;
// Copy in the new bounds.
bounds[lowerIndex].Value = lowerValues[axis];
bounds[lowerIndex].ProxyId = proxyId;
bounds[upperIndex].Value = upperValues[axis];
bounds[upperIndex].ProxyId = proxyId;
bounds[lowerIndex].StabbingCount = lowerIndex == 0 ? (ushort)0 : bounds[lowerIndex - 1].StabbingCount;
bounds[upperIndex].StabbingCount = bounds[upperIndex - 1].StabbingCount;
// Adjust the stabbing count between the new bounds.
for (int index = lowerIndex; index < upperIndex; ++index)
{
++bounds[index].StabbingCount;
}
// Adjust the all the affected bound indices.
for (int index = lowerIndex; index < boundCount + 2; ++index)
{
Proxy proxy_ = _proxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy_.LowerBounds[axis] = (ushort)index;
}
else
{
proxy_.UpperBounds[axis] = (ushort)index;
}
}
}
++_proxyCount;
Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies);
// Create pairs if the AABB is in range.
for (int i = 0; i < _queryResultCount; ++i)
{
Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid);
_pairManager.AddBufferedPair(proxyId, _queryResults[i]);
}
_pairManager.Commit();
if (IsValidate)
{
Validate();
}
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
return proxyId;
}
public BroadPhase(AABB worldAABB, PairCallback callback)
{
_pairManager = new PairManager();
_pairManager.Initialize(this, callback);
Box2DXDebug.Assert(worldAABB.IsValid);
_worldAABB = worldAABB;
_proxyCount = 0;
Vec2 d = worldAABB.UpperBound - worldAABB.LowerBound;
_quantizationFactor.X = (float)BROADPHASE_MAX / d.X;
_quantizationFactor.Y = (float)BROADPHASE_MAX / d.Y;
for (ushort i = 0; i < Settings.MaxProxies - 1; ++i)
{
_proxyPool[i] = new Proxy();
_proxyPool[i].Next = (ushort)(i + 1);
_proxyPool[i].TimeStamp = 0;
_proxyPool[i].OverlapCount = BroadPhase.Invalid;
_proxyPool[i].UserData = null;
}
_proxyPool[Settings.MaxProxies - 1] = new Proxy();
_proxyPool[Settings.MaxProxies - 1].Next = PairManager.NullProxy;
_proxyPool[Settings.MaxProxies - 1].TimeStamp = 0;
_proxyPool[Settings.MaxProxies - 1].OverlapCount = BroadPhase.Invalid;
_proxyPool[Settings.MaxProxies - 1].UserData = null;
_freeProxy = 0;
_timeStamp = 1;
_queryResultCount = 0;
for (int i = 0; i < 2; i++)
{
_bounds[i] = new Bound[(2 * Settings.MaxProxies)];
}
int bCount = 2 * Settings.MaxProxies;
for (int j = 0; j < 2; j++)
for (int k = 0; k < bCount; k++)
_bounds[j][k] = new Bound();
}
private static int BinarySearch(Bound[] bounds, int count, ushort value)
{
int low = 0;
int high = count - 1;
while (low <= high)
{
int mid = (low + high) >> 1;
if (bounds[mid].Value > value)
{
high = mid - 1;
}
else if (bounds[mid].Value < value)
{
low = mid + 1;
}
else
{
return (ushort)mid;
}
}
return low;
}
private void Query(out int lowerQueryOut, out int upperQueryOut,
ushort lowerValue, ushort upperValue,
Bound[] bounds, int boundCount, int axis)
{
int lowerQuery = BinarySearch(bounds, boundCount, lowerValue);
int upperQuery = BinarySearch(bounds, boundCount, upperValue);
// Easy case: lowerQuery <= lowerIndex(i) < upperQuery
// Solution: search query range for min bounds.
for (int i = lowerQuery; i < upperQuery; ++i)
{
if (bounds[i].IsLower)
{
IncrementOverlapCount(bounds[i].ProxyId);
}
}
// Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
// Solution: use the stabbing count to search down the bound array.
if (lowerQuery > 0)
{
int i = lowerQuery - 1;
int s = bounds[i].StabbingCount;
// Find the s overlaps.
while (s != 0)
{
Box2DXDebug.Assert(i >= 0);
if (bounds[i].IsLower)
{
Proxy proxy = _proxyPool[bounds[i].ProxyId];
if (lowerQuery <= proxy.UpperBounds[axis])
{
IncrementOverlapCount(bounds[i].ProxyId);
--s;
}
}
--i;
}
}
lowerQueryOut = lowerQuery;
upperQueryOut = upperQuery;
}
// Call MoveProxy as many times as you like, then when you are done
// call Commit to finalized the proxy pairs (for your time step).
public void MoveProxy(int proxyId, AABB aabb)
{
if (proxyId == PairManager.NullProxy || Settings.MaxProxies <= proxyId)
{
Box2DXDebug.Assert(false);
return;
}
if (aabb.IsValid == false)
{
Box2DXDebug.Assert(false);
return;
}
int boundCount = 2 * _proxyCount;
Proxy proxy = _proxyPool[proxyId];
// Get new bound values
BoundValues newValues = new BoundValues();;
ComputeBounds(out newValues.LowerValues, out newValues.UpperValues, aabb);
// Get old bound values
BoundValues oldValues = new BoundValues();
for (int axis = 0; axis < 2; ++axis)
{
oldValues.LowerValues[axis] = _bounds[axis][proxy.LowerBounds[axis]].Value;
oldValues.UpperValues[axis] = _bounds[axis][proxy.UpperBounds[axis]].Value;
}
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex = proxy.LowerBounds[axis];
int upperIndex = proxy.UpperBounds[axis];
ushort lowerValue = newValues.LowerValues[axis];
ushort upperValue = newValues.UpperValues[axis];
int deltaLower = lowerValue - bounds[lowerIndex].Value;
int deltaUpper = upperValue - bounds[upperIndex].Value;
bounds[lowerIndex].Value = lowerValue;
bounds[upperIndex].Value = upperValue;
//
// Expanding adds overlaps
//
// Should we move the lower bound down?
if (deltaLower < 0)
{
int index = lowerIndex;
while (index > 0 && lowerValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = _proxyPool[prevBound.ProxyId];
++prevBound.StabbingCount;
if (prevBound.IsUpper == true)
{
if (TestOverlap(newValues, prevProxy))
{
_pairManager.AddBufferedPair(proxyId, prevProxyId);
}
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
else
{
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
--proxy.LowerBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
// Should we move the upper bound up?
if (deltaUpper > 0)
{
int index = upperIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= upperValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = _proxyPool[nextProxyId];
++nextBound.StabbingCount;
if (nextBound.IsLower == true)
{
if (TestOverlap(newValues, nextProxy))
{
_pairManager.AddBufferedPair(proxyId, nextProxyId);
}
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
else
{
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
++proxy.UpperBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
//
// Shrinking removes overlaps
//
// Should we move the lower bound up?
if (deltaLower > 0)
{
int index = lowerIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= lowerValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = _proxyPool[nextProxyId];
--nextBound.StabbingCount;
if (nextBound.IsUpper)
{
if (TestOverlap(oldValues, nextProxy))
{
_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
}
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
else
{
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
++proxy.LowerBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
// Should we move the upper bound down?
if (deltaUpper < 0)
{
int index = upperIndex;
while (index > 0 && upperValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = _proxyPool[prevProxyId];
--prevBound.StabbingCount;
if (prevBound.IsLower == true)
{
if (TestOverlap(oldValues, prevProxy))
{
_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
}
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
else
{
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
--proxy.UpperBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
}
if (IsValidate)
{
Validate();
}
}
public void DestroyProxy(int proxyId)
{
Box2DXDebug.Assert(0 < _proxyCount && _proxyCount <= Settings.MaxProxies);
Proxy proxy = _proxyPool[proxyId];
Box2DXDebug.Assert(proxy.IsValid);
int boundCount = 2 * _proxyCount;
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex = proxy.LowerBounds[axis];
int upperIndex = proxy.UpperBounds[axis];
ushort lowerValue = bounds[lowerIndex].Value;
ushort upperValue = bounds[upperIndex].Value;
#warning "Check this"
//memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
Bound[] tmp = new Bound[upperIndex - lowerIndex - 1];
for (int i = 0; i < (upperIndex - lowerIndex - 1); i++)
{
tmp[i] = bounds[lowerIndex + 1 + i].Clone();
}
for (int i = 0; i < (upperIndex - lowerIndex - 1); i++)
{
bounds[lowerIndex + i] = tmp[i];
}
//memmove(bounds + upperIndex - 1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound));
tmp = new Bound[boundCount - upperIndex - 1];
for (int i = 0; i < (boundCount - upperIndex - 1); i++)
{
tmp[i] = bounds[upperIndex + 1 + i].Clone();
}
for (int i = 0; i < (boundCount - upperIndex - 1); i++)
{
bounds[upperIndex - 1 + i] = tmp[i];
}
// Fix bound indices.
for (int index = lowerIndex; index < boundCount - 2; ++index)
{
Proxy proxy_ = _proxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy_.LowerBounds[axis] = (ushort)index;
}
else
{
proxy_.UpperBounds[axis] = (ushort)index;
}
}
// Fix stabbing count.
for (int index = lowerIndex; index < upperIndex - 1; ++index)
{
--bounds[index].StabbingCount;
}
// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
Query(out lowerIndex, out upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
}
Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies);
for (int i = 0; i < _queryResultCount; ++i)
{
Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid);
_pairManager.RemoveBufferedPair(proxyId, _queryResults[i]);
}
_pairManager.Commit();
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
// Return the proxy to the pool.
proxy.UserData = null;
proxy.OverlapCount = BroadPhase.Invalid;
proxy.LowerBounds[0] = BroadPhase.Invalid;
proxy.LowerBounds[1] = BroadPhase.Invalid;
proxy.UpperBounds[0] = BroadPhase.Invalid;
proxy.UpperBounds[1] = BroadPhase.Invalid;
proxy.Next = _freeProxy;
_freeProxy = (ushort)proxyId;
--_proxyCount;
if (IsValidate)
{
Validate();
}
}
public void MoveProxy(int proxyId, AABB aabb)
{
if (proxyId == (int)PairManager.NullProxy || Settings.MaxProxies <= proxyId)
{
Box2DXDebug.Assert(false);
}
else
{
if (!aabb.IsValid)
{
Box2DXDebug.Assert(false);
}
else
{
int num = 2 * this._proxyCount;
Proxy proxy = this._proxyPool[proxyId];
BoundValues boundValues = new BoundValues();
this.ComputeBounds(out boundValues.LowerValues, out boundValues.UpperValues, aabb);
BoundValues boundValues2 = new BoundValues();
for (int i = 0; i < 2; i++)
{
boundValues2.LowerValues[i] = this._bounds[i][(int)proxy.LowerBounds[i]].Value;
boundValues2.UpperValues[i] = this._bounds[i][(int)proxy.UpperBounds[i]].Value;
}
for (int i = 0; i < 2; i++)
{
Bound[] array = this._bounds[i];
int num2 = (int)proxy.LowerBounds[i];
int num3 = (int)proxy.UpperBounds[i];
ushort num4 = boundValues.LowerValues[i];
ushort num5 = boundValues.UpperValues[i];
int num6 = (int)(num4 - array[num2].Value);
int num7 = (int)(num5 - array[num3].Value);
array[num2].Value = num4;
array[num3].Value = num5;
if (num6 < 0)
{
int num8 = num2;
while (num8 > 0 && num4 < array[num8 - 1].Value)
{
Bound bound = array[num8];
Bound bound2 = array[num8 - 1];
int proxyId2 = (int)bound2.ProxyId;
Proxy proxy2 = this._proxyPool[(int)bound2.ProxyId];
Bound expr_18A = bound2;
expr_18A.StabbingCount += 1;
if (bound2.IsUpper)
{
if (this.TestOverlap(boundValues, proxy2))
{
this._pairManager.AddBufferedPair(proxyId, proxyId2);
}
ushort[] expr_1DA_cp_0 = proxy2.UpperBounds;
int expr_1DA_cp_1 = i;
expr_1DA_cp_0[expr_1DA_cp_1] += 1;
Bound expr_1EA = bound;
expr_1EA.StabbingCount += 1;
}
else
{
ushort[] expr_20A_cp_0 = proxy2.LowerBounds;
int expr_20A_cp_1 = i;
expr_20A_cp_0[expr_20A_cp_1] += 1;
Bound expr_21A = bound;
expr_21A.StabbingCount -= 1;
}
ushort[] expr_236_cp_0 = proxy.LowerBounds;
int expr_236_cp_1 = i;
expr_236_cp_0[expr_236_cp_1] -= 1;
Box2DX.Common.Math.Swap <Bound>(ref array[num8], ref array[num8 - 1]);
num8--;
}
}
if (num7 > 0)
{
int num8 = num3;
while (num8 < num - 1 && array[num8 + 1].Value <= num5)
{
Bound bound = array[num8];
Bound bound3 = array[num8 + 1];
int proxyId3 = (int)bound3.ProxyId;
Proxy proxy3 = this._proxyPool[proxyId3];
Bound expr_2C9 = bound3;
expr_2C9.StabbingCount += 1;
if (bound3.IsLower)
{
if (this.TestOverlap(boundValues, proxy3))
{
this._pairManager.AddBufferedPair(proxyId, proxyId3);
}
ushort[] expr_319_cp_0 = proxy3.LowerBounds;
int expr_319_cp_1 = i;
expr_319_cp_0[expr_319_cp_1] -= 1;
Bound expr_329 = bound;
expr_329.StabbingCount += 1;
}
else
{
ushort[] expr_349_cp_0 = proxy3.UpperBounds;
int expr_349_cp_1 = i;
expr_349_cp_0[expr_349_cp_1] -= 1;
Bound expr_359 = bound;
expr_359.StabbingCount -= 1;
}
ushort[] expr_375_cp_0 = proxy.UpperBounds;
int expr_375_cp_1 = i;
expr_375_cp_0[expr_375_cp_1] += 1;
Box2DX.Common.Math.Swap <Bound>(ref array[num8], ref array[num8 + 1]);
num8++;
}
}
if (num6 > 0)
{
int num8 = num2;
while (num8 < num - 1 && array[num8 + 1].Value <= num4)
{
Bound bound = array[num8];
Bound bound3 = array[num8 + 1];
int proxyId3 = (int)bound3.ProxyId;
Proxy proxy3 = this._proxyPool[proxyId3];
Bound expr_40D = bound3;
expr_40D.StabbingCount -= 1;
if (bound3.IsUpper)
{
if (this.TestOverlap(boundValues2, proxy3))
{
this._pairManager.RemoveBufferedPair(proxyId, proxyId3);
}
ushort[] expr_45D_cp_0 = proxy3.UpperBounds;
int expr_45D_cp_1 = i;
expr_45D_cp_0[expr_45D_cp_1] -= 1;
Bound expr_46D = bound;
expr_46D.StabbingCount -= 1;
}
else
{
ushort[] expr_48D_cp_0 = proxy3.LowerBounds;
int expr_48D_cp_1 = i;
expr_48D_cp_0[expr_48D_cp_1] -= 1;
Bound expr_49D = bound;
expr_49D.StabbingCount += 1;
}
ushort[] expr_4B9_cp_0 = proxy.LowerBounds;
int expr_4B9_cp_1 = i;
expr_4B9_cp_0[expr_4B9_cp_1] += 1;
Box2DX.Common.Math.Swap <Bound>(ref array[num8], ref array[num8 + 1]);
num8++;
}
}
if (num7 < 0)
{
int num8 = num3;
while (num8 > 0 && num5 < array[num8 - 1].Value)
{
Bound bound = array[num8];
Bound bound2 = array[num8 - 1];
int proxyId2 = (int)bound2.ProxyId;
Proxy proxy2 = this._proxyPool[proxyId2];
Bound expr_551 = bound2;
expr_551.StabbingCount -= 1;
if (bound2.IsLower)
{
if (this.TestOverlap(boundValues2, proxy2))
{
this._pairManager.RemoveBufferedPair(proxyId, proxyId2);
}
ushort[] expr_5A1_cp_0 = proxy2.LowerBounds;
int expr_5A1_cp_1 = i;
expr_5A1_cp_0[expr_5A1_cp_1] += 1;
Bound expr_5B1 = bound;
expr_5B1.StabbingCount -= 1;
}
else
{
ushort[] expr_5D1_cp_0 = proxy2.UpperBounds;
int expr_5D1_cp_1 = i;
expr_5D1_cp_0[expr_5D1_cp_1] += 1;
Bound expr_5E1 = bound;
expr_5E1.StabbingCount += 1;
}
ushort[] expr_5FD_cp_0 = proxy.UpperBounds;
int expr_5FD_cp_1 = i;
expr_5FD_cp_0[expr_5FD_cp_1] -= 1;
Box2DX.Common.Math.Swap <Bound>(ref array[num8], ref array[num8 - 1]);
num8--;
}
}
}
if (BroadPhase.IsValidate)
{
this.Validate();
}
}
}
}
public ushort CreateProxy(AABB aabb, object userData)
{
Box2DXDebug.Assert(this._proxyCount < Settings.MaxProxies);
Box2DXDebug.Assert(this._freeProxy != PairManager.NullProxy);
ushort freeProxy = this._freeProxy;
Proxy proxy = this._proxyPool[(int)freeProxy];
this._freeProxy = proxy.Next;
proxy.OverlapCount = 0;
proxy.UserData = userData;
int num = 2 * this._proxyCount;
ushort[] array = new ushort[2];
ushort[] array2 = new ushort[2];
this.ComputeBounds(out array, out array2, aabb);
for (int i = 0; i < 2; i++)
{
Bound[] array3 = this._bounds[i];
int num2;
int num3;
this.Query(out num2, out num3, array[i], array2[i], array3, num, i);
Bound[] array4 = new Bound[num - num3];
for (int j = 0; j < num - num3; j++)
{
array4[j] = array3[num3 + j].Clone();
}
for (int j = 0; j < num - num3; j++)
{
array3[num3 + 2 + j] = array4[j];
}
array4 = new Bound[num3 - num2];
for (int j = 0; j < num3 - num2; j++)
{
array4[j] = array3[num2 + j].Clone();
}
for (int j = 0; j < num3 - num2; j++)
{
array3[num2 + 1 + j] = array4[j];
}
num3++;
array3[num2].Value = array[i];
array3[num2].ProxyId = freeProxy;
array3[num3].Value = array2[i];
array3[num3].ProxyId = freeProxy;
array3[num2].StabbingCount = ((num2 == 0) ? (ushort)0 : array3[num2 - 1].StabbingCount);
array3[num3].StabbingCount = array3[num3 - 1].StabbingCount;
for (int k = num2; k < num3; k++)
{
Bound expr_1E4 = array3[k];
expr_1E4.StabbingCount += 1;
}
for (int k = num2; k < num + 2; k++)
{
Proxy proxy2 = this._proxyPool[(int)array3[k].ProxyId];
if (array3[k].IsLower)
{
proxy2.LowerBounds[i] = (ushort)k;
}
else
{
proxy2.UpperBounds[i] = (ushort)k;
}
}
}
this._proxyCount++;
Box2DXDebug.Assert(this._queryResultCount < Settings.MaxProxies);
for (int j = 0; j < this._queryResultCount; j++)
{
Box2DXDebug.Assert((int)this._queryResults[j] < Settings.MaxProxies);
Box2DXDebug.Assert(this._proxyPool[(int)this._queryResults[j]].IsValid);
this._pairManager.AddBufferedPair((int)freeProxy, (int)this._queryResults[j]);
}
this._pairManager.Commit();
if (BroadPhase.IsValidate)
{
this.Validate();
}
this._queryResultCount = 0;
this.IncrementTimeStamp();
return(freeProxy);
}
public void DestroyProxy(int proxyId)
{
Box2DXDebug.Assert(0 < _proxyCount && _proxyCount <= Settings.MaxProxies);
Proxy proxy = _proxyPool[proxyId];
Box2DXDebug.Assert(proxy.IsValid);
int boundCount = 2 * _proxyCount;
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex = proxy.LowerBounds[axis];
int upperIndex = proxy.UpperBounds[axis];
ushort lowerValue = bounds[lowerIndex].Value;
ushort upperValue = bounds[upperIndex].Value;
#warning "Check this"
//memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
Bound[] tmp = new Bound[upperIndex - lowerIndex - 1];
for (int i = 0; i < (upperIndex - lowerIndex - 1); i++)
{
tmp[i] = bounds[lowerIndex + 1 + i].Clone();
}
for (int i = 0; i < (upperIndex - lowerIndex - 1); i++)
{
bounds[lowerIndex + i] = tmp[i];
}
//memmove(bounds + upperIndex - 1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound));
tmp = new Bound[boundCount - upperIndex - 1];
for (int i = 0; i < (boundCount - upperIndex - 1); i++)
{
tmp[i] = bounds[upperIndex + 1 + i].Clone();
}
for (int i = 0; i < (boundCount - upperIndex - 1); i++)
{
bounds[upperIndex - 1 + i] = tmp[i];
}
// Fix bound indices.
for (int index = lowerIndex; index < boundCount - 2; ++index)
{
Proxy proxy_ = _proxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy_.LowerBounds[axis] = (ushort)index;
}
else
{
proxy_.UpperBounds[axis] = (ushort)index;
}
}
// Fix stabbing count.
for (int index = lowerIndex; index < upperIndex - 1; ++index)
{
--bounds[index].StabbingCount;
}
// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
Query(out lowerIndex, out upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
}
Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies);
for (int i = 0; i < _queryResultCount; ++i)
{
Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid);
_pairManager.RemoveBufferedPair(proxyId, _queryResults[i]);
}
_pairManager.Commit();
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
// Return the proxy to the pool.
proxy.UserData = null;
proxy.OverlapCount = BroadPhase.Invalid;
proxy.LowerBounds[0] = BroadPhase.Invalid;
proxy.LowerBounds[1] = BroadPhase.Invalid;
proxy.UpperBounds[0] = BroadPhase.Invalid;
proxy.UpperBounds[1] = BroadPhase.Invalid;
proxy.Next = _freeProxy;
_freeProxy = (ushort)proxyId;
--_proxyCount;
if (IsValidate)
{
Validate();
}
}