/**
         * Query an AABB for overlapping proxies. The callback class
         * is called for each proxy that overlaps the supplied AABB.
         */
        public void Query(BroadPhaseQueryCallback callback, b2AABB aabb)
        {
            List <float> lowerValues = new List <float>();
            List <float> upperValues = new List <float>();

            ComputeBounds(lowerValues, upperValues, aabb);

            uint        lowerIndex    = 0;
            uint        upperIndex    = 0;
            List <uint> lowerIndexOut = new List <uint>();

            lowerIndexOut.Add(lowerIndex);
            List <uint> upperIndexOut = new List <uint>();

            upperIndexOut.Add(upperIndex);
            QueryAxis(lowerIndexOut, upperIndexOut, (uint)lowerValues[0], (uint)upperValues[0], m_bounds[0], (uint)(2 * m_proxyCount), 0);
            QueryAxis(lowerIndexOut, upperIndexOut, (uint)lowerValues[1], (uint)upperValues[1], m_bounds[1], (uint)(2 * m_proxyCount), 1);

            //b2Settings.b2Assert(m_queryResultCount < b2Settings.b2_maxProxies);

            // TODO: Don't be lazy, transform QueryAxis to directly call callback
            for (int i = 0; i < m_queryResultCount; ++i)
            {
                b2Proxy proxy = (b2Proxy)m_queryResults[i];
                //b2Settings.b2Assert(proxy.IsValid());
                if (!callback(proxy))
                {
                    break;
                }
            }

            // Prepare for next query.
            m_queryResultCount = 0;
            IncrementTimeStamp();
        }
        // Buffer a pair for removal.
        public void RemoveBufferedPair(b2Proxy proxy1, b2Proxy proxy2)
        {
            //b2Settings.b2Assert(proxy1 && proxy2);

            b2Pair pair = Find(proxy1, proxy2);

            if (pair == null)
            {
                // The pair never existed. This is legal (due to collision filtering).
                return;
            }

            // If this pair is not in the pair buffer ...
            if (pair.IsBuffered() == false)
            {
                // This must be an old pair.
                //b2Settings.b2Assert(pair.IsFinal() == true);

                pair.SetBuffered();
                m_pairBuffer[m_pairBufferCount] = pair;
                ++m_pairBufferCount;

                //b2Settings.b2Assert(m_pairBufferCount <= m_pairCount);
            }

            pair.SetRemoved();

            if (b2BroadPhase.s_validate)
            {
                ValidateBuffer();
            }
        }
        // This one is only used for validation.
        private bool TestOverlapValidate(b2Proxy p1, b2Proxy p2)
        {
            for (int axis = 0; axis < 2; ++axis)
            {
                List <b2Bound> bounds = m_bounds[axis];

                //b2Settings.b2Assert(p1.lowerBounds[axis] < 2 * m_proxyCount);
                //b2Settings.b2Assert(p1.upperBounds[axis] < 2 * m_proxyCount);
                //b2Settings.b2Assert(p2.lowerBounds[axis] < 2 * m_proxyCount);
                //b2Settings.b2Assert(p2.upperBounds[axis] < 2 * m_proxyCount);

                b2Bound bound1 = bounds[(int)p1.lowerBounds[axis]];
                b2Bound bound2 = bounds[(int)p2.upperBounds[axis]];
                if (bound1.value > bound2.value)
                {
                    return(false);
                }

                bound1 = bounds[(int)p1.upperBounds[axis]];
                bound2 = bounds[(int)p2.lowerBounds[axis]];
                if (bound1.value < bound2.value)
                {
                    return(false);
                }
            }

            return(true);
        }
        /*
         * As proxies are created and moved, many pairs are created and destroyed. Even worse, the same
         * pair may be added and removed multiple times in a single time step of the physics engine. To reduce
         * traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the
         * end of the physics step. This is done by buffering all the RemovePair requests. AddPair
         * requests are processed immediately because we need the hash table entry for quick lookup.
         *
         * All user user callbacks are delayed until the buffered pairs are confirmed in Commit.
         * This is very important because the user callbacks may be very expensive and client logic
         * may be harmed if pairs are added and removed within the same time step.
         *
         * Buffer a pair for addition.
         * We may add a pair that is not in the pair manager or pair buffer.
         * We may add a pair that is already in the pair manager and pair buffer.
         * If the added pair is not a new pair, then it must be in the pair buffer (because RemovePair was called).
         */
        public void AddBufferedPair(b2Proxy proxy1, b2Proxy proxy2)
        {
            //b2Settings.b2Assert(proxy1 && proxy2);

            b2Pair pair = AddPair(proxy1, proxy2);

            // If this pair is not in the pair buffer ...
            if (pair.IsBuffered() == false)
            {
                // This must be a newly added pair.
                //b2Settings.b2Assert(pair.IsFinal() == false);

                // Add it to the pair buffer.
                pair.SetBuffered();
                m_pairBuffer[m_pairBufferCount] = pair;
                ++m_pairBufferCount;
                //b2Settings.b2Assert(m_pairBufferCount <= m_pairCount);
            }

            // Confirm this pair for the subsequent call to Commit.
            pair.ClearRemoved();

            if (b2BroadPhase.s_validate)
            {
                ValidateBuffer();
            }
        }
        // Remove a pair, return the pair's userData.
        private object RemovePair(b2Proxy proxy1, b2Proxy proxy2)
        {
            //b2Settings.b2Assert(m_pairCount > 0);

            b2Pair pair = proxy1.pairs[proxy2];

            if (pair == null)
            {
                //b2Settings.b2Assert(false);
                return(null);
            }

            object userData = pair.userData;

            //delete proxy1.pairs[proxy2];
            //delete proxy2.pairs[proxy1];
            proxy1.pairs.Remove(proxy2);
            proxy2.pairs.Remove(proxy1);

            // Scrub
            pair.next     = m_freePair;
            pair.proxy1   = null;
            pair.proxy2   = null;
            pair.userData = null;
            pair.status   = 0;

            m_freePair = pair;
            --m_pairCount;
            return(userData);
        }
//private:

        // Add a pair and return the new pair. If the pair already exists,
        // no new pair is created and the old one is returned.
        private b2Pair AddPair(b2Proxy proxy1, b2Proxy proxy2)
        {
            b2Pair pair = proxy1.pairs[proxy2];

            if (pair != null)
            {
                return(pair);
            }

            if (m_freePair == null)
            {
                m_freePair = new b2Pair();
                m_pairs.Add(m_freePair);
            }
            pair       = m_freePair;
            m_freePair = pair.next;

            pair.proxy1   = proxy1;
            pair.proxy2   = proxy2;
            pair.status   = 0;
            pair.userData = null;
            pair.next     = null;

            proxy1.pairs[proxy2] = pair;
            proxy2.pairs[proxy1] = pair;

            ++m_pairCount;

            return(pair);
        }
        public void Commit(Action <object, object> callback)
        {
            int i;

            int removeCount = 0;

            for (i = 0; i < m_pairBufferCount; ++i)
            {
                b2Pair pair = (b2Pair)m_pairBuffer[i];
                //b2Settings.b2Assert(pair.IsBuffered());
                pair.ClearBuffered();

                //b2Settings.b2Assert(pair.proxy1 && pair.proxy2);

                b2Proxy proxy1 = pair.proxy1;
                b2Proxy proxy2 = pair.proxy2;

                //b2Settings.b2Assert(proxy1.IsValid());
                //b2Settings.b2Assert(proxy2.IsValid());

                if (pair.IsRemoved())
                {
                    // It is possible a pair was added then removed before a commit. Therefore,
                    // we should be careful not to tell the user the pair was removed when the
                    // the user didn't receive a matching add.
                    //if (pair.IsFinal() == true)
                    //{
                    //	m_callback.PairRemoved(proxy1.userData, proxy2.userData, pair.userData);
                    //}

                    // Store the ids so we can actually remove the pair below.
                    //m_pairBuffer[removeCount] = pair;
                    //++removeCount;
                }
                else
                {
                    //b2Settings.b2Assert(m_broadPhase.TestOverlap(proxy1, proxy2) == true);

                    if (pair.IsFinal() == false)
                    {
                        //pair.userData = m_callback.PairAdded(proxy1.userData, proxy2.userData);
                        //pair.SetFinal();
                        callback(proxy1.userData, proxy2.userData);
                    }
                }
            }

            //for (i = 0; i < removeCount; ++i)
            //{
            //	pair = m_pairBuffer[i]
            //	RemovePair(pair.proxy1, pair.proxy2);
            //}

            m_pairBufferCount = 0;

            if (b2BroadPhase.s_validate)
            {
                ValidateTable();
            }
        }
        /**
         * Get the AABB for a proxy.
         */
        public b2AABB GetFatAABB(object proxy_)
        {
            b2AABB  aabb  = new b2AABB();
            b2Proxy proxy = proxy_ as b2Proxy;

            aabb.lowerBound.x = m_worldAABB.lowerBound.x + m_bounds[0][(int)proxy.lowerBounds[0]].value / m_quantizationFactor.x;
            aabb.lowerBound.y = m_worldAABB.lowerBound.y + m_bounds[1][(int)proxy.lowerBounds[1]].value / m_quantizationFactor.y;
            aabb.upperBound.x = m_worldAABB.lowerBound.x + m_bounds[0][(int)proxy.upperBounds[0]].value / m_quantizationFactor.x;
            aabb.upperBound.y = m_worldAABB.lowerBound.y + m_bounds[1][(int)proxy.upperBounds[1]].value / m_quantizationFactor.y;
            return(aabb);
        }
 private void IncrementOverlapCount(b2Proxy proxy)
 {
     if (proxy.timeStamp < m_timeStamp)
     {
         proxy.timeStamp    = m_timeStamp;
         proxy.overlapCount = 1;
     }
     else
     {
         proxy.overlapCount = 2;
         //b2Settings.b2Assert(m_queryResultCount < b2Settings.b2_maxProxies);
         m_queryResults[m_queryResultCount] = proxy;
         ++m_queryResultCount;
     }
 }
        private void QueryAxis(List <uint> lowerQueryOut, List <uint> upperQueryOut, uint lowerValue, uint upperValue, List <b2Bound> bounds, uint boundCount, int axis)
        {
            uint    lowerQuery = BinarySearch(bounds, (int)boundCount, lowerValue);
            uint    upperQuery = BinarySearch(bounds, (int)boundCount, upperValue);
            b2Bound bound;

            // Easy case: lowerQuery <= lowerIndex(i) < upperQuery
            // Solution: search query range for min bounds.
            for (uint j = lowerQuery; j < upperQuery; ++j)
            {
                bound = bounds[(int)j];
                if (bound.IsLower())
                {
                    IncrementOverlapCount(bound.proxy);
                }
            }

            // Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
            // Solution: use the stabbing count to search down the bound array.
            if (lowerQuery > 0)
            {
                int i = (int)(lowerQuery - 1);
                bound = bounds[i];
                int s = (int)bound.stabbingCount;

                // Find the s overlaps.
                while (s != 0)
                {
                    //b2Settings.b2Assert(i >= 0);
                    bound = bounds[i];
                    if (bound.IsLower())
                    {
                        b2Proxy proxy = bound.proxy;
                        if (lowerQuery <= proxy.upperBounds[axis])
                        {
                            IncrementOverlapCount(bound.proxy);
                            --s;
                        }
                    }
                    --i;
                }
            }

            lowerQueryOut[0] = lowerQuery;
            upperQueryOut[0] = upperQuery;
        }
        public bool TestOverlap(object proxyA, object proxyB)
        {
            b2Proxy proxyA_ = proxyA as b2Proxy;
            b2Proxy proxyB_ = proxyB as b2Proxy;

            if (proxyA_.lowerBounds[0] > proxyB_.upperBounds[0])
            {
                return(false);
            }
            if (proxyB_.lowerBounds[0] > proxyA_.upperBounds[0])
            {
                return(false);
            }
            if (proxyA_.lowerBounds[1] > proxyB_.upperBounds[1])
            {
                return(false);
            }
            if (proxyB_.lowerBounds[1] > proxyA_.upperBounds[1])
            {
                return(false);
            }
            return(true);
        }
        public bool TestOverlapBound(b2BoundValues b, b2Proxy p)
        {
            for (int axis = 0; axis < 2; ++axis)
            {
                List <b2Bound> bounds = m_bounds[axis];

                //b2Settings.b2Assert(p.lowerBounds[axis] < 2 * m_proxyCount);
                //b2Settings.b2Assert(p.upperBounds[axis] < 2 * m_proxyCount);

                b2Bound bound = bounds[(int)p.upperBounds[axis]];
                if (b.lowerValues[axis] > bound.value)
                {
                    return(false);
                }

                bound = bounds[(int)p.lowerBounds[axis]];
                if (b.upperValues[axis] < bound.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(object proxy_, b2AABB aabb, b2Vec2 displacement)
        {
            b2Proxy proxy = proxy_ as b2Proxy;

            List <uint> as3arr;
            int         as3int;

            int     axis;
            uint    index;
            b2Bound bound;
            b2Bound prevBound;
            b2Bound nextBound;
            uint    nextProxyId;
            b2Proxy nextProxy;

            if (proxy == null)
            {
                //b2Settings.b2Assert(false);
                return;
            }

            if (aabb.IsValid() == false)
            {
                //b2Settings.b2Assert(false);
                return;
            }

            uint boundCount = (uint)(2 * m_proxyCount);

            // Get new bound values
            b2BoundValues newValues = new b2BoundValues();

            ComputeBounds(newValues.lowerValues, newValues.upperValues, aabb);

            // Get old bound values
            b2BoundValues oldValues = new b2BoundValues();

            for (axis = 0; axis < 2; ++axis)
            {
                bound = m_bounds[axis][(int)proxy.lowerBounds[axis]];
                oldValues.lowerValues[axis] = bound.value;
                bound = m_bounds[axis][(int)proxy.upperBounds[axis]];
                oldValues.upperValues[axis] = bound.value;
            }

            for (axis = 0; axis < 2; ++axis)
            {
                List <b2Bound> bounds = m_bounds[axis];

                uint lowerIndex = proxy.lowerBounds[axis];
                uint upperIndex = proxy.upperBounds[axis];

                uint lowerValue = (uint)newValues.lowerValues[axis];
                uint upperValue = (uint)newValues.upperValues[axis];

                bound = bounds[(int)lowerIndex];
                int deltaLower = (int)(lowerValue - bound.value);
                bound.value = lowerValue;

                bound = bounds[(int)upperIndex];
                int deltaUpper = (int)(upperValue - bound.value);
                bound.value = upperValue;

                //
                // Expanding adds overlaps
                //

                // Should we move the lower bound down?
                if (deltaLower < 0)
                {
                    index = lowerIndex;
                    while (index > 0 && lowerValue < (bounds[(int)(index - 1)] as b2Bound).value)
                    {
                        bound     = bounds[(int)index];
                        prevBound = bounds[(int)(index - 1)];

                        b2Proxy prevProxy = prevBound.proxy;

                        prevBound.stabbingCount++;

                        if (prevBound.IsUpper() == true)
                        {
                            if (TestOverlapBound(newValues, prevProxy))
                            {
                                m_pairManager.AddBufferedPair(proxy, prevProxy);
                            }

                            //prevProxy.upperBounds[axis]++;
                            as3arr = prevProxy.upperBounds;
                            as3int = (int)as3arr[axis];
                            as3int++;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount++;
                        }
                        else
                        {
                            //prevProxy.lowerBounds[axis]++;
                            as3arr = prevProxy.lowerBounds;
                            as3int = (int)as3arr[axis];
                            as3int++;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount--;
                        }

                        //proxy.lowerBounds[axis]--;
                        as3arr = proxy.lowerBounds;
                        as3int = (int)as3arr[axis];
                        as3int--;
                        as3arr[axis] = (uint)as3int;

                        // swap
                        //var temp:b2Bound = bound;
                        //bound = prevEdge;
                        //prevEdge = temp;
                        bound.Swap(prevBound);
                        //b2Math.Swap(bound, prevEdge);
                        --index;
                    }
                }

                // Should we move the upper bound up?
                if (deltaUpper > 0)
                {
                    index = upperIndex;
                    while (index < boundCount - 1 && (bounds[(int)(index + 1)] as b2Bound).value <= upperValue)
                    {
                        bound     = bounds[(int)index];
                        nextBound = bounds[(int)(index + 1)];
                        nextProxy = nextBound.proxy;

                        nextBound.stabbingCount++;

                        if (nextBound.IsLower() == true)
                        {
                            if (TestOverlapBound(newValues, nextProxy))
                            {
                                m_pairManager.AddBufferedPair(proxy, nextProxy);
                            }

                            //nextProxy.lowerBounds[axis]--;
                            as3arr = nextProxy.lowerBounds;
                            as3int = (int)as3arr[axis];
                            as3int--;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount++;
                        }
                        else
                        {
                            //nextProxy.upperBounds[axis]--;
                            as3arr = nextProxy.upperBounds;
                            as3int = (int)as3arr[axis];
                            as3int--;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount--;
                        }

                        //proxy.upperBounds[axis]++;
                        as3arr = proxy.upperBounds;
                        as3int = (int)as3arr[axis];
                        as3int++;
                        as3arr[axis] = (uint)as3int;

                        // swap
                        //var temp:b2Bound = bound;
                        //bound = nextEdge;
                        //nextEdge = temp;
                        bound.Swap(nextBound);
                        //b2Math.Swap(bound, nextEdge);
                        index++;
                    }
                }

                //
                // Shrinking removes overlaps
                //

                // Should we move the lower bound up?
                if (deltaLower > 0)
                {
                    index = lowerIndex;
                    while (index < boundCount - 1 && (bounds[(int)(index + 1)] as b2Bound).value <= lowerValue)
                    {
                        bound     = bounds[(int)index];
                        nextBound = bounds[(int)(index + 1)];

                        nextProxy = nextBound.proxy;

                        nextBound.stabbingCount--;

                        if (nextBound.IsUpper())
                        {
                            if (TestOverlapBound(oldValues, nextProxy))
                            {
                                m_pairManager.RemoveBufferedPair(proxy, nextProxy);
                            }

                            //nextProxy.upperBounds[axis]--;
                            as3arr = nextProxy.upperBounds;
                            as3int = (int)as3arr[axis];
                            as3int--;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount--;
                        }
                        else
                        {
                            //nextProxy.lowerBounds[axis]--;
                            as3arr = nextProxy.lowerBounds;
                            as3int = (int)as3arr[axis];
                            as3int--;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount++;
                        }

                        //proxy.lowerBounds[axis]++;
                        as3arr = proxy.lowerBounds;
                        as3int = (int)as3arr[axis];
                        as3int++;
                        as3arr[axis] = (uint)as3int;

                        // swap
                        //var temp:b2Bound = bound;
                        //bound = nextEdge;
                        //nextEdge = temp;
                        bound.Swap(nextBound);
                        //b2Math.Swap(bound, nextEdge);
                        index++;
                    }
                }

                // Should we move the upper bound down?
                if (deltaUpper < 0)
                {
                    index = upperIndex;
                    while (index > 0 && upperValue < (bounds[(int)(index - 1)] as b2Bound).value)
                    {
                        bound     = bounds[(int)index];
                        prevBound = bounds[(int)(index - 1)];

                        b2Proxy prevProxy = prevBound.proxy;

                        prevBound.stabbingCount--;

                        if (prevBound.IsLower() == true)
                        {
                            if (TestOverlapBound(oldValues, prevProxy))
                            {
                                m_pairManager.RemoveBufferedPair(proxy, prevProxy);
                            }

                            //prevProxy.lowerBounds[axis]++;
                            as3arr = prevProxy.lowerBounds;
                            as3int = (int)as3arr[axis];
                            as3int++;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount--;
                        }
                        else
                        {
                            //prevProxy.upperBounds[axis]++;
                            as3arr = prevProxy.upperBounds;
                            as3int = (int)as3arr[axis];
                            as3int++;
                            as3arr[axis] = (uint)as3int;

                            bound.stabbingCount++;
                        }

                        //proxy.upperBounds[axis]--;
                        as3arr = proxy.upperBounds;
                        as3int = (int)as3arr[axis];
                        as3int--;
                        as3arr[axis] = (uint)as3int;

                        // swap
                        //var temp:b2Bound = bound;
                        //bound = prevEdge;
                        //prevEdge = temp;
                        bound.Swap(prevBound);
                        //b2Math.Swap(bound, prevEdge);
                        index--;
                    }
                }
            }
        }
        public void DestroyProxy(object proxy_)
        {
            b2Proxy proxy = proxy_ as b2Proxy;
            b2Bound tBound1;
            b2Bound tBound2;

            //b2Settings.b2Assert(proxy.IsValid());

            int boundCount = 2 * m_proxyCount;

            for (int axis = 0; axis < 2; ++axis)
            {
                List <b2Bound> bounds = m_bounds[axis];

                uint lowerIndex = proxy.lowerBounds[axis];
                uint upperIndex = proxy.upperBounds[axis];
                tBound1 = bounds[(int)lowerIndex];
                uint lowerValue = tBound1.value;
                tBound2 = bounds[(int)upperIndex];
                uint upperValue = tBound2.value;

                bounds.RemoveRange((int)upperIndex, 1);
                bounds.RemoveRange((int)lowerIndex, 1);
                bounds.Add(tBound1);
                bounds.Add(tBound2);


                // Fix bound indices.
                int tEnd = boundCount - 2;
                for (uint index = lowerIndex; index < tEnd; ++index)
                {
                    tBound1 = bounds[(int)index];
                    b2Proxy proxy2 = tBound1.proxy;
                    if (tBound1.IsLower())
                    {
                        proxy2.lowerBounds[axis] = index;
                    }
                    else
                    {
                        proxy2.upperBounds[axis] = index;
                    }
                }

                // Fix stabbing count.
                tEnd = (int)upperIndex - 1;
                for (int index2 = (int)lowerIndex; index2 < tEnd; ++index2)
                {
                    tBound1 = bounds[index2];
                    tBound1.stabbingCount--;
                }

                // Query for pairs to be removed. lowerIndex and upperIndex are not needed.
                // make lowerIndex and upper output using an array and do this for others if compiler doesn't pick them up
                List <uint> ignore = new List <uint>();
                QueryAxis(ignore, ignore, lowerValue, upperValue, bounds, (uint)(boundCount - 2), axis);
            }

            //b2Settings.b2Assert(m_queryResultCount < b2Settings.b2_maxProxies);

            for (int i = 0; i < m_queryResultCount; ++i)
            {
                //b2Settings.b2Assert(m_proxyPool[m_queryResults[i]].IsValid());

                m_pairManager.RemoveBufferedPair(proxy, (b2Proxy)m_queryResults[i]);
            }

            // Prepare for next query.
            m_queryResultCount = 0;
            IncrementTimeStamp();

            // Return the proxy to the pool.
            proxy.userData       = null;
            proxy.overlapCount   = b2_invalid;
            proxy.lowerBounds[0] = b2_invalid;
            proxy.lowerBounds[1] = b2_invalid;
            proxy.upperBounds[0] = b2_invalid;
            proxy.upperBounds[1] = b2_invalid;

            proxy.next  = m_freeProxy;
            m_freeProxy = proxy;
            --m_proxyCount;
        }
        // Create and destroy proxies. These call Flush first.
        public object CreateProxy(b2AABB aabb, object userData)
        {
            uint    index;
            b2Proxy proxy;
            int     i;
            int     j;

            //b2Settings.b2Assert(m_proxyCount < b2_maxProxies);
            //b2Settings.b2Assert(m_freeProxy != b2Pair.b2_nullProxy);

            if (m_freeProxy == null)
            {
                // As all proxies are allocated, m_proxyCount == m_proxyPool.length
                m_freeProxy              = m_proxyPool[m_proxyCount] = new b2Proxy();
                m_freeProxy.next         = null;
                m_freeProxy.timeStamp    = 0;
                m_freeProxy.overlapCount = b2_invalid;
                m_freeProxy.userData     = null;

                for (i = 0; i < 2; i++)
                {
                    j = m_proxyCount * 2;
                    m_bounds[i][j++] = new b2Bound();
                    m_bounds[i][j]   = new b2Bound();
                }
            }

            proxy       = m_freeProxy;
            m_freeProxy = proxy.next;

            proxy.overlapCount = 0;
            proxy.userData     = userData;

            uint boundCount = (uint)(2 * m_proxyCount);

            List <float> lowerValues = new List <float>();
            List <float> upperValues = new List <float>();

            ComputeBounds(lowerValues, upperValues, aabb);

            for (int axis = 0; axis < 2; ++axis)
            {
                List <b2Bound> bounds        = m_bounds[axis];
                uint           lowerIndex    = 0;
                uint           upperIndex    = 0;
                List <uint>    lowerIndexOut = new List <uint>();
                lowerIndexOut.Add(lowerIndex);
                List <uint> upperIndexOut = new List <uint>();
                upperIndexOut.Add(upperIndex);
                QueryAxis(lowerIndexOut, upperIndexOut, (uint)lowerValues[axis], (uint)upperValues[axis], bounds, boundCount, axis);
                lowerIndex = lowerIndexOut[0];
                upperIndex = upperIndexOut[0];

                //bounds.splice(upperIndex, 0, bounds[bounds.length - 1]);
                //bounds.length--;
                //bounds.splice(lowerIndex, 0, bounds[bounds.length - 1]);
                //bounds.length--;
                bounds.Insert((int)upperIndex, bounds[bounds.Count - 1]);
                bounds.RemoveAt(bounds.Count - 1);
                bounds.Insert((int)lowerIndex, bounds[bounds.Count - 1]);
                bounds.RemoveAt(bounds.Count - 1);

                // The upper index has increased because of the lower bound insertion.
                ++upperIndex;

                // Copy in the new bounds.
                b2Bound tBound1 = bounds[(int)lowerIndex];
                b2Bound tBound2 = bounds[(int)upperIndex];
                tBound1.value = (uint)lowerValues[axis];
                tBound1.proxy = proxy;
                tBound2.value = (uint)upperValues[axis];
                tBound2.proxy = proxy;

                b2Bound tBoundAS3 = bounds[(int)(lowerIndex - 1)];
                tBound1.stabbingCount = lowerIndex == 0 ? 0 : tBoundAS3.stabbingCount;
                tBoundAS3             = bounds[(int)(upperIndex - 1)];
                tBound2.stabbingCount = tBoundAS3.stabbingCount;

                // Adjust the stabbing count between the new bounds.
                for (index = lowerIndex; index < upperIndex; ++index)
                {
                    tBoundAS3 = bounds[(int)index];
                    tBoundAS3.stabbingCount++;
                }

                // Adjust the all the affected bound indices.
                for (index = lowerIndex; index < boundCount + 2; ++index)
                {
                    tBound1 = bounds[(int)index];
                    b2Proxy proxy2 = tBound1.proxy;
                    if (tBound1.IsLower())
                    {
                        proxy2.lowerBounds[axis] = index;
                    }
                    else
                    {
                        proxy2.upperBounds[axis] = index;
                    }
                }
            }

            ++m_proxyCount;

            //b2Settings.b2Assert(m_queryResultCount < b2Settings.b2_maxProxies);

            for (i = 0; i < m_queryResultCount; ++i)
            {
                //b2Settings.b2Assert(m_queryResults[i] < b2_maxProxies);
                //b2Settings.b2Assert(m_proxyPool[m_queryResults[i]].IsValid());

                m_pairManager.AddBufferedPair(proxy, (b2Proxy)m_queryResults[i]);
            }

            // Prepare for next query.
            m_queryResultCount = 0;
            IncrementTimeStamp();

            return(proxy);
        }
 private b2Pair Find(b2Proxy proxy1, b2Proxy proxy2)
 {
     return(proxy1.pairs[proxy2]);
 }