source: 2013/30/MiskaK/MW2(My Warfare 2)/Paranneltu Jypeli/Physics2DDotNet/Detectors/SweepAndPruneDetector.cs @ 4507

Revision 4507, 13.9 KB checked in by anlakane, 6 years ago (diff)

Talletus.

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
25
26
27#if UseDouble
28using Scalar = System.Double;
29#else
30using Scalar = System.Single;
31#endif
32using System;
33using System.Collections.Generic;
34
35using AdvanceMath.Geometry2D;
36using System.Runtime.Serialization;
37namespace Physics2DDotNet.Detectors
38{
39
40    /// <summary>
41    /// The Sweep and Prune detector should be O(nlogn), but can be O(n^2) if everything is colliding. 
42    /// </summary>
43#if !(WINDOWS_PHONE || XBOX)
44    [Serializable]
45#endif
46    public sealed class SweepAndPruneDetector : BroadPhaseCollisionDetector
47    {
48#if !(WINDOWS_PHONE || XBOX)
49        [Serializable]
50#endif
51        sealed class IntList
52        {
53            static int[] Default = new int[0];
54            int[] array = Default;
55            int length = 0;
56            int bloom = 0; // based off the idea of a bloom filter.
57            public int Count
58            {
59                get { return length; }
60            }
61            public void Add(int item)
62            {
63                if (array.Length == length)
64                {
65                    if (array.Length == 0)
66                    {
67                        this.array = new int[4];
68                    }
69                    else
70                    {
71                        int newLenght = array.Length * 2;
72                        int[] newArray = new int[newLenght];
73                        this.array.CopyTo(newArray, 0);
74                        this.array = newArray;
75                    }
76                }
77                this.array[length++] = item;
78                //adds it to the bloom filter
79                bloom = bloom | item;
80            }
81            public void Sort()
82            {
83                Array.Sort<int>(array, 0, length);
84            }
85            public bool Contains(int item)
86            {
87                //check the bloom filter to see if it is in it.
88                if ((bloom & item) != item) { return false; }
89                int min = 0;
90                int max = length - 1;
91                while (min <= max)
92                {
93                    int index = ((max - min) >> 1) + min;
94                    int value = array[index];
95                    if (value < item)
96                    {
97                        min = index + 1;
98                    }
99                    else if (value > item)
100                    {
101                        max = index - 1;
102                    }
103                    else
104                    {
105                        return true;
106                    }
107                }
108                return false;
109            }
110            public void Clear()
111            {
112                length = 0;
113                bloom = 0;
114            }
115        }
116#if !(WINDOWS_PHONE || XBOX)
117        [Serializable]
118#endif
119        sealed class StubComparer : IComparer<Stub>
120        {
121            public int Compare(Stub left, Stub right)
122            {
123                if (left.value < right.value) { return -1; }
124                else if (left.value > right.value) { return 1; }
125                else { return ((left == right) ? (0) : ((left.begin) ? (-1) : (1))); }
126            }
127        }
128#if !(WINDOWS_PHONE || XBOX)
129        [Serializable]
130#endif
131        sealed class Wrapper 
132#if !WINDOWS_PHONE
133            : IDeserializationCallback
134#endif
135        {
136            public int beginCount;
137            public IntList colliders = new IntList();
138            public int collisions;
139#if !WINDOWS_PHONE
140            [NonSerialized]
141#endif
142            public LinkedListNode<Wrapper> node;
143            public Body body;
144            public bool shouldAddNode;
145            Stub xBegin;
146            Stub xEnd;
147            Stub yBegin;
148            Stub yEnd;
149            public Wrapper(Body body)
150            {
151                this.body = body;
152                this.node = new LinkedListNode<Wrapper>(this);
153                xBegin = new Stub(this, true); 
154                xEnd = new Stub(this, false); 
155                yBegin = new Stub(this, true); 
156                yEnd = new Stub(this, false); 
157            }
158            public void AddStubs(List<Stub> xStubs, List<Stub> yStubs)
159            {
160                xStubs.Add(xBegin);
161                xStubs.Add(xEnd);
162
163                yStubs.Add(yBegin);
164                yStubs.Add(yEnd);
165            }
166            public void Update()
167            {
168                collisions = 0;
169                beginCount = -1;
170                colliders.Clear();
171                BoundingRectangle rect = body.Rectangle;
172                shouldAddNode = rect.Min.X != rect.Max.X || rect.Min.Y != rect.Max.Y;
173               
174                xBegin.value = rect.Min.X;
175                xEnd.value = rect.Max.X;
176
177                yBegin.value = rect.Min.Y;
178                yEnd.value = rect.Max.Y;
179            }
180
181#if !WINDOWS_PHONE
182            void IDeserializationCallback.OnDeserialization(object sender)
183            {
184                this.node = new LinkedListNode<Wrapper>(this);
185            }
186#endif
187        }
188#if !(WINDOWS_PHONE || XBOX)
189        [Serializable]
190#endif
191        sealed class Stub
192        {
193            public Wrapper wrapper;
194            public bool begin;
195            public Scalar value;
196            public Stub(Wrapper wrapper, bool begin)
197            {
198                this.wrapper = wrapper;
199                this.begin = begin;
200            }
201        }
202
203        static StubComparer comparer = new StubComparer();
204
205        static bool WrapperIsRemoved(Wrapper wrapper)
206        {
207            return !wrapper.body.IsAdded;
208        }
209        static bool StubIsRemoved(Stub stub)
210        {
211            return !stub.wrapper.body.IsAdded;
212        }
213        List<Wrapper> wrappers;
214        List<Stub> xStubs;
215        List<Stub> yStubs;
216        int lastXCount = 0;
217        int lastYCount = 0;
218
219        public SweepAndPruneDetector()
220        {
221            this.wrappers = new List<Wrapper>();
222            this.xStubs = new List<Stub>();
223            this.yStubs = new List<Stub>();
224        }
225       
226        protected internal override void AddBodyRange(List<Body> collection)
227        {
228            int wrappercount = collection.Count + wrappers.Count;
229            if (wrappers.Capacity < wrappercount)
230            {
231                wrappers.Capacity = wrappercount;
232            }
233            int nodeCount = collection.Count * 2 + xStubs.Count;
234            if (xStubs.Capacity < nodeCount)
235            {
236                xStubs.Capacity = nodeCount;
237                yStubs.Capacity = nodeCount;
238            }
239            foreach (Body item in collection)
240            {
241                Wrapper wrapper = new Wrapper(item);
242                wrappers.Add(wrapper);
243                wrapper.AddStubs(xStubs, yStubs);
244            }
245        }
246        protected internal override void Clear()
247        {
248            wrappers.Clear();
249            xStubs.Clear();
250            yStubs.Clear();
251        }
252        protected internal override void RemoveExpiredBodies()
253        {
254            wrappers.RemoveAll(WrapperIsRemoved);
255            xStubs.RemoveAll(StubIsRemoved);
256            yStubs.RemoveAll(StubIsRemoved);
257        }
258
259        private void Update()
260        {
261            for (int index = 0; index < wrappers.Count; ++index)
262            {
263                wrappers[index].Update();
264            }
265            xStubs.Sort(comparer);
266            yStubs.Sort(comparer);
267        }
268
269        public override void Detect(TimeStep step)
270        {
271            Update();
272            int count1 = 0;
273            int count2 = 0;
274            int beginCount = 0;
275            List<Stub> list1, list2;
276            LinkedList<Wrapper> currentBodies = new LinkedList<Wrapper>();
277            LinkedListNode<Wrapper> node;
278            Stub stub;
279            Wrapper wrapper;
280            Body body1, body2;
281
282            //this puts the axis that collided the least last round
283            //as the first axis to be tested. to reduce the number
284            //of values added to Wrapper.colliders
285            bool ySmall = lastXCount > lastYCount;
286            if (ySmall)
287            {
288                list1 = yStubs;
289                list2 = xStubs;
290            }
291            else
292            {
293                list1 = xStubs;
294                list2 = yStubs;
295            }
296            //test the first axis.
297            for (int index = 0; index < list1.Count; ++index)
298            {
299                stub = list1[index];
300                wrapper = stub.wrapper;
301
302                if (stub.begin)
303                {
304                    body1 = wrapper.body;
305                    node = currentBodies.First;
306                    while (node != null)
307                    {
308                        count1++;
309                        body2 = node.Value.body;
310                        if ((body1.Mass.MassInv != 0 || body2.Mass.MassInv != 0) &&
311                            Body.CanCollide(body1, body2))
312                        {
313                            if (body1.ID > body2.ID)
314                            {
315                                node.Value.colliders.Add(body1.ID);
316                            }
317                            else
318                            {
319                                wrapper.colliders.Add(body2.ID);
320                            }
321                            node.Value.collisions++;
322                            wrapper.collisions++;
323                        }
324                        node = node.Next;
325                    }
326                    if (wrapper.shouldAddNode)
327                    {
328                        currentBodies.AddLast(wrapper.node);
329                    }
330                }
331                else
332                {
333                    if (wrapper.shouldAddNode)
334                    {
335                        currentBodies.Remove(wrapper.node);
336                    }
337                    if (wrapper.colliders.Count > 0)
338                    {
339                        wrapper.colliders.Sort();
340                    }
341                }
342            }
343            //if no collisions on the first axis exit.
344            if (count1 == 0)
345            {
346                if (ySmall) { lastYCount = 0; }
347                else { lastXCount = 0; }
348                return;
349            }
350            //tests the second axis.
351            for (int index = 0; index < list2.Count; ++index)
352            {
353                stub = list2[index];
354                wrapper = stub.wrapper;
355                if (stub.begin)
356                {
357                    beginCount++;
358                    if (wrapper.collisions == 0) //wrapper.colliders.Count == 0)
359                    {
360                        count2 += currentBodies.Count;
361                        wrapper.beginCount = beginCount;
362                    }
363                    else
364                    {
365                        body1 = wrapper.body;
366                        node = currentBodies.First;
367                        while (node != null)
368                        {
369                            body2 = node.Value.body;
370                            count2++;
371                            bool collided;
372                            if (body1.ID > body2.ID)
373                            {
374                                collided = node.Value.colliders.Contains(body1.ID);
375                            }
376                            else
377                            {
378                                collided = wrapper.colliders.Contains(body2.ID);
379                            }
380                            if (collided)//node.Value.colliders.Contains(body1.ID))
381                            {
382                                this.OnCollision(step, body1, body2);
383                                //this.OnCollision(dt, body1, node.Value.body);
384                            }
385                            node = node.Next;
386                        }
387                        if (wrapper.shouldAddNode)
388                        {
389                            currentBodies.AddLast(wrapper.node);
390                        }
391                    }
392                }
393                else
394                {
395                    if (wrapper.beginCount > 0)
396                    {
397                        count2 += beginCount - wrapper.beginCount;
398                    }
399                    else if (wrapper.shouldAddNode)
400                    {
401                        currentBodies.Remove(wrapper.node);
402                    }
403                }
404            }
405
406            if (ySmall)
407            {
408                lastYCount = count1;
409                lastXCount = count2;
410            }
411            else
412            {
413                lastXCount = count1;
414                lastYCount = count2;
415            }
416        }
417    }
418}
Note: See TracBrowser for help on using the repository browser.