aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs623
1 files changed, 0 insertions, 623 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
deleted file mode 100644
index 168d947..0000000
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
+++ /dev/null
@@ -1,623 +0,0 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class AxisSweep3: OverlappingPairCache
30 {
31 Vector3 _worldAabbMin;
32 Vector3 _worldAabbMax;
33
34 Vector3 _quantize;
35
36 int _numHandles;
37 int _maxHandles;
38
39 Handle[] _handles;
40 Edge[][] _edges = new Edge[3][];
41
42 ushort _firstFreeHandle;
43
44 int _invalidPair;
45
46 public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles)
47 : base()
48 {
49 BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767);
50
51 // init bounds
52 _worldAabbMin = worldAabbMin;
53 _worldAabbMax = worldAabbMax;
54
55 Vector3 aabbSize = _worldAabbMax - _worldAabbMin;
56 _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize;
57
58 // allocate handles buffer and put all handles on free list
59 _handles = new Handle[maxHandles];
60 for (int i = 0; i < maxHandles; i++)
61 _handles[i] = new Handle();
62 _maxHandles = maxHandles;
63 _numHandles = 0;
64
65 // handle 0 is reserved as the null index, and is also used as the sentinel
66 _firstFreeHandle = 1;
67 {
68 for (int i = _firstFreeHandle; i < maxHandles; i++)
69 {
70 _handles[i].NextFree = (ushort)(i + 1);
71 }
72 _handles[maxHandles - 1].NextFree = 0;
73 }
74
75 {
76 // allocate edge buffers
77 for (int i = 0; i < 3; i++)
78 {
79 _edges[i] = new Edge[maxHandles * 2];
80 for (int j = 0; j < maxHandles * 2; j++)
81 {
82 _edges[i][j] = new Edge();
83 }
84 }
85 }
86 //removed overlap management
87
88 // make boundary sentinels
89
90 _handles[0].ClientData = 0;
91
92 for (int axis = 0; axis < 3; axis++)
93 {
94 _handles[0].MinEdges[axis] = 0;
95 _handles[0].MaxEdges[axis] = 1;
96
97 _edges[axis][0].Position = 0;
98 _edges[axis][0].Handle = 0;
99 _edges[axis][1].Position = 0xffff;
100 _edges[axis][1].Handle = 0;
101 }
102 }
103
104 public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
105 {
106 ushort[] min = new ushort[3], max = new ushort[3];
107 Quantize(out min, aabbMin, 0);
108 Quantize(out max, aabbMax, 1);
109
110 ushort handle = AllocateHandle();
111 Handle oHandle = GetHandle(handle);
112
113 oHandle.HandleID = handle;
114 oHandle.ClientData = owner;
115 oHandle.CollisionFilterGroup = collisionFilterGroup;
116 oHandle.CollisionFilterMask = collisionFilterMask;
117
118 int limit = _numHandles * 2;
119
120 // (Gluk )
121 // ( Inside )
122 for (int axis = 0; axis < 3; axis++)
123 {
124 _handles[0].MaxEdges[axis] += 2;
125
126 _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position;
127 _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle;
128
129 _edges[axis][limit - 1].Position = min[axis];
130 _edges[axis][limit - 1].Handle = handle;
131
132 _edges[axis][limit].Position = max[axis];
133 _edges[axis][limit].Handle = handle;
134
135 oHandle.MinEdges[axis] = (ushort)(limit - 1);
136 oHandle.MaxEdges[axis] = (ushort)limit;
137 }
138
139 SortMinDown(0, oHandle.MinEdges[0], false);
140 SortMaxDown(0, oHandle.MaxEdges[0], false);
141 SortMinDown(1, oHandle.MinEdges[1], false);
142 SortMaxDown(1, oHandle.MaxEdges[1], false);
143 SortMinDown(2, oHandle.MinEdges[2], true);
144 SortMaxDown(2, oHandle.MaxEdges[2], true);
145
146 return handle;
147 }
148
149 public void RemoveHandle(ushort handle)
150 {
151 Handle pHandle = GetHandle(handle);
152
153 //explicitly remove the pairs containing the proxy
154 //we could do it also in the sortMinUp (passing true)
155 //todo: compare performance
156 RemoveOverlappingPairsContainingProxy(pHandle);
157
158
159 // compute current limit of edge arrays
160 int limit = _numHandles * 2;
161 int axis;
162
163 for (axis = 0; axis < 3; axis++)
164 {
165 _handles[0].MaxEdges[axis] -= 2;
166 }
167
168 // remove the edges by sorting them up to the end of the list
169 for (axis = 0; axis < 3; axis++)
170 {
171 Edge[] pEdges = _edges[axis];
172 ushort max = pHandle.MaxEdges[axis];
173 pEdges[max].Position = 0xffff;
174
175 SortMaxUp(axis, max, false);
176
177 ushort i = pHandle.MinEdges[axis];
178 pEdges[i].Position = 0xffff;
179
180 SortMinUp(axis, i, false);
181
182 pEdges[limit - 1].Handle = 0;
183 pEdges[limit - 1].Position = 0xffff;
184 }
185
186 // free the handle
187 FreeHandle(handle);
188 }
189
190 public override void ProcessAllOverlappingPairs(IOverlapCallback callback)
191 {
192 OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort));
193
194 if (_invalidPair != 0)
195 OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair);
196 _invalidPair = 0;
197
198 BroadphasePair previousPair = new BroadphasePair();
199 previousPair.ProxyA = null;
200 previousPair.ProxyB = null;
201 previousPair.CollisionAlgorithm = null;
202
203 List<BroadphasePair> removal = new List<BroadphasePair>();
204
205 for (int i = 0; i < OverlappingPairs.Count; i++)
206 {
207 bool isDuplicate = (OverlappingPairs[i] == previousPair);
208 previousPair = OverlappingPairs[i];
209 bool needsRemoval;
210 if (!isDuplicate)
211 {
212 bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB);
213 if (hasOverlap)
214 {
215 needsRemoval = callback.ProcessOverlap(ref previousPair);
216 }
217 else
218 {
219 needsRemoval = true;
220 }
221 }
222 else
223 {
224 needsRemoval = true;
225 BulletDebug.Assert(previousPair.CollisionAlgorithm == null);
226 }
227
228 if (needsRemoval)
229 {
230 removal.Add(previousPair);
231 }
232 }
233
234 for (int i = 0; i < removal.Count; i++)
235 {
236 BroadphasePair pair = removal[i];
237 CleanOverlappingPair(ref pair);
238 pair.ProxyA = null;
239 pair.ProxyB = null;
240 _invalidPair++;
241 OverlappingPairCount--;
242 }
243 }
244
245 private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
246 {
247 if (proxyA == null || proxyB == null)
248 return false;
249
250 Handle handleA = proxyA as Handle;
251 Handle handleB = proxyB as Handle;
252
253 for (int axis = 0; axis < 3; axis++)
254 {
255 if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] ||
256 handleB.MaxEdges[axis] < handleA.MinEdges[axis])
257 {
258 return false;
259 }
260 }
261 return true;
262 }
263
264 private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB)
265 {
266 for (int axis = 0; axis < 3; axis++)
267 {
268 if (axis != ignoreAxis)
269 {
270 if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] ||
271 pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis])
272 {
273 return false;
274 }
275 }
276 }
277
278 return true;
279 }
280
281 private ushort AllocateHandle()
282 {
283 ushort handle = _firstFreeHandle;
284 _firstFreeHandle = GetHandle(handle).NextFree;
285 _numHandles++;
286
287 return handle;
288 }
289
290 private void FreeHandle(ushort handle)
291 {
292 BulletDebug.Assert(handle > 0 && handle < _maxHandles);
293
294 GetHandle(handle).NextFree = _firstFreeHandle;
295 _firstFreeHandle = handle;
296
297 _numHandles--;
298 }
299
300 private Handle GetHandle(ushort handle)
301 {
302 return _handles[handle];
303 }
304
305 private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax)
306 {
307 Handle pHandle = GetHandle(handle);
308
309 // quantize the new bounds
310 ushort[] min = new ushort[3];
311 ushort[] max = new ushort[3];
312 Quantize(out min, aabbMin, 0);
313 Quantize(out max, aabbMax, 1);
314
315 // update changed edges
316 for (int axis = 0; axis < 3; axis++)
317 {
318 ushort emin = pHandle.MinEdges[axis];
319 ushort emax = pHandle.MaxEdges[axis];
320
321 int dmin = (int)min[axis] - (int)_edges[axis][emin].Position;
322 int dmax = (int)max[axis] - (int)_edges[axis][emax].Position;
323
324 _edges[axis][emin].Position = min[axis];
325 _edges[axis][emax].Position = max[axis];
326
327 // expand (only adds overlaps)
328 if (dmin < 0)
329 SortMinDown(axis, emin, true);
330
331 if (dmax > 0)
332 SortMaxUp(axis, emax, true);
333
334 // shrink (only removes overlaps)
335 if (dmin > 0)
336 SortMinUp(axis, emin, true);
337
338 if (dmax < 0)
339 SortMaxDown(axis, emax, true);
340 }
341 }
342
343 private void Quantize(out ushort[] result, Vector3 point, int isMax)
344 {
345 Vector3 clampedPoint = new Vector3(
346 point.X,
347 point.Y,
348 point.Z
349 );
350
351 MathHelper.SetMax(ref clampedPoint, _worldAabbMin);
352 MathHelper.SetMin(ref clampedPoint, _worldAabbMax);
353
354 Vector3 v = (clampedPoint - _worldAabbMin) * _quantize;
355
356 result = new ushort[3];
357 result[0] = (ushort)(((int)v.X & 0xfffe) | isMax);
358 result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax);
359 result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax);
360 }
361
362 private void SortMinDown(int axis, ushort edge, bool updateOverlaps)
363 {
364 Edge pEdge = _edges[axis][edge];
365 Edge pPrev = _edges[axis][edge - 1];
366 Handle pHandleEdge = GetHandle(pEdge.Handle);
367
368 while (pEdge.Position < pPrev.Position)
369 {
370 Handle pHandlePrev = GetHandle(pPrev.Handle);
371
372 if (pPrev.IsMax())
373 {
374 // if previous edge is a maximum check the bounds and add an overlap if necessary
375 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev))
376 {
377 AddOverlappingPair(pHandleEdge, pHandlePrev);
378 }
379
380 // update edge reference in other handle
381 pHandlePrev.MaxEdges[axis]++;
382 }
383 else
384 pHandlePrev.MinEdges[axis]++;
385
386 pHandleEdge.MinEdges[axis]--;
387
388 // swap the edges
389 pEdge.Swap(ref pPrev);
390
391 // decrement
392 edge--;
393 pEdge = _edges[axis][edge];
394 pPrev = _edges[axis][edge - 1];
395 }
396 }
397
398 private void SortMinUp(int axis, ushort edge, bool updateOverlaps)
399 {
400 Edge pEdge = _edges[axis][edge];
401 Edge pNext = _edges[axis][edge + 1];
402 Handle pHandleEdge = GetHandle(pEdge.Handle);
403
404 while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position))
405 {
406 Handle pHandleNext = GetHandle(pNext.Handle);
407
408 if (pNext.IsMax())
409 {
410 // if next edge is maximum remove any overlap between the two handles
411 if (updateOverlaps)
412 {
413 //Handle handle0 = GetHandle(pEdge.Handle);
414 //Handle handle1 = GetHandle(pNext.Handle);
415 //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1);
416 //RemoveOverlappingPair(tmpPair);
417 }
418
419 // update edge reference in other handle
420 pHandleNext.MaxEdges[axis]--;
421 }
422 else
423 pHandleNext.MinEdges[axis]--;
424
425 pHandleEdge.MinEdges[axis]++;
426
427 // swap the edges
428 pEdge.Swap(ref pNext);
429
430 // increment
431 edge++;
432 pEdge = _edges[axis][edge];
433 pNext = _edges[axis][edge + 1];
434 }
435 }
436
437 private void SortMaxDown(int axis, ushort edge, bool updateOverlaps)
438 {
439 Edge pEdge = _edges[axis][edge];
440 Edge pPrev = _edges[axis][edge - 1];
441 Handle pHandleEdge = GetHandle(pEdge.Handle);
442
443 while (pEdge.Position < pPrev.Position)
444 {
445 Handle pHandlePrev = GetHandle(pPrev.Handle);
446
447 if (!pPrev.IsMax())
448 {
449 // if previous edge was a minimum remove any overlap between the two handles
450 if (updateOverlaps)
451 {
452 //this is done during the overlappingpairarray iteration/narrowphase collision
453 //Handle handle0 = GetHandle(pEdge.Handle);
454 //Handle handle1 = GetHandle(pPrev.Handle);
455 //BroadphasePair pair = FindPair(handle0, handle1);
456
457 //if (pair != null)
458 //{
459 // RemoveOverlappingPair(pair);
460 //}
461 }
462
463 // update edge reference in other handle
464 pHandlePrev.MinEdges[axis]++; ;
465 }
466 else
467 pHandlePrev.MaxEdges[axis]++;
468
469 pHandleEdge.MaxEdges[axis]--;
470
471 // swap the edges
472 pEdge.Swap(ref pPrev);
473
474 // decrement
475 edge--;
476 pEdge = _edges[axis][edge];
477 pPrev = _edges[axis][edge - 1];
478 }
479 }
480
481 private void SortMaxUp(int axis, ushort edge, bool updateOverlaps)
482 {
483 Edge pEdge = _edges[axis][edge];
484 Edge pNext = _edges[axis][edge + 1];
485 Handle pHandleEdge = GetHandle(pEdge.Handle);
486
487 while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position))
488 {
489 Handle pHandleNext = GetHandle(pNext.Handle);
490
491 if (!pNext.IsMax())
492 {
493 // if next edge is a minimum check the bounds and add an overlap if necessary
494 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext))
495 {
496 Handle handle0 = GetHandle(pEdge.Handle);
497 Handle handle1 = GetHandle(pNext.Handle);
498 AddOverlappingPair(handle0, handle1);
499 }
500
501 // update edge reference in other handle
502 pHandleNext.MinEdges[axis]--;
503 }
504 else
505 pHandleNext.MaxEdges[axis]--;
506
507 pHandleEdge.MaxEdges[axis]++;
508
509 // swap the edges
510 pEdge.Swap(ref pNext);
511
512 // increment
513 edge++;
514 pEdge = _edges[axis][edge];
515 pNext = _edges[axis][edge + 1];
516 }
517 }
518
519 #region Abstract
520
521 public override void RefreshOverlappingPairs()
522 {
523 }
524
525 public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
526 {
527 ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask);
528
529 Handle handle = GetHandle(handleId);
530
531 return handle;
532 }
533
534 public override void DestroyProxy(BroadphaseProxy proxy)
535 {
536 Handle handle = proxy as Handle;
537 RemoveHandle(handle.HandleID);
538 }
539
540 public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)
541 {
542 Handle handle = proxy as Handle;
543 UpdateHandle(handle.HandleID, aabbMin, aabbMax);
544 }
545 #endregion
546 }
547
548 public class Edge
549 {
550 ushort position;
551 ushort handle;
552
553 public ushort Position
554 {
555 get { return position; }
556 set { position = value; }
557 }
558
559 public ushort Handle
560 {
561 get { return handle; }
562 set { handle = value; }
563 }
564
565 public bool IsMax()
566 {
567 return (position & (ushort)1) == 1;
568 }
569
570 public void Swap(ref Edge e)
571 {
572 ushort tmpPosition = this.position;
573 ushort tmpHandle = this.handle;
574 this.position = e.position;
575 this.handle = e.handle;
576 e.position = tmpPosition;
577 e.handle = tmpHandle;
578 }
579 }
580
581 public class Handle: BroadphaseProxy
582 {
583 ushort[] minEdges, maxEdges;
584 ushort pad;
585 ushort handleID;
586
587 public ushort[] MinEdges
588 {
589 get { return minEdges; }
590 set { minEdges = value; }
591 }
592
593 public ushort[] MaxEdges
594 {
595 get { return maxEdges; }
596 set { maxEdges = value; }
597 }
598
599 public ushort HandleID
600 {
601 get { return handleID; }
602 set { handleID = value; }
603 }
604
605 public ushort Pad
606 {
607 get { return pad; }
608 set { pad = value; }
609 }
610
611 public ushort NextFree
612 {
613 get { return minEdges[0]; }
614 set { minEdges[0] = value;}
615 }
616
617 public Handle()
618 {
619 minEdges = new ushort[3];
620 maxEdges = new ushort[3];
621 }
622 }
623}
generated by cgit v1.1 at 2024-08-26 15:40:25 +0000