internal bool TestOverlap(BoundValues b, Proxy p)
{
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
if (b.LowerValues[axis] > bounds[p.UpperBounds[axis]].Value)
{
return(false);
}
if (b.UpperValues[axis] < bounds[p.LowerBounds[axis]].Value)
{
return(false);
}
}
return(true);
}
internal bool TestOverlap(BoundValues b, Proxy p)
{
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
Box2DNetDebug.Assert(p.LowerBounds[axis] < 2 * _proxyCount);
Box2DNetDebug.Assert(p.UpperBounds[axis] < 2 * _proxyCount);
if (b.LowerValues[axis] > bounds[p.UpperBounds[axis]].Value)
{
return(false);
}
if (b.UpperValues[axis] < bounds[p.LowerBounds[axis]].Value)
{
return(false);
}
}
return(true);
}
// 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)
{
Box2DNetDebug.Assert(false);
return;
}
if (aabb.IsValid == false)
{
Box2DNetDebug.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();
}
}