source: 2014/24/EemeliK/Zombieland/Jypeli/Physics2DDotNet/Detectors/FrameCoherentSAPDetector.cs @ 10335

Revision 5974, 28.1 KB checked in by empaheik, 5 years ago (diff)
Line 
1#region MIT License
2/*
3 * Copyright (c) 2005-2008 Jonathan Mark Porter. http://physics2d.googlepages.com/
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights to
8 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
9 * the Software, and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
16 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
17 * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
18 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
20 * OTHER DEALINGS IN THE SOFTWARE.
21 */
22#endregion
23
24// Contributor: Andrew D. Jones
25
26#if UseDouble
27using Scalar = System.Double;
28#else
29using Scalar = System.Single;
30#endif
31
32#if !WINDOWS_PHONE
33
34using System;
35
36using AdvanceMath.Geometry2D;
37using System.Collections;
38#if !(WINDOWS_PHONE || XBOX)
39using System.Collections.Specialized;
40#endif
41using System.Collections.Generic;
42using System.Text;
43
44namespace Physics2DDotNet.Detectors
45{
46    /// <summary>
47    /// Full name is Frame Coherent Sweep and Prune.
48    /// This class is used to isolate the AABB pairs that are currently in a collision
49    /// state without having to check all pair combinations. It relies heavily on frame
50    /// coherence or the idea that objects will typically be near their last position
51    /// from frame to frame. The class caches the various state information and doesn't
52    /// update it unless an extent on an axis "swaps" positions with its neighbor.
53    /// Note: If your application has "teleporting" objects or objects that are
54    /// extremely high-speed in relation to other objects, then this Sweep and Prune
55    /// method may breakdown.
56    /// </summary>
57    public sealed class FrameCoherentSAPDetector : BroadPhaseCollisionDetector
58    {
59        /// <summary>
60        /// This class keeps a list of information that relates extents to geometries.
61        /// </summary>
62        sealed class ExtentInfoList : List<ExtentInfo>
63        {
64            public FrameCoherentSAPDetector owner;
65            public ExtentInfoList(FrameCoherentSAPDetector sap) { owner = sap; }
66
67            public void MoveUnderConsiderationToOverlaps()
68            {
69                for (int i = 0; i < Count; i++)
70                {
71                    if (this[i].underConsideration.Count == 0)
72                        continue;
73
74                    Body g1 = this[i].body;
75                    BoundingRectangle aabb1 = g1.Rectangle;
76
77                    // First transfer those under consideration to overlaps,
78                    // for, they have been considered...
79                    int startIndex = this[i].overlaps.Count;
80                    this[i].overlaps.AddRange(this[i].underConsideration);
81                    this[i].underConsideration.Clear();
82
83                    for (int j = startIndex; j < this[i].overlaps.Count; j++)
84                    {
85                        Body g2 = this[i].overlaps[j];
86
87                        // It is possible that we may test the same pair of geometries
88                        // for both extents (x and y), however, I'm banking on that
89                        // one of the extents has probably already been cached and
90                        // therefore, won't be checked.
91                        if (FrameCoherentSAPDetector.TestForCollisions(g1, g2) == false)
92                            continue;
93
94                        owner.collisionPairs.AddPair(g1, g2);
95                    }
96                }
97            }
98        }
99
100        /// <summary>
101        /// This class is used to keep track of the pairs of geometry that need to be
102        /// passed on to the narrow phase. The keys stored in the dictionary are
103        /// the actual geometry pairs (the boolean value is currently unused).
104        /// NOTE: May eventually want to add OnEnterCollisionState /
105        /// OnExitCollisionState callbacks which might be useful for debugging
106        /// or possibly in user applications.
107        /// </summary>
108        sealed class CollisionPairDictionary : Dictionary<CollisionPair, bool>
109        {
110            public void RemovePair(Body g1, Body g2)
111            {
112                CollisionPair cp = new CollisionPair(g1, g2);
113                Remove(cp);
114
115                // May want a OnDeactivatedCollision here.
116                // For example, if you were highlighting colliding
117                // ABV's for debugging, this callback would let you
118                // know to stop.
119            }
120
121            public void AddPair(Body g1, Body g2)
122            {
123                CollisionPair cp = new CollisionPair(g1, g2);
124
125                // This check is a trade-off. In many cases, we don't need to perform
126                // this check and we could just do a "try" block instead, however,
127                // when exceptions are thrown, they are mega-slow... so checking for
128                // the key is really the best option all round.
129                if (ContainsKey(cp))
130                    return;
131
132                Add(cp, true);
133            }
134        }
135
136        /// <summary>
137        /// Houses collision pairs as geom1 and geom2. The pairs are always ordered such
138        /// that the lower id geometry is first. This allows the CollisionPairDictionary
139        /// to have a consistent key / hash code for a pair of geometry.
140        /// </summary>
141        struct CollisionPair
142        {
143            public Body body1;
144            public Body body2;
145
146            public CollisionPair(Body body1, Body body2)
147            {
148                System.Diagnostics.Debug.Assert(body1 != body2);
149
150                if (body1.ID < body2.ID)
151                {
152                    this.body1 = body1;
153                    this.body2 = body2;
154                }
155                else
156                {
157                    this.body1 = body2;
158                    this.body2 = body1;
159                }
160            }
161
162            public override int GetHashCode()
163            {
164                // Arbitrarly choose 20000 as a number of colliders that we won't
165                // approach any time soon.
166                return (body1.ID * 20000 + body2.ID);
167            }
168        }
169
170        /// <summary>
171        /// This class represents a single extent of an AABB on a single axis. It has a
172        /// reference to ExtentInfo which has information about the geometry it belongs
173        /// to.
174        /// </summary>
175        sealed class Extent
176        {
177            public bool isMin;
178            public Scalar value;
179            public ExtentInfo info;
180            public Extent(ExtentInfo info, Scalar value, bool isMin)
181            {
182                this.info = info;
183                this.value = value;
184                this.isMin = isMin;
185            }
186        }
187
188        /// <summary>
189        /// Represents a lists of extents for a given axis. This list will be kept
190        /// sorted incrementally.
191        /// </summary>
192        sealed class ExtentList : List<Extent>
193        {
194            public FrameCoherentSAPDetector owner;
195            public ExtentList(FrameCoherentSAPDetector sap) { owner = sap; }
196
197            /// <summary>
198            /// Inserts a new Extent into the already sorted list. As the ExtentList
199            /// class is currently derived from the generic List class, insertions
200            /// of new geometry (and extents) are going to be somewhat slow right
201            /// off the bat. Additionally, this function currently performs
202            /// linear insertion. Two big optimizations in the future would be to
203            /// (1) make this function perform a binary search and (2) allow for
204            /// a "hint" of what index to start with. The reason for this is because
205            /// there is always a min and max extents that need inserting and we
206            /// know the max extent is always after the min extent.
207            /// </summary>
208            /*private int InsertIntoSortedList(Extent newExtent)
209            {
210                // List<> is not the most speedy for insertion, however, since
211                // we don't plan to do this except for when geometry is added
212                // to the system, we go ahead an use List<> instead of LinkedList<>
213                // This code, btw, assumes the list is already sorted and that
214                // the new entry just needs to be inserted.
215                //
216                // Optimization Note: A binary search could be used here and would
217                // improve speed for when adding geometry.
218                int insertAt = 0;
219
220                // Check for empty list
221                if (this.Count == 0)
222                {
223                    this.Add(newExtent);
224                    return 0;
225                }
226
227                while (insertAt < this.Count &&
228                    (this[insertAt].value < newExtent.value ||
229                    (this[insertAt].value == newExtent.value &&
230                    this[insertAt].isMin && !newExtent.isMin)))
231                {
232                    insertAt++;
233                }
234
235                this.Insert(insertAt, newExtent);
236                return insertAt;
237            }
238
239            /// <summary>
240            /// Incrementally inserts the min/max extents into the ExtentList. As it
241            /// does so, the method ensures that overlap records, the collisionpair
242            /// map, and all other book-keeping is up todate.
243            /// </summary>
244            /// <param name="ourInfo">The extent info for a give axis</param>
245            public void IncrementalInsertExtent(ExtentInfo ourInfo)
246            {
247                Extent min = ourInfo.min;
248                Extent max = ourInfo.max;
249
250                System.Diagnostics.Debug.Assert(min.value < max.value);
251
252                int iMin = InsertIntoSortedList(min);
253                int iMax = InsertIntoSortedList(max);
254
255                Body ourGeom = ourInfo.body;
256
257                // As this is a newly inserted extent, we need to update the overlap
258                // information.
259
260                // RULE 1: Traverse from min to max. Look for other "min" Extents
261                // and when found, add our wrapper/geometry to their list.
262                int iCurr = iMin + 1;
263                while (iCurr != iMax)
264                {
265                    if (this[iCurr].isMin)
266                        this[iCurr].info.underConsideration.Add(ourGeom);
267                    iCurr++;
268                }
269
270                // RULE 2: From min, traverse to the left until we encounter
271                // another "min" extent. If we find one, we add its geometry
272                // to our underConsideration list and go to RULE 3, otherwise
273                // there is no more work to do and we can exit.
274                iCurr = iMin - 1;
275                while (iCurr >= 0 && this[iCurr].isMin == false)
276                    iCurr--;
277
278                if (iCurr < 0)
279                    return;
280
281                List<Body> ourUnderConsideration = ourInfo.underConsideration;
282                Extent currExtent = this[iCurr];
283
284                ourUnderConsideration.Add(currExtent.info.body);
285
286                // RULE 3: Now that we have found a "min" extent, we take
287                // its existing overlap list and copy it into our underConsideration
288                // list. All except for ourselves.
289                ourUnderConsideration.AddRange(currExtent.info.underConsideration);
290                ourUnderConsideration.Remove(ourGeom); // just in case
291
292                // RULE 4: Move from the found extent back toward our "min" extent.
293                // Whenever and "max" extent is found, we remove its reference
294                // from our extents list.
295                while (iCurr != iMin)
296                {
297                    if (currExtent.isMin == false)
298                    {
299                        ourUnderConsideration.Remove(currExtent.info.body);
300
301                        if (ourInfo.overlaps.Remove(currExtent.info.body))
302                        {
303                            owner.collisionPairs.RemovePair(ourGeom,
304                                currExtent.info.body);
305                        }
306                    }
307                    currExtent = this[++iCurr];
308                }
309            }*/
310
311            /// <summary>
312            /// Incrementally sorts ExtentList. It is assumed that there is a high level
313            /// of frame coherence and that much of the list is already fairly well
314            /// sorted. This algorithm makes use of "insert sort" which is notoriously
315            /// slow - except for when a list is already almost sorted - which is the
316            /// case when there is high frame coherence.
317            /// </summary>
318            public void IncrementalSort()
319            {
320                if (Count < 2)
321                    return;
322
323                for (int i = 0; i < Count - 1; i++)
324                {
325                    int evalCnt = i + 1;
326                    Extent evalExtent = this[evalCnt];
327                    Extent currExtent = this[i];
328
329                    if (currExtent.value <= evalExtent.value)
330                        continue;
331
332                    Extent savedExtent = evalExtent;
333
334                    if (evalExtent.isMin)
335                    {
336                        while (currExtent.value > evalExtent.value)
337                        {
338                            if (currExtent.isMin)
339                            {
340                                // Begin extent is inserted before another begin extent.
341                                // So, Inserted extent's object looses reference to
342                                // non-inserted extent and Non-inserted extent gains
343                                // reference to inserted extent's object.
344                                evalExtent.info.underConsideration.Remove(
345                                    currExtent.info.body);
346
347                                if (evalExtent.info.overlaps.Remove(
348                                    currExtent.info.body))
349                                {
350                                    owner.collisionPairs.RemovePair(
351                                        evalExtent.info.body,
352                                        currExtent.info.body);
353                                }
354
355                                // Add extent
356                                currExtent.info.underConsideration.Add(
357                                    evalExtent.info.body);
358                            }
359                            else
360                            {
361                                // "min" extent inserted before the max extent.
362                                // Inserted extent gains reference to non-inserted extent.
363                                evalExtent.info.underConsideration.Add(
364                                    currExtent.info.body);
365                            }
366
367                            this[evalCnt--] = this[i--];
368                            if (i < 0)
369                                break;
370                            currExtent = this[i];
371                        }
372                    }
373                    else
374                    {
375                        while (currExtent.value > evalExtent.value)
376                        {
377                            if (currExtent.isMin)
378                            {
379                                // Ending extent inserted before a beginning extent
380                                // the non inserted extent looses a reference to the
381                                // inserted one
382                                currExtent.info.underConsideration.Remove(
383                                    evalExtent.info.body);
384
385                                if (currExtent.info.overlaps.Remove(
386                                    evalExtent.info.body))
387                                {
388                                    owner.collisionPairs.RemovePair(
389                                        evalExtent.info.body,
390                                        currExtent.info.body);
391                                }
392                            }
393                            this[evalCnt--] = this[i--];
394                            if (i < 0)
395                                break;
396                            currExtent = this[i];
397                        }
398                    }
399                    this[evalCnt] = savedExtent;
400                }
401            }
402        }
403
404        /// <summary>
405        /// This class contains represents additional extent info for a particular axis
406        /// It has a reference to the geometry whose extents are being tracked. It
407        /// also has a min and max extent reference into the ExtentList itself.
408        /// The class keeps track of overlaps with other geometries.
409        /// </summary>
410        sealed class ExtentInfo
411        {
412            public Body body; // Specific to Farseer
413            public Extent min;
414            public Extent max;
415            public List<Body> overlaps;
416            public List<Body> underConsideration;
417
418            public ExtentInfo(Body g, Scalar min, Scalar max)
419            {
420                this.body = g;
421                this.underConsideration = new List<Body>();
422                this.overlaps = new List<Body>();
423                this.min = new Extent(this, min, true);
424                this.max = new Extent(this, max, false);
425            }
426        }
427
428
429        /// <summary>
430        /// Test AABB collisions between two geometries. Tests include checking if the
431        /// geometries are enabled, static, in the right collision categories, etc.
432        /// </summary>
433        /// <returns>Returns true if there is a collision, false otherwise</returns>
434        static bool TestForCollisions(Body g1, Body g2)
435        {
436
437
438
439            //TMP
440            /*            AABB aabb1 = new AABB();
441                        AABB aabb2 = new AABB();
442                        aabb1.min = g1.aabb.min;
443                        aabb1.max = g1.aabb.max;
444                        aabb2.min = g2.aabb.min;
445                        aabb2.max = g2.aabb.max;
446                        aabb1.min.X -= fTol;
447                        aabb1.min.Y -= fTol;
448                        aabb1.max.X += fTol;
449                        aabb1.max.Y += fTol;
450                        aabb2.min.X -= fTol;
451                        aabb2.min.Y -= fTol;
452                        aabb2.max.X += fTol;
453                        aabb2.max.Y += fTol;
454                        if (AABB.Intersect(aabb1, aabb2) == false)
455                            return false;
456            */
457            /* if (AABB.Intersect(g1.Rectangle, g2.Rectangle) == false)
458                 return false;*/
459            if (!g1.Rectangle.Intersects(g2.Rectangle))
460            {
461                return false;
462            }
463            if (!Body.CanCollide(g1, g2))
464            {
465                return false;
466            }
467
468            return true;
469        }
470
471        ExtentList xExtentList;
472        ExtentList yExtentList;
473        ExtentInfoList xInfoList;
474        ExtentInfoList yInfoList;
475        CollisionPairDictionary collisionPairs;
476        //static public Scalar fTol = 1.5f; //.01f;
477        TimeStep step;
478
479        public FrameCoherentSAPDetector()
480        {
481            this.xExtentList = new ExtentList(this);
482            this.yExtentList = new ExtentList(this);
483            this.xInfoList = new ExtentInfoList(this);
484            this.yInfoList = new ExtentInfoList(this);
485            this.collisionPairs = new CollisionPairDictionary();
486        }
487
488        protected internal override void RemoveExpiredBodies()
489        {
490            xInfoList.RemoveAll(delegate(ExtentInfo i) { return !i.body.IsAdded; });
491            xExtentList.RemoveAll(delegate(Extent n) { return !n.info.body.IsAdded; });
492            yInfoList.RemoveAll(delegate(ExtentInfo i) { return !i.body.IsAdded; });
493            yExtentList.RemoveAll(delegate(Extent n) { return !n.info.body.IsAdded; });
494            // We force a non-incremental update because that will insure that the
495            // collisionPairs get recreated and that the geometry isn't being held
496            // by overlaps, etc. Its just easier this way.
497            ForceNonIncrementalUpdate();
498        }
499
500        //TODO: do a merge sort insertion
501        protected internal override void AddBodyRange(List<Body> collection)
502        {
503            foreach (Body b in collection)
504            {
505                AddGeom(b);
506            }
507            ForceNonIncrementalUpdate();
508        }
509
510        public override void Detect(TimeStep step)
511        {
512            this.step = step;
513            this.Run();
514        }
515        protected internal override void Clear()
516        {
517            xExtentList.Clear();
518            yExtentList.Clear();
519            xInfoList.Clear();
520            yInfoList.Clear();
521            collisionPairs.Clear();
522        }
523
524        /// <summary>
525        /// This method is used by the PhysicsSimulator to notify Sweep and Prune that
526        /// new geometry is to be tracked.
527        /// </summary>
528        /// <param name="g">The geometry to be added</param>
529        void AddGeom(Body g)
530        {
531            ExtentInfo xExtentInfo = new ExtentInfo(g, g.Rectangle.Min.X, g.Rectangle.Max.X);
532            xInfoList.Add(xExtentInfo);
533            xExtentList.Add(xExtentInfo.min);
534            xExtentList.Add(xExtentInfo.max);
535
536            ExtentInfo yExtentInfo = new ExtentInfo(g, g.Rectangle.Min.Y, g.Rectangle.Max.Y);
537            yInfoList.Add(yExtentInfo);
538            yExtentList.Add(yExtentInfo.min);
539            yExtentList.Add(yExtentInfo.max);
540        }
541
542
543        /// <summary>
544        /// Updates the values in the x and y extent lists by the changing aabb values.
545        /// </summary>
546        private void UpdateExtentValues()
547        {
548            System.Diagnostics.Debug.Assert(xInfoList.Count == yInfoList.Count);
549            for (int i = 0; i < xInfoList.Count; i++)
550            {
551                ExtentInfo xInfo = xInfoList[i];
552                ExtentInfo yInfo = yInfoList[i];
553                System.Diagnostics.Debug.Assert(xInfo.body == yInfo.body);
554                BoundingRectangle aabb = xInfo.body.Rectangle;
555
556                xInfo.min.value = aabb.Min.X;
557                xInfo.max.value = aabb.Max.X;
558                yInfo.min.value = aabb.Min.Y;
559                yInfo.max.value = aabb.Max.Y;
560
561                /*                xInfo.min.value = aabb.min.X - fTol;
562                                xInfo.max.value = aabb.max.X + fTol;
563                                yInfo.min.value = aabb.min.Y - fTol;
564                                yInfo.max.value = aabb.max.Y + fTol;*/
565            }
566        }
567
568        /// <summary>
569        /// Iterates over the collision pairs and creates arbiters.
570        /// </summary>
571        private void HandleCollisions()
572        {
573            foreach (CollisionPair cp in collisionPairs.Keys)
574            {
575                // Note: Possible optimization. Maybe arbiter can be cached into value of
576                // collisionPairs? Currently, the collisionPairs hash doesn't use its
577                // value parameter - its just an unused bool value.
578                /* Arbiter arbiter;
579                 arbiter = physicsSimulator.arbiterPool.Fetch();
580                 arbiter.ConstructArbiter(cp.geom1, cp.geom2,
581                     physicsSimulator.allowedPenetration,
582                     physicsSimulator.biasFactor,
583                     physicsSimulator.maxContactsToDetect,
584                     physicsSimulator.maxContactsToResolve);
585
586                 if (!physicsSimulator.arbiterList.Contains(arbiter))
587                     physicsSimulator.arbiterList.Add(arbiter);
588                 else
589                     physicsSimulator.arbiterPool.Release(arbiter);*/
590                OnCollision(step, cp.body1, cp.body2);
591            }
592        }
593
594        bool bForce = false;
595        /// <summary>
596        /// Just calls Update.
597        /// </summary>
598        void Run()
599        {
600            if (bForce)
601                ForceNonIncrementalUpdate();
602            else
603                Update();
604        }
605
606        /// <summary>
607        /// Incrementally updates the system. Assumes relatively good frame coherence.
608        /// </summary>
609        void Update()
610        {
611            UpdateExtentValues();
612
613            xExtentList.IncrementalSort();
614            yExtentList.IncrementalSort();
615
616            xInfoList.MoveUnderConsiderationToOverlaps();
617            yInfoList.MoveUnderConsiderationToOverlaps();
618
619            HandleCollisions();
620        }
621
622        /// <summary>
623        /// This function can be used for times when frame-coherence is temporarily lost
624        /// or when it is simply more convenient to completely rebuild all the cached
625        /// data instead of incrementally updating it. Currently it is used after
626        /// removing disposed/removed geometries. If your application had an object
627        /// that teleported across the universe or some other situation where
628        /// frame-coherence was lost, you might consider this function.
629        /// </summary>
630        public void ForceNonIncrementalUpdate()
631        {
632            UpdateExtentValues();
633
634            // First, wipe out the collision records
635            collisionPairs.Clear();
636
637            // And clear out all the overlap records
638            System.Diagnostics.Debug.Assert(xInfoList.Count == yInfoList.Count);
639            for (int i = 0; i < xInfoList.Count; i++)
640            {
641                xInfoList[i].overlaps.Clear();
642                xInfoList[i].underConsideration.Clear();
643                yInfoList[i].overlaps.Clear();
644                yInfoList[i].underConsideration.Clear();
645            }
646
647            // Force sort
648            xExtentList.Sort(delegate(Extent l, Extent r)
649                { return l.value.CompareTo(r.value); });
650            yExtentList.Sort(delegate(Extent l, Extent r)
651                { return l.value.CompareTo(r.value); });
652
653            // Rebuild overlap information
654            List<Body> overlaps = new List<Body>();
655            for (int i = 0; i < 2; i++)
656            {
657                overlaps.Clear();
658
659                ExtentList extentList = null;
660                if (i == 0)
661                    extentList = xExtentList;
662                else
663                    extentList = yExtentList;
664
665                foreach (Extent extent in extentList)
666                {
667                    if (extent.isMin)
668                    {
669                        // Add whatever is currently in overlaps to this
670                        extent.info.overlaps.AddRange(overlaps);
671
672                        // Now add, this geom to overlaps
673                        overlaps.Add(extent.info.body);
674                    }
675                    else
676                    {
677                        // remove this geom from overlaps
678                        overlaps.Remove(extent.info.body);
679
680                        // Test this geom against its overlaps for collisionpairs
681                        Body thisGeom = extent.info.body;
682                        foreach (Body g in extent.info.overlaps)
683                        {
684                            if (FrameCoherentSAPDetector.TestForCollisions(thisGeom, g) == false)
685                                continue;
686
687                            collisionPairs.AddPair(thisGeom, g);
688                        }
689                    }
690                }
691            }
692            HandleCollisions();
693        }
694    }
695}
696
697#endif
Note: See TracBrowser for help on using the repository browser.