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