Jitter.Collision.CollisionSystemPersistentSAP.RemoveEntity C# (CSharp) Method

        public override bool RemoveEntity(IBroadphaseEntity body)
        {
            if (body.BroadphaseTag > bodyList.Count || bodyList[body.BroadphaseTag] != body) return false;

            int count;

            count = 0;
            for (int i = 0; i < axis1.Count; i++)
            { if (axis1[i].Body == body) { count++; axis1.RemoveAt(i); if (count == 2) break; i--; } }

            count = 0;
            for (int i = 0; i < axis2.Count; i++)
            { if (axis2[i].Body == body) { count++; axis2.RemoveAt(i); if (count == 2) break; i--; } }

            count = 0;
            for (int i = 0; i < axis3.Count; i++)
            { if (axis3[i].Body == body) { count++; axis3.RemoveAt(i); if (count == 2) break; i--; } }

            foreach (var pair in fullOverlaps) if (pair.body1 == body || pair.body2 == body) depricated.Push(pair);
            while (depricated.Count > 0) fullOverlaps.Remove(depricated.Pop());

            IBroadphaseEntity lastBody = bodyList[bodyList.Count - 1];

            if (body == lastBody)
            {
                for (int i = 0; i < bodyList.Count; i++)
                {
                    t2bM.SetValue(body.BroadphaseTag, i, 0);
                }

                bodyList.RemoveAt(body.BroadphaseTag);
            }
            else
            {
                for (int i = 0; i < bodyList.Count; i++)
                {
                    int value = t2bM.GetValue(lastBody.BroadphaseTag, i);
                    t2bM.SetValue(body.BroadphaseTag, i, value);
                }

                bodyList.RemoveAt(lastBody.BroadphaseTag);
                bodyList[body.BroadphaseTag] = lastBody;

                lastBody.BroadphaseTag = body.BroadphaseTag;
            }

            ResizeMatrix(MatrixGrowFactor);

            return true;
        }