private void SortAxis(List <SweepPoint> axis)
        {
            for (int j = 1; j < axis.Count; j++)
            {
                SweepPoint keyelement = axis[j];
                FP         key        = keyelement.Value;

                int i = j - 1;

                while (i >= 0 && axis[i].Value > key)
                {
                    SweepPoint swapper = axis[i];

                    if (keyelement.Begin && !swapper.Begin)
                    {
                        if (CheckBoundingBoxes(swapper.Body, keyelement.Body))
                        {
                            lock (fullOverlaps) fullOverlaps.Add(new OverlapPair(swapper.Body, keyelement.Body));
                        }
                    }

                    if (!keyelement.Begin && swapper.Begin)
                    {
                        lock (fullOverlaps) fullOverlaps.Remove(new OverlapPair(swapper.Body, keyelement.Body));
                    }

                    axis[i + 1] = swapper;
                    i           = i - 1;
                }
                axis[i + 1] = keyelement;
            }
        }
        private void DirtySortAxis(List <SweepPoint> axis)
        {
            axis.Sort(QuickSort);
            activeList.Clear();

            for (int i = 0; i < axis.Count; i++)
            {
                SweepPoint keyelement = axis[i];

                if (keyelement.Begin)
                {
                    foreach (IBroadphaseEntity body in activeList)
                    {
                        if (CheckBoundingBoxes(body, keyelement.Body))
                        {
                            fullOverlaps.Add(new OverlapPair(body, keyelement.Body));
                        }
                    }

                    activeList.Add(keyelement.Body);
                }
                else
                {
                    activeList.Remove(keyelement.Body);
                }
            }
        }
        private int QuickSort(SweepPoint sweepPoint1, SweepPoint sweepPoint2)
        {
            FP val1 = sweepPoint1.Value;
            FP val2 = sweepPoint2.Value;

            if (val1 > val2)
            {
                return(1);
            }
            else if (val2 > val1)
            {
                return(-1);
            }
            else
            {
                return(0);
            }
        }
        public void Restore(CollisionSystemPersistentSAP cs)
        {
            cs.bodyList.Clear();
            cs.bodyList.AddRange(bodyList);

            cs.axis1.Clear();
            for (index = 0, length = axis1.Count; index < length; index++)
            {
                SweetPointClone sp = axis1[index];

                SweepPoint spN = new SweepPoint(null, false, 0);
                sp.Restore(spN);
                cs.axis1.Add(spN);
            }

            cs.axis2.Clear();
            for (index = 0, length = axis2.Count; index < length; index++)
            {
                SweetPointClone sp = axis2[index];

                SweepPoint spN = new SweepPoint(null, false, 0);
                sp.Restore(spN);
                cs.axis2.Add(spN);
            }

            cs.axis3.Clear();
            for (index = 0, length = axis3.Count; index < length; index++)
            {
                SweetPointClone sp = axis3[index];

                SweepPoint spN = new SweepPoint(null, false, 0);
                sp.Restore(spN);
                cs.axis3.Add(spN);
            }

            cs.fullOverlaps.Clear();
            cs.fullOverlaps.AddRange(fullOverlaps);

            cs.activeList.Clear();
            cs.activeList.AddRange(activeList);

            cs.swapOrder = swapOrder;
        }