diff options
Stage 1 of adding Darok's bulletX plugin: adding the ModifiedBulletX project (which is based on the BulletXNA project, but modified to not use XNA).
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Collision')
81 files changed, 11967 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs new file mode 100644 index 0000000..12692ea --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs | |||
@@ -0,0 +1,623 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace 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 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs new file mode 100644 index 0000000..dcfd0d8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs | |||
@@ -0,0 +1,68 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | /// Dispatcher uses these types | ||
29 | /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave | ||
30 | /// to facilitate type checking | ||
31 | public enum BroadphaseNativeTypes | ||
32 | { | ||
33 | // polyhedral convex shapes | ||
34 | Box, | ||
35 | Triangle, | ||
36 | Tetrahedral, | ||
37 | ConvexTriangleMesh, | ||
38 | ConvexHull, | ||
39 | //implicit convex shapes | ||
40 | ImplicitConvexShapes, | ||
41 | Sphere, | ||
42 | MultiSphere, | ||
43 | Capsule, | ||
44 | Cone, | ||
45 | Convex, | ||
46 | Cylinder, | ||
47 | MinkowskiSum, | ||
48 | MinkowskiDifference, | ||
49 | //concave shapes | ||
50 | ConcaveShapesStart, | ||
51 | //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! | ||
52 | TriangleMesh, | ||
53 | //used for demo integration FAST/Swift collision library and Bullet | ||
54 | FastConcaveMesh, | ||
55 | //terrain | ||
56 | Terrain, | ||
57 | //Used for GIMPACT Trimesh integration | ||
58 | Gimpact, | ||
59 | |||
60 | Empty, | ||
61 | StaticPlane, | ||
62 | ConcaveShapesEnd, | ||
63 | |||
64 | Compound, | ||
65 | |||
66 | MaxBroadphaseCollisionTypes, | ||
67 | } | ||
68 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs new file mode 100644 index 0000000..2b9e114 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs | |||
@@ -0,0 +1,113 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class BroadphasePair | ||
29 | { | ||
30 | private BroadphaseProxy _proxyA; | ||
31 | private BroadphaseProxy _proxyB; | ||
32 | |||
33 | private CollisionAlgorithm _algorithm; | ||
34 | private object _userInfo; | ||
35 | |||
36 | public BroadphasePair() | ||
37 | { | ||
38 | } | ||
39 | |||
40 | public BroadphasePair(BroadphasePair other) | ||
41 | { | ||
42 | _proxyA = other._proxyA; | ||
43 | _proxyB = other._proxyB; | ||
44 | |||
45 | _algorithm = other._algorithm; | ||
46 | _userInfo = null; | ||
47 | } | ||
48 | |||
49 | public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | ||
50 | { | ||
51 | _proxyA = proxyA; | ||
52 | _proxyB = proxyB; | ||
53 | |||
54 | _algorithm = null; | ||
55 | _userInfo = null; | ||
56 | } | ||
57 | |||
58 | public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } } | ||
59 | public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } } | ||
60 | |||
61 | public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } } | ||
62 | public object UserInfo { get { return _userInfo; } set { _userInfo = value; } } | ||
63 | |||
64 | public override int GetHashCode() | ||
65 | { | ||
66 | return _proxyA.GetHashCode() ^ _proxyB.GetHashCode(); | ||
67 | } | ||
68 | |||
69 | public override bool Equals(object obj) | ||
70 | { | ||
71 | if (obj is BroadphasePair) | ||
72 | return this == (BroadphasePair)obj; | ||
73 | return false; | ||
74 | } | ||
75 | |||
76 | public static int ComparisonSort(BroadphasePair a, BroadphasePair b) | ||
77 | { | ||
78 | int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1; | ||
79 | int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1; | ||
80 | int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1; | ||
81 | int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1; | ||
82 | int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1; | ||
83 | int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1; | ||
84 | |||
85 | if (aAId > bAId || | ||
86 | (a.ProxyA == b.ProxyA && aBId > bBId) || | ||
87 | (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId)) | ||
88 | return -1; | ||
89 | else | ||
90 | return 1; | ||
91 | } | ||
92 | |||
93 | public static bool operator ==(BroadphasePair a, BroadphasePair b) | ||
94 | { | ||
95 | if (object.Equals(a, null) && object.Equals(b, null)) | ||
96 | return true; | ||
97 | if (object.Equals(a, null) || object.Equals(b, null)) | ||
98 | return false; | ||
99 | |||
100 | return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB); | ||
101 | } | ||
102 | |||
103 | public static bool operator !=(BroadphasePair a, BroadphasePair b) | ||
104 | { | ||
105 | if (object.Equals(a, null) && object.Equals(b, null)) | ||
106 | return true; | ||
107 | if (object.Equals(a, null) || object.Equals(b, null)) | ||
108 | return false; | ||
109 | |||
110 | return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB); | ||
111 | } | ||
112 | } | ||
113 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs new file mode 100644 index 0000000..aed82d5 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs | |||
@@ -0,0 +1,91 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class BroadphaseProxy | ||
29 | { | ||
30 | //Usually the client CollisionObject or Rigidbody class | ||
31 | private object _clientObject; | ||
32 | private CollisionFilterGroups _collisionFilterGroup; | ||
33 | private CollisionFilterGroups _collisionFilterMask; | ||
34 | private readonly int _comparisonID; | ||
35 | |||
36 | private static int _globalCount = 0; | ||
37 | |||
38 | public BroadphaseProxy() | ||
39 | { | ||
40 | _comparisonID = _globalCount++; | ||
41 | } | ||
42 | |||
43 | public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) | ||
44 | : this() | ||
45 | { | ||
46 | _clientObject = userData; | ||
47 | _collisionFilterGroup = collisionFilterGroup; | ||
48 | _collisionFilterMask = collisionFilterMask; | ||
49 | } | ||
50 | |||
51 | public object ClientData { get { return _clientObject; } set { _clientObject = value; } } | ||
52 | public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } } | ||
53 | public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } } | ||
54 | internal int ComparisonID { get { return _comparisonID; } } | ||
55 | |||
56 | public static bool IsPolyhedral(BroadphaseNativeTypes proxyType) | ||
57 | { | ||
58 | return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes); | ||
59 | } | ||
60 | |||
61 | public static bool IsConvex(BroadphaseNativeTypes proxyType) | ||
62 | { | ||
63 | return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart); | ||
64 | } | ||
65 | |||
66 | public static bool IsConcave(BroadphaseNativeTypes proxyType) | ||
67 | { | ||
68 | return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) && | ||
69 | (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd)); | ||
70 | } | ||
71 | public static bool IsCompound(BroadphaseNativeTypes proxyType) | ||
72 | { | ||
73 | return (proxyType == BroadphaseNativeTypes.Compound); | ||
74 | } | ||
75 | public static bool IsInfinite(BroadphaseNativeTypes proxyType) | ||
76 | { | ||
77 | return (proxyType == BroadphaseNativeTypes.StaticPlane); | ||
78 | } | ||
79 | |||
80 | //optional filtering to cull potential collisions | ||
81 | public enum CollisionFilterGroups | ||
82 | { | ||
83 | Default = 1, | ||
84 | Static = 2, | ||
85 | Kinematic = 4, | ||
86 | Debris = 8, | ||
87 | Sensor = 16, | ||
88 | All = Default | Static | Kinematic | Debris | Sensor, | ||
89 | } | ||
90 | } | ||
91 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs new file mode 100644 index 0000000..1e59ad8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs | |||
@@ -0,0 +1,51 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | /// <summary> | ||
29 | /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher. | ||
30 | /// It is persistent over frames | ||
31 | /// </summary> | ||
32 | public abstract class CollisionAlgorithm | ||
33 | { | ||
34 | private IDispatcher _dispatcher; | ||
35 | private readonly int _comparisonID = 0; | ||
36 | |||
37 | private static int _globalCount = 0; | ||
38 | |||
39 | public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
40 | { | ||
41 | _comparisonID = _globalCount++; | ||
42 | _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher; | ||
43 | } | ||
44 | |||
45 | protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } | ||
46 | internal int ComparisonID { get { return _comparisonID; } } | ||
47 | |||
48 | public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); | ||
49 | public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut); | ||
50 | } | ||
51 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs new file mode 100644 index 0000000..381752a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs | |||
@@ -0,0 +1,42 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public struct CollisionAlgorithmConstructionInfo | ||
29 | { | ||
30 | private IDispatcher _dispatcher; | ||
31 | private PersistentManifold _manifold; | ||
32 | |||
33 | public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher) | ||
34 | { | ||
35 | _dispatcher = dispatcher; | ||
36 | _manifold = null; | ||
37 | } | ||
38 | |||
39 | public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } } | ||
40 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | ||
41 | } | ||
42 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs new file mode 100644 index 0000000..8c0367f --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs | |||
@@ -0,0 +1,54 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public enum DispatchFunction | ||
29 | { | ||
30 | Discrete = 1, | ||
31 | Continuous, | ||
32 | } | ||
33 | |||
34 | public class DispatcherInfo | ||
35 | { | ||
36 | private float _timeStep; | ||
37 | private int _stepCount; | ||
38 | private DispatchFunction _dispatchFunc = DispatchFunction.Discrete; | ||
39 | private float _timeOfImpact = 1; | ||
40 | private bool _useContinuous; | ||
41 | private bool _enableSatConvex; | ||
42 | private bool _enableSpu; | ||
43 | private IDebugDraw _debugDraw; | ||
44 | |||
45 | public float TimeStep { get { return _timeStep; } set { _timeStep = value; } } | ||
46 | public int StepCount { get { return _stepCount; } set { _stepCount = value; } } | ||
47 | public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } } | ||
48 | public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } } | ||
49 | public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } } | ||
50 | public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } } | ||
51 | public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } } | ||
52 | public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } } | ||
53 | } | ||
54 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs new file mode 100644 index 0000000..89618af --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs | |||
@@ -0,0 +1,36 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public interface IBroadphase | ||
30 | { | ||
31 | BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); | ||
32 | void DestroyProxy(BroadphaseProxy proxy); | ||
33 | void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax); | ||
34 | void CleanProxyFromPairs(BroadphaseProxy proxy); | ||
35 | } | ||
36 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs new file mode 100644 index 0000000..52831b7 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs | |||
@@ -0,0 +1,42 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public interface IDispatcher | ||
29 | { | ||
30 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold); | ||
31 | CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB); | ||
32 | PersistentManifold GetNewManifold(object bodyA, object bodyB); | ||
33 | void ReleaseManifold(PersistentManifold manifold); | ||
34 | void ClearManifold(PersistentManifold manifold); | ||
35 | bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB); | ||
36 | bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB); | ||
37 | void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo); | ||
38 | PersistentManifold GetManifoldByIndex(int index); | ||
39 | |||
40 | int ManifoldCount { get; } | ||
41 | } | ||
42 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs new file mode 100644 index 0000000..4e128da --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs | |||
@@ -0,0 +1,33 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public interface IOverlapCallback | ||
29 | { | ||
30 | //return true for deletion of the pair | ||
31 | bool ProcessOverlap(ref BroadphasePair pair); | ||
32 | } | ||
33 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs new file mode 100644 index 0000000..165912a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs | |||
@@ -0,0 +1,159 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public abstract class OverlappingPairCache : IBroadphase | ||
29 | { | ||
30 | private static int _overlappingPairCount = 0; | ||
31 | private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>(); | ||
32 | //during the dispatch, check that user doesn't destroy/create proxy | ||
33 | private bool _blockedForChanges; | ||
34 | |||
35 | public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } } | ||
36 | public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } } | ||
37 | |||
38 | public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } } | ||
39 | |||
40 | public void RemoveOverlappingPair(BroadphasePair pair) | ||
41 | { | ||
42 | if (!_overlappingPairs.Contains(pair)) | ||
43 | return; | ||
44 | |||
45 | CleanOverlappingPair(ref pair); | ||
46 | _overlappingPairs.Remove(pair); | ||
47 | } | ||
48 | |||
49 | public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | ||
50 | { | ||
51 | //don't add overlap with own | ||
52 | bool test = proxyA != proxyB; | ||
53 | BulletDebug.Assert(proxyA != proxyB); | ||
54 | |||
55 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) | ||
56 | return; | ||
57 | |||
58 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); | ||
59 | _overlappingPairs.Add(pair); | ||
60 | _overlappingPairCount++; | ||
61 | } | ||
62 | |||
63 | //this FindPair becomes really slow. Either sort the list to speedup the query, or | ||
64 | //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. | ||
65 | //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) | ||
66 | //Also we can use a 2D bitmap, which can be useful for a future GPU implementation | ||
67 | public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB) | ||
68 | { | ||
69 | if (!NeedsBroadphaseCollision(proxyA, proxyB)) | ||
70 | return null; | ||
71 | |||
72 | BroadphasePair pair = new BroadphasePair(proxyA, proxyB); | ||
73 | for (int i = 0; i < _overlappingPairs.Count; i++) | ||
74 | { | ||
75 | if (_overlappingPairs[i] == pair) | ||
76 | { | ||
77 | return _overlappingPairs[i]; | ||
78 | } | ||
79 | } | ||
80 | |||
81 | return null; | ||
82 | } | ||
83 | |||
84 | public void CleanProxyFromPairs(BroadphaseProxy proxy) | ||
85 | { | ||
86 | for (int i = 0; i < _overlappingPairs.Count; i++) | ||
87 | { | ||
88 | BroadphasePair pair = _overlappingPairs[i]; | ||
89 | if (pair.ProxyA == proxy || | ||
90 | pair.ProxyB == proxy) | ||
91 | { | ||
92 | CleanOverlappingPair(ref pair); | ||
93 | _overlappingPairs[i] = pair; | ||
94 | } | ||
95 | } | ||
96 | } | ||
97 | |||
98 | public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy) | ||
99 | { | ||
100 | for (int i = _overlappingPairs.Count - 1; i >= 0; i--) | ||
101 | { | ||
102 | BroadphasePair pair = _overlappingPairs[i]; | ||
103 | if (pair.ProxyA == proxy || | ||
104 | pair.ProxyB == proxy) | ||
105 | { | ||
106 | RemoveOverlappingPair(pair); | ||
107 | i++; | ||
108 | } | ||
109 | } | ||
110 | } | ||
111 | |||
112 | public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) | ||
113 | { | ||
114 | bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0; | ||
115 | collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0); | ||
116 | |||
117 | return collides; | ||
118 | } | ||
119 | |||
120 | public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback) | ||
121 | { | ||
122 | List<BroadphasePair> deleting = new List<BroadphasePair>(); | ||
123 | for (int i = 0; i < _overlappingPairs.Count; i++) | ||
124 | { | ||
125 | BroadphasePair p = _overlappingPairs[i]; | ||
126 | if (callback.ProcessOverlap(ref p)) | ||
127 | { | ||
128 | CleanOverlappingPair(ref p); | ||
129 | deleting.Add(p); | ||
130 | _overlappingPairCount--; | ||
131 | } | ||
132 | } | ||
133 | |||
134 | for (int i = 0; i < deleting.Count; i++) | ||
135 | _overlappingPairs.Remove(deleting[i]); | ||
136 | } | ||
137 | |||
138 | public void CleanOverlappingPair(ref BroadphasePair pair) | ||
139 | { | ||
140 | if (pair.CollisionAlgorithm != null) | ||
141 | { | ||
142 | if (pair.CollisionAlgorithm is IDisposable) | ||
143 | (pair.CollisionAlgorithm as IDisposable).Dispose(); | ||
144 | pair.CollisionAlgorithm = null; | ||
145 | } | ||
146 | } | ||
147 | |||
148 | public abstract void RefreshOverlappingPairs(); | ||
149 | |||
150 | #region IBroadphase Members | ||
151 | public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask); | ||
152 | |||
153 | public abstract void DestroyProxy(BroadphaseProxy proxy); | ||
154 | |||
155 | public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax); | ||
156 | |||
157 | #endregion | ||
158 | } | ||
159 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs new file mode 100644 index 0000000..1dc3f34 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs | |||
@@ -0,0 +1,128 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SimpleBroadphase : OverlappingPairCache | ||
30 | { | ||
31 | private int _maxProxies; | ||
32 | private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>(); | ||
33 | |||
34 | public SimpleBroadphase() | ||
35 | : this(16384) { } | ||
36 | |||
37 | public SimpleBroadphase(int maxProxies) | ||
38 | : base() | ||
39 | { | ||
40 | _maxProxies = maxProxies; | ||
41 | } | ||
42 | |||
43 | public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | ||
44 | { | ||
45 | if (_proxies.Count >= _maxProxies) | ||
46 | { | ||
47 | BulletDebug.Assert(false); | ||
48 | return null; //should never happen, but don't let the game crash ;-) | ||
49 | } | ||
50 | BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z); | ||
51 | |||
52 | SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask); | ||
53 | _proxies.Add(proxy); | ||
54 | |||
55 | return proxy; | ||
56 | } | ||
57 | |||
58 | |||
59 | public override void DestroyProxy(BroadphaseProxy proxy) | ||
60 | { | ||
61 | RemoveOverlappingPairsContainingProxy(proxy); | ||
62 | _proxies.Remove(proxy as SimpleBroadphaseProxy); | ||
63 | } | ||
64 | |||
65 | public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax) | ||
66 | { | ||
67 | SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy); | ||
68 | simpleProxy.Minimum = aabbMin; | ||
69 | simpleProxy.Maximum = aabbMax; | ||
70 | } | ||
71 | |||
72 | private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy) | ||
73 | { | ||
74 | return proxy as SimpleBroadphaseProxy; | ||
75 | } | ||
76 | |||
77 | public override void RefreshOverlappingPairs() | ||
78 | { | ||
79 | for (int i = 0; i < _proxies.Count; i++) | ||
80 | { | ||
81 | SimpleBroadphaseProxy proxyA = _proxies[i]; | ||
82 | |||
83 | for (int j = i + 1; j < _proxies.Count; j++) | ||
84 | { | ||
85 | SimpleBroadphaseProxy proxyB = _proxies[j]; | ||
86 | |||
87 | if (AabbOverlap(proxyA, proxyB)) | ||
88 | { | ||
89 | if (FindPair(proxyA, proxyB) == null) | ||
90 | { | ||
91 | AddOverlappingPair(proxyA, proxyB); | ||
92 | } | ||
93 | } | ||
94 | } | ||
95 | } | ||
96 | |||
97 | CheckOverlapCallback check = new CheckOverlapCallback(); | ||
98 | ProcessAllOverlappingPairs(check); | ||
99 | } | ||
100 | |||
101 | public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB) | ||
102 | { | ||
103 | return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X && | ||
104 | proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y && | ||
105 | proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z; | ||
106 | } | ||
107 | |||
108 | private void Validate() | ||
109 | { | ||
110 | for (int i = 0; i < _proxies.Count; i++) | ||
111 | { | ||
112 | for (int j = i + 1; j < _proxies.Count; j++) | ||
113 | { | ||
114 | if (_proxies[i] == _proxies[j]) | ||
115 | throw new BulletException(); | ||
116 | } | ||
117 | } | ||
118 | } | ||
119 | } | ||
120 | |||
121 | public class CheckOverlapCallback : IOverlapCallback | ||
122 | { | ||
123 | public bool ProcessOverlap(ref BroadphasePair pair) | ||
124 | { | ||
125 | return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy)); | ||
126 | } | ||
127 | } | ||
128 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs new file mode 100644 index 0000000..f473ef2 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs | |||
@@ -0,0 +1,46 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SimpleBroadphaseProxy : BroadphaseProxy | ||
30 | { | ||
31 | private Vector3 _min; | ||
32 | private Vector3 _max; | ||
33 | |||
34 | public SimpleBroadphaseProxy() { } | ||
35 | |||
36 | public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask) | ||
37 | : base(userData, collisionFilterGroup, collisionFilterMask) | ||
38 | { | ||
39 | _min = minPoint; | ||
40 | _max = maxPoint; | ||
41 | } | ||
42 | |||
43 | public Vector3 Minimum { get { return _min; } set { _min = value; } } | ||
44 | public Vector3 Maximum { get { return _max; } set { _max = value; } } | ||
45 | } | ||
46 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs new file mode 100644 index 0000000..ba638d1 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs | |||
@@ -0,0 +1,59 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback | ||
30 | { | ||
31 | CollisionWorld.RayResultCallback _resultCallback; | ||
32 | CollisionObject _collisionObject; | ||
33 | TriangleMeshShape _triangleMesh; | ||
34 | |||
35 | public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to, | ||
36 | CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh) | ||
37 | : base(from, to) | ||
38 | { | ||
39 | _resultCallback = resultCallback; | ||
40 | _collisionObject = collisionObject; | ||
41 | _triangleMesh = triangleMesh; | ||
42 | } | ||
43 | |||
44 | public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex) | ||
45 | { | ||
46 | CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo(); | ||
47 | shapeInfo.ShapePart = partId; | ||
48 | shapeInfo.TriangleIndex = triangleIndex; | ||
49 | |||
50 | CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult | ||
51 | (_collisionObject, | ||
52 | shapeInfo, | ||
53 | hitNormalLocal, | ||
54 | hitFraction); | ||
55 | |||
56 | return _resultCallback.AddSingleResult(rayResult); | ||
57 | } | ||
58 | } | ||
59 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs new file mode 100644 index 0000000..4e3903b --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs | |||
@@ -0,0 +1,40 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CollisionAlgorithmCreateFunction | ||
30 | { | ||
31 | private bool _swapped; | ||
32 | |||
33 | public bool IsSwapped { get { return _swapped; } set { _swapped = value; } } | ||
34 | |||
35 | public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) | ||
36 | { | ||
37 | return null; | ||
38 | } | ||
39 | } | ||
40 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs new file mode 100644 index 0000000..75d35b0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs | |||
@@ -0,0 +1,280 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); | ||
30 | |||
31 | public class CollisionDispatcher : IDispatcher | ||
32 | { | ||
33 | private List<PersistentManifold> _manifolds = new List<PersistentManifold>(); | ||
34 | |||
35 | //private bool _useIslands; | ||
36 | private NearCallback _nearCallback; | ||
37 | |||
38 | //private ManifoldResult _defaultManifoldResult; | ||
39 | |||
40 | private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes]; | ||
41 | |||
42 | //default CreationFunctions, filling the m_doubleDispatch table | ||
43 | private CollisionAlgorithmCreateFunction _convexConvexCreateFunc; | ||
44 | private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc; | ||
45 | private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc; | ||
46 | private CollisionAlgorithmCreateFunction _compoundCreateFunc; | ||
47 | private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc; | ||
48 | private CollisionAlgorithmCreateFunction _emptyCreateFunc; | ||
49 | |||
50 | private int _count; | ||
51 | private static int _manifoldCount = 0; | ||
52 | |||
53 | public CollisionDispatcher() | ||
54 | { | ||
55 | NearCallback = DefaultNearCallback; | ||
56 | //_useIslands = true; | ||
57 | //default CreationFunctions, filling the m_doubleDispatch table | ||
58 | _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(); | ||
59 | _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); | ||
60 | _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); | ||
61 | _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc(); | ||
62 | _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc(); | ||
63 | _emptyCreateFunc = new EmptyAlgorithm.CreateFunc(); | ||
64 | |||
65 | for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++) | ||
66 | { | ||
67 | for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++) | ||
68 | { | ||
69 | _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j); | ||
70 | if (_doubleDispatch[(int)i, (int)j] == null) | ||
71 | throw new BulletException(); | ||
72 | } | ||
73 | } | ||
74 | } | ||
75 | |||
76 | public int Count { get { return _count; } set { _count = value; } } | ||
77 | public int ManifoldCount { get { return _manifolds.Count; } } | ||
78 | public List<PersistentManifold> Manifolds { get { return _manifolds; } } | ||
79 | |||
80 | public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } } | ||
81 | |||
82 | public PersistentManifold GetManifoldByIndex(int index) | ||
83 | { | ||
84 | return _manifolds[index]; | ||
85 | } | ||
86 | |||
87 | //registerCollisionCreateFunc allows registration of custom/alternative collision create functions | ||
88 | public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc) | ||
89 | { | ||
90 | _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc; | ||
91 | } | ||
92 | |||
93 | public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB) | ||
94 | { | ||
95 | _manifoldCount++; | ||
96 | |||
97 | CollisionObject body0 = bodyA as CollisionObject; | ||
98 | CollisionObject body1 = bodyB as CollisionObject; | ||
99 | |||
100 | PersistentManifold manifold = new PersistentManifold(body0, body1); | ||
101 | _manifolds.Add(manifold); | ||
102 | |||
103 | return manifold; | ||
104 | } | ||
105 | |||
106 | public virtual void ReleaseManifold(PersistentManifold manifold) | ||
107 | { | ||
108 | _manifoldCount--; | ||
109 | |||
110 | ClearManifold(manifold); | ||
111 | _manifolds.Remove(manifold); | ||
112 | } | ||
113 | |||
114 | |||
115 | public virtual void ClearManifold(PersistentManifold manifold) | ||
116 | { | ||
117 | manifold.ClearManifold(); | ||
118 | } | ||
119 | |||
120 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB) | ||
121 | { | ||
122 | return FindAlgorithm(bodyA, bodyB, null); | ||
123 | } | ||
124 | |||
125 | public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold) | ||
126 | { | ||
127 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); | ||
128 | collisionAlgorithmConstructionInfo.Dispatcher = this; | ||
129 | collisionAlgorithmConstructionInfo.Manifold = sharedManifold; | ||
130 | CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB); | ||
131 | return collisionAlgorithm; | ||
132 | } | ||
133 | |||
134 | /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1) | ||
135 | { | ||
136 | return internalFindAlgorithm(body0, body1, null); | ||
137 | } | ||
138 | |||
139 | public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) | ||
140 | { | ||
141 | m_count++; | ||
142 | |||
143 | CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); | ||
144 | ci.m_dispatcher = this; | ||
145 | |||
146 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex()) | ||
147 | { | ||
148 | return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1); | ||
149 | } | ||
150 | |||
151 | if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave()) | ||
152 | { | ||
153 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); | ||
154 | } | ||
155 | |||
156 | if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave()) | ||
157 | { | ||
158 | return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); | ||
159 | } | ||
160 | |||
161 | if (body0.getCollisionShape().isCompound()) | ||
162 | { | ||
163 | return new CompoundCollisionAlgorithm(ci, body0, body1, false); | ||
164 | } | ||
165 | else | ||
166 | { | ||
167 | if (body1.getCollisionShape().isCompound()) | ||
168 | { | ||
169 | return new CompoundCollisionAlgorithm(ci, body0, body1, true); | ||
170 | } | ||
171 | } | ||
172 | |||
173 | //failed to find an algorithm | ||
174 | return new EmptyAlgorithm(ci); | ||
175 | }*/ | ||
176 | |||
177 | public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB) | ||
178 | { | ||
179 | if (bodyA == null || bodyB == null) | ||
180 | throw new BulletException(); | ||
181 | |||
182 | bool needsCollision = true; | ||
183 | |||
184 | //broadphase filtering already deals with this | ||
185 | /*if ((body0.isStaticObject() || body0.isKinematicObject()) && | ||
186 | (body1.isStaticObject() || body1.isKinematicObject())) | ||
187 | { | ||
188 | printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); | ||
189 | }*/ | ||
190 | |||
191 | if ((!bodyA.IsActive) && (!bodyB.IsActive)) | ||
192 | needsCollision = false; | ||
193 | |||
194 | return needsCollision; | ||
195 | } | ||
196 | |||
197 | public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB) | ||
198 | { | ||
199 | //here you can do filtering | ||
200 | bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse; | ||
201 | hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject); | ||
202 | return hasResponse; | ||
203 | } | ||
204 | |||
205 | public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo) | ||
206 | { | ||
207 | CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this); | ||
208 | pairCache.ProcessAllOverlappingPairs(collisionCallback); | ||
209 | } | ||
210 | |||
211 | private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB) | ||
212 | { | ||
213 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB)) | ||
214 | { | ||
215 | return _convexConvexCreateFunc; | ||
216 | } | ||
217 | |||
218 | if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB)) | ||
219 | { | ||
220 | return _convexConcaveCreateFunc; | ||
221 | } | ||
222 | |||
223 | if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA)) | ||
224 | { | ||
225 | return _swappedConvexConcaveCreateFunc; | ||
226 | } | ||
227 | |||
228 | if (BroadphaseProxy.IsCompound(proxyTypeA)) | ||
229 | { | ||
230 | return _compoundCreateFunc; | ||
231 | } | ||
232 | else | ||
233 | { | ||
234 | if (BroadphaseProxy.IsCompound(proxyTypeB)) | ||
235 | { | ||
236 | return _swappedCompoundCreateFunc; | ||
237 | } | ||
238 | } | ||
239 | |||
240 | //failed to find an algorithm | ||
241 | return _emptyCreateFunc; | ||
242 | } | ||
243 | |||
244 | public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } } | ||
245 | |||
246 | //by default, Bullet will use this near callback | ||
247 | public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) | ||
248 | { | ||
249 | CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject; | ||
250 | CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject; | ||
251 | |||
252 | if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB)) | ||
253 | { | ||
254 | //dispatcher will keep algorithms persistent in the collision pair | ||
255 | if (collisionPair.CollisionAlgorithm == null) | ||
256 | { | ||
257 | collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB); | ||
258 | } | ||
259 | |||
260 | if (collisionPair.CollisionAlgorithm != null) | ||
261 | { | ||
262 | ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB); | ||
263 | |||
264 | if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete) | ||
265 | { | ||
266 | //discrete collision detection query | ||
267 | collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); | ||
268 | } | ||
269 | else | ||
270 | { | ||
271 | //continuous collision detection query, time of impact (toi) | ||
272 | float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult); | ||
273 | if (dispatchInfo.TimeOfImpact > timeOfImpact) | ||
274 | dispatchInfo.TimeOfImpact = timeOfImpact; | ||
275 | } | ||
276 | } | ||
277 | } | ||
278 | } | ||
279 | } | ||
280 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs new file mode 100644 index 0000000..4e9cf58 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs | |||
@@ -0,0 +1,163 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public enum ActivationState | ||
30 | { | ||
31 | Nothing = 0, | ||
32 | Active, | ||
33 | IslandSleeping, | ||
34 | WantsDeactivation, | ||
35 | DisableDeactivation, | ||
36 | DisableSimulation, | ||
37 | } | ||
38 | |||
39 | public enum CollisionOptions | ||
40 | { | ||
41 | StaticObject = 1, | ||
42 | KinematicObject = 2, | ||
43 | NoContactResponse = 4, | ||
44 | CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution) | ||
45 | } | ||
46 | |||
47 | /// <summary> | ||
48 | /// btCollisionObject can be used to manage collision detection objects. | ||
49 | /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. | ||
50 | /// They can be added to the btCollisionWorld. | ||
51 | /// </summary> | ||
52 | public class CollisionObject | ||
53 | { | ||
54 | protected Matrix _worldTransform; | ||
55 | private BroadphaseProxy _broadphase; | ||
56 | private CollisionShape _collisionShape; | ||
57 | |||
58 | //m_interpolationWorldTransform is used for CCD and interpolation | ||
59 | //it can be either previous or future (predicted) transform | ||
60 | private Matrix _interpolationWorldTransform; | ||
61 | |||
62 | private CollisionOptions _collisionFlags; | ||
63 | |||
64 | private int _islandTag; | ||
65 | private ActivationState _activationState; | ||
66 | private float _deactivationTime; | ||
67 | |||
68 | private float _friction; | ||
69 | private float _restitution; | ||
70 | |||
71 | //users can point to their objects, m_userPointer is not used by Bullet | ||
72 | private object _userData; | ||
73 | |||
74 | //m_internalOwner one is used by optional Bullet high level interface | ||
75 | private object _internalOwner; | ||
76 | |||
77 | //time of impact calculation | ||
78 | private float _hitFraction; | ||
79 | |||
80 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: | ||
81 | private float _ccdSweptSphereRadius; | ||
82 | |||
83 | // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold | ||
84 | private float _ccdSquareMotionThreshold; | ||
85 | //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) | ||
86 | //without destroying the continuous interpolated motion (which uses this interpolation velocities) | ||
87 | private Vector3 _interpolationLinearVelocity; | ||
88 | private Vector3 _interpolationAngularVelocity; | ||
89 | |||
90 | private int _companionID; | ||
91 | |||
92 | public CollisionObject() | ||
93 | { | ||
94 | _activationState = ActivationState.Active; | ||
95 | _hitFraction = 1; | ||
96 | } | ||
97 | |||
98 | public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } } | ||
99 | public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } } | ||
100 | public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } } | ||
101 | |||
102 | public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } } | ||
103 | public bool MergesSimulationIslands | ||
104 | { | ||
105 | get | ||
106 | { | ||
107 | //static objects, kinematic and object without contact response don't merge islands | ||
108 | return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0; | ||
109 | } | ||
110 | } | ||
111 | |||
112 | public ActivationState ActivationState | ||
113 | { | ||
114 | get { return _activationState; } | ||
115 | set | ||
116 | { | ||
117 | if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation)) | ||
118 | _activationState = value; | ||
119 | } | ||
120 | } | ||
121 | |||
122 | public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } } | ||
123 | public float Restitution { get { return _restitution; } set { _restitution = value; } } | ||
124 | public float Friction { get { return _friction; } set { _friction = value; } } | ||
125 | public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } } | ||
126 | public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } } | ||
127 | public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } } | ||
128 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } | ||
129 | public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } } | ||
130 | public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } } | ||
131 | public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } } | ||
132 | public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } } | ||
133 | public int IslandTag { get { return _islandTag; } set { _islandTag = value; } } | ||
134 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
135 | public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } } | ||
136 | //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm | ||
137 | public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } } | ||
138 | // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold | ||
139 | public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } } | ||
140 | //users can point to their objects, userPointer is not used by Bullet | ||
141 | public object UserData { get { return _userData; } set { _userData = value; } } | ||
142 | public int CompanionID { get { return _companionID; } set { _companionID = value; } } | ||
143 | |||
144 | public void ForceActivationState(ActivationState newState) | ||
145 | { | ||
146 | _activationState = newState; | ||
147 | } | ||
148 | |||
149 | public void Activate() | ||
150 | { | ||
151 | Activate(false); | ||
152 | } | ||
153 | |||
154 | public void Activate(bool forceActivation) | ||
155 | { | ||
156 | if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0) | ||
157 | { | ||
158 | ActivationState = ActivationState.Active; | ||
159 | _deactivationTime = 0; | ||
160 | } | ||
161 | } | ||
162 | } | ||
163 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs new file mode 100644 index 0000000..7a85731 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs | |||
@@ -0,0 +1,48 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CollisionPairCallback : IOverlapCallback | ||
30 | { | ||
31 | private DispatcherInfo _dispatchInfo; | ||
32 | private CollisionDispatcher _dispatcher; | ||
33 | |||
34 | public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) | ||
35 | { | ||
36 | _dispatchInfo = dispatchInfo; | ||
37 | _dispatcher = dispatcher; | ||
38 | } | ||
39 | |||
40 | #region IOverlapCallback Members | ||
41 | public bool ProcessOverlap(ref BroadphasePair pair) | ||
42 | { | ||
43 | _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo); | ||
44 | return false; | ||
45 | } | ||
46 | #endregion | ||
47 | } | ||
48 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs new file mode 100644 index 0000000..968e219 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs | |||
@@ -0,0 +1,358 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CollisionWorld | ||
30 | { | ||
31 | private List<CollisionObject> _collisionObjects = new List<CollisionObject>(); | ||
32 | private IDispatcher _dispatcher; | ||
33 | private OverlappingPairCache _broadphasePairCache; | ||
34 | private bool _ownsDispatcher; | ||
35 | private bool _ownsBroadphasePairCache; | ||
36 | private DispatcherInfo _dispatchInfo = new DispatcherInfo(); | ||
37 | |||
38 | /// <summary> | ||
39 | /// this constructor doesn't own the dispatcher and paircache/broadphase | ||
40 | /// </summary> | ||
41 | /// <param name="dispatcher"></param> | ||
42 | /// <param name="pairCache"></param> | ||
43 | public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache) | ||
44 | { | ||
45 | _dispatcher = dispatcher; | ||
46 | _broadphasePairCache = pairCache; | ||
47 | _ownsDispatcher = false; | ||
48 | _ownsBroadphasePairCache = false; | ||
49 | } | ||
50 | |||
51 | public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } } | ||
52 | public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } } | ||
53 | public IBroadphase Broadphase { get { return _broadphasePairCache; } } | ||
54 | public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } } | ||
55 | public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } } | ||
56 | public int CollisionObjectsCount { get { return _collisionObjects.Count; } } | ||
57 | protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } } | ||
58 | protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } } | ||
59 | |||
60 | // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback | ||
61 | // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. | ||
62 | public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback) | ||
63 | { | ||
64 | Matrix rayFromTrans, rayToTrans; | ||
65 | |||
66 | rayFromTrans = Matrix.Identity; | ||
67 | rayFromTrans.Translation = rayFromWorld; | ||
68 | |||
69 | rayToTrans = Matrix.Identity; | ||
70 | rayToTrans.Translation = rayToWorld; | ||
71 | |||
72 | // brute force go over all objects. Once there is a broadphase, use that, or | ||
73 | // add a raycast against aabb first. | ||
74 | |||
75 | foreach (CollisionObject collisionObject in _collisionObjects) | ||
76 | { | ||
77 | //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); | ||
78 | Vector3 collisionObjectAabbMin, collisionObjectAabbMax; | ||
79 | collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax); | ||
80 | |||
81 | float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing | ||
82 | Vector3 hitNormal = new Vector3(); | ||
83 | |||
84 | //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax)) | ||
85 | if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) | ||
86 | { | ||
87 | RayTestSingle(rayFromTrans, rayToTrans, | ||
88 | collisionObject, collisionObject.CollisionShape, | ||
89 | collisionObject.WorldTransform, resultCallback); | ||
90 | |||
91 | } | ||
92 | } | ||
93 | } | ||
94 | |||
95 | // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. | ||
96 | // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape. | ||
97 | // This allows more customization. | ||
98 | public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, | ||
99 | CollisionObject collisionObject, | ||
100 | CollisionShape collisionShape, | ||
101 | Matrix colObjWorldTransform, | ||
102 | RayResultCallback resultCallback) | ||
103 | { | ||
104 | SphereShape pointShape=new SphereShape(0.0f); | ||
105 | |||
106 | if (collisionShape.IsConvex) | ||
107 | { | ||
108 | CastResult castResult = new CastResult(); | ||
109 | castResult.Fraction = 1f;//?? | ||
110 | |||
111 | ConvexShape convexShape = collisionShape as ConvexShape; | ||
112 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); | ||
113 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); | ||
114 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); | ||
115 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); | ||
116 | |||
117 | if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) | ||
118 | { | ||
119 | //add hit | ||
120 | if (castResult.Normal.LengthSquared() > 0.0001f) | ||
121 | { | ||
122 | castResult.Normal.Normalize(); | ||
123 | if (castResult.Fraction < resultCallback.ClosestHitFraction) | ||
124 | { | ||
125 | |||
126 | CollisionWorld.LocalRayResult localRayResult = new LocalRayResult | ||
127 | ( | ||
128 | collisionObject, | ||
129 | new LocalShapeInfo(), | ||
130 | castResult.Normal, | ||
131 | castResult.Fraction | ||
132 | ); | ||
133 | |||
134 | resultCallback.AddSingleResult(localRayResult); | ||
135 | } | ||
136 | } | ||
137 | } | ||
138 | else | ||
139 | { | ||
140 | if (collisionShape.IsConcave) | ||
141 | { | ||
142 | |||
143 | TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; | ||
144 | |||
145 | Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); | ||
146 | |||
147 | Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); | ||
148 | Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); | ||
149 | |||
150 | BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); | ||
151 | rcb.HitFraction = resultCallback.ClosestHitFraction; | ||
152 | |||
153 | Vector3 rayAabbMinLocal = rayFromLocal; | ||
154 | MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); | ||
155 | Vector3 rayAabbMaxLocal = rayFromLocal; | ||
156 | MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); | ||
157 | |||
158 | triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); | ||
159 | } | ||
160 | else | ||
161 | { | ||
162 | //todo: use AABB tree or other BVH acceleration structure! | ||
163 | if (collisionShape.IsCompound) | ||
164 | { | ||
165 | CompoundShape compoundShape = collisionShape as CompoundShape; | ||
166 | for (int i = 0; i < compoundShape.ChildShapeCount; i++) | ||
167 | { | ||
168 | Matrix childTrans = compoundShape.GetChildTransform(i); | ||
169 | CollisionShape childCollisionShape = compoundShape.GetChildShape(i); | ||
170 | Matrix childWorldTrans = colObjWorldTransform * childTrans; | ||
171 | RayTestSingle(rayFromTrans, rayToTrans, | ||
172 | collisionObject, | ||
173 | childCollisionShape, | ||
174 | childWorldTrans, | ||
175 | resultCallback); | ||
176 | } | ||
177 | } | ||
178 | } | ||
179 | } | ||
180 | } | ||
181 | } | ||
182 | |||
183 | public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask) | ||
184 | { | ||
185 | //check that the object isn't already added | ||
186 | if (!_collisionObjects.Contains(collisionObject)) | ||
187 | { | ||
188 | _collisionObjects.Add(collisionObject); | ||
189 | |||
190 | //calculate new AABB | ||
191 | Matrix trans = collisionObject.WorldTransform; | ||
192 | |||
193 | Vector3 minAabb; | ||
194 | Vector3 maxAabb; | ||
195 | collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb); | ||
196 | |||
197 | BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType; | ||
198 | collisionObject.Broadphase = Broadphase.CreateProxy( | ||
199 | minAabb, | ||
200 | maxAabb, | ||
201 | type, | ||
202 | collisionObject, | ||
203 | collisionFilterGroup, | ||
204 | collisionFilterMask | ||
205 | ); | ||
206 | } | ||
207 | } | ||
208 | |||
209 | public void AddCollisionObject(CollisionObject collisionObject) | ||
210 | { | ||
211 | AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default); | ||
212 | } | ||
213 | |||
214 | public void RemoveCollisionObject(CollisionObject collisionObject) | ||
215 | { | ||
216 | BroadphaseProxy bp = collisionObject.Broadphase; | ||
217 | if (bp != null) | ||
218 | { | ||
219 | // | ||
220 | // only clear the cached algorithms | ||
221 | // | ||
222 | Broadphase.CleanProxyFromPairs(bp); | ||
223 | Broadphase.DestroyProxy(bp); | ||
224 | collisionObject.Broadphase = null; | ||
225 | } | ||
226 | |||
227 | _collisionObjects.Remove(collisionObject); | ||
228 | } | ||
229 | |||
230 | public virtual void PerformDiscreteCollisionDetection() | ||
231 | { | ||
232 | DispatcherInfo dispatchInfo = DispatchInfo; | ||
233 | //update aabb (of all moved objects) | ||
234 | |||
235 | Vector3 aabbMin, aabbMax; | ||
236 | for (int i = 0; i < _collisionObjects.Count; i++) | ||
237 | { | ||
238 | _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax); | ||
239 | _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax); | ||
240 | } | ||
241 | |||
242 | _broadphasePairCache.RefreshOverlappingPairs(); | ||
243 | |||
244 | IDispatcher dispatcher = Dispatcher; | ||
245 | if (dispatcher != null) | ||
246 | dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo); | ||
247 | } | ||
248 | |||
249 | public void Dispose(bool disposing) | ||
250 | { | ||
251 | if (disposing) | ||
252 | { | ||
253 | //clean up remaining objects | ||
254 | foreach (CollisionObject collisionObject in _collisionObjects) | ||
255 | { | ||
256 | BroadphaseProxy bp = collisionObject.Broadphase; | ||
257 | if (bp != null) | ||
258 | { | ||
259 | // | ||
260 | // only clear the cached algorithms | ||
261 | // | ||
262 | Broadphase.CleanProxyFromPairs(bp); | ||
263 | Broadphase.DestroyProxy(bp); | ||
264 | } | ||
265 | } | ||
266 | } | ||
267 | } | ||
268 | |||
269 | /// <summary> | ||
270 | /// LocalShapeInfo gives extra information for complex shapes | ||
271 | /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart | ||
272 | /// </summary> | ||
273 | public struct LocalShapeInfo | ||
274 | { | ||
275 | private int _shapePart; | ||
276 | private int _triangleIndex; | ||
277 | |||
278 | public int ShapePart { get { return _shapePart; } set { _shapePart = value; } } | ||
279 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | ||
280 | } | ||
281 | |||
282 | public struct LocalRayResult | ||
283 | { | ||
284 | private CollisionObject _collisionObject; | ||
285 | private LocalShapeInfo _localShapeInfo; | ||
286 | private Vector3 _hitNormalLocal; | ||
287 | private float _hitFraction; | ||
288 | |||
289 | public LocalRayResult(CollisionObject collisionObject, | ||
290 | LocalShapeInfo localShapeInfo, | ||
291 | Vector3 hitNormalLocal, | ||
292 | float hitFraction) | ||
293 | { | ||
294 | _collisionObject = collisionObject; | ||
295 | _localShapeInfo = localShapeInfo; | ||
296 | _hitNormalLocal = hitNormalLocal; | ||
297 | _hitFraction = hitFraction; | ||
298 | } | ||
299 | |||
300 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } | ||
301 | public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } } | ||
302 | public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } } | ||
303 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
304 | } | ||
305 | |||
306 | /// <summary> | ||
307 | /// RayResultCallback is used to report new raycast results | ||
308 | /// </summary> | ||
309 | public abstract class RayResultCallback | ||
310 | { | ||
311 | private float _closestHitFraction; | ||
312 | |||
313 | public RayResultCallback() | ||
314 | { | ||
315 | _closestHitFraction = 1; | ||
316 | } | ||
317 | |||
318 | public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } } | ||
319 | public bool HasHit { get { return _closestHitFraction < 1; } } | ||
320 | |||
321 | public abstract float AddSingleResult(LocalRayResult rayResult); | ||
322 | } | ||
323 | |||
324 | public class ClosestRayResultCallback : RayResultCallback | ||
325 | { | ||
326 | private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction | ||
327 | private Vector3 _rayToWorld; | ||
328 | |||
329 | private Vector3 _hitNormalWorld; | ||
330 | private Vector3 _hitPointWorld; | ||
331 | private CollisionObject _collisionObject; | ||
332 | |||
333 | public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld) | ||
334 | { | ||
335 | _rayFromWorld = rayFromWorld; | ||
336 | _rayToWorld = rayToWorld; | ||
337 | _collisionObject = null; | ||
338 | } | ||
339 | |||
340 | public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } } | ||
341 | public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } } | ||
342 | public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } } | ||
343 | public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } } | ||
344 | public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } } | ||
345 | |||
346 | public override float AddSingleResult(LocalRayResult rayResult) | ||
347 | { | ||
348 | //caller already does the filter on the m_closestHitFraction | ||
349 | //assert(rayResult.m_hitFraction <= m_closestHitFraction); | ||
350 | ClosestHitFraction = rayResult.HitFraction; | ||
351 | _collisionObject = rayResult.CollisionObject; | ||
352 | _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform); | ||
353 | MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld); | ||
354 | return rayResult.HitFraction; | ||
355 | } | ||
356 | } | ||
357 | } | ||
358 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs new file mode 100644 index 0000000..3fec7d0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs | |||
@@ -0,0 +1,157 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using System.Diagnostics; | ||
26 | using MonoXnaCompactMaths; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | public class CompoundCollisionAlgorithm : CollisionAlgorithm | ||
31 | { | ||
32 | private List<CollisionAlgorithm> _childCollisionAlgorithms; | ||
33 | private bool _isSwapped; | ||
34 | |||
35 | public CompoundCollisionAlgorithm( | ||
36 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, | ||
37 | CollisionObject bodyA, | ||
38 | CollisionObject bodyB, bool isSwapped) | ||
39 | : base(collisionAlgorithmConstructionInfo) | ||
40 | { | ||
41 | //Begin | ||
42 | _isSwapped = isSwapped; | ||
43 | |||
44 | CollisionObject collisionObject = isSwapped ? bodyB : bodyA; | ||
45 | CollisionObject otherObject = isSwapped ? bodyA : bodyB; | ||
46 | |||
47 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | ||
48 | |||
49 | CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape; | ||
50 | int childrenNumber = compoundShape.ChildShapeCount; | ||
51 | int index = 0; | ||
52 | |||
53 | _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber); | ||
54 | |||
55 | for (index = 0; index < childrenNumber; index++) | ||
56 | { | ||
57 | CollisionShape childShape = compoundShape.GetChildShape(index); | ||
58 | CollisionShape orgShape = collisionObject.CollisionShape; | ||
59 | |||
60 | collisionObject.CollisionShape = childShape; | ||
61 | _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject); | ||
62 | collisionObject.CollisionShape = orgShape; | ||
63 | } | ||
64 | } | ||
65 | |||
66 | public override void ProcessCollision( | ||
67 | CollisionObject bodyA, | ||
68 | CollisionObject bodyB, | ||
69 | DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
70 | { | ||
71 | //Begin | ||
72 | |||
73 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyB; | ||
74 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; | ||
75 | |||
76 | //Debug.Assert(collisionObject.getCollisionShape().isCompound()); | ||
77 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | ||
78 | |||
79 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; | ||
80 | |||
81 | int childrenNumber = _childCollisionAlgorithms.Count; | ||
82 | |||
83 | for (int i = 0; i < childrenNumber; i++) | ||
84 | { | ||
85 | CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape; | ||
86 | |||
87 | Matrix orgTransform = collisionObject.WorldTransform; | ||
88 | CollisionShape orgShape = collisionObject.CollisionShape; | ||
89 | |||
90 | Matrix childTransform = compoundShape.GetChildTransform(i); | ||
91 | Matrix newChildWorld = orgTransform * childTransform; | ||
92 | |||
93 | collisionObject.WorldTransform = newChildWorld; | ||
94 | collisionObject.CollisionShape = childShape; | ||
95 | _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut); | ||
96 | |||
97 | collisionObject.CollisionShape = orgShape; | ||
98 | collisionObject.WorldTransform = orgTransform; | ||
99 | } | ||
100 | } | ||
101 | |||
102 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
103 | { | ||
104 | CollisionObject collisionObject = _isSwapped ? bodyB : bodyA; | ||
105 | CollisionObject otherObject = _isSwapped ? bodyA : bodyB; | ||
106 | |||
107 | BulletDebug.Assert(collisionObject.CollisionShape.IsCompound); | ||
108 | |||
109 | CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape; | ||
110 | |||
111 | float hitFraction = 1.0f; | ||
112 | |||
113 | for (int i = 0; i < _childCollisionAlgorithms.Count; i++) | ||
114 | { | ||
115 | CollisionShape childShape = compoundShape.GetChildShape(i); | ||
116 | |||
117 | Matrix orgTransform = collisionObject.WorldTransform; | ||
118 | CollisionShape orgShape = collisionObject.CollisionShape; | ||
119 | |||
120 | Matrix childTransform = compoundShape.GetChildTransform(i); | ||
121 | Matrix newChildWorld = orgTransform * childTransform; | ||
122 | collisionObject.WorldTransform = newChildWorld; | ||
123 | |||
124 | collisionObject.CollisionShape = childShape; | ||
125 | float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact( | ||
126 | collisionObject, otherObject, dispatchInfo, resultOut | ||
127 | ); | ||
128 | |||
129 | if (frac < hitFraction) | ||
130 | { | ||
131 | hitFraction = frac; | ||
132 | } | ||
133 | |||
134 | collisionObject.CollisionShape = orgShape; | ||
135 | collisionObject.WorldTransform = orgTransform; | ||
136 | } | ||
137 | |||
138 | return hitFraction; | ||
139 | } | ||
140 | |||
141 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
142 | { | ||
143 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
144 | { | ||
145 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | ||
146 | } | ||
147 | }; | ||
148 | |||
149 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction | ||
150 | { | ||
151 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
152 | { | ||
153 | return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | ||
154 | } | ||
155 | }; | ||
156 | } | ||
157 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs new file mode 100644 index 0000000..c4949f0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs | |||
@@ -0,0 +1,189 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm | ||
30 | { | ||
31 | private bool _isSwapped; | ||
32 | private ConvexTriangleCallback _convexTriangleCallback; | ||
33 | |||
34 | public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | ||
35 | : base(collisionAlgorithmConstructionInfo) | ||
36 | { | ||
37 | _isSwapped = isSwapped; | ||
38 | _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); | ||
39 | } | ||
40 | |||
41 | public void ClearCache() | ||
42 | { | ||
43 | _convexTriangleCallback.ClearCache(); | ||
44 | } | ||
45 | |||
46 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
47 | { | ||
48 | CollisionObject convexBody = _isSwapped ? bodyB : bodyA; | ||
49 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | ||
50 | |||
51 | if (triBody.CollisionShape.IsConcave) | ||
52 | { | ||
53 | CollisionObject triOb = triBody; | ||
54 | ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; | ||
55 | |||
56 | if (convexBody.CollisionShape.IsConvex) | ||
57 | { | ||
58 | float collisionMarginTriangle = concaveShape.Margin; | ||
59 | |||
60 | resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); | ||
61 | _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); | ||
62 | |||
63 | //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. | ||
64 | //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); | ||
65 | |||
66 | _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); | ||
67 | concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); | ||
68 | } | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
73 | { | ||
74 | CollisionObject convexbody = _isSwapped ? bodyB : bodyA; | ||
75 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | ||
76 | |||
77 | |||
78 | //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) | ||
79 | |||
80 | //only perform CCD above a certain threshold, this prevents blocking on the long run | ||
81 | //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... | ||
82 | float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); | ||
83 | if (squareMot0 < convexbody.CcdSquareMotionThreshold) | ||
84 | { | ||
85 | return 1; | ||
86 | } | ||
87 | |||
88 | Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); | ||
89 | Matrix convexFromLocal = triInv * convexbody.WorldTransform; | ||
90 | Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; | ||
91 | |||
92 | if (triBody.CollisionShape.IsConcave) | ||
93 | { | ||
94 | Vector3 rayAabbMin = convexFromLocal.Translation; | ||
95 | MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); | ||
96 | Vector3 rayAabbMax = convexFromLocal.Translation; | ||
97 | MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); | ||
98 | float ccdRadius0 = convexbody.CcdSweptSphereRadius; | ||
99 | rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | ||
100 | rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | ||
101 | |||
102 | float curHitFraction = 1f; //is this available? | ||
103 | LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, | ||
104 | convexbody.CcdSweptSphereRadius, curHitFraction); | ||
105 | |||
106 | raycastCallback.HitFraction = convexbody.HitFraction; | ||
107 | |||
108 | CollisionObject concavebody = triBody; | ||
109 | |||
110 | ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; | ||
111 | |||
112 | if (triangleMesh != null) | ||
113 | { | ||
114 | triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); | ||
115 | } | ||
116 | |||
117 | if (raycastCallback.HitFraction < convexbody.HitFraction) | ||
118 | { | ||
119 | convexbody.HitFraction = raycastCallback.HitFraction; | ||
120 | return raycastCallback.HitFraction; | ||
121 | } | ||
122 | } | ||
123 | |||
124 | return 1; | ||
125 | } | ||
126 | |||
127 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
128 | { | ||
129 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
130 | { | ||
131 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | ||
132 | } | ||
133 | } | ||
134 | |||
135 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction | ||
136 | { | ||
137 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
138 | { | ||
139 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | ||
140 | } | ||
141 | } | ||
142 | |||
143 | private class LocalTriangleSphereCastCallback : ITriangleCallback | ||
144 | { | ||
145 | private Matrix _ccdSphereFromTrans; | ||
146 | private Matrix _ccdSphereToTrans; | ||
147 | private Matrix _meshTransform; | ||
148 | |||
149 | private float _ccdSphereRadius; | ||
150 | private float _hitFraction; | ||
151 | |||
152 | public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) | ||
153 | { | ||
154 | _ccdSphereFromTrans = from; | ||
155 | _ccdSphereToTrans = to; | ||
156 | _ccdSphereRadius = ccdSphereRadius; | ||
157 | _hitFraction = hitFraction; | ||
158 | } | ||
159 | |||
160 | public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } | ||
161 | public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } | ||
162 | public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } | ||
163 | public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } | ||
164 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
165 | |||
166 | public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) | ||
167 | { | ||
168 | //do a swept sphere for now | ||
169 | Matrix ident = Matrix.Identity; | ||
170 | CastResult castResult = new CastResult(); | ||
171 | castResult.Fraction = _hitFraction; | ||
172 | SphereShape pointShape = new SphereShape(_ccdSphereRadius); | ||
173 | TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); | ||
174 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); | ||
175 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); | ||
176 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); | ||
177 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); | ||
178 | //local space? | ||
179 | |||
180 | if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, | ||
181 | ident, ident, castResult)) | ||
182 | { | ||
183 | if (_hitFraction > castResult.Fraction) | ||
184 | _hitFraction = castResult.Fraction; | ||
185 | } | ||
186 | } | ||
187 | } | ||
188 | } | ||
189 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs new file mode 100644 index 0000000..8fa4837 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs | |||
@@ -0,0 +1,193 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable | ||
29 | { | ||
30 | private const bool DisableCcd = false; | ||
31 | private GjkPairDetector _gjkPairDetector; | ||
32 | private bool _ownManifold; | ||
33 | private PersistentManifold _manifold; | ||
34 | private bool _lowLevelOfDetail; | ||
35 | |||
36 | public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
37 | : base(collisionAlgorithmConstructionInfo) | ||
38 | { | ||
39 | _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver); | ||
40 | _ownManifold = false; | ||
41 | _manifold = manifold; | ||
42 | _lowLevelOfDetail = false; | ||
43 | } | ||
44 | |||
45 | public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } } | ||
46 | public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } } | ||
47 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | ||
48 | |||
49 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
50 | { | ||
51 | if (_manifold == null) | ||
52 | { | ||
53 | //swapped? | ||
54 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | ||
55 | _ownManifold = true; | ||
56 | } | ||
57 | resultOut.SetPersistentManifold(_manifold); | ||
58 | |||
59 | ConvexShape min0 = bodyA.CollisionShape as ConvexShape; | ||
60 | ConvexShape min1 = bodyB.CollisionShape as ConvexShape; | ||
61 | |||
62 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
63 | |||
64 | //TODO: if (dispatchInfo.m_useContinuous) | ||
65 | _gjkPairDetector.setMinkowskiA(min0); | ||
66 | _gjkPairDetector.setMinkowskiB(min1); | ||
67 | input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold; | ||
68 | input.MaximumDistanceSquared *= input.MaximumDistanceSquared; | ||
69 | |||
70 | // input.m_maximumDistanceSquared = 1e30f; | ||
71 | |||
72 | input.TransformA = bodyA.WorldTransform; | ||
73 | input.TransformB = bodyB.WorldTransform; | ||
74 | |||
75 | _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw); | ||
76 | } | ||
77 | |||
78 | public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
79 | { | ||
80 | //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold | ||
81 | |||
82 | //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold | ||
83 | //col0->m_worldTransform, | ||
84 | float resultFraction = 1f; | ||
85 | |||
86 | float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared(); | ||
87 | float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared(); | ||
88 | |||
89 | if (squareMotA < colA.CcdSquareMotionThreshold && | ||
90 | squareMotB < colB.CcdSquareMotionThreshold) | ||
91 | return resultFraction; | ||
92 | |||
93 | if (DisableCcd) | ||
94 | return 1f; | ||
95 | |||
96 | //An adhoc way of testing the Continuous Collision Detection algorithms | ||
97 | //One object is approximated as a sphere, to simplify things | ||
98 | //Starting in penetration should report no time of impact | ||
99 | //For proper CCD, better accuracy and handling of 'allowed' penetration should be added | ||
100 | //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) | ||
101 | |||
102 | // Convex0 against sphere for Convex1 | ||
103 | { | ||
104 | ConvexShape convexA = colA.CollisionShape as ConvexShape; | ||
105 | |||
106 | SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation | ||
107 | CastResult result = new CastResult(); | ||
108 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); | ||
109 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); | ||
110 | //Simplification, one object is simplified as a sphere | ||
111 | GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex); | ||
112 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); | ||
113 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, | ||
114 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) | ||
115 | { | ||
116 | //store result.m_fraction in both bodies | ||
117 | if (colA.HitFraction > result.Fraction) | ||
118 | colA.HitFraction = result.Fraction; | ||
119 | |||
120 | if (colB.HitFraction > result.Fraction) | ||
121 | colB.HitFraction = result.Fraction; | ||
122 | |||
123 | if (resultFraction > result.Fraction) | ||
124 | resultFraction = result.Fraction; | ||
125 | } | ||
126 | } | ||
127 | |||
128 | // Sphere (for convex0) against Convex1 | ||
129 | { | ||
130 | ConvexShape convexB = colB.CollisionShape as ConvexShape; | ||
131 | |||
132 | SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation | ||
133 | CastResult result = new CastResult(); | ||
134 | VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); | ||
135 | //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); | ||
136 | ///Simplification, one object is simplified as a sphere | ||
137 | GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex); | ||
138 | //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); | ||
139 | if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform, | ||
140 | colB.WorldTransform, colB.InterpolationWorldTransform, result)) | ||
141 | { | ||
142 | //store result.m_fraction in both bodies | ||
143 | if (colA.HitFraction > result.Fraction) | ||
144 | colA.HitFraction = result.Fraction; | ||
145 | |||
146 | if (colB.HitFraction > result.Fraction) | ||
147 | colB.HitFraction = result.Fraction; | ||
148 | |||
149 | if (resultFraction > result.Fraction) | ||
150 | resultFraction = result.Fraction; | ||
151 | } | ||
152 | } | ||
153 | return resultFraction; | ||
154 | } | ||
155 | |||
156 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
157 | { | ||
158 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | ||
159 | private ISimplexSolver _simplexSolver; | ||
160 | //private bool _ownsSolvers; | ||
161 | |||
162 | public CreateFunc() | ||
163 | { | ||
164 | //_ownsSolvers = true; | ||
165 | _simplexSolver = new VoronoiSimplexSolver(); | ||
166 | _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver(); | ||
167 | } | ||
168 | |||
169 | public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
170 | { | ||
171 | //_ownsSolvers = false; | ||
172 | _simplexSolver = simplexSolver; | ||
173 | _penetrationDepthSolver = penetrationDepthSolver; | ||
174 | } | ||
175 | |||
176 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
177 | { | ||
178 | return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver); | ||
179 | } | ||
180 | } | ||
181 | |||
182 | #region IDisposable Members | ||
183 | public void Dispose() | ||
184 | { | ||
185 | if (_ownManifold) | ||
186 | { | ||
187 | if (_manifold != null) | ||
188 | Dispatcher.ReleaseManifold(_manifold); | ||
189 | } | ||
190 | } | ||
191 | #endregion | ||
192 | } | ||
193 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs new file mode 100644 index 0000000..5355817 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs | |||
@@ -0,0 +1,130 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class ConvexTriangleCallback : ITriangleCallback, IDisposable | ||
30 | { | ||
31 | private CollisionObject _convexBody; | ||
32 | private CollisionObject _triBody; | ||
33 | |||
34 | private Vector3 _aabbMin; | ||
35 | private Vector3 _aabbMax; | ||
36 | |||
37 | private ManifoldResult _resultOut; | ||
38 | |||
39 | private IDispatcher _dispatcher; | ||
40 | private DispatcherInfo _dispatchInfo; | ||
41 | private float _collisionMarginTriangle; | ||
42 | |||
43 | private int _triangleCount; | ||
44 | |||
45 | private PersistentManifold _manifold; | ||
46 | |||
47 | public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | ||
48 | { | ||
49 | _dispatcher = dispatcher; | ||
50 | _dispatchInfo = null; | ||
51 | _convexBody = isSwapped ? bodyB : bodyA; | ||
52 | _triBody = isSwapped ? bodyA : bodyB; | ||
53 | |||
54 | // create the manifold from the dispatcher 'manifold pool' | ||
55 | _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody); | ||
56 | ClearCache(); | ||
57 | } | ||
58 | |||
59 | public Vector3 AabbMin { get { return _aabbMin; } } | ||
60 | public Vector3 AabbMax { get { return _aabbMax; } } | ||
61 | public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } } | ||
62 | public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } } | ||
63 | |||
64 | public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
65 | { | ||
66 | _dispatchInfo = dispatchInfo; | ||
67 | _collisionMarginTriangle = collisionMarginTriangle; | ||
68 | _resultOut = resultOut; | ||
69 | |||
70 | //recalc aabbs | ||
71 | Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform; | ||
72 | CollisionShape convexShape = _convexBody.CollisionShape; | ||
73 | //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); | ||
74 | convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax); | ||
75 | float extraMargin = collisionMarginTriangle; | ||
76 | Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin); | ||
77 | |||
78 | _aabbMax += extra; | ||
79 | _aabbMin -= extra; | ||
80 | } | ||
81 | |||
82 | public void ClearCache() | ||
83 | { | ||
84 | _dispatcher.ClearManifold(_manifold); | ||
85 | } | ||
86 | |||
87 | #region ITriangleCallback Members | ||
88 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
89 | { | ||
90 | //aabb filter is already applied! | ||
91 | CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo(); | ||
92 | collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher; | ||
93 | |||
94 | CollisionObject collisionObject = _triBody; | ||
95 | |||
96 | //debug drawing of the overlapping triangles | ||
97 | /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) | ||
98 | { | ||
99 | Vector3 color = new Vector3(255, 255, 0); | ||
100 | btTransform & tr = ob->WorldTransform; | ||
101 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color); | ||
102 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color); | ||
103 | m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color); | ||
104 | }*/ | ||
105 | |||
106 | if (_convexBody.CollisionShape.IsConvex) | ||
107 | { | ||
108 | TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); | ||
109 | triangleShape.Margin=_collisionMarginTriangle; | ||
110 | |||
111 | CollisionShape tempShape = collisionObject.CollisionShape; | ||
112 | collisionObject.CollisionShape = triangleShape; | ||
113 | |||
114 | CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold); | ||
115 | |||
116 | _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex); | ||
117 | collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut); | ||
118 | collisionObject.CollisionShape = tempShape; | ||
119 | } | ||
120 | } | ||
121 | #endregion | ||
122 | #region IDisposable Members | ||
123 | public void Dispose() | ||
124 | { | ||
125 | ClearCache(); | ||
126 | _dispatcher.ReleaseManifold(_manifold); | ||
127 | } | ||
128 | #endregion | ||
129 | } | ||
130 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs new file mode 100644 index 0000000..13cb3d9 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs | |||
@@ -0,0 +1,52 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | /// <summary> | ||
29 | /// EmptyAlgorithm is a stub for unsupported collision pairs. | ||
30 | /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. | ||
31 | /// </summary> | ||
32 | public class EmptyAlgorithm : CollisionAlgorithm | ||
33 | { | ||
34 | public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
35 | : base(collisionAlgorithmConstructionInfo) { } | ||
36 | |||
37 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { } | ||
38 | |||
39 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
40 | { | ||
41 | return 1f; | ||
42 | } | ||
43 | |||
44 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
45 | { | ||
46 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
47 | { | ||
48 | return new EmptyAlgorithm(collisionAlgorithmConstructionInfo); | ||
49 | } | ||
50 | }; | ||
51 | } | ||
52 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs new file mode 100644 index 0000000..63f37ea --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs | |||
@@ -0,0 +1,147 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB); | ||
30 | |||
31 | public class ManifoldResult : DiscreteCollisionDetectorInterface.Result | ||
32 | { | ||
33 | private PersistentManifold _manifold; | ||
34 | private static ContactAddedCallback _contactAddedCallback = null; | ||
35 | |||
36 | //we need this for compounds | ||
37 | private Matrix _rootTransA; | ||
38 | private Matrix _rootTransB; | ||
39 | |||
40 | private CollisionObject _bodyA; | ||
41 | private CollisionObject _bodyB; | ||
42 | private int _partIdA; | ||
43 | private int _partIdB; | ||
44 | private int _indexA; | ||
45 | private int _indexB; | ||
46 | |||
47 | public ManifoldResult() | ||
48 | { | ||
49 | } | ||
50 | |||
51 | public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB) | ||
52 | { | ||
53 | _bodyA = bodyA; | ||
54 | _bodyB = bodyB; | ||
55 | _rootTransA = bodyA.WorldTransform; | ||
56 | _rootTransB = bodyB.WorldTransform; | ||
57 | } | ||
58 | |||
59 | public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } } | ||
60 | |||
61 | public void SetPersistentManifold(PersistentManifold manifold) | ||
62 | { | ||
63 | _manifold = manifold; | ||
64 | } | ||
65 | |||
66 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) | ||
67 | { | ||
68 | _partIdA = partIdA; | ||
69 | _partIdB = partIdB; | ||
70 | _indexA = indexA; | ||
71 | _indexB = indexB; | ||
72 | } | ||
73 | |||
74 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
75 | { | ||
76 | if (_manifold == null) | ||
77 | throw new BulletException("Manifold Pointer is null."); | ||
78 | |||
79 | //order in manifold needs to match | ||
80 | |||
81 | if (depth > PersistentManifold.ContactBreakingThreshold) | ||
82 | return; | ||
83 | |||
84 | bool isSwapped = _manifold.BodyA != _bodyA; | ||
85 | |||
86 | Vector3 pointA = pointInWorld + normalOnBInWorld * depth; | ||
87 | Vector3 localA; | ||
88 | Vector3 localB; | ||
89 | |||
90 | if (isSwapped) | ||
91 | { | ||
92 | localA = MathHelper.InvXForm(_rootTransB, pointA); | ||
93 | localB = MathHelper.InvXForm(_rootTransA, pointInWorld); | ||
94 | } | ||
95 | else | ||
96 | { | ||
97 | localA = MathHelper.InvXForm(_rootTransA, pointA); | ||
98 | localB = MathHelper.InvXForm(_rootTransB, pointInWorld); | ||
99 | } | ||
100 | |||
101 | ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth); | ||
102 | |||
103 | int insertIndex = _manifold.GetCacheEntry(newPt); | ||
104 | |||
105 | newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB); | ||
106 | newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB); | ||
107 | |||
108 | //User can override friction and/or restitution | ||
109 | if (_contactAddedCallback != null && | ||
110 | //and if either of the two bodies requires custom material | ||
111 | ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 || | ||
112 | (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0)) | ||
113 | { | ||
114 | //experimental feature info, for per-triangle material etc. | ||
115 | CollisionObject obj0 = isSwapped ? _bodyB : _bodyA; | ||
116 | CollisionObject obj1 = isSwapped ? _bodyA : _bodyB; | ||
117 | _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB); | ||
118 | } | ||
119 | |||
120 | if (insertIndex >= 0) | ||
121 | { | ||
122 | _manifold.ReplaceContactPoint(newPt, insertIndex); | ||
123 | } | ||
124 | else | ||
125 | { | ||
126 | _manifold.AddManifoldPoint(newPt); | ||
127 | } | ||
128 | } | ||
129 | |||
130 | private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB) | ||
131 | { | ||
132 | float friction = bodyA.Friction * bodyB.Friction; | ||
133 | |||
134 | float MaxFriction = 10; | ||
135 | if (friction < -MaxFriction) | ||
136 | friction = -MaxFriction; | ||
137 | if (friction > MaxFriction) | ||
138 | friction = MaxFriction; | ||
139 | return friction; | ||
140 | } | ||
141 | |||
142 | private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB) | ||
143 | { | ||
144 | return bodyA.Restitution * bodyB.Restitution; | ||
145 | } | ||
146 | } | ||
147 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs new file mode 100644 index 0000000..957843b --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs | |||
@@ -0,0 +1,304 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SimulationIslandManager | ||
30 | { | ||
31 | private UnionFind _unionFind = new UnionFind(); | ||
32 | |||
33 | public void InitUnionFind(int n) | ||
34 | { | ||
35 | _unionFind.Reset(n); | ||
36 | } | ||
37 | |||
38 | public UnionFind UnionFind { get { return _unionFind; } } | ||
39 | |||
40 | public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher) | ||
41 | { | ||
42 | InitUnionFind(world.CollisionObjectsCount); | ||
43 | |||
44 | // put the index into m_controllers into m_tag | ||
45 | int index = 0; | ||
46 | for (int i = 0; i < world.CollisionObjects.Count; i++) | ||
47 | { | ||
48 | world.CollisionObjects[i].IslandTag = index; | ||
49 | world.CollisionObjects[i].HitFraction = 1; | ||
50 | world.CollisionObjects[i].CompanionID = -1; | ||
51 | index++; | ||
52 | } | ||
53 | // do the union find | ||
54 | FindUnions(dispatcher); | ||
55 | } | ||
56 | |||
57 | public virtual void StoreIslandActivationState(CollisionWorld world) | ||
58 | { | ||
59 | // put the islandId ('find' value) into m_tag | ||
60 | int index = 0; | ||
61 | for (int i = 0; i < world.CollisionObjects.Count; i++) | ||
62 | { | ||
63 | if (world.CollisionObjects[i].MergesSimulationIslands) | ||
64 | { | ||
65 | world.CollisionObjects[i].IslandTag = _unionFind.Find(index); | ||
66 | world.CollisionObjects[i].CompanionID = -1; | ||
67 | } | ||
68 | else | ||
69 | { | ||
70 | world.CollisionObjects[i].IslandTag = -1; | ||
71 | world.CollisionObjects[i].CompanionID = -2; | ||
72 | } | ||
73 | index++; | ||
74 | } | ||
75 | } | ||
76 | |||
77 | public void FindUnions(IDispatcher dispatcher) | ||
78 | { | ||
79 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | ||
80 | { | ||
81 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | ||
82 | //static objects (invmass 0.f) don't merge ! | ||
83 | |||
84 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | ||
85 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | ||
86 | |||
87 | if (((colObjA != null) && (colObjA.MergesSimulationIslands)) && | ||
88 | ((colObjB != null) && (colObjB.MergesSimulationIslands))) | ||
89 | { | ||
90 | _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag); | ||
91 | } | ||
92 | } | ||
93 | } | ||
94 | |||
95 | public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback) | ||
96 | { | ||
97 | //we are going to sort the unionfind array, and store the element id in the size | ||
98 | //afterwards, we clean unionfind, to make sure no-one uses it anymore | ||
99 | UnionFind.SortIslands(); | ||
100 | int numElem = UnionFind.ElementCount; | ||
101 | |||
102 | int endIslandIndex = 1; | ||
103 | int startIslandIndex; | ||
104 | |||
105 | //update the sleeping state for bodies, if all are sleeping | ||
106 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | ||
107 | { | ||
108 | int islandId = UnionFind[startIslandIndex].ID; | ||
109 | for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | ||
110 | { | ||
111 | } | ||
112 | |||
113 | //int numSleeping = 0; | ||
114 | |||
115 | bool allSleeping = true; | ||
116 | |||
117 | int idx; | ||
118 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
119 | { | ||
120 | int i = UnionFind[idx].Size; | ||
121 | |||
122 | CollisionObject colObjA = collisionObjects[i]; | ||
123 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
124 | { | ||
125 | Console.WriteLine("error in island management"); | ||
126 | } | ||
127 | |||
128 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
129 | if (colObjA.IslandTag == islandId) | ||
130 | { | ||
131 | if (colObjA.ActivationState == ActivationState.Active) | ||
132 | { | ||
133 | allSleeping = false; | ||
134 | } | ||
135 | if (colObjA.ActivationState == ActivationState.DisableDeactivation) | ||
136 | { | ||
137 | allSleeping = false; | ||
138 | } | ||
139 | } | ||
140 | } | ||
141 | |||
142 | |||
143 | if (allSleeping) | ||
144 | { | ||
145 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
146 | { | ||
147 | int i = UnionFind[idx].Size; | ||
148 | CollisionObject colObjA = collisionObjects[i]; | ||
149 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
150 | { | ||
151 | Console.WriteLine("error in island management"); | ||
152 | } | ||
153 | |||
154 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
155 | |||
156 | if (colObjA.IslandTag == islandId) | ||
157 | { | ||
158 | colObjA.ActivationState =ActivationState.IslandSleeping; | ||
159 | } | ||
160 | } | ||
161 | } | ||
162 | else | ||
163 | { | ||
164 | for (idx = startIslandIndex; idx < endIslandIndex; idx++) | ||
165 | { | ||
166 | int i = UnionFind[idx].Size; | ||
167 | |||
168 | CollisionObject colObjA = collisionObjects[i]; | ||
169 | if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1)) | ||
170 | { | ||
171 | Console.WriteLine("error in island management"); | ||
172 | } | ||
173 | |||
174 | BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1)); | ||
175 | |||
176 | if (colObjA.IslandTag == islandId) | ||
177 | { | ||
178 | if (colObjA.ActivationState == ActivationState.IslandSleeping) | ||
179 | { | ||
180 | colObjA.ActivationState = ActivationState.WantsDeactivation; | ||
181 | } | ||
182 | } | ||
183 | } | ||
184 | } | ||
185 | } | ||
186 | |||
187 | //int maxNumManifolds = dispatcher.ManifoldCount; | ||
188 | List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount); | ||
189 | |||
190 | for (int i = 0; i < dispatcher.ManifoldCount; i++) | ||
191 | { | ||
192 | PersistentManifold manifold = dispatcher.GetManifoldByIndex(i); | ||
193 | |||
194 | CollisionObject colObjA = manifold.BodyA as CollisionObject; | ||
195 | CollisionObject colObjB = manifold.BodyB as CollisionObject; | ||
196 | |||
197 | //todo: check sleeping conditions! | ||
198 | if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) || | ||
199 | ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping)) | ||
200 | { | ||
201 | |||
202 | //kinematic objects don't merge islands, but wake up all connected objects | ||
203 | if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping) | ||
204 | { | ||
205 | colObjB.Activate(); | ||
206 | } | ||
207 | if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping) | ||
208 | { | ||
209 | colObjA.Activate(); | ||
210 | } | ||
211 | |||
212 | //filtering for response | ||
213 | if (dispatcher.NeedsResponse(colObjA, colObjB)) | ||
214 | islandmanifold.Add(manifold); | ||
215 | } | ||
216 | } | ||
217 | |||
218 | int numManifolds = islandmanifold.Count; | ||
219 | |||
220 | // Sort manifolds, based on islands | ||
221 | // Sort the vector using predicate and std::sort | ||
222 | islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate)); | ||
223 | |||
224 | //now process all active islands (sets of manifolds for now) | ||
225 | int startManifoldIndex = 0; | ||
226 | int endManifoldIndex = 1; | ||
227 | |||
228 | List<CollisionObject> islandBodies = new List<CollisionObject>(); | ||
229 | |||
230 | for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) | ||
231 | { | ||
232 | int islandId = UnionFind[startIslandIndex].ID; | ||
233 | bool islandSleeping = false; | ||
234 | for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++) | ||
235 | { | ||
236 | int i = UnionFind[endIslandIndex].Size; | ||
237 | CollisionObject colObjA = collisionObjects[i]; | ||
238 | islandBodies.Add(colObjA); | ||
239 | if (!colObjA.IsActive) | ||
240 | islandSleeping = true; | ||
241 | } | ||
242 | |||
243 | //find the accompanying contact manifold for this islandId | ||
244 | int numIslandManifolds = 0; | ||
245 | List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds); | ||
246 | |||
247 | if (startManifoldIndex < numManifolds) | ||
248 | { | ||
249 | int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]); | ||
250 | if (curIslandID == islandId) | ||
251 | { | ||
252 | for (int k = startManifoldIndex; k < islandmanifold.Count; k++) | ||
253 | { | ||
254 | startManifold.Add(islandmanifold[k]); | ||
255 | } | ||
256 | for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { } | ||
257 | |||
258 | // Process the actual simulation, only if not sleeping/deactivated | ||
259 | numIslandManifolds = endManifoldIndex - startManifoldIndex; | ||
260 | } | ||
261 | } | ||
262 | |||
263 | if (!islandSleeping) | ||
264 | { | ||
265 | callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId); | ||
266 | } | ||
267 | |||
268 | if (numIslandManifolds != 0) | ||
269 | { | ||
270 | startManifoldIndex = endManifoldIndex; | ||
271 | } | ||
272 | |||
273 | islandBodies.Clear(); | ||
274 | } | ||
275 | } | ||
276 | |||
277 | private static int GetIslandId(PersistentManifold lhs) | ||
278 | { | ||
279 | int islandId; | ||
280 | CollisionObject rcolObjA = lhs.BodyA as CollisionObject; | ||
281 | CollisionObject rcolObjB = lhs.BodyB as CollisionObject; | ||
282 | islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag; | ||
283 | return islandId; | ||
284 | } | ||
285 | |||
286 | private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs) | ||
287 | { | ||
288 | int rIslandIdA, lIslandIdB; | ||
289 | rIslandIdA = GetIslandId(rhs); | ||
290 | lIslandIdB = GetIslandId(lhs); | ||
291 | //return lIslandId0 < rIslandId0; | ||
292 | if (lIslandIdB < rIslandIdA) | ||
293 | return -1; | ||
294 | //else if (lIslandIdB > rIslandIdA) | ||
295 | // return 1; | ||
296 | return 1; | ||
297 | } | ||
298 | |||
299 | public interface IIslandCallback | ||
300 | { | ||
301 | void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID); | ||
302 | } | ||
303 | } | ||
304 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs new file mode 100644 index 0000000..8933638 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs | |||
@@ -0,0 +1,270 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// SphereBoxCollisionAlgorithm provides sphere-box collision detection. | ||
31 | /// Other features are frame-coherency (persistent data) and collision response. | ||
32 | /// </summary> | ||
33 | public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable | ||
34 | { | ||
35 | private bool _ownManifold; | ||
36 | private PersistentManifold _manifold; | ||
37 | private bool _isSwapped; | ||
38 | |||
39 | public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped) | ||
40 | : base(collisionAlgorithmConstructionInfo) | ||
41 | { | ||
42 | _ownManifold = false; | ||
43 | _manifold = manifold; | ||
44 | _isSwapped = isSwapped; | ||
45 | |||
46 | CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA; | ||
47 | CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB; | ||
48 | |||
49 | if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject)) | ||
50 | { | ||
51 | _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject); | ||
52 | _ownManifold = true; | ||
53 | } | ||
54 | } | ||
55 | |||
56 | public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius) | ||
57 | { | ||
58 | pointOnBox = new Vector3(); | ||
59 | pointOnSphere = new Vector3(); | ||
60 | |||
61 | float margins; | ||
62 | Vector3[] bounds = new Vector3[2]; | ||
63 | BoxShape boxShape = boxObject.CollisionShape as BoxShape; | ||
64 | |||
65 | bounds[0] = -boxShape.HalfExtents; | ||
66 | bounds[1] = boxShape.HalfExtents; | ||
67 | |||
68 | margins = boxShape.Margin; //also add sphereShape margin? | ||
69 | |||
70 | Matrix m44T = boxObject.WorldTransform; | ||
71 | |||
72 | Vector3[] boundsVec = new Vector3[2]; | ||
73 | float penetration; | ||
74 | |||
75 | boundsVec[0] = bounds[0]; | ||
76 | boundsVec[1] = bounds[1]; | ||
77 | |||
78 | Vector3 marginsVec = new Vector3(margins, margins, margins); | ||
79 | |||
80 | // add margins | ||
81 | bounds[0] += marginsVec; | ||
82 | bounds[1] -= marginsVec; | ||
83 | |||
84 | ///////////////////////////////////////////////// | ||
85 | |||
86 | Vector3 tmp, prel, normal, v3P; | ||
87 | Vector3[] n = new Vector3[6]; | ||
88 | float sep = 10000000.0f, sepThis; | ||
89 | |||
90 | n[0] = new Vector3(-1.0f, 0.0f, 0.0f); | ||
91 | n[1] = new Vector3(0.0f, -1.0f, 0.0f); | ||
92 | n[2] = new Vector3(0.0f, 0.0f, -1.0f); | ||
93 | n[3] = new Vector3(1.0f, 0.0f, 0.0f); | ||
94 | n[4] = new Vector3(0.0f, 1.0f, 0.0f); | ||
95 | n[5] = new Vector3(0.0f, 0.0f, 1.0f); | ||
96 | |||
97 | // convert point in local space | ||
98 | prel = MathHelper.InvXForm(m44T, sphereCenter); | ||
99 | |||
100 | bool found = false; | ||
101 | |||
102 | v3P = prel; | ||
103 | |||
104 | for (int i = 0; i < 6; i++) | ||
105 | { | ||
106 | int j = i < 3 ? 0 : 1; | ||
107 | if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f) | ||
108 | { | ||
109 | v3P = v3P - n[i] * sepThis; | ||
110 | found = true; | ||
111 | } | ||
112 | } | ||
113 | |||
114 | // | ||
115 | |||
116 | if (found) | ||
117 | { | ||
118 | bounds[0] = boundsVec[0]; | ||
119 | bounds[1] = boundsVec[1]; | ||
120 | |||
121 | normal = Vector3.Normalize(prel - v3P); | ||
122 | pointOnBox = v3P + normal * margins; | ||
123 | pointOnSphere = prel - normal * radius; | ||
124 | |||
125 | if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f) | ||
126 | { | ||
127 | return 1.0f; | ||
128 | } | ||
129 | |||
130 | // transform back in world space | ||
131 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); | ||
132 | pointOnBox = tmp; | ||
133 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); | ||
134 | pointOnSphere = tmp; | ||
135 | float seps2 = (pointOnBox - pointOnSphere).LengthSquared(); | ||
136 | |||
137 | //if this fails, fallback into deeper penetration case, below | ||
138 | if (seps2 > MathHelper.Epsilon) | ||
139 | { | ||
140 | sep = -(float)Math.Sqrt(seps2); | ||
141 | normal = (pointOnBox - pointOnSphere); | ||
142 | normal *= 1f / sep; | ||
143 | } | ||
144 | return sep; | ||
145 | } | ||
146 | |||
147 | ////////////////////////////////////////////////// | ||
148 | // Deep penetration case | ||
149 | |||
150 | penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]); | ||
151 | |||
152 | bounds[0] = boundsVec[0]; | ||
153 | bounds[1] = boundsVec[1]; | ||
154 | |||
155 | if (penetration <= 0.0f) | ||
156 | return (penetration - margins); | ||
157 | else | ||
158 | return 1.0f; | ||
159 | } | ||
160 | |||
161 | public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax) | ||
162 | { | ||
163 | Vector3[] bounds = new Vector3[2]; | ||
164 | |||
165 | bounds[0] = aabbMin; | ||
166 | bounds[1] = aabbMax; | ||
167 | |||
168 | Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3(); | ||
169 | Vector3[] n = new Vector3[6]; | ||
170 | float sep = -10000000.0f, sepThis; | ||
171 | |||
172 | n[0] = new Vector3(-1.0f, 0.0f, 0.0f); | ||
173 | n[1] = new Vector3(0.0f, -1.0f, 0.0f); | ||
174 | n[2] = new Vector3(0.0f, 0.0f, -1.0f); | ||
175 | n[3] = new Vector3(1.0f, 0.0f, 0.0f); | ||
176 | n[4] = new Vector3(0.0f, 1.0f, 0.0f); | ||
177 | n[5] = new Vector3(0.0f, 0.0f, 1.0f); | ||
178 | |||
179 | Matrix m44T = boxObject.WorldTransform; | ||
180 | |||
181 | // convert point in local space | ||
182 | prel = MathHelper.InvXForm(m44T, sphereCenter); | ||
183 | |||
184 | /////////// | ||
185 | |||
186 | for (int i = 0; i < 6; i++) | ||
187 | { | ||
188 | int j = i < 3 ? 0 : 1; | ||
189 | if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f; | ||
190 | if (sepThis > sep) | ||
191 | { | ||
192 | p0 = bounds[j]; | ||
193 | normal = n[i]; | ||
194 | sep = sepThis; | ||
195 | } | ||
196 | } | ||
197 | |||
198 | pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0))); | ||
199 | pointOnSphere = pointOnBox + normal * sep; | ||
200 | |||
201 | // transform back in world space | ||
202 | tmp = MathHelper.MatrixToVector(m44T, pointOnBox); | ||
203 | pointOnBox = tmp; | ||
204 | tmp = MathHelper.MatrixToVector(m44T, pointOnSphere); | ||
205 | pointOnSphere = tmp; | ||
206 | normal = Vector3.Normalize(pointOnBox - pointOnSphere); | ||
207 | |||
208 | return sep; | ||
209 | } | ||
210 | |||
211 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
212 | { | ||
213 | if (_manifold == null) | ||
214 | return; | ||
215 | |||
216 | CollisionObject sphereObject = _isSwapped ? bodyB : bodyA; | ||
217 | CollisionObject boxObject = _isSwapped ? bodyA : bodyB; | ||
218 | |||
219 | SphereShape sphereA = sphereObject.CollisionShape as SphereShape; | ||
220 | |||
221 | Vector3 pOnBox, pOnSphere; | ||
222 | Vector3 sphereCenter = sphereObject.WorldTransform.Translation; | ||
223 | float radius = sphereA.Radius; | ||
224 | |||
225 | float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius); | ||
226 | |||
227 | if (dist < MathHelper.Epsilon) | ||
228 | { | ||
229 | Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere); | ||
230 | |||
231 | // report a contact. internally this will be kept persistent, and contact reduction is done | ||
232 | resultOut.SetPersistentManifold(_manifold); | ||
233 | resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist); | ||
234 | } | ||
235 | } | ||
236 | |||
237 | public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
238 | { | ||
239 | //not yet | ||
240 | return 1; | ||
241 | } | ||
242 | |||
243 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
244 | { | ||
245 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
246 | { | ||
247 | if (!IsSwapped) | ||
248 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | ||
249 | else | ||
250 | return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | ||
251 | } | ||
252 | } | ||
253 | |||
254 | #region IDisposable Members | ||
255 | public void Dispose() | ||
256 | { | ||
257 | Dispose(true); | ||
258 | } | ||
259 | |||
260 | public void Dispose(bool disposing) | ||
261 | { | ||
262 | if (disposing && _ownManifold) | ||
263 | { | ||
264 | if (_manifold != null) | ||
265 | Dispatcher.ReleaseManifold(_manifold); | ||
266 | } | ||
267 | } | ||
268 | #endregion | ||
269 | } | ||
270 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs new file mode 100644 index 0000000..7a76d24 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs | |||
@@ -0,0 +1,104 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SphereSphereCollisionAlgorithm : CollisionAlgorithm | ||
30 | { | ||
31 | private bool _ownManifold; | ||
32 | private PersistentManifold _manifold; | ||
33 | |||
34 | public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
35 | : base(collisionAlgorithmConstructionInfo) | ||
36 | { | ||
37 | _ownManifold = false; | ||
38 | _manifold = manifold; | ||
39 | |||
40 | if (_manifold == null) | ||
41 | { | ||
42 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | ||
43 | _ownManifold = true; | ||
44 | } | ||
45 | } | ||
46 | |||
47 | public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
48 | : base(collisionAlgorithmConstructionInfo) { } | ||
49 | |||
50 | ~SphereSphereCollisionAlgorithm() | ||
51 | { | ||
52 | if (_ownManifold) | ||
53 | { | ||
54 | if (_manifold != null) | ||
55 | Dispatcher.ReleaseManifold(_manifold); | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
60 | { | ||
61 | if (_manifold == null) | ||
62 | return; | ||
63 | |||
64 | SphereShape sphereA = bodyA.CollisionShape as SphereShape; | ||
65 | SphereShape sphereB = bodyB.CollisionShape as SphereShape; | ||
66 | |||
67 | Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation; | ||
68 | float len = diff.Length(); | ||
69 | float radiusA = sphereA.Radius; | ||
70 | float radiusB = sphereB.Radius; | ||
71 | |||
72 | //if distance positive, don't generate a new contact | ||
73 | if (len > (radiusA + radiusB)) | ||
74 | return; | ||
75 | |||
76 | //distance (negative means penetration) | ||
77 | float dist = len - (radiusA + radiusB); | ||
78 | |||
79 | Vector3 normalOnSurfaceB = diff / len; | ||
80 | //point on A (worldspace) | ||
81 | Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB; | ||
82 | //point on B (worldspace) | ||
83 | Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB; | ||
84 | |||
85 | // report a contact. internally this will be kept persistent, and contact reduction is done | ||
86 | resultOut.SetPersistentManifold(_manifold); | ||
87 | resultOut.AddContactPoint(normalOnSurfaceB, posB, dist); | ||
88 | } | ||
89 | |||
90 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
91 | { | ||
92 | //not yet | ||
93 | return 1f; | ||
94 | } | ||
95 | |||
96 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
97 | { | ||
98 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
99 | { | ||
100 | return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB); | ||
101 | } | ||
102 | } | ||
103 | } | ||
104 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs new file mode 100644 index 0000000..1ca5cfb --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs | |||
@@ -0,0 +1,100 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection. | ||
31 | /// Other features are frame-coherency (persistent data) and collision response. | ||
32 | /// Also provides the most basic sample for custom/user btCollisionAlgorithm | ||
33 | /// </summary> | ||
34 | public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable | ||
35 | { | ||
36 | private bool _ownManifold; | ||
37 | private PersistentManifold _manifold; | ||
38 | private bool _isSwapped; | ||
39 | |||
40 | public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | ||
41 | : base(collisionAlgorithmConstructionInfo) | ||
42 | { | ||
43 | _ownManifold = false; | ||
44 | _manifold = manifold; | ||
45 | _isSwapped = isSwapped; | ||
46 | |||
47 | if (_manifold == null) | ||
48 | { | ||
49 | _manifold = Dispatcher.GetNewManifold(bodyA, bodyB); | ||
50 | _ownManifold = true; | ||
51 | } | ||
52 | } | ||
53 | |||
54 | public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo) | ||
55 | : base(collisionAlgorithmConstructionInfo) { } | ||
56 | |||
57 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
58 | { | ||
59 | if (_manifold == null) | ||
60 | return; | ||
61 | |||
62 | SphereShape sphere = bodyA.CollisionShape as SphereShape; | ||
63 | TriangleShape triangle = bodyB.CollisionShape as TriangleShape; | ||
64 | |||
65 | /// report a contact. internally this will be kept persistent, and contact reduction is done | ||
66 | resultOut.SetPersistentManifold(_manifold); | ||
67 | SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle); | ||
68 | |||
69 | DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
70 | input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds | ||
71 | input.TransformA = bodyA.WorldTransform; | ||
72 | input.TransformB = bodyB.WorldTransform; | ||
73 | |||
74 | detector.GetClosestPoints(input, resultOut, null); | ||
75 | } | ||
76 | |||
77 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | ||
78 | { | ||
79 | //not yet | ||
80 | return 1f; | ||
81 | } | ||
82 | |||
83 | public class CreateFunc : CollisionAlgorithmCreateFunction | ||
84 | { | ||
85 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | ||
86 | { | ||
87 | return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped); | ||
88 | } | ||
89 | } | ||
90 | |||
91 | #region IDisposable Members | ||
92 | public void Dispose() | ||
93 | { | ||
94 | if (_ownManifold) | ||
95 | if (_manifold != null) | ||
96 | Dispatcher.ReleaseManifold(_manifold); | ||
97 | } | ||
98 | #endregion | ||
99 | } | ||
100 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs new file mode 100644 index 0000000..865754a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs | |||
@@ -0,0 +1,214 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class SphereTriangleDetector : DiscreteCollisionDetectorInterface | ||
30 | { | ||
31 | private SphereShape _sphere; | ||
32 | private TriangleShape _triangle; | ||
33 | private const int MaxOverlap = 0; | ||
34 | |||
35 | public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle) | ||
36 | { | ||
37 | this._sphere = sphere; | ||
38 | this._triangle = triangle; | ||
39 | } | ||
40 | |||
41 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) | ||
42 | { | ||
43 | Matrix transformA = input.TransformA; | ||
44 | Matrix transformB = input.TransformB; | ||
45 | |||
46 | Vector3 point = new Vector3(); | ||
47 | Vector3 normal = new Vector3(); | ||
48 | Single timeOfImpact = 1.0f; | ||
49 | Single depth = 0.0f; | ||
50 | |||
51 | //move sphere into triangle space | ||
52 | Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA); | ||
53 | |||
54 | if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact)) | ||
55 | output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth); | ||
56 | } | ||
57 | |||
58 | /// <summary> | ||
59 | /// See also geometrictools.com | ||
60 | /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv | ||
61 | /// </summary> | ||
62 | /// <param name="from"></param> | ||
63 | /// <param name="to"></param> | ||
64 | /// <param name="p"></param> | ||
65 | /// <param name="nearest"></param> | ||
66 | /// <returns></returns> | ||
67 | private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest) | ||
68 | { | ||
69 | Vector3 diff = point - from; | ||
70 | Vector3 v = to - from; | ||
71 | float t = Vector3.Dot(v, diff); | ||
72 | |||
73 | if (t > 0) | ||
74 | { | ||
75 | float dotVV = Vector3.Dot(v, v); | ||
76 | if (t < dotVV) | ||
77 | { | ||
78 | t /= dotVV; | ||
79 | diff -= t * v; | ||
80 | } | ||
81 | else | ||
82 | { | ||
83 | t = 1; | ||
84 | diff -= v; | ||
85 | } | ||
86 | } | ||
87 | else | ||
88 | t = 0; | ||
89 | |||
90 | nearest = from + t * v; | ||
91 | return Vector3.Dot(diff, diff); | ||
92 | } | ||
93 | |||
94 | private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact) | ||
95 | { | ||
96 | Vector3[] vertices = _triangle.Vertices; | ||
97 | Vector3 c = sphereCenter; | ||
98 | float r = _sphere.Radius; | ||
99 | |||
100 | Vector3 delta = new Vector3(); | ||
101 | |||
102 | Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); | ||
103 | normal = Vector3.Normalize(normal); | ||
104 | Vector3 p1ToCentre = c - vertices[0]; | ||
105 | float distanceFromPlane = Vector3.Dot(p1ToCentre, normal); | ||
106 | |||
107 | if (distanceFromPlane < 0) | ||
108 | { | ||
109 | //triangle facing the other way | ||
110 | distanceFromPlane *= -1; | ||
111 | normal *= -1; | ||
112 | } | ||
113 | |||
114 | float contactMargin = PersistentManifold.ContactBreakingThreshold; | ||
115 | bool isInsideContactPlane = distanceFromPlane < r + contactMargin; | ||
116 | bool isInsideShellPlane = distanceFromPlane < r; | ||
117 | |||
118 | float deltaDotNormal = Vector3.Dot(delta, normal); | ||
119 | if (!isInsideShellPlane && deltaDotNormal >= 0.0f) | ||
120 | return false; | ||
121 | |||
122 | // Check for contact / intersection | ||
123 | bool hasContact = false; | ||
124 | Vector3 contactPoint = new Vector3(); | ||
125 | if (isInsideContactPlane) | ||
126 | { | ||
127 | if (FaceContains(c, vertices, normal)) | ||
128 | { | ||
129 | // Inside the contact wedge - touches a point on the shell plane | ||
130 | hasContact = true; | ||
131 | contactPoint = c - normal * distanceFromPlane; | ||
132 | } | ||
133 | else | ||
134 | { | ||
135 | // Could be inside one of the contact capsules | ||
136 | float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); | ||
137 | Vector3 nearestOnEdge = new Vector3(); | ||
138 | for (int i = 0; i < _triangle.EdgeCount; i++) | ||
139 | { | ||
140 | Vector3 pa, pb; | ||
141 | _triangle.GetEdge(i, out pa, out pb); | ||
142 | |||
143 | float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge); | ||
144 | if (distanceSqr < contactCapsuleRadiusSqr) | ||
145 | { | ||
146 | // Yep, we're inside a capsule | ||
147 | hasContact = true; | ||
148 | contactPoint = nearestOnEdge; | ||
149 | } | ||
150 | } | ||
151 | } | ||
152 | } | ||
153 | |||
154 | if (hasContact) | ||
155 | { | ||
156 | Vector3 contactToCentre = c - contactPoint; | ||
157 | float distanceSqr = contactToCentre.LengthSquared(); | ||
158 | if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap)) | ||
159 | { | ||
160 | float distance = (float)Math.Sqrt(distanceSqr); | ||
161 | resultNormal = contactToCentre; | ||
162 | resultNormal = Vector3.Normalize(resultNormal); | ||
163 | point = contactPoint; | ||
164 | depth = -(r - distance); | ||
165 | return true; | ||
166 | } | ||
167 | |||
168 | if (Vector3.Dot(delta, contactToCentre) >= 0.0f) | ||
169 | return false; | ||
170 | |||
171 | // Moving towards the contact point -> collision | ||
172 | point = contactPoint; | ||
173 | timeOfImpact = 0.0f; | ||
174 | return true; | ||
175 | } | ||
176 | return false; | ||
177 | } | ||
178 | |||
179 | private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p) | ||
180 | { | ||
181 | Vector3 p1 = vertices[0]; | ||
182 | Vector3 p2 = vertices[1]; | ||
183 | Vector3 p3 = vertices[2]; | ||
184 | |||
185 | Vector3 edge1 = p2 - p1; | ||
186 | Vector3 edge2 = p3 - p2; | ||
187 | Vector3 edge3 = p1 - p3; | ||
188 | |||
189 | Vector3 p1ToP = p - p1; | ||
190 | Vector3 p2ToP = p - p2; | ||
191 | Vector3 p3ToP = p - p3; | ||
192 | |||
193 | Vector3 edge1Normal = Vector3.Cross(edge1, normal); | ||
194 | Vector3 edge2Normal = Vector3.Cross(edge2, normal); | ||
195 | Vector3 edge3Normal = Vector3.Cross(edge3, normal); | ||
196 | |||
197 | float r1, r2, r3; | ||
198 | r1 = Vector3.Dot(edge1Normal, p1ToP); | ||
199 | r2 = Vector3.Dot(edge2Normal, p2ToP); | ||
200 | r3 = Vector3.Dot(edge3Normal, p3ToP); | ||
201 | if ((r1 > 0 && r2 > 0 && r3 > 0) || | ||
202 | (r1 <= 0 && r2 <= 0 && r3 <= 0)) | ||
203 | return true; | ||
204 | return false; | ||
205 | } | ||
206 | |||
207 | private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal) | ||
208 | { | ||
209 | Vector3 lp = p; | ||
210 | Vector3 lnormal = normal; | ||
211 | return PointInTriangle(vertices, lnormal, lp); | ||
212 | } | ||
213 | } | ||
214 | } \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs new file mode 100644 index 0000000..a825fad --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs | |||
@@ -0,0 +1,151 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public class UnionFind : IDisposable | ||
29 | { | ||
30 | private List<Element> _elements = new List<Element>(); | ||
31 | |||
32 | public int ElementCount | ||
33 | { | ||
34 | get { return _elements.Count; } | ||
35 | } | ||
36 | |||
37 | public void SortIslands() | ||
38 | { | ||
39 | for (int i = 0; i < _elements.Count; i++) | ||
40 | { | ||
41 | _elements[i].ID = Find(i); | ||
42 | _elements[i].Size = i; | ||
43 | } | ||
44 | |||
45 | _elements.Sort(Sort); | ||
46 | } | ||
47 | |||
48 | private static int Sort(Element x, Element y) | ||
49 | { | ||
50 | if (x.ID < y.ID) return -1; | ||
51 | //else if (x.ID > y.ID) return 1; | ||
52 | else return 0; | ||
53 | } | ||
54 | |||
55 | public void Reset(int number) | ||
56 | { | ||
57 | Allocate(number); | ||
58 | |||
59 | for (int i = 0; i < number; i++) | ||
60 | { | ||
61 | Element element = new Element(); | ||
62 | element.ID = i; | ||
63 | element.Size = 1; | ||
64 | _elements.Insert(i, element); | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public bool IsRoot(int index) | ||
69 | { | ||
70 | return (_elements[index].Size == index); | ||
71 | } | ||
72 | |||
73 | public Element this[int index] | ||
74 | { | ||
75 | get { return _elements[index]; } | ||
76 | } | ||
77 | |||
78 | public void Allocate(int number) | ||
79 | { | ||
80 | //Does nothing | ||
81 | _elements = new List<Element>(number); | ||
82 | } | ||
83 | |||
84 | public bool Find(int i, int j) | ||
85 | { | ||
86 | return (Find(i) == Find(j)); | ||
87 | } | ||
88 | |||
89 | public int Find(int i) | ||
90 | { | ||
91 | while (i != _elements[i].ID) | ||
92 | { | ||
93 | //Element element = _elements[i]; | ||
94 | //element.ID = _elements[_elements[i].ID].ID; | ||
95 | _elements[i].ID = _elements[_elements[i].ID].ID; | ||
96 | i = _elements[i].ID; | ||
97 | } | ||
98 | |||
99 | return i; | ||
100 | } | ||
101 | |||
102 | public void Unite(int p, int q) | ||
103 | { | ||
104 | int i = Find(p), j = Find(q); | ||
105 | if (i == j) | ||
106 | return; | ||
107 | |||
108 | //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) | ||
109 | //if (_elements[i].Size < _elements[j].Size) | ||
110 | //{ | ||
111 | // Element element = _elements[i]; | ||
112 | // element.ID = j; | ||
113 | // _elements[i] = element; | ||
114 | |||
115 | // element = _elements[j]; | ||
116 | // element.Size += _elements[i].Size; | ||
117 | // _elements[j] = element; | ||
118 | //} | ||
119 | //else | ||
120 | //{ | ||
121 | // Element element = _elements[j]; | ||
122 | // element.ID = i; | ||
123 | // _elements[j] = element; | ||
124 | |||
125 | // element = _elements[i]; | ||
126 | // element.Size += _elements[j].Size; | ||
127 | // _elements[i] = element; | ||
128 | //} | ||
129 | _elements[i].ID = j; | ||
130 | _elements[j].Size += _elements[i].Size; | ||
131 | } | ||
132 | |||
133 | #region IDisposable Members | ||
134 | |||
135 | public void Dispose() | ||
136 | { | ||
137 | _elements.Clear(); | ||
138 | } | ||
139 | |||
140 | #endregion | ||
141 | } | ||
142 | |||
143 | public class Element | ||
144 | { | ||
145 | private int _id; | ||
146 | private int _size; | ||
147 | |||
148 | public int ID { get { return _id; } set { _id = value; } } | ||
149 | public int Size { get { return _size; } set { _size = value; } } | ||
150 | } | ||
151 | } \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs new file mode 100644 index 0000000..b75c2b6 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs | |||
@@ -0,0 +1,215 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). | ||
31 | /// </summary> | ||
32 | public class BUSimplex1to4 : PolyhedralConvexShape | ||
33 | { | ||
34 | private int _numVertices = 0; | ||
35 | private Vector3[] _vertices = new Vector3[4]; | ||
36 | |||
37 | public BUSimplex1to4() { } | ||
38 | |||
39 | public BUSimplex1to4(Vector3 pointA) | ||
40 | { | ||
41 | AddVertex(pointA); | ||
42 | } | ||
43 | |||
44 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB) | ||
45 | { | ||
46 | AddVertex(pointA); | ||
47 | AddVertex(pointB); | ||
48 | } | ||
49 | |||
50 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC) | ||
51 | { | ||
52 | AddVertex(pointA); | ||
53 | AddVertex(pointB); | ||
54 | AddVertex(pointC); | ||
55 | } | ||
56 | |||
57 | public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD) | ||
58 | { | ||
59 | AddVertex(pointA); | ||
60 | AddVertex(pointB); | ||
61 | AddVertex(pointC); | ||
62 | AddVertex(pointD); | ||
63 | } | ||
64 | |||
65 | protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } } | ||
66 | |||
67 | public override int VertexCount | ||
68 | { | ||
69 | get | ||
70 | { | ||
71 | return _numVertices; | ||
72 | } | ||
73 | } | ||
74 | |||
75 | public override int EdgeCount | ||
76 | { | ||
77 | get | ||
78 | { | ||
79 | //euler formula, F-E+V = 2, so E = F+V-2 | ||
80 | switch (_numVertices) | ||
81 | { | ||
82 | case 0: return 0; | ||
83 | case 1: return 0; | ||
84 | case 2: return 1; | ||
85 | case 3: return 3; | ||
86 | case 4: return 6; | ||
87 | } | ||
88 | return 0; | ||
89 | } | ||
90 | } | ||
91 | |||
92 | public override int PlaneCount | ||
93 | { | ||
94 | get | ||
95 | { | ||
96 | switch (_numVertices) | ||
97 | { | ||
98 | case 0: | ||
99 | return 0; | ||
100 | case 1: | ||
101 | return 0; | ||
102 | case 2: | ||
103 | return 0; | ||
104 | case 3: | ||
105 | return 2; | ||
106 | case 4: | ||
107 | return 4; | ||
108 | } | ||
109 | return 0; | ||
110 | } | ||
111 | } | ||
112 | |||
113 | public override BroadphaseNativeTypes ShapeType | ||
114 | { | ||
115 | get | ||
116 | { | ||
117 | return BroadphaseNativeTypes.Tetrahedral; | ||
118 | } | ||
119 | } | ||
120 | |||
121 | public override string Name | ||
122 | { | ||
123 | get | ||
124 | { | ||
125 | return "BUSimplex1to4"; | ||
126 | } | ||
127 | } | ||
128 | |||
129 | public void AddVertex(Vector3 v) | ||
130 | { | ||
131 | _vertices[_numVertices++] = v; | ||
132 | } | ||
133 | |||
134 | public void Reset() | ||
135 | { | ||
136 | _numVertices = 0; | ||
137 | } | ||
138 | |||
139 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
140 | { | ||
141 | switch (_numVertices) | ||
142 | { | ||
143 | case 2: | ||
144 | pa = _vertices[0]; | ||
145 | pb = _vertices[1]; | ||
146 | return; | ||
147 | case 3: | ||
148 | switch (i) | ||
149 | { | ||
150 | case 0: | ||
151 | pa = _vertices[0]; | ||
152 | pb = _vertices[1]; | ||
153 | return; | ||
154 | case 1: | ||
155 | pa = _vertices[1]; | ||
156 | pb = _vertices[2]; | ||
157 | return; | ||
158 | case 2: | ||
159 | pa = _vertices[2]; | ||
160 | pb = _vertices[0]; | ||
161 | return; | ||
162 | } | ||
163 | break; | ||
164 | case 4: | ||
165 | switch (i) | ||
166 | { | ||
167 | case 0: | ||
168 | pa = _vertices[0]; | ||
169 | pb = _vertices[1]; | ||
170 | return; | ||
171 | case 1: | ||
172 | pa = _vertices[1]; | ||
173 | pb = _vertices[2]; | ||
174 | return; | ||
175 | case 2: | ||
176 | pa = _vertices[2]; | ||
177 | pb = _vertices[0]; | ||
178 | return; | ||
179 | case 3: | ||
180 | pa = _vertices[0]; | ||
181 | pb = _vertices[3]; | ||
182 | return; | ||
183 | case 4: | ||
184 | pa = _vertices[1]; | ||
185 | pb = _vertices[3]; | ||
186 | return; | ||
187 | case 5: | ||
188 | pa = _vertices[2]; | ||
189 | pb = _vertices[3]; | ||
190 | return; | ||
191 | } | ||
192 | break; | ||
193 | } | ||
194 | |||
195 | pa = new Vector3(); | ||
196 | pb = new Vector3(); | ||
197 | } | ||
198 | |||
199 | public override void GetVertex(int i, out Vector3 vtx) | ||
200 | { | ||
201 | vtx = _vertices[i]; | ||
202 | } | ||
203 | |||
204 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
205 | { | ||
206 | planeNormal = new Vector3(); | ||
207 | planeSupport = new Vector3(); | ||
208 | } | ||
209 | |||
210 | public override bool IsInside(Vector3 pt, float tolerance) | ||
211 | { | ||
212 | return false; | ||
213 | } | ||
214 | } | ||
215 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs new file mode 100644 index 0000000..ba6f3b7 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs | |||
@@ -0,0 +1,316 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class BoxShape : PolyhedralConvexShape | ||
30 | { | ||
31 | public BoxShape(Vector3 boxHalfExtents) | ||
32 | { | ||
33 | ImplicitShapeDimensions = boxHalfExtents; | ||
34 | } | ||
35 | |||
36 | public override int VertexCount | ||
37 | { | ||
38 | get | ||
39 | { | ||
40 | return 8; | ||
41 | } | ||
42 | } | ||
43 | |||
44 | public override int EdgeCount | ||
45 | { | ||
46 | get | ||
47 | { | ||
48 | return 12; | ||
49 | } | ||
50 | } | ||
51 | |||
52 | public override BroadphaseNativeTypes ShapeType | ||
53 | { | ||
54 | get | ||
55 | { | ||
56 | return BroadphaseNativeTypes.Box; | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public override string Name | ||
61 | { | ||
62 | get | ||
63 | { | ||
64 | return "Box"; | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public override int PreferredPenetrationDirectionsCount | ||
69 | { | ||
70 | get | ||
71 | { | ||
72 | return 6; | ||
73 | } | ||
74 | } | ||
75 | |||
76 | public override int PlaneCount | ||
77 | { | ||
78 | get | ||
79 | { | ||
80 | return 6; | ||
81 | } | ||
82 | } | ||
83 | |||
84 | public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } } | ||
85 | |||
86 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
87 | { | ||
88 | int edgeVert0 = 0; | ||
89 | int edgeVert1 = 0; | ||
90 | |||
91 | switch (i) | ||
92 | { | ||
93 | case 0: | ||
94 | edgeVert0 = 0; | ||
95 | edgeVert1 = 1; | ||
96 | break; | ||
97 | case 1: | ||
98 | edgeVert0 = 0; | ||
99 | edgeVert1 = 2; | ||
100 | break; | ||
101 | case 2: | ||
102 | edgeVert0 = 1; | ||
103 | edgeVert1 = 3; | ||
104 | |||
105 | break; | ||
106 | case 3: | ||
107 | edgeVert0 = 2; | ||
108 | edgeVert1 = 3; | ||
109 | break; | ||
110 | case 4: | ||
111 | edgeVert0 = 0; | ||
112 | edgeVert1 = 4; | ||
113 | break; | ||
114 | case 5: | ||
115 | edgeVert0 = 1; | ||
116 | edgeVert1 = 5; | ||
117 | |||
118 | break; | ||
119 | case 6: | ||
120 | edgeVert0 = 2; | ||
121 | edgeVert1 = 6; | ||
122 | break; | ||
123 | case 7: | ||
124 | edgeVert0 = 3; | ||
125 | edgeVert1 = 7; | ||
126 | break; | ||
127 | case 8: | ||
128 | edgeVert0 = 4; | ||
129 | edgeVert1 = 5; | ||
130 | break; | ||
131 | case 9: | ||
132 | edgeVert0 = 4; | ||
133 | edgeVert1 = 6; | ||
134 | break; | ||
135 | case 10: | ||
136 | edgeVert0 = 5; | ||
137 | edgeVert1 = 7; | ||
138 | break; | ||
139 | case 11: | ||
140 | edgeVert0 = 6; | ||
141 | edgeVert1 = 7; | ||
142 | break; | ||
143 | default: | ||
144 | throw new BulletException(); | ||
145 | |||
146 | } | ||
147 | |||
148 | GetVertex(edgeVert0, out pa); | ||
149 | GetVertex(edgeVert1, out pb); | ||
150 | } | ||
151 | |||
152 | public override void GetVertex(int i, out Vector3 vtx) | ||
153 | { | ||
154 | Vector3 halfExtents = HalfExtents; | ||
155 | |||
156 | vtx = new Vector3( | ||
157 | halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1), | ||
158 | halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1), | ||
159 | halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2)); | ||
160 | } | ||
161 | |||
162 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
163 | { | ||
164 | //this plane might not be aligned... | ||
165 | Vector4 plane; | ||
166 | GetPlaneEquation(out plane, i); | ||
167 | planeNormal = new Vector3(plane.X, plane.Y, plane.Z); | ||
168 | planeSupport = LocalGetSupportingVertex(-planeNormal); | ||
169 | } | ||
170 | |||
171 | public override bool IsInside(Vector3 pt, float tolerance) | ||
172 | { | ||
173 | Vector3 halfExtents = HalfExtents; | ||
174 | |||
175 | //btScalar minDist = 2*tolerance; | ||
176 | |||
177 | bool result = (pt.X <= ( halfExtents.X + tolerance)) && | ||
178 | (pt.X >= (-halfExtents.X - tolerance)) && | ||
179 | (pt.Y <= ( halfExtents.Y + tolerance)) && | ||
180 | (pt.Y >= (-halfExtents.Y - tolerance)) && | ||
181 | (pt.Z <= ( halfExtents.Z + tolerance)) && | ||
182 | (pt.Z >= (-halfExtents.Z - tolerance)); | ||
183 | |||
184 | return result; | ||
185 | } | ||
186 | |||
187 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
188 | { | ||
189 | Vector3 halfExtents = HalfExtents; | ||
190 | |||
191 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | ||
192 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | ||
193 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | ||
194 | } | ||
195 | |||
196 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
197 | { | ||
198 | Vector3 halfExtents = HalfExtents; | ||
199 | Vector3 margin = new Vector3(Margin, Margin, Margin); | ||
200 | halfExtents -= margin; | ||
201 | |||
202 | return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | ||
203 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | ||
204 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | ||
205 | } | ||
206 | |||
207 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
208 | { | ||
209 | Vector3 halfExtents = HalfExtents; | ||
210 | Vector3 margin = new Vector3(Margin, Margin, Margin); | ||
211 | halfExtents -= margin; | ||
212 | |||
213 | for (int i = 0; i < vectors.Length; i++) | ||
214 | { | ||
215 | Vector3 vec = vectors[i]; | ||
216 | supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X, | ||
217 | vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y, | ||
218 | vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z); | ||
219 | } | ||
220 | } | ||
221 | |||
222 | public virtual void GetPlaneEquation(out Vector4 plane, int i) | ||
223 | { | ||
224 | Vector3 halfExtents = HalfExtents; | ||
225 | |||
226 | switch (i) | ||
227 | { | ||
228 | case 0: | ||
229 | plane = new Vector4(1, 0, 0, 0); | ||
230 | plane.W = -halfExtents.X; | ||
231 | break; | ||
232 | case 1: | ||
233 | plane = new Vector4(-1, 0, 0, 0); | ||
234 | plane.W = -halfExtents.X; | ||
235 | break; | ||
236 | case 2: | ||
237 | plane = new Vector4(0, 1, 0, 0); | ||
238 | plane.W = -halfExtents.Y; | ||
239 | break; | ||
240 | case 3: | ||
241 | plane = new Vector4(0, -1, 0, 0); | ||
242 | plane.W = -halfExtents.Y; | ||
243 | break; | ||
244 | case 4: | ||
245 | plane = new Vector4(0, 0, 1, 0); | ||
246 | plane.W = -halfExtents.Z; | ||
247 | break; | ||
248 | case 5: | ||
249 | plane = new Vector4(0, 0, -1, 0); | ||
250 | plane.W = -halfExtents.Z; | ||
251 | break; | ||
252 | default: | ||
253 | throw new BulletException(); | ||
254 | } | ||
255 | } | ||
256 | |||
257 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | ||
258 | { | ||
259 | switch (index) | ||
260 | { | ||
261 | case 0: | ||
262 | penetrationVector = new Vector3(1, 0, 0); | ||
263 | break; | ||
264 | case 1: | ||
265 | penetrationVector = new Vector3(-1, 0, 0); | ||
266 | break; | ||
267 | case 2: | ||
268 | penetrationVector = new Vector3(0, 1, 0); | ||
269 | break; | ||
270 | case 3: | ||
271 | penetrationVector = new Vector3(0, -1, 0); | ||
272 | break; | ||
273 | case 4: | ||
274 | penetrationVector = new Vector3(0, 0, 1); | ||
275 | break; | ||
276 | case 5: | ||
277 | penetrationVector = new Vector3(0, 0, -1); | ||
278 | break; | ||
279 | default: | ||
280 | throw new BulletException(); | ||
281 | } | ||
282 | } | ||
283 | |||
284 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
285 | { | ||
286 | Vector3 halfExtents = HalfExtents; | ||
287 | |||
288 | Matrix abs_b = MathHelper.Absolute(t); | ||
289 | Vector3 center = t.Translation; | ||
290 | Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); | ||
291 | Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); | ||
292 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); | ||
293 | Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents), | ||
294 | Vector3.Dot(row2, halfExtents), | ||
295 | Vector3.Dot(row3, halfExtents)); | ||
296 | extent += new Vector3(Margin, Margin, Margin); | ||
297 | |||
298 | aabbMin = center - extent; | ||
299 | aabbMax = center + extent; | ||
300 | } | ||
301 | |||
302 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
303 | { | ||
304 | Vector3 halfExtents = HalfExtents; | ||
305 | |||
306 | float lx = 2f * (halfExtents.X); | ||
307 | float ly = 2f * (halfExtents.Y); | ||
308 | float lz = 2f * (halfExtents.Z); | ||
309 | |||
310 | inertia = new Vector3(); | ||
311 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); | ||
312 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); | ||
313 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); | ||
314 | } | ||
315 | } | ||
316 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs new file mode 100644 index 0000000..00247a0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs | |||
@@ -0,0 +1,83 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | class MyNodeOverlapCallback : INodeOverlapCallback | ||
30 | { | ||
31 | StridingMeshInterface _meshInterface; | ||
32 | ITriangleCallback _callback; | ||
33 | Vector3[] _triangle = new Vector3[3]; | ||
34 | |||
35 | public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface) | ||
36 | { | ||
37 | _meshInterface = meshInterface; | ||
38 | _callback = callback; | ||
39 | } | ||
40 | |||
41 | public void ProcessNode(OptimizedBvhNode node) | ||
42 | { | ||
43 | List<Vector3> verts; | ||
44 | List<int> indicies; | ||
45 | int numtriangles; | ||
46 | |||
47 | _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart); | ||
48 | Vector3 meshScaling = _meshInterface.Scaling; | ||
49 | |||
50 | for (int j = 0; j < 3; j++) | ||
51 | { | ||
52 | _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling; | ||
53 | } | ||
54 | |||
55 | _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex); | ||
56 | _meshInterface.UnLockReadOnlyVertexBase(node.SubPart); | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public class BvhTriangleMeshShape : TriangleMeshShape | ||
61 | { | ||
62 | OptimizedBvh _bvh = new OptimizedBvh(); | ||
63 | |||
64 | public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface) | ||
65 | { | ||
66 | _bvh.Build(meshInterface); | ||
67 | } | ||
68 | |||
69 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
70 | { | ||
71 | MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface); | ||
72 | _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); | ||
73 | } | ||
74 | |||
75 | public override string Name | ||
76 | { | ||
77 | get | ||
78 | { | ||
79 | return "BvhTriangleMesh"; | ||
80 | } | ||
81 | } | ||
82 | } | ||
83 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs new file mode 100644 index 0000000..a9ce3be --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs | |||
@@ -0,0 +1,148 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// CollisionShape provides generic interface for collidable objects | ||
31 | /// </summary> | ||
32 | public abstract class CollisionShape | ||
33 | { | ||
34 | //debugging support | ||
35 | private string _tempDebug; | ||
36 | |||
37 | public abstract string Name { get; } | ||
38 | public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } } | ||
39 | |||
40 | public bool IsPolyhedral | ||
41 | { | ||
42 | get | ||
43 | { | ||
44 | return BroadphaseProxy.IsPolyhedral(ShapeType); | ||
45 | } | ||
46 | } | ||
47 | |||
48 | public bool IsConvex | ||
49 | { | ||
50 | get | ||
51 | { | ||
52 | return BroadphaseProxy.IsConvex(ShapeType); | ||
53 | } | ||
54 | } | ||
55 | public bool IsConcave | ||
56 | { | ||
57 | get | ||
58 | { | ||
59 | return BroadphaseProxy.IsConcave(ShapeType); | ||
60 | } | ||
61 | } | ||
62 | public bool IsCompound | ||
63 | { | ||
64 | get | ||
65 | { | ||
66 | return BroadphaseProxy.IsCompound(ShapeType); | ||
67 | } | ||
68 | } | ||
69 | |||
70 | //isInfinite is used to catch simulation error (aabb check) | ||
71 | public bool IsInfinite | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | return BroadphaseProxy.IsInfinite(ShapeType); | ||
76 | } | ||
77 | } | ||
78 | |||
79 | public abstract float Margin { get; set; } | ||
80 | public abstract Vector3 LocalScaling { get; set; } | ||
81 | public abstract BroadphaseNativeTypes ShapeType { get; } | ||
82 | |||
83 | |||
84 | public virtual void GetBoundingSphere(out Vector3 center, out float radius) | ||
85 | { | ||
86 | Matrix tr = Matrix.Identity; | ||
87 | Vector3 aabbMin, aabbMax; | ||
88 | |||
89 | GetAabb(tr, out aabbMin, out aabbMax); | ||
90 | |||
91 | radius = (aabbMax - aabbMin).Length() * 0.5f; | ||
92 | center = (aabbMin + aabbMax) * 0.5f; | ||
93 | } | ||
94 | |||
95 | public virtual float GetAngularMotionDisc() | ||
96 | { | ||
97 | Vector3 center; | ||
98 | float disc; | ||
99 | GetBoundingSphere(out center, out disc); | ||
100 | disc += center.Length(); | ||
101 | return disc; | ||
102 | } | ||
103 | |||
104 | //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) | ||
105 | //result is conservative | ||
106 | public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax) | ||
107 | { | ||
108 | //start with static aabb | ||
109 | GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax); | ||
110 | |||
111 | float temporalAabbMaxx = temporalAabbMax.X; | ||
112 | float temporalAabbMaxy = temporalAabbMax.Y; | ||
113 | float temporalAabbMaxz = temporalAabbMax.Z; | ||
114 | float temporalAabbMinx = temporalAabbMin.X; | ||
115 | float temporalAabbMiny = temporalAabbMin.Y; | ||
116 | float temporalAabbMinz = temporalAabbMin.Z; | ||
117 | |||
118 | // add linear motion | ||
119 | Vector3 linMotion = linearVelocity * timeStep; | ||
120 | //todo: simd would have a vector max/min operation, instead of per-element access | ||
121 | if (linMotion.X > 0) | ||
122 | temporalAabbMaxx += linMotion.X; | ||
123 | else | ||
124 | temporalAabbMinx += linMotion.X; | ||
125 | if (linMotion.Y > 0) | ||
126 | temporalAabbMaxy += linMotion.Y; | ||
127 | else | ||
128 | temporalAabbMiny += linMotion.Y; | ||
129 | if (linMotion.Z > 0) | ||
130 | temporalAabbMaxz += linMotion.Z; | ||
131 | else | ||
132 | temporalAabbMinz += linMotion.Z; | ||
133 | |||
134 | //add conservative angular motion | ||
135 | float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep; | ||
136 | Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion); | ||
137 | temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); | ||
138 | temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); | ||
139 | |||
140 | temporalAabbMin -= angularMotion3d; | ||
141 | temporalAabbMax += angularMotion3d; | ||
142 | } | ||
143 | |||
144 | public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax); | ||
145 | |||
146 | public abstract void CalculateLocalInertia(float mass, out Vector3 inertia); | ||
147 | } | ||
148 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs new file mode 100644 index 0000000..08aeec8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs | |||
@@ -0,0 +1,183 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// CompoundShape allows to store multiple other CollisionShapes | ||
31 | /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. | ||
32 | /// </summary> | ||
33 | public class CompoundShape : CollisionShape | ||
34 | { | ||
35 | private List<Matrix> _childTransforms = new List<Matrix>(); | ||
36 | private List<CollisionShape> _childShapes = new List<CollisionShape>(); | ||
37 | private Vector3 _localAabbMin; | ||
38 | private Vector3 _localAabbMax; | ||
39 | |||
40 | private OptimizedBvh _aabbTree; | ||
41 | private float _collisionMargin; | ||
42 | private Vector3 _localScaling; | ||
43 | |||
44 | public CompoundShape() | ||
45 | { | ||
46 | _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f); | ||
47 | _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | ||
48 | _aabbTree = null; | ||
49 | _collisionMargin = 0f; | ||
50 | _localScaling = new Vector3(1f, 1f, 1f); | ||
51 | } | ||
52 | |||
53 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
54 | { | ||
55 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); | ||
56 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); | ||
57 | |||
58 | Matrix abs_b = MathHelper.Absolute(t); | ||
59 | |||
60 | Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13); | ||
61 | Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23); | ||
62 | Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33); | ||
63 | |||
64 | Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X, | ||
65 | Vector3.Dot(row2, localCenter) + t.Translation.Y, | ||
66 | Vector3.Dot(row3, localCenter) + t.Translation.Z); | ||
67 | |||
68 | Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents), | ||
69 | Vector3.Dot(row2, localHalfExtents), | ||
70 | Vector3.Dot(row3, localHalfExtents)); | ||
71 | |||
72 | aabbMin = center - extent; | ||
73 | aabbMax = center + extent; | ||
74 | } | ||
75 | |||
76 | public override BroadphaseNativeTypes ShapeType | ||
77 | { | ||
78 | get | ||
79 | { | ||
80 | return BroadphaseNativeTypes.Compound; | ||
81 | } | ||
82 | } | ||
83 | |||
84 | public override Vector3 LocalScaling | ||
85 | { | ||
86 | get | ||
87 | { | ||
88 | return _localScaling; | ||
89 | } | ||
90 | set | ||
91 | { | ||
92 | _localScaling = value; | ||
93 | } | ||
94 | } | ||
95 | |||
96 | public override string Name | ||
97 | { | ||
98 | get | ||
99 | { | ||
100 | return "Compound"; | ||
101 | } | ||
102 | } | ||
103 | |||
104 | public override float Margin | ||
105 | { | ||
106 | get | ||
107 | { | ||
108 | return _collisionMargin; | ||
109 | } | ||
110 | set | ||
111 | { | ||
112 | _collisionMargin = value; | ||
113 | } | ||
114 | } | ||
115 | |||
116 | public int ChildShapeCount { get { return _childShapes.Count; } } | ||
117 | //this is optional, but should make collision queries faster, by culling non-overlapping nodes | ||
118 | public OptimizedBvh AabbTree { get { return _aabbTree; } } | ||
119 | |||
120 | public CollisionShape GetChildShape(int index) | ||
121 | { | ||
122 | return _childShapes[index]; | ||
123 | } | ||
124 | |||
125 | public Matrix GetChildTransform(int index) | ||
126 | { | ||
127 | return _childTransforms[index]; | ||
128 | } | ||
129 | |||
130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
131 | { | ||
132 | //approximation: take the inertia from the aabb for now | ||
133 | Matrix ident = Matrix.Identity; | ||
134 | Vector3 aabbMin, aabbMax; | ||
135 | GetAabb(ident, out aabbMin, out aabbMax); | ||
136 | |||
137 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
138 | |||
139 | float lx = 2f * (halfExtents.X); | ||
140 | float ly = 2f * (halfExtents.Y); | ||
141 | float lz = 2f * (halfExtents.Z); | ||
142 | |||
143 | inertia = new Vector3(); | ||
144 | inertia.X = mass / (12.0f) * (ly * ly + lz * lz); | ||
145 | inertia.Y = mass / (12.0f) * (lx * lx + lz * lz); | ||
146 | inertia.Z = mass / (12.0f) * (lx * lx + ly * ly); | ||
147 | } | ||
148 | |||
149 | public void AddChildShape(Matrix localTransform, CollisionShape shape) | ||
150 | { | ||
151 | _childTransforms.Add(localTransform); | ||
152 | _childShapes.Add(shape); | ||
153 | |||
154 | //extend the local aabbMin/aabbMax | ||
155 | Vector3 localAabbMin, localAabbMax; | ||
156 | shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); | ||
157 | if (_localAabbMin.X > localAabbMin.X) | ||
158 | { | ||
159 | _localAabbMin.X = localAabbMin.X; | ||
160 | } | ||
161 | if (_localAabbMax.X < localAabbMax.X) | ||
162 | { | ||
163 | _localAabbMax.X = localAabbMax.X; | ||
164 | } | ||
165 | if (_localAabbMin.Y > localAabbMin.Y) | ||
166 | { | ||
167 | _localAabbMin.Y = localAabbMin.Y; | ||
168 | } | ||
169 | if (_localAabbMax.Y < localAabbMax.Y) | ||
170 | { | ||
171 | _localAabbMax.Y = localAabbMax.Y; | ||
172 | } | ||
173 | if (_localAabbMin.Z > localAabbMin.Z) | ||
174 | { | ||
175 | _localAabbMin.Z = localAabbMin.Z; | ||
176 | } | ||
177 | if (_localAabbMax.Z < localAabbMax.Z) | ||
178 | { | ||
179 | _localAabbMax.Z = localAabbMax.Z; | ||
180 | } | ||
181 | } | ||
182 | } | ||
183 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs new file mode 100644 index 0000000..1e773f0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs | |||
@@ -0,0 +1,55 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class ConcaveShape : CollisionShape | ||
30 | { | ||
31 | private float _collisionMargin; | ||
32 | |||
33 | public ConcaveShape() { } | ||
34 | |||
35 | public float CollisionMargin | ||
36 | { | ||
37 | get { return _collisionMargin; } | ||
38 | set { _collisionMargin = value; } | ||
39 | } | ||
40 | |||
41 | public override float Margin | ||
42 | { | ||
43 | get | ||
44 | { | ||
45 | return _collisionMargin; | ||
46 | } | ||
47 | set | ||
48 | { | ||
49 | _collisionMargin = value; | ||
50 | } | ||
51 | } | ||
52 | |||
53 | public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax); | ||
54 | } | ||
55 | } \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs new file mode 100644 index 0000000..361c277 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs | |||
@@ -0,0 +1,208 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ConeShape implements a Cone shape, around the X axis | ||
31 | /// </summary> | ||
32 | public class ConeShapeX : ConeShape | ||
33 | { | ||
34 | public ConeShapeX(float radius, float height) | ||
35 | : base(radius, height) | ||
36 | { | ||
37 | ConeUpIndex = 0; | ||
38 | } | ||
39 | } | ||
40 | |||
41 | /// <summary> | ||
42 | /// ConeShape implements a Cone shape, around the Z axis | ||
43 | /// </summary> | ||
44 | public class ConeShapeZ : ConeShape | ||
45 | { | ||
46 | public ConeShapeZ(float radius, float height) | ||
47 | : base(radius, height) | ||
48 | { | ||
49 | ConeUpIndex = 2; | ||
50 | } | ||
51 | } | ||
52 | |||
53 | /// <summary> | ||
54 | /// ConeShape implements a Cone shape, around the Y axis | ||
55 | /// </summary> | ||
56 | public class ConeShape : ConvexShape | ||
57 | { | ||
58 | private float _sinAngle; | ||
59 | private float _radius; | ||
60 | private float _height; | ||
61 | private int[] _coneIndices = new int[3]; | ||
62 | |||
63 | public ConeShape(float radius, float height) | ||
64 | { | ||
65 | _radius = radius; | ||
66 | _height = height; | ||
67 | ConeUpIndex = 1; | ||
68 | _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); | ||
69 | } | ||
70 | |||
71 | public float Radius { get { return _radius; } } | ||
72 | public float Height { get { return _height; } } | ||
73 | |||
74 | public override BroadphaseNativeTypes ShapeType | ||
75 | { | ||
76 | get | ||
77 | { | ||
78 | return BroadphaseNativeTypes.Cone; | ||
79 | } | ||
80 | } | ||
81 | |||
82 | public override string Name | ||
83 | { | ||
84 | get | ||
85 | { | ||
86 | return "Cone"; | ||
87 | } | ||
88 | } | ||
89 | |||
90 | //choose upAxis index | ||
91 | public int ConeUpIndex | ||
92 | { | ||
93 | get { return _coneIndices[1]; } | ||
94 | set | ||
95 | { | ||
96 | switch (value) | ||
97 | { | ||
98 | case 0: | ||
99 | _coneIndices[0] = 1; | ||
100 | _coneIndices[1] = 0; | ||
101 | _coneIndices[2] = 2; | ||
102 | break; | ||
103 | case 1: | ||
104 | _coneIndices[0] = 0; | ||
105 | _coneIndices[1] = 1; | ||
106 | _coneIndices[2] = 2; | ||
107 | break; | ||
108 | case 2: | ||
109 | _coneIndices[0] = 0; | ||
110 | _coneIndices[1] = 2; | ||
111 | _coneIndices[2] = 1; | ||
112 | break; | ||
113 | default: | ||
114 | BulletDebug.Assert(false); | ||
115 | break; | ||
116 | } | ||
117 | } | ||
118 | } | ||
119 | |||
120 | private Vector3 ConeLocalSupport(Vector3 v) | ||
121 | { | ||
122 | float halfHeight = _height * 0.5f; | ||
123 | bool condition; | ||
124 | |||
125 | if (_coneIndices[1] == 0) | ||
126 | condition = v.X > v.Length() * _sinAngle; | ||
127 | else if (_coneIndices[1] == 1) | ||
128 | condition = v.Y > v.Length() * _sinAngle; | ||
129 | else | ||
130 | condition = v.Z > v.Length() * _sinAngle; | ||
131 | |||
132 | if (condition) | ||
133 | { | ||
134 | Vector3 tmp = new Vector3(); | ||
135 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); | ||
136 | return tmp; | ||
137 | } | ||
138 | else | ||
139 | { | ||
140 | float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) | ||
141 | + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2])); | ||
142 | if (s > MathHelper.Epsilon) | ||
143 | { | ||
144 | float d = _radius / s; | ||
145 | Vector3 tmp = new Vector3(); | ||
146 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); | ||
147 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | ||
148 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); | ||
149 | return tmp; | ||
150 | } | ||
151 | else | ||
152 | { | ||
153 | Vector3 tmp = new Vector3(); | ||
154 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | ||
155 | return tmp; | ||
156 | } | ||
157 | } | ||
158 | } | ||
159 | |||
160 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
161 | { | ||
162 | return ConeLocalSupport(vec); | ||
163 | } | ||
164 | |||
165 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
166 | { | ||
167 | for (int i = 0; i < vectors.Length; i++) | ||
168 | supportVerticesOut[i] = ConeLocalSupport(vectors[i]); | ||
169 | } | ||
170 | |||
171 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
172 | { | ||
173 | Matrix identity = Matrix.Identity; | ||
174 | Vector3 aabbMin, aabbMax; | ||
175 | GetAabb(identity, out aabbMin, out aabbMax); | ||
176 | |||
177 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
178 | |||
179 | float margin = Margin; | ||
180 | |||
181 | float lx = 2f * (halfExtents.X + margin); | ||
182 | float ly = 2f * (halfExtents.Y + margin); | ||
183 | float lz = 2f * (halfExtents.Z + margin); | ||
184 | float x2 = lx * lx; | ||
185 | float y2 = ly * ly; | ||
186 | float z2 = lz * lz; | ||
187 | float scaledmass = mass * 0.08333333f; | ||
188 | |||
189 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); | ||
190 | } | ||
191 | |||
192 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
193 | { | ||
194 | Vector3 supVertex = ConeLocalSupport(vec); | ||
195 | if (Margin != 0) | ||
196 | { | ||
197 | Vector3 vecnorm = vec; | ||
198 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
199 | { | ||
200 | vecnorm = new Vector3(-1f, -1f, -1f); | ||
201 | } | ||
202 | vecnorm = Vector3.Normalize(vecnorm); | ||
203 | supVertex += Margin * vecnorm; | ||
204 | } | ||
205 | return supVertex; | ||
206 | } | ||
207 | } | ||
208 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs new file mode 100644 index 0000000..eb78533 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs | |||
@@ -0,0 +1,184 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) | ||
31 | /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. | ||
32 | /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash. | ||
33 | /// (memory is much slower then the cpu) | ||
34 | /// </summary> | ||
35 | public class ConvexHullShape : PolyhedralConvexShape | ||
36 | { | ||
37 | private List<Vector3> _points = new List<Vector3>(); | ||
38 | |||
39 | public ConvexHullShape() { } | ||
40 | |||
41 | public override int VertexCount | ||
42 | { | ||
43 | get | ||
44 | { | ||
45 | return _points.Count; | ||
46 | } | ||
47 | } | ||
48 | |||
49 | public override int EdgeCount | ||
50 | { | ||
51 | get | ||
52 | { | ||
53 | return _points.Count; | ||
54 | } | ||
55 | } | ||
56 | |||
57 | public override int PlaneCount | ||
58 | { | ||
59 | get | ||
60 | { | ||
61 | return 0; | ||
62 | } | ||
63 | } | ||
64 | |||
65 | public override BroadphaseNativeTypes ShapeType | ||
66 | { | ||
67 | get | ||
68 | { | ||
69 | return BroadphaseNativeTypes.ConvexHull; | ||
70 | } | ||
71 | } | ||
72 | |||
73 | public override string Name | ||
74 | { | ||
75 | get | ||
76 | { | ||
77 | return "Convex"; | ||
78 | } | ||
79 | } | ||
80 | |||
81 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
82 | { | ||
83 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
84 | |||
85 | if (Margin != 0) | ||
86 | { | ||
87 | Vector3 vecnorm = vec; | ||
88 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
89 | { | ||
90 | vecnorm=new Vector3(-1, -1, -1); | ||
91 | } | ||
92 | vecnorm = Vector3.Normalize(vecnorm); | ||
93 | supVertex += Margin * vecnorm; | ||
94 | } | ||
95 | return supVertex; | ||
96 | } | ||
97 | |||
98 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) | ||
99 | { | ||
100 | Vector3 supVec = new Vector3(); | ||
101 | float newDot, maxDot = -1e30f; | ||
102 | |||
103 | Vector3 vec = vec0; | ||
104 | float lenSqr = vec.LengthSquared(); | ||
105 | if (lenSqr < 0.0001f) | ||
106 | { | ||
107 | vec = new Vector3(1, 0, 0); | ||
108 | } | ||
109 | else | ||
110 | { | ||
111 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
112 | vec *= rlen; | ||
113 | } | ||
114 | |||
115 | for (int i = 0; i < _points.Count; i++) | ||
116 | { | ||
117 | Vector3 vtx = _points[i] * LocalScaling; | ||
118 | |||
119 | newDot = Vector3.Dot(vec, vtx); | ||
120 | if (newDot > maxDot) | ||
121 | { | ||
122 | maxDot = newDot; | ||
123 | supVec = vtx; | ||
124 | } | ||
125 | } | ||
126 | return supVec; | ||
127 | } | ||
128 | |||
129 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
130 | { | ||
131 | float newDot; | ||
132 | //use 'w' component of supportVerticesOut? | ||
133 | /*{ | ||
134 | for (int i = 0; i < numVectors; i++) | ||
135 | { | ||
136 | supportVerticesOut[i][3] = -1e30f; | ||
137 | } | ||
138 | }*/ | ||
139 | #warning Warning! | ||
140 | for (int i = 0; i < _points.Count; i++) | ||
141 | { | ||
142 | Vector3 vtx = _points[i] * LocalScaling; | ||
143 | |||
144 | for (int j = 0; j < vectors.Length; j++) | ||
145 | { | ||
146 | newDot = Vector3.Dot(vectors[j], vtx); | ||
147 | if (newDot > -1e30f) | ||
148 | { | ||
149 | //WARNING: don't swap next lines, the w component would get overwritten! | ||
150 | supportVerticesOut[j] = vtx; | ||
151 | //supportVerticesOut[j][3] = newDot; | ||
152 | #warning Warning! | ||
153 | } | ||
154 | } | ||
155 | } | ||
156 | } | ||
157 | |||
158 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
159 | { | ||
160 | int index0 = i % _points.Count; | ||
161 | int index1 = (i + 1) % _points.Count; | ||
162 | pa = _points[index0] * LocalScaling; | ||
163 | pb = _points[index1] * LocalScaling; | ||
164 | } | ||
165 | |||
166 | public override void GetVertex(int i, out Vector3 vtx) | ||
167 | { | ||
168 | vtx = _points[i] * LocalScaling; | ||
169 | } | ||
170 | |||
171 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
172 | { | ||
173 | planeNormal = new Vector3(); | ||
174 | planeSupport = new Vector3(); | ||
175 | BulletDebug.Assert(false); | ||
176 | } | ||
177 | |||
178 | public override bool IsInside(Vector3 pt, float tolerance) | ||
179 | { | ||
180 | BulletDebug.Assert(false); | ||
181 | return false; | ||
182 | } | ||
183 | } | ||
184 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs new file mode 100644 index 0000000..02d678e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs | |||
@@ -0,0 +1,141 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ConvexShape is an abstract shape interface. | ||
31 | /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. | ||
32 | /// used in combination with GJK or btConvexCast | ||
33 | /// </summary> | ||
34 | public abstract class ConvexShape : CollisionShape | ||
35 | { | ||
36 | private const int _maxPreferredPenetrationDirections = 10; | ||
37 | private const float _convexDistanceMargin = 0.04f; | ||
38 | |||
39 | private Vector3 _localScaling; | ||
40 | private Vector3 _implicitShapeDimensions; | ||
41 | private float _collisionMargin; | ||
42 | |||
43 | public ConvexShape() | ||
44 | : base() | ||
45 | { | ||
46 | _localScaling = Vector3.One; | ||
47 | _collisionMargin = ConvexDistanceMargin; | ||
48 | } | ||
49 | |||
50 | public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } } | ||
51 | public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } } | ||
52 | |||
53 | public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } } | ||
54 | public virtual int PreferredPenetrationDirectionsCount { get { return 0; } } | ||
55 | |||
56 | protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } } | ||
57 | |||
58 | public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | ||
59 | { | ||
60 | penetrationVector = new Vector3(); | ||
61 | BulletDebug.Assert(false); | ||
62 | } | ||
63 | |||
64 | public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec); | ||
65 | //notice that the vectors should be unit length | ||
66 | public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut); | ||
67 | |||
68 | /// <summary> | ||
69 | /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version | ||
70 | /// </summary> | ||
71 | /// <param name="t"></param> | ||
72 | /// <param name="aabbMin"></param> | ||
73 | /// <param name="aabbMax"></param> | ||
74 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
75 | { | ||
76 | GetAabbSlow(t, out aabbMin, out aabbMax); | ||
77 | } | ||
78 | |||
79 | public override Vector3 LocalScaling | ||
80 | { | ||
81 | get | ||
82 | { | ||
83 | return _localScaling; | ||
84 | } | ||
85 | set | ||
86 | { | ||
87 | _localScaling = value; | ||
88 | } | ||
89 | } | ||
90 | |||
91 | public override float Margin | ||
92 | { | ||
93 | get | ||
94 | { | ||
95 | return _collisionMargin; | ||
96 | } | ||
97 | set | ||
98 | { | ||
99 | _collisionMargin = value; | ||
100 | } | ||
101 | } | ||
102 | |||
103 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
104 | { | ||
105 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
106 | |||
107 | if (Margin != 0f) | ||
108 | { | ||
109 | Vector3 vecnorm = vec; | ||
110 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
111 | { | ||
112 | vecnorm = new Vector3(-1f, -1f, -1f); | ||
113 | } | ||
114 | vecnorm.Normalize(); | ||
115 | supVertex += Margin * vecnorm; | ||
116 | } | ||
117 | return supVertex; | ||
118 | } | ||
119 | |||
120 | public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
121 | { | ||
122 | float margin = Margin; | ||
123 | aabbMax = new Vector3(); | ||
124 | aabbMin = new Vector3(); | ||
125 | |||
126 | for (int i = 0; i < 3; i++) | ||
127 | { | ||
128 | Vector3 vec = new Vector3(0f, 0f, 0f); | ||
129 | MathHelper.SetElement(ref vec, i, 1); | ||
130 | |||
131 | Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)); | ||
132 | |||
133 | Vector3 tmp = MathHelper.MatrixToVector(t, sv); | ||
134 | MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin); | ||
135 | MathHelper.SetElement(ref vec, i, -1f); | ||
136 | tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t))); | ||
137 | MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin); | ||
138 | } | ||
139 | } | ||
140 | } | ||
141 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs new file mode 100644 index 0000000..0fd3fa7 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs | |||
@@ -0,0 +1,185 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead. | ||
31 | /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. | ||
32 | /// </summary> | ||
33 | public class ConvexTriangleMeshShape : PolyhedralConvexShape | ||
34 | { | ||
35 | private StridingMeshInterface _stridingMesh; | ||
36 | |||
37 | public ConvexTriangleMeshShape(StridingMeshInterface meshInterface) | ||
38 | { | ||
39 | _stridingMesh = meshInterface; | ||
40 | } | ||
41 | |||
42 | public StridingMeshInterface getStridingMesh() | ||
43 | { | ||
44 | return _stridingMesh; | ||
45 | } | ||
46 | |||
47 | public override int VertexCount | ||
48 | { | ||
49 | get | ||
50 | { | ||
51 | return 0; | ||
52 | } | ||
53 | } | ||
54 | |||
55 | public override int EdgeCount | ||
56 | { | ||
57 | get | ||
58 | { | ||
59 | return 0; | ||
60 | } | ||
61 | } | ||
62 | |||
63 | public override int PlaneCount | ||
64 | { | ||
65 | get | ||
66 | { | ||
67 | return 0; | ||
68 | } | ||
69 | } | ||
70 | |||
71 | public override Vector3 LocalScaling | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | return base.LocalScaling; | ||
76 | } | ||
77 | set | ||
78 | { | ||
79 | _stridingMesh.Scaling = value; | ||
80 | } | ||
81 | } | ||
82 | |||
83 | public override BroadphaseNativeTypes ShapeType | ||
84 | { | ||
85 | get | ||
86 | { | ||
87 | return BroadphaseNativeTypes.ConvexTriangleMesh; | ||
88 | } | ||
89 | } | ||
90 | |||
91 | public override string Name | ||
92 | { | ||
93 | get | ||
94 | { | ||
95 | return "ConvexTrimesh"; | ||
96 | } | ||
97 | } | ||
98 | |||
99 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
100 | { | ||
101 | pa = new Vector3(); | ||
102 | pb = new Vector3(); | ||
103 | BulletDebug.Assert(false); | ||
104 | } | ||
105 | |||
106 | public override void GetVertex(int i, out Vector3 vtx) | ||
107 | { | ||
108 | vtx = new Vector3(); | ||
109 | BulletDebug.Assert(false); | ||
110 | } | ||
111 | |||
112 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
113 | { | ||
114 | planeNormal = new Vector3(); | ||
115 | planeSupport = new Vector3(); | ||
116 | BulletDebug.Assert(false); | ||
117 | } | ||
118 | |||
119 | public override bool IsInside(Vector3 pt, float tolerance) | ||
120 | { | ||
121 | BulletDebug.Assert(false); | ||
122 | return false; | ||
123 | } | ||
124 | |||
125 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
126 | { | ||
127 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
128 | |||
129 | if (Margin != 0) | ||
130 | { | ||
131 | Vector3 vecnorm = vec; | ||
132 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
133 | { | ||
134 | vecnorm = new Vector3(-1, -1, -1); | ||
135 | } | ||
136 | vecnorm = Vector3.Normalize(vecnorm); | ||
137 | supVertex += Margin * vecnorm; | ||
138 | } | ||
139 | return supVertex; | ||
140 | } | ||
141 | |||
142 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0) | ||
143 | { | ||
144 | Vector3 supVec = new Vector3(); | ||
145 | |||
146 | Vector3 vec = vec0; | ||
147 | float lenSqr = vec.LengthSquared(); | ||
148 | if (lenSqr < 0.0001f) | ||
149 | { | ||
150 | vec = new Vector3(1, 0, 0); | ||
151 | } | ||
152 | else | ||
153 | { | ||
154 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
155 | vec *= rlen; | ||
156 | } | ||
157 | |||
158 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); | ||
159 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
160 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | ||
161 | supVec = supportCallback.SupportVertexLocal; | ||
162 | |||
163 | return supVec; | ||
164 | } | ||
165 | |||
166 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
167 | { | ||
168 | //use 'w' component of supportVerticesOut? | ||
169 | /*{ | ||
170 | for (int i = 0; i < numVectors; i++) | ||
171 | { | ||
172 | supportVerticesOut[i][3] = -1e30f; | ||
173 | } | ||
174 | }*/ | ||
175 | for (int j = 0; j < vectors.Length; j++) | ||
176 | { | ||
177 | Vector3 vec = vectors[j]; | ||
178 | LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec); | ||
179 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
180 | _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | ||
181 | supportVerticesOut[j] = supportCallback.SupportVertexLocal; | ||
182 | } | ||
183 | } | ||
184 | } | ||
185 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs new file mode 100644 index 0000000..9a86eaf --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs | |||
@@ -0,0 +1,136 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// implements cylinder shape interface | ||
31 | /// </summary> | ||
32 | public class CylinderShape : BoxShape | ||
33 | { | ||
34 | public CylinderShape(Vector3 halfExtents) | ||
35 | : base(halfExtents) | ||
36 | { | ||
37 | } | ||
38 | |||
39 | public override BroadphaseNativeTypes ShapeType | ||
40 | { | ||
41 | get | ||
42 | { | ||
43 | return BroadphaseNativeTypes.Cylinder; | ||
44 | } | ||
45 | } | ||
46 | |||
47 | public virtual int UpAxis | ||
48 | { | ||
49 | get | ||
50 | { | ||
51 | return 1; | ||
52 | } | ||
53 | } | ||
54 | |||
55 | public virtual float Radius | ||
56 | { | ||
57 | get | ||
58 | { | ||
59 | return HalfExtents.Z; | ||
60 | } | ||
61 | } | ||
62 | |||
63 | //debugging | ||
64 | public override string Name | ||
65 | { | ||
66 | get | ||
67 | { | ||
68 | return "CylinderY"; | ||
69 | } | ||
70 | } | ||
71 | |||
72 | //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version | ||
73 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
74 | { | ||
75 | GetAabbSlow(t, out aabbMin, out aabbMax); | ||
76 | } | ||
77 | |||
78 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
79 | { | ||
80 | return CylinderLocalSupportY(HalfExtents, vec); | ||
81 | } | ||
82 | |||
83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
84 | { | ||
85 | for (int i = 0; i < vectors.Length; i++) | ||
86 | { | ||
87 | supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]); | ||
88 | } | ||
89 | } | ||
90 | |||
91 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
92 | { | ||
93 | |||
94 | Vector3 supVertex; | ||
95 | supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
96 | |||
97 | if (Margin != 0) | ||
98 | { | ||
99 | Vector3 vecnorm = vec; | ||
100 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
101 | { | ||
102 | vecnorm=new Vector3(-1, -1, -1); | ||
103 | } | ||
104 | vecnorm = Vector3.Normalize(vecnorm); | ||
105 | supVertex += Margin * vecnorm; | ||
106 | } | ||
107 | return supVertex; | ||
108 | } | ||
109 | |||
110 | private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v) | ||
111 | { | ||
112 | float radius = halfExtents.X; | ||
113 | float halfHeight = halfExtents.Y; | ||
114 | |||
115 | Vector3 tmp = new Vector3(); | ||
116 | float d; | ||
117 | |||
118 | float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z); | ||
119 | if (s != 0) | ||
120 | { | ||
121 | d = radius / s; | ||
122 | tmp.X = v.X * d; | ||
123 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; | ||
124 | tmp.Z = v.Z * d; | ||
125 | return tmp; | ||
126 | } | ||
127 | else | ||
128 | { | ||
129 | tmp.X = radius; | ||
130 | tmp.Y = v.Y < 0 ? -halfHeight : halfHeight; | ||
131 | tmp.Z = 0; | ||
132 | return tmp; | ||
133 | } | ||
134 | } | ||
135 | } | ||
136 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs new file mode 100644 index 0000000..9dc8e2d --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs | |||
@@ -0,0 +1,100 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CylinderShapeX : CylinderShape | ||
30 | { | ||
31 | public CylinderShapeX(Vector3 halfExtents) | ||
32 | : base(halfExtents) { } | ||
33 | |||
34 | public override int UpAxis | ||
35 | { | ||
36 | get | ||
37 | { | ||
38 | return 0; | ||
39 | } | ||
40 | } | ||
41 | |||
42 | public override float Radius | ||
43 | { | ||
44 | get | ||
45 | { | ||
46 | return HalfExtents.Y; | ||
47 | } | ||
48 | } | ||
49 | |||
50 | //debugging | ||
51 | public override string Name | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | return "CylinderX"; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
60 | { | ||
61 | return CylinderLocalSupportX(HalfExtents, vec); | ||
62 | } | ||
63 | |||
64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
65 | { | ||
66 | for (int i = 0; i < vectors.Length; i++) | ||
67 | { | ||
68 | supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]); | ||
69 | } | ||
70 | } | ||
71 | |||
72 | private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v) | ||
73 | { | ||
74 | //mapping depends on how cylinder local orientation is | ||
75 | // extents of the cylinder is: X,Y is for radius, and Z for height | ||
76 | float radius = halfExtents.Y; | ||
77 | float halfHeight = halfExtents.X; | ||
78 | |||
79 | Vector3 tmp = new Vector3(); | ||
80 | float d; | ||
81 | |||
82 | float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z); | ||
83 | if (s != 0) | ||
84 | { | ||
85 | d = radius / s; | ||
86 | tmp.Y = v.Y * d; | ||
87 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; | ||
88 | tmp.Z = v.Z * d; | ||
89 | return tmp; | ||
90 | } | ||
91 | else | ||
92 | { | ||
93 | tmp.Y = radius; | ||
94 | tmp.X = v.X < 0 ? -halfHeight : halfHeight; | ||
95 | tmp.Z = 0; | ||
96 | return tmp; | ||
97 | } | ||
98 | } | ||
99 | } | ||
100 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs new file mode 100644 index 0000000..1913e9c --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs | |||
@@ -0,0 +1,100 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class CylinderShapeZ : CylinderShape | ||
30 | { | ||
31 | public CylinderShapeZ(Vector3 halfExtents) | ||
32 | : base(halfExtents) { } | ||
33 | |||
34 | public override int UpAxis | ||
35 | { | ||
36 | get | ||
37 | { | ||
38 | return 2; | ||
39 | } | ||
40 | } | ||
41 | |||
42 | public override float Radius | ||
43 | { | ||
44 | get | ||
45 | { | ||
46 | return HalfExtents.X; | ||
47 | } | ||
48 | } | ||
49 | |||
50 | //debugging | ||
51 | public override string Name | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | return "CylinderZ"; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
60 | { | ||
61 | return CylinderLocalSupportZ(HalfExtents, vec); | ||
62 | } | ||
63 | |||
64 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
65 | { | ||
66 | for (int i = 0; i < vectors.Length; i++) | ||
67 | { | ||
68 | supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]); | ||
69 | } | ||
70 | } | ||
71 | |||
72 | Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v) | ||
73 | { | ||
74 | //mapping depends on how cylinder local orientation is | ||
75 | // extents of the cylinder is: X,Y is for radius, and Z for height | ||
76 | float radius = halfExtents.X; | ||
77 | float halfHeight = halfExtents.Z; | ||
78 | |||
79 | Vector3 tmp = new Vector3(); | ||
80 | float d; | ||
81 | |||
82 | float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y); | ||
83 | if (s != 0) | ||
84 | { | ||
85 | d = radius / s; | ||
86 | tmp.X = v.X * d; | ||
87 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; | ||
88 | tmp.Y = v.Y * d; | ||
89 | return tmp; | ||
90 | } | ||
91 | else | ||
92 | { | ||
93 | tmp.X = radius; | ||
94 | tmp.Z = v.Z < 0 ? -halfHeight : halfHeight; | ||
95 | tmp.Y = 0; | ||
96 | return tmp; | ||
97 | } | ||
98 | } | ||
99 | } | ||
100 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs new file mode 100644 index 0000000..b094c39 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs | |||
@@ -0,0 +1,80 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | using System.Diagnostics; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | public class EmptyShape : ConcaveShape | ||
31 | { | ||
32 | private Vector3 _localScaling; | ||
33 | |||
34 | public override string Name | ||
35 | { | ||
36 | get | ||
37 | { | ||
38 | return "Empty"; | ||
39 | } | ||
40 | } | ||
41 | |||
42 | public override BroadphaseNativeTypes ShapeType | ||
43 | { | ||
44 | get | ||
45 | { | ||
46 | return BroadphaseNativeTypes.Empty; | ||
47 | } | ||
48 | } | ||
49 | |||
50 | public override Vector3 LocalScaling | ||
51 | { | ||
52 | get | ||
53 | { | ||
54 | return _localScaling; | ||
55 | } | ||
56 | set | ||
57 | { | ||
58 | _localScaling = value; | ||
59 | } | ||
60 | } | ||
61 | |||
62 | public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax) | ||
63 | { | ||
64 | throw new Exception("The method or operation is not implemented."); | ||
65 | } | ||
66 | |||
67 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
68 | { | ||
69 | Vector3 margin = new Vector3(Margin, Margin, Margin); | ||
70 | aabbMin = t.Translation - margin; | ||
71 | aabbMax = t.Translation + margin; | ||
72 | } | ||
73 | |||
74 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
75 | { | ||
76 | inertia = new Vector3(); | ||
77 | BulletDebug.Assert(false); | ||
78 | } | ||
79 | } | ||
80 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs new file mode 100644 index 0000000..5073096 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs | |||
@@ -0,0 +1,55 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class FilteredCallback : ITriangleIndexCallback | ||
30 | { | ||
31 | private ITriangleCallback _callback; | ||
32 | private Vector3 _aabbMin; | ||
33 | private Vector3 _aabbMax; | ||
34 | |||
35 | public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
36 | { | ||
37 | _callback = callback; | ||
38 | _aabbMin = aabbMin; | ||
39 | _aabbMax = aabbMax; | ||
40 | } | ||
41 | |||
42 | public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } } | ||
43 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } | ||
44 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } | ||
45 | |||
46 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | ||
47 | { | ||
48 | if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax)) | ||
49 | { | ||
50 | //check aabb in triangle-space, before doing this | ||
51 | _callback.ProcessTriangle(triangle, partId, triangleIndex); | ||
52 | } | ||
53 | } | ||
54 | } | ||
55 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs new file mode 100644 index 0000000..b88d804 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs | |||
@@ -0,0 +1,33 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public interface ITriangleIndexCallback | ||
30 | { | ||
31 | void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex); | ||
32 | } | ||
33 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs new file mode 100644 index 0000000..951707d --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs | |||
@@ -0,0 +1,58 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class LocalSupportVertexCallback : ITriangleIndexCallback | ||
30 | { | ||
31 | private Vector3 _supportVertexLocal; | ||
32 | private float _maxDot; | ||
33 | private Vector3 _supportVecLocal; | ||
34 | |||
35 | public LocalSupportVertexCallback(Vector3 supportVecLocal) | ||
36 | { | ||
37 | _supportVertexLocal = new Vector3(); | ||
38 | _maxDot = -1e30f; | ||
39 | _supportVecLocal = supportVecLocal; | ||
40 | } | ||
41 | |||
42 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } | ||
43 | public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } | ||
44 | |||
45 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | ||
46 | { | ||
47 | for (int i = 0; i < 3; i++) | ||
48 | { | ||
49 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); | ||
50 | if (dot > _maxDot) | ||
51 | { | ||
52 | _maxDot = dot; | ||
53 | _supportVertexLocal = triangle[i]; | ||
54 | } | ||
55 | } | ||
56 | } | ||
57 | } | ||
58 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs new file mode 100644 index 0000000..6f70cae --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs | |||
@@ -0,0 +1,99 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. | ||
31 | /// </summary> | ||
32 | public class MinkowskiSumShape : ConvexShape | ||
33 | { | ||
34 | private Matrix _transformA; | ||
35 | private Matrix _transformB; | ||
36 | private ConvexShape _shapeA; | ||
37 | private ConvexShape _shapeB; | ||
38 | |||
39 | public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) | ||
40 | { | ||
41 | _shapeA = shapeA; | ||
42 | _shapeB = shapeB; | ||
43 | _transformA = Matrix.Identity; | ||
44 | _transformB = Matrix.Identity; | ||
45 | } | ||
46 | |||
47 | public Matrix TransformA { get { return _transformA; } set { _transformA = value; } } | ||
48 | public Matrix TransformB { get { return _transformB; } set { _transformB = value; } } | ||
49 | public ConvexShape ShapeA { get { return _shapeA; } } | ||
50 | public ConvexShape ShapeB { get { return _shapeB; } } | ||
51 | |||
52 | public override float Margin | ||
53 | { | ||
54 | get | ||
55 | { | ||
56 | return _shapeA.Margin + _shapeB.Margin; | ||
57 | } | ||
58 | set | ||
59 | { | ||
60 | base.Margin = value; | ||
61 | } | ||
62 | } | ||
63 | |||
64 | public override BroadphaseNativeTypes ShapeType | ||
65 | { | ||
66 | get | ||
67 | { | ||
68 | return BroadphaseNativeTypes.MinkowskiDifference; | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public override string Name | ||
73 | { | ||
74 | get | ||
75 | { | ||
76 | return "MinkowskiSum"; | ||
77 | } | ||
78 | } | ||
79 | |||
80 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
81 | { | ||
82 | Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA))); | ||
83 | Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB))); | ||
84 | return supVertexA + supVertexB; | ||
85 | } | ||
86 | |||
87 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
88 | { | ||
89 | for (int i = 0; i < vectors.Length; i++) | ||
90 | supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]); | ||
91 | } | ||
92 | |||
93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
94 | { | ||
95 | inertia = new Vector3(); | ||
96 | BulletDebug.Assert(false); | ||
97 | } | ||
98 | } | ||
99 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs new file mode 100644 index 0000000..9959c01 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs | |||
@@ -0,0 +1,154 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) | ||
31 | /// </summary> | ||
32 | public class MultiSphereShape : ConvexShape | ||
33 | { | ||
34 | private const int _maxNumSpheres = 5; | ||
35 | private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; | ||
36 | private float[] _radi = new float[MaxNumSpheres]; | ||
37 | private Vector3 _inertiaHalfExtents; | ||
38 | |||
39 | private int m_numSpheres; | ||
40 | |||
41 | public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) | ||
42 | { | ||
43 | _inertiaHalfExtents = inertiaHalfExtents; | ||
44 | float startMargin = 1e30f; | ||
45 | |||
46 | m_numSpheres = numSpheres; | ||
47 | for (int i = 0; i < m_numSpheres; i++) | ||
48 | { | ||
49 | _localPositions[i] = positions[i]; | ||
50 | _radi[i] = radi[i]; | ||
51 | if (radi[i] < startMargin) | ||
52 | startMargin = radi[i]; | ||
53 | } | ||
54 | Margin = startMargin; | ||
55 | } | ||
56 | |||
57 | public static int MaxNumSpheres { get { return _maxNumSpheres; } } | ||
58 | |||
59 | public override BroadphaseNativeTypes ShapeType | ||
60 | { | ||
61 | get | ||
62 | { | ||
63 | return BroadphaseNativeTypes.MultiSphere; | ||
64 | } | ||
65 | } | ||
66 | |||
67 | public override string Name | ||
68 | { | ||
69 | get | ||
70 | { | ||
71 | return "MultiSphere"; | ||
72 | } | ||
73 | } | ||
74 | |||
75 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) | ||
76 | { | ||
77 | Vector3 supVec = new Vector3(); | ||
78 | |||
79 | float maxDot = -1e30f; | ||
80 | |||
81 | |||
82 | Vector3 vec = vecA; | ||
83 | float lenSqr = vec.LengthSquared(); | ||
84 | if (lenSqr < 0.0001f) | ||
85 | { | ||
86 | vec = new Vector3(1, 0, 0); | ||
87 | } | ||
88 | else | ||
89 | { | ||
90 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
91 | vec *= rlen; | ||
92 | } | ||
93 | |||
94 | Vector3 vtx; | ||
95 | float newDot; | ||
96 | |||
97 | for (int i = 0; i < m_numSpheres; i++) | ||
98 | { | ||
99 | vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; | ||
100 | newDot = Vector3.Dot(vec, vtx); | ||
101 | if (newDot > maxDot) | ||
102 | { | ||
103 | maxDot = newDot; | ||
104 | supVec = vtx; | ||
105 | } | ||
106 | } | ||
107 | |||
108 | return supVec; | ||
109 | } | ||
110 | |||
111 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
112 | { | ||
113 | for (int j = 0; j < vectors.Length; j++) | ||
114 | { | ||
115 | float maxDot = -1e30f; | ||
116 | Vector3 vtx; | ||
117 | float newDot; | ||
118 | |||
119 | for (int i = 0; i < m_numSpheres; i++) | ||
120 | { | ||
121 | vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; | ||
122 | newDot = Vector3.Dot(vectors[j], vtx); | ||
123 | if (newDot > maxDot) | ||
124 | { | ||
125 | maxDot = newDot; | ||
126 | supportVerticesOut[j] = vtx; | ||
127 | } | ||
128 | } | ||
129 | } | ||
130 | } | ||
131 | |||
132 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
133 | { | ||
134 | //as an approximation, take the inertia of the box that bounds the spheres | ||
135 | Matrix ident = Matrix.Identity; | ||
136 | Vector3 halfExtents = _inertiaHalfExtents; | ||
137 | |||
138 | float margin = ConvexDistanceMargin; | ||
139 | |||
140 | float lx = 2f * (halfExtents.X + margin); | ||
141 | float ly = 2f * (halfExtents.Y + margin); | ||
142 | float lz = 2f * (halfExtents.Z + margin); | ||
143 | float x2 = lx * lx; | ||
144 | float y2 = ly * ly; | ||
145 | float z2 = lz * lz; | ||
146 | float scaledmass = mass * 0.08333333f; | ||
147 | |||
148 | inertia = new Vector3(); | ||
149 | inertia.X = scaledmass * (y2 + z2); | ||
150 | inertia.Y = scaledmass * (x2 + z2); | ||
151 | inertia.Z = scaledmass * (x2 + y2); | ||
152 | } | ||
153 | } | ||
154 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs new file mode 100644 index 0000000..fac344e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs | |||
@@ -0,0 +1,32 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | namespace XnaDevRu.BulletX | ||
27 | { | ||
28 | public interface INodeOverlapCallback | ||
29 | { | ||
30 | void ProcessNode(OptimizedBvhNode node); | ||
31 | } | ||
32 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs new file mode 100644 index 0000000..c22c990 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs | |||
@@ -0,0 +1,293 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) | ||
31 | /// </summary> | ||
32 | public class OptimizedBvh | ||
33 | { | ||
34 | private static int _maxIterations = 0; | ||
35 | private OptimizedBvhNode _rootNode; | ||
36 | |||
37 | private OptimizedBvhNode[] _contiguousNodes; | ||
38 | private int _curNodeIndex; | ||
39 | |||
40 | private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>(); | ||
41 | |||
42 | public OptimizedBvh() { } | ||
43 | |||
44 | public void Build(StridingMeshInterface triangles) | ||
45 | { | ||
46 | NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes); | ||
47 | |||
48 | Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | ||
49 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
50 | |||
51 | triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax); | ||
52 | |||
53 | //now we have an array of leafnodes in m_leafNodes | ||
54 | _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count]; | ||
55 | for (int i = 0; i < _contiguousNodes.Length; i++) | ||
56 | _contiguousNodes[i] = new OptimizedBvhNode(); | ||
57 | _curNodeIndex = 0; | ||
58 | |||
59 | _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count); | ||
60 | } | ||
61 | |||
62 | public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) | ||
63 | { | ||
64 | OptimizedBvhNode internalNode; | ||
65 | |||
66 | int splitAxis, splitIndex, i; | ||
67 | int numIndices = endIndex - startIndex; | ||
68 | int curIndex = _curNodeIndex; | ||
69 | |||
70 | if (numIndices <= 0) | ||
71 | throw new BulletException(); | ||
72 | |||
73 | if (numIndices == 1) | ||
74 | { | ||
75 | _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex]; | ||
76 | //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); | ||
77 | return leafNodes[startIndex]; | ||
78 | } | ||
79 | |||
80 | //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. | ||
81 | splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex); | ||
82 | |||
83 | splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis); | ||
84 | |||
85 | internalNode = _contiguousNodes[_curNodeIndex++]; | ||
86 | |||
87 | internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | ||
88 | internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); | ||
89 | |||
90 | for (i = startIndex; i < endIndex; i++) | ||
91 | { | ||
92 | internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax); | ||
93 | internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin); | ||
94 | } | ||
95 | |||
96 | //internalNode->m_escapeIndex; | ||
97 | internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex); | ||
98 | internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex); | ||
99 | |||
100 | internalNode.EscapeIndex = _curNodeIndex - curIndex; | ||
101 | return internalNode; | ||
102 | } | ||
103 | |||
104 | public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex) | ||
105 | { | ||
106 | Vector3 means = new Vector3(); | ||
107 | Vector3 variance = new Vector3(); | ||
108 | int numIndices = endIndex - startIndex; | ||
109 | |||
110 | for (int i = startIndex; i < endIndex; i++) | ||
111 | { | ||
112 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
113 | means += center; | ||
114 | } | ||
115 | means *= (1f / (float)numIndices); | ||
116 | |||
117 | for (int i = startIndex; i < endIndex; i++) | ||
118 | { | ||
119 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
120 | Vector3 diff2 = center - means; | ||
121 | diff2 = diff2 * diff2; | ||
122 | variance += diff2; | ||
123 | } | ||
124 | variance *= (1f / ((float)numIndices - 1)); | ||
125 | |||
126 | return MathHelper.MaxAxis(variance); | ||
127 | } | ||
128 | |||
129 | public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis) | ||
130 | { | ||
131 | int splitIndex = startIndex; | ||
132 | int numIndices = endIndex - startIndex; | ||
133 | float splitValue; | ||
134 | |||
135 | Vector3 means = new Vector3(); | ||
136 | for (int i = startIndex; i < endIndex; i++) | ||
137 | { | ||
138 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
139 | means += center; | ||
140 | } | ||
141 | means *= (1f / (float)numIndices); | ||
142 | |||
143 | if (splitAxis == 0) | ||
144 | splitValue = means.X; | ||
145 | else if (splitAxis == 1) | ||
146 | splitValue = means.Y; | ||
147 | else if (splitAxis == 2) | ||
148 | splitValue = means.Z; | ||
149 | else | ||
150 | throw new ArgumentException(); | ||
151 | |||
152 | //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. | ||
153 | for (int i = startIndex; i < endIndex; i++) | ||
154 | { | ||
155 | Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin); | ||
156 | float centerSplit; | ||
157 | |||
158 | if (splitAxis == 0) | ||
159 | centerSplit = means.X; | ||
160 | else if (splitAxis == 1) | ||
161 | centerSplit = means.Y; | ||
162 | else if (splitAxis == 2) | ||
163 | centerSplit = means.Z; | ||
164 | else | ||
165 | throw new ArgumentException(); | ||
166 | |||
167 | if (centerSplit > splitValue) | ||
168 | { | ||
169 | //swap | ||
170 | OptimizedBvhNode tmp = leafNodes[i]; | ||
171 | leafNodes[i] = leafNodes[splitIndex]; | ||
172 | leafNodes[splitIndex] = tmp; | ||
173 | splitIndex++; | ||
174 | } | ||
175 | } | ||
176 | if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1))) | ||
177 | { | ||
178 | splitIndex = startIndex + (numIndices >> 1); | ||
179 | } | ||
180 | return splitIndex; | ||
181 | } | ||
182 | |||
183 | public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | ||
184 | { | ||
185 | bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); | ||
186 | if (aabbOverlap) | ||
187 | { | ||
188 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); | ||
189 | if (isLeafNode) | ||
190 | { | ||
191 | nodeCallback.ProcessNode(rootNode); | ||
192 | } | ||
193 | else | ||
194 | { | ||
195 | WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax); | ||
196 | WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax); | ||
197 | } | ||
198 | } | ||
199 | } | ||
200 | |||
201 | public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | ||
202 | { | ||
203 | int escapeIndex, curIndex = 0; | ||
204 | int walkIterations = 0; | ||
205 | bool aabbOverlap, isLeafNode; | ||
206 | int rootNodeIndex = 0; | ||
207 | OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex]; | ||
208 | |||
209 | while (curIndex < _curNodeIndex) | ||
210 | { | ||
211 | //catch bugs in tree data | ||
212 | if (walkIterations >= _curNodeIndex) | ||
213 | throw new BulletException(); | ||
214 | |||
215 | walkIterations++; | ||
216 | aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax); | ||
217 | isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null); | ||
218 | |||
219 | if (isLeafNode && aabbOverlap) | ||
220 | { | ||
221 | nodeCallback.ProcessNode(rootNode); | ||
222 | } | ||
223 | |||
224 | if (aabbOverlap || isLeafNode) | ||
225 | { | ||
226 | rootNodeIndex++; // this | ||
227 | curIndex++; | ||
228 | if (rootNodeIndex < rootNodeArray.Length) | ||
229 | rootNode = rootNodeArray[rootNodeIndex]; | ||
230 | } | ||
231 | else | ||
232 | { | ||
233 | escapeIndex = rootNode.EscapeIndex; | ||
234 | rootNodeIndex += escapeIndex; // and this | ||
235 | curIndex += escapeIndex; | ||
236 | if (rootNodeIndex < rootNodeArray.Length) | ||
237 | rootNode = rootNodeArray[rootNodeIndex]; | ||
238 | } | ||
239 | |||
240 | } | ||
241 | |||
242 | if (_maxIterations < walkIterations) | ||
243 | _maxIterations = walkIterations; | ||
244 | } | ||
245 | |||
246 | public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) | ||
247 | { | ||
248 | //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) | ||
249 | //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); | ||
250 | //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax); | ||
251 | WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax); | ||
252 | } | ||
253 | |||
254 | public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { } | ||
255 | } | ||
256 | |||
257 | public class NodeTriangleCallback : ITriangleIndexCallback | ||
258 | { | ||
259 | private List<OptimizedBvhNode> _triangleNodes; | ||
260 | |||
261 | public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes) | ||
262 | { | ||
263 | _triangleNodes = triangleNodes; | ||
264 | } | ||
265 | |||
266 | public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } } | ||
267 | |||
268 | public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex) | ||
269 | { | ||
270 | |||
271 | OptimizedBvhNode node = new OptimizedBvhNode(); | ||
272 | node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f); | ||
273 | node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f); | ||
274 | |||
275 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]); | ||
276 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]); | ||
277 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]); | ||
278 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]); | ||
279 | node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]); | ||
280 | node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]); | ||
281 | |||
282 | node.EscapeIndex = -1; | ||
283 | node.LeftChild = null; | ||
284 | node.RightChild = null; | ||
285 | |||
286 | //for child nodes | ||
287 | node.SubPart = partId; | ||
288 | node.TriangleIndex = triangleIndex; | ||
289 | |||
290 | _triangleNodes.Add(node); | ||
291 | } | ||
292 | } | ||
293 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs new file mode 100644 index 0000000..d43f2fb --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs | |||
@@ -0,0 +1,63 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// OptimizedBvhNode contains both internal and leaf node information. | ||
31 | /// It hasn't been optimized yet for storage. Some obvious optimizations are: | ||
32 | /// Removal of the pointers (can already be done, they are not used for traversal) | ||
33 | /// and storing aabbmin/max as quantized integers. | ||
34 | /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle | ||
35 | /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans | ||
36 | /// </summary> | ||
37 | public class OptimizedBvhNode | ||
38 | { | ||
39 | private Vector3 _aabbMin; | ||
40 | private Vector3 _aabbMax; | ||
41 | |||
42 | //these 2 pointers are obsolete, the stackless traversal just uses the escape index | ||
43 | private OptimizedBvhNode _leftChild; | ||
44 | private OptimizedBvhNode _rightChild; | ||
45 | |||
46 | private int _escapeIndex; | ||
47 | |||
48 | //for child nodes | ||
49 | private int _subPart; | ||
50 | private int _triangleIndex; | ||
51 | |||
52 | public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } } | ||
53 | public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } } | ||
54 | |||
55 | public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } } | ||
56 | public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } } | ||
57 | |||
58 | public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } } | ||
59 | |||
60 | public int SubPart { get { return _subPart; } set { _subPart = value; } } | ||
61 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | ||
62 | } | ||
63 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs new file mode 100644 index 0000000..f61371a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs | |||
@@ -0,0 +1,133 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class PolyhedralConvexShape : ConvexShape | ||
30 | { | ||
31 | public PolyhedralConvexShape() | ||
32 | { | ||
33 | //m_optionalHull = null; | ||
34 | } | ||
35 | |||
36 | public abstract int VertexCount { get; } | ||
37 | public abstract int EdgeCount { get; } | ||
38 | public abstract int PlaneCount { get; } | ||
39 | |||
40 | public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB); | ||
41 | public abstract void GetVertex(int i, out Vector3 vertex); | ||
42 | public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i); | ||
43 | // abstract int getIndex(int i); | ||
44 | |||
45 | public abstract bool IsInside(Vector3 point, float tolerance); | ||
46 | |||
47 | // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp | ||
48 | //public class Hull m_optionalHull; | ||
49 | |||
50 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
51 | { | ||
52 | Vector3 supVec = new Vector3(); | ||
53 | |||
54 | float maxDot = -1e30f; | ||
55 | |||
56 | float lenSqr = vec.LengthSquared(); | ||
57 | if (lenSqr < 0.0001f) | ||
58 | { | ||
59 | vec = new Vector3(1, 0, 0); | ||
60 | } | ||
61 | else | ||
62 | { | ||
63 | float rlen = 1f / (float)Math.Sqrt(lenSqr); | ||
64 | vec *= rlen; | ||
65 | } | ||
66 | |||
67 | Vector3 vtx; | ||
68 | float newDot; | ||
69 | |||
70 | for (int i = 0; i < VertexCount; i++) | ||
71 | { | ||
72 | GetVertex(i, out vtx); | ||
73 | newDot = Vector3.Dot(vec, vtx); | ||
74 | if (newDot > maxDot) | ||
75 | { | ||
76 | maxDot = newDot; | ||
77 | supVec = vtx; | ||
78 | } | ||
79 | } | ||
80 | return supVec; | ||
81 | } | ||
82 | |||
83 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
84 | { | ||
85 | #warning Think about this | ||
86 | /*Vector3 vtx; | ||
87 | float newDot; | ||
88 | |||
89 | for (int i = 0; i < vectors.Length; i++) | ||
90 | { | ||
91 | supportVerticesOut[i][3] = -1e30f; | ||
92 | } | ||
93 | |||
94 | for (int j = 0; j < vectors.Length; j++) | ||
95 | { | ||
96 | Vector3 vec = vectors[j]; | ||
97 | |||
98 | for (int i = 0; i < getNumVertices(); i++) | ||
99 | { | ||
100 | getVertex(i, out vtx); | ||
101 | newDot = Vector3.Dot(vec,vtx); | ||
102 | if (newDot > supportVerticesOut[j][3]) | ||
103 | { | ||
104 | //WARNING: don't swap next lines, the w component would get overwritten! | ||
105 | supportVerticesOut[j] = vtx; | ||
106 | supportVerticesOut[j][3] = newDot; | ||
107 | } | ||
108 | } | ||
109 | }*/ | ||
110 | } | ||
111 | |||
112 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
113 | { | ||
114 | //not yet, return box inertia | ||
115 | float margin = Margin; | ||
116 | |||
117 | Matrix ident = Matrix.Identity; | ||
118 | Vector3 aabbMin, aabbMax; | ||
119 | GetAabb(ident, out aabbMin, out aabbMax); | ||
120 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
121 | |||
122 | float lx = 2f * (halfExtents.X + margin); | ||
123 | float ly = 2f * (halfExtents.Y + margin); | ||
124 | float lz = 2f * (halfExtents.Z + margin); | ||
125 | float x2 = lx * lx; | ||
126 | float y2 = ly * ly; | ||
127 | float z2 = lz * lz; | ||
128 | float scaledmass = mass * 0.08333333f; | ||
129 | |||
130 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); | ||
131 | } | ||
132 | } | ||
133 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs new file mode 100644 index 0000000..c660a6f --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs | |||
@@ -0,0 +1,116 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// btSphereShape implements an implicit (getSupportingVertex) Sphere | ||
31 | /// </summary> | ||
32 | public class SphereShape : ConvexShape | ||
33 | { | ||
34 | public SphereShape(float radius) | ||
35 | : base() | ||
36 | { | ||
37 | Vector3 temp = ImplicitShapeDimensions; | ||
38 | temp.X = radius; | ||
39 | ImplicitShapeDimensions = temp; | ||
40 | } | ||
41 | |||
42 | public float Radius { get { return ImplicitShapeDimensions.X; } } | ||
43 | |||
44 | public override BroadphaseNativeTypes ShapeType | ||
45 | { | ||
46 | get | ||
47 | { | ||
48 | return BroadphaseNativeTypes.Sphere; | ||
49 | } | ||
50 | } | ||
51 | |||
52 | public override string Name | ||
53 | { | ||
54 | get | ||
55 | { | ||
56 | return "Sphere"; | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
61 | { | ||
62 | return new Vector3(); | ||
63 | } | ||
64 | |||
65 | /// <summary> | ||
66 | /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case | ||
67 | /// this means, non-uniform scaling is not supported anymore | ||
68 | /// </summary> | ||
69 | public override float Margin | ||
70 | { | ||
71 | get | ||
72 | { | ||
73 | return LocalScaling.X * Radius + base.Margin; | ||
74 | } | ||
75 | set | ||
76 | { | ||
77 | base.Margin = value; | ||
78 | } | ||
79 | } | ||
80 | |||
81 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
82 | { | ||
83 | if (supportVerticesOut != null) | ||
84 | for (int i = 0; i < supportVerticesOut.Length; i++) | ||
85 | supportVerticesOut[i] = new Vector3(); | ||
86 | } | ||
87 | |||
88 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
89 | { | ||
90 | float elem = 0.4f * mass * Margin * Margin; | ||
91 | inertia = new Vector3(elem, elem, elem); | ||
92 | } | ||
93 | |||
94 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
95 | { | ||
96 | Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); | ||
97 | |||
98 | Vector3 vecnorm = vec; | ||
99 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
100 | { | ||
101 | vecnorm = new Vector3(-1f, -1f, -1f); | ||
102 | } | ||
103 | vecnorm.Normalize(); | ||
104 | supVertex += Margin * vecnorm; | ||
105 | return supVertex; | ||
106 | } | ||
107 | |||
108 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
109 | { | ||
110 | Vector3 center = t.Translation; | ||
111 | Vector3 extent = new Vector3(Margin, Margin, Margin); | ||
112 | aabbMin = center - extent; | ||
113 | aabbMax = center + extent; | ||
114 | } | ||
115 | } | ||
116 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs new file mode 100644 index 0000000..49604a0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs | |||
@@ -0,0 +1,124 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class StaticPlaneShape : ConcaveShape | ||
30 | { | ||
31 | private Vector3 _localAabbMin; | ||
32 | private Vector3 _localAabbMax; | ||
33 | |||
34 | private Vector3 _planeNormal; | ||
35 | private float _planeConstant; | ||
36 | private Vector3 _localScaling; | ||
37 | |||
38 | public StaticPlaneShape(Vector3 planeNormal, float planeConstant) | ||
39 | { | ||
40 | _planeNormal = planeNormal; | ||
41 | _planeConstant = planeConstant; | ||
42 | _localScaling = new Vector3(); | ||
43 | } | ||
44 | |||
45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } | ||
46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } | ||
47 | |||
48 | protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } } | ||
49 | protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } } | ||
50 | |||
51 | public override BroadphaseNativeTypes ShapeType | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | return BroadphaseNativeTypes.StaticPlane; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public override Vector3 LocalScaling | ||
60 | { | ||
61 | get | ||
62 | { | ||
63 | return _localScaling; | ||
64 | } | ||
65 | set | ||
66 | { | ||
67 | _localScaling = value; | ||
68 | } | ||
69 | } | ||
70 | |||
71 | public override string Name | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | return "StaticPlane"; | ||
76 | } | ||
77 | } | ||
78 | |||
79 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
80 | { | ||
81 | Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f); | ||
82 | |||
83 | Vector3 center = _planeNormal * _planeConstant; | ||
84 | aabbMin = center + infvec * _planeNormal; | ||
85 | aabbMax = aabbMin; | ||
86 | MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal); | ||
87 | MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal); | ||
88 | |||
89 | aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); | ||
90 | aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
91 | } | ||
92 | |||
93 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
94 | { | ||
95 | //moving concave objects not supported | ||
96 | inertia = new Vector3(); | ||
97 | } | ||
98 | |||
99 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) { | ||
100 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | ||
101 | float radius = halfExtents.Length(); | ||
102 | Vector3 center = (aabbMax + aabbMin) * 0.5f; | ||
103 | |||
104 | //this is where the triangles are generated, given AABB and plane equation (normal/constant) | ||
105 | Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3(); | ||
106 | |||
107 | //tangentDir0/tangentDir1 can be precalculated | ||
108 | MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1); | ||
109 | |||
110 | Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal; | ||
111 | |||
112 | Vector3[] triangle = new Vector3[3]; | ||
113 | triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; | ||
114 | triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; | ||
115 | triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; | ||
116 | callback.ProcessTriangle(triangle, 0, 0); | ||
117 | |||
118 | triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; | ||
119 | triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; | ||
120 | triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; | ||
121 | callback.ProcessTriangle(triangle, 0, 1); | ||
122 | } | ||
123 | } | ||
124 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs new file mode 100644 index 0000000..2a2ce27 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs | |||
@@ -0,0 +1,115 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// PHY_ScalarType enumerates possible scalar types. | ||
31 | /// See the StridingMeshInterface for its use | ||
32 | /// </summary> | ||
33 | public enum PHY_ScalarType | ||
34 | { | ||
35 | PHY_FLOAT, | ||
36 | PHY_DOUBLE, | ||
37 | PHY_INTEGER, | ||
38 | PHY_SHORT, | ||
39 | PHY_FIXEDPOINT88 | ||
40 | } | ||
41 | |||
42 | /// <summary> | ||
43 | /// StridingMeshInterface is the interface class for high performance access to triangle meshes | ||
44 | /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. | ||
45 | /// </summary> | ||
46 | public abstract class StridingMeshInterface | ||
47 | { | ||
48 | protected Vector3 _scaling; | ||
49 | |||
50 | public StridingMeshInterface() | ||
51 | { | ||
52 | _scaling = new Vector3(1f,1f,1f); | ||
53 | } | ||
54 | |||
55 | public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
56 | { | ||
57 | int numtotalphysicsverts = 0; | ||
58 | int numtriangles, gfxindex; | ||
59 | int part, graphicssubparts = SubPartsCount(); | ||
60 | Vector3[] triangle = new Vector3[3]; | ||
61 | List<Vector3> verts; | ||
62 | List<int> indicies; | ||
63 | |||
64 | Vector3 meshScaling = Scaling; | ||
65 | |||
66 | //if the number of parts is big, the performance might drop due to the innerloop switch on indextype | ||
67 | for (part = 0; part < graphicssubparts; part++) | ||
68 | { | ||
69 | GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part); | ||
70 | numtotalphysicsverts += numtriangles * 3; //upper bound | ||
71 | |||
72 | for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) | ||
73 | { | ||
74 | triangle[0] = verts[indicies[gfxindex * 3 + 0]]; | ||
75 | triangle[1] = verts[indicies[gfxindex * 3 + 1]]; | ||
76 | triangle[2] = verts[indicies[gfxindex * 3 + 2]]; | ||
77 | |||
78 | callback.ProcessTriangleIndex(triangle, part, gfxindex); | ||
79 | } | ||
80 | |||
81 | UnLockReadOnlyVertexBase(part); | ||
82 | } | ||
83 | } | ||
84 | |||
85 | |||
86 | // get read and write access to a subpart of a triangle mesh | ||
87 | // this subpart has a continuous array of vertices and indices | ||
88 | // in this way the mesh can be handled as chunks of memory with striding | ||
89 | // very similar to OpenGL vertexarray support | ||
90 | // make a call to unLockVertexBase when the read and write access is finished | ||
91 | public abstract void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); | ||
92 | |||
93 | public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart); | ||
94 | |||
95 | // unLockVertexBase finishes the access to a subpart of the triangle mesh | ||
96 | // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished | ||
97 | public abstract void UnLockVertexBase(int subpart); | ||
98 | |||
99 | public abstract void UnLockReadOnlyVertexBase(int subpart); | ||
100 | |||
101 | |||
102 | // getNumSubParts returns the number of seperate subparts | ||
103 | // each subpart has a continuous array of vertices and indices | ||
104 | public abstract int SubPartsCount(); | ||
105 | |||
106 | public abstract void PreallocateVertices(int numverts); | ||
107 | public abstract void PreallocateIndices(int numindices); | ||
108 | |||
109 | public Vector3 Scaling | ||
110 | { | ||
111 | get { return _scaling; } | ||
112 | set { _scaling = value; } | ||
113 | } | ||
114 | } | ||
115 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs new file mode 100644 index 0000000..ffb5813 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs | |||
@@ -0,0 +1,67 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | internal class SupportVertexCallback : ITriangleCallback | ||
30 | { | ||
31 | private Vector3 _supportVertexLocal; | ||
32 | |||
33 | private Matrix _worldTransform; | ||
34 | private float _maxDot; | ||
35 | private Vector3 _supportVecLocal; | ||
36 | |||
37 | public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans) | ||
38 | { | ||
39 | _supportVertexLocal = new Vector3(); | ||
40 | _worldTransform = trans; | ||
41 | _maxDot = -1e30f; | ||
42 | _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform); | ||
43 | } | ||
44 | |||
45 | public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } } | ||
46 | public float MaxDot { get { return _maxDot; } set { _maxDot = value; } } | ||
47 | public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } } | ||
48 | |||
49 | public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } } | ||
50 | public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } } | ||
51 | |||
52 | #region ITriangleCallback Members | ||
53 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
54 | { | ||
55 | for (int i = 0; i < 3; i++) | ||
56 | { | ||
57 | float dot = Vector3.Dot(_supportVecLocal, triangle[i]); | ||
58 | if (dot > _maxDot) | ||
59 | { | ||
60 | _maxDot = dot; | ||
61 | _supportVertexLocal = triangle[i]; | ||
62 | } | ||
63 | } | ||
64 | } | ||
65 | #endregion | ||
66 | } | ||
67 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs new file mode 100644 index 0000000..158e42a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs | |||
@@ -0,0 +1,80 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class Triangle | ||
30 | { | ||
31 | private Vector3 _vertexA; | ||
32 | private Vector3 _vertexB; | ||
33 | private Vector3 _vertexC; | ||
34 | private int _partId; | ||
35 | private int _triangleIndex; | ||
36 | |||
37 | public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } } | ||
38 | public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } } | ||
39 | public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } } | ||
40 | public int PartId { get { return _partId; } set { _partId = value; } } | ||
41 | public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } } | ||
42 | } | ||
43 | |||
44 | /// <summary> | ||
45 | /// example usage of this class: | ||
46 | /// TriangleBuffer triBuf; | ||
47 | /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax); | ||
48 | /// for (int i = 0; i < triBuf.getNumTriangles(); i++) | ||
49 | /// { | ||
50 | /// Triangle tri = triBuf.getTriangle(i); | ||
51 | /// //do something useful here with the triangle | ||
52 | /// } | ||
53 | /// </summary> | ||
54 | public class TriangleBuffer : ITriangleCallback | ||
55 | { | ||
56 | private List<Triangle> _triangleBuffer = new List<Triangle>(); | ||
57 | |||
58 | public int TriangleCount { get { return _triangleBuffer.Count; } } | ||
59 | public Triangle this[int index] { get { return _triangleBuffer[index]; } } | ||
60 | |||
61 | public void ClearBuffer() | ||
62 | { | ||
63 | _triangleBuffer.Clear(); | ||
64 | } | ||
65 | |||
66 | #region ITriangleCallback Members | ||
67 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
68 | { | ||
69 | Triangle tri = new Triangle(); | ||
70 | tri.VertexA = triangle[0]; | ||
71 | tri.VertexB = triangle[1]; | ||
72 | tri.VertexC = triangle[2]; | ||
73 | tri.PartId = partID; | ||
74 | tri.TriangleIndex = triangleIndex; | ||
75 | |||
76 | _triangleBuffer.Add(tri); | ||
77 | } | ||
78 | #endregion | ||
79 | } | ||
80 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs new file mode 100644 index 0000000..ac892c2 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs | |||
@@ -0,0 +1,33 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public interface ITriangleCallback | ||
30 | { | ||
31 | void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex); | ||
32 | } | ||
33 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs new file mode 100644 index 0000000..6d0d7b5 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs | |||
@@ -0,0 +1,136 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements | ||
31 | /// instead of the number of indices, we pass the number of triangles | ||
32 | /// </summary> | ||
33 | public struct IndexedMesh | ||
34 | { | ||
35 | private int _numTriangles; | ||
36 | private int[] _triangleIndexBase; | ||
37 | private int _triangleIndexStride; | ||
38 | private int _numVertices; | ||
39 | private Vector3[] _vertexBase; | ||
40 | private int _vertexStride; | ||
41 | |||
42 | public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) | ||
43 | { | ||
44 | _numTriangles = numTriangleIndices; | ||
45 | _triangleIndexBase = triangleIndexBase; | ||
46 | _triangleIndexStride = triangleIndexStride; | ||
47 | _vertexBase = vertexBase; | ||
48 | _numVertices = numVertices; | ||
49 | _vertexStride = vertexStride; | ||
50 | } | ||
51 | |||
52 | public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase) | ||
53 | { | ||
54 | _numTriangles = triangleIndexBase.Length; | ||
55 | _triangleIndexBase = triangleIndexBase; | ||
56 | _triangleIndexStride = 32; | ||
57 | _vertexBase = vertexBase; | ||
58 | _numVertices = vertexBase.Length; | ||
59 | _vertexStride = 24; | ||
60 | } | ||
61 | |||
62 | public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } } | ||
63 | public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } } | ||
64 | public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } } | ||
65 | public int VertexCount { get { return _numVertices; } set { _numVertices = value; } } | ||
66 | public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } } | ||
67 | public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } } | ||
68 | } | ||
69 | |||
70 | /// <summary> | ||
71 | /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. | ||
72 | /// Additional meshes can be added using addIndexedMesh | ||
73 | /// </summary> | ||
74 | public class TriangleIndexVertexArray : StridingMeshInterface | ||
75 | { | ||
76 | List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>(); | ||
77 | |||
78 | public TriangleIndexVertexArray() { } | ||
79 | |||
80 | public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride) | ||
81 | { | ||
82 | IndexedMesh mesh = new IndexedMesh(); | ||
83 | mesh.TriangleCount = numTriangleIndices; | ||
84 | mesh.TriangleIndexBase = triangleIndexBase; | ||
85 | mesh.TriangleIndexStride = triangleIndexStride; | ||
86 | mesh.VertexBase = vertexBase; | ||
87 | mesh.VertexCount = numVertices; | ||
88 | mesh.VertexStride = vertexStride; | ||
89 | |||
90 | AddIndexedMesh(mesh); | ||
91 | } | ||
92 | |||
93 | public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase) | ||
94 | : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { } | ||
95 | |||
96 | public void AddIndexedMesh(IndexedMesh indexedMesh) | ||
97 | { | ||
98 | _indexedMeshes.Add(indexedMesh); | ||
99 | } | ||
100 | |||
101 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
102 | { | ||
103 | throw new Exception("The method or operation is not implemented."); | ||
104 | } | ||
105 | |||
106 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
107 | { | ||
108 | throw new Exception("The method or operation is not implemented."); | ||
109 | } | ||
110 | |||
111 | public override void UnLockVertexBase(int subpart) | ||
112 | { | ||
113 | throw new Exception("The method or operation is not implemented."); | ||
114 | } | ||
115 | |||
116 | public override void UnLockReadOnlyVertexBase(int subpart) | ||
117 | { | ||
118 | throw new Exception("The method or operation is not implemented."); | ||
119 | } | ||
120 | |||
121 | public override int SubPartsCount() | ||
122 | { | ||
123 | return _indexedMeshes.Count; | ||
124 | } | ||
125 | |||
126 | public override void PreallocateVertices(int numverts) | ||
127 | { | ||
128 | throw new Exception("The method or operation is not implemented."); | ||
129 | } | ||
130 | |||
131 | public override void PreallocateIndices(int numindices) | ||
132 | { | ||
133 | throw new Exception("The method or operation is not implemented."); | ||
134 | } | ||
135 | } | ||
136 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs new file mode 100644 index 0000000..1426355 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs | |||
@@ -0,0 +1,102 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | |||
26 | using MonoXnaCompactMaths; | ||
27 | |||
28 | namespace XnaDevRu.BulletX | ||
29 | { | ||
30 | class TriangleMesh : StridingMeshInterface | ||
31 | { | ||
32 | int _numTriangles; | ||
33 | List<Vector3> _verts; | ||
34 | |||
35 | public TriangleMesh() | ||
36 | { | ||
37 | _numTriangles = 0; | ||
38 | _verts = new List<Vector3>(); | ||
39 | } | ||
40 | |||
41 | void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2) | ||
42 | { | ||
43 | _verts.Add(vertex0); | ||
44 | _verts.Add(vertex1); | ||
45 | _verts.Add(vertex2); | ||
46 | _numTriangles++; | ||
47 | } | ||
48 | |||
49 | public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
50 | { | ||
51 | verts = new List<Vector3>(); | ||
52 | for (int i = 0; i < 3; i++) | ||
53 | { | ||
54 | verts.Add(_verts[subpart * 3 + i]); | ||
55 | } | ||
56 | indicies = new List<int>(); | ||
57 | indicies.Add(0); | ||
58 | indicies.Add(1); | ||
59 | indicies.Add(2); | ||
60 | numfaces = 1; | ||
61 | } | ||
62 | |||
63 | public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart) | ||
64 | { | ||
65 | verts = new List<Vector3>(); | ||
66 | for (int i = 0; i < 3; i++) | ||
67 | { | ||
68 | verts.Add(_verts[subpart * 3 + i]); | ||
69 | } | ||
70 | indicies = new List<int>(); | ||
71 | indicies.Add(0); | ||
72 | indicies.Add(1); | ||
73 | indicies.Add(2); | ||
74 | numfaces = 1; | ||
75 | } | ||
76 | |||
77 | public override void UnLockVertexBase(int subpart) | ||
78 | { | ||
79 | |||
80 | } | ||
81 | |||
82 | public override void UnLockReadOnlyVertexBase(int subpart) | ||
83 | { | ||
84 | |||
85 | } | ||
86 | |||
87 | public override int SubPartsCount() | ||
88 | { | ||
89 | return _numTriangles; | ||
90 | } | ||
91 | |||
92 | public override void PreallocateVertices(int numverts) | ||
93 | { | ||
94 | |||
95 | } | ||
96 | |||
97 | public override void PreallocateIndices(int numindices) | ||
98 | { | ||
99 | |||
100 | } | ||
101 | } | ||
102 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs new file mode 100644 index 0000000..bc8f09e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs | |||
@@ -0,0 +1,160 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. | ||
31 | /// </summary> | ||
32 | public class TriangleMeshShape : ConcaveShape | ||
33 | { | ||
34 | private StridingMeshInterface _meshInterface; | ||
35 | private Vector3 _localAabbMin; | ||
36 | private Vector3 _localAabbMax; | ||
37 | |||
38 | public TriangleMeshShape(StridingMeshInterface meshInterface) | ||
39 | { | ||
40 | this._meshInterface = meshInterface; | ||
41 | RecalcLocalAabb(); | ||
42 | } | ||
43 | |||
44 | protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } } | ||
45 | protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } } | ||
46 | protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } } | ||
47 | |||
48 | public override BroadphaseNativeTypes ShapeType | ||
49 | { | ||
50 | get | ||
51 | { | ||
52 | return BroadphaseNativeTypes.TriangleMesh; | ||
53 | } | ||
54 | } | ||
55 | |||
56 | public override Vector3 LocalScaling | ||
57 | { | ||
58 | get | ||
59 | { | ||
60 | return _meshInterface.Scaling; | ||
61 | } | ||
62 | set | ||
63 | { | ||
64 | _meshInterface.Scaling = value; | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public override string Name | ||
69 | { | ||
70 | get | ||
71 | { | ||
72 | return "TriangleMesh"; | ||
73 | } | ||
74 | } | ||
75 | |||
76 | public void RecalcLocalAabb() | ||
77 | { | ||
78 | { | ||
79 | Vector3 vec = new Vector3(); | ||
80 | vec.X = 1f; | ||
81 | Vector3 tmp = LocalGetSupportingVertex(vec); | ||
82 | _localAabbMax.X = tmp.X + CollisionMargin; | ||
83 | vec.X = -1f; | ||
84 | tmp = LocalGetSupportingVertex(vec); | ||
85 | _localAabbMin.X = tmp.X - CollisionMargin; | ||
86 | } | ||
87 | { | ||
88 | Vector3 vec = new Vector3(); | ||
89 | vec.Y = 1f; | ||
90 | Vector3 tmp = LocalGetSupportingVertex(vec); | ||
91 | _localAabbMax.Y = tmp.Y + CollisionMargin; | ||
92 | vec.Y = -1f; | ||
93 | tmp = LocalGetSupportingVertex(vec); | ||
94 | _localAabbMin.Y = tmp.Y - CollisionMargin; | ||
95 | } | ||
96 | { | ||
97 | Vector3 vec = new Vector3(); | ||
98 | vec.Z = 1f; | ||
99 | Vector3 tmp = LocalGetSupportingVertex(vec); | ||
100 | _localAabbMax.Z = tmp.Z + CollisionMargin; | ||
101 | vec.Z = -1f; | ||
102 | tmp = LocalGetSupportingVertex(vec); | ||
103 | _localAabbMin.Z = tmp.Z - CollisionMargin; | ||
104 | } | ||
105 | } | ||
106 | |||
107 | public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
108 | { | ||
109 | LocalProcessAllTriangles(callback, aabbMax, aabbMax); | ||
110 | } | ||
111 | |||
112 | protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) | ||
113 | { | ||
114 | FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); | ||
115 | _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax); | ||
116 | } | ||
117 | |||
118 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
119 | { | ||
120 | Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin); | ||
121 | Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin); | ||
122 | |||
123 | Matrix abs_b = MathHelper.Absolute(t); | ||
124 | |||
125 | Vector3 center = MathHelper.MatrixToVector(t, localCenter); | ||
126 | |||
127 | Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents), | ||
128 | Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents), | ||
129 | Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents)); | ||
130 | extent += new Vector3(Margin, Margin, Margin); | ||
131 | |||
132 | aabbMin = center - extent; | ||
133 | aabbMax = center + extent; | ||
134 | } | ||
135 | |||
136 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
137 | { | ||
138 | inertia = new Vector3(); | ||
139 | //moving concave objects not supported | ||
140 | BulletDebug.Assert(false); | ||
141 | } | ||
142 | |||
143 | public virtual Vector3 LocalGetSupportingVertex(Vector3 vec) | ||
144 | { | ||
145 | Vector3 supportVertex; | ||
146 | Matrix ident = Matrix.Identity; | ||
147 | SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); | ||
148 | Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f); | ||
149 | LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax); | ||
150 | supportVertex = supportCallback.SupportVertexLocal; | ||
151 | return supportVertex; | ||
152 | } | ||
153 | |||
154 | public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
155 | { | ||
156 | BulletDebug.Assert(false); | ||
157 | return LocalGetSupportingVertex(vec); | ||
158 | } | ||
159 | } | ||
160 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs new file mode 100644 index 0000000..59ffad0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs | |||
@@ -0,0 +1,187 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class TriangleShape : PolyhedralConvexShape | ||
30 | { | ||
31 | private Vector3[] _vertices = new Vector3[3]; | ||
32 | |||
33 | public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC) | ||
34 | { | ||
35 | _vertices[0] = pointA; | ||
36 | _vertices[1] = pointB; | ||
37 | _vertices[2] = pointC; | ||
38 | } | ||
39 | |||
40 | public override int PreferredPenetrationDirectionsCount | ||
41 | { | ||
42 | get | ||
43 | { | ||
44 | return 2; | ||
45 | } | ||
46 | } | ||
47 | |||
48 | public Vector3[] Vertices | ||
49 | { | ||
50 | get | ||
51 | { | ||
52 | return _vertices; | ||
53 | } | ||
54 | } | ||
55 | |||
56 | public override int VertexCount | ||
57 | { | ||
58 | get | ||
59 | { | ||
60 | return 3; | ||
61 | } | ||
62 | } | ||
63 | |||
64 | public override int EdgeCount | ||
65 | { | ||
66 | get | ||
67 | { | ||
68 | return 3; | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public override int PlaneCount | ||
73 | { | ||
74 | get | ||
75 | { | ||
76 | return 1; | ||
77 | } | ||
78 | } | ||
79 | |||
80 | public override BroadphaseNativeTypes ShapeType | ||
81 | { | ||
82 | get | ||
83 | { | ||
84 | return BroadphaseNativeTypes.Triangle; | ||
85 | } | ||
86 | } | ||
87 | |||
88 | public override string Name | ||
89 | { | ||
90 | get | ||
91 | { | ||
92 | return "Triangle"; | ||
93 | } | ||
94 | } | ||
95 | |||
96 | public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector) | ||
97 | { | ||
98 | CalculateNormal(out penetrationVector); | ||
99 | if (index != 0) | ||
100 | penetrationVector *= -1f; | ||
101 | } | ||
102 | |||
103 | public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport) | ||
104 | { | ||
105 | CalculateNormal(out planeNormal); | ||
106 | planeSupport = _vertices[0]; | ||
107 | } | ||
108 | |||
109 | public void CalculateNormal(out Vector3 normal) | ||
110 | { | ||
111 | normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0])); | ||
112 | } | ||
113 | |||
114 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | ||
115 | { | ||
116 | Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2])); | ||
117 | return _vertices[MathHelper.MaxAxis(dots)]; | ||
118 | } | ||
119 | |||
120 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | ||
121 | { | ||
122 | for (int i = 0; i < vectors.Length; i++) | ||
123 | { | ||
124 | Vector3 dir = vectors[i]; | ||
125 | Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2])); | ||
126 | supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)]; | ||
127 | } | ||
128 | } | ||
129 | |||
130 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | ||
131 | { | ||
132 | inertia = new Vector3(); | ||
133 | BulletDebug.Assert(false); | ||
134 | } | ||
135 | |||
136 | public override void GetEdge(int i, out Vector3 pa, out Vector3 pb) | ||
137 | { | ||
138 | GetVertex(i, out pa); | ||
139 | GetVertex((i + 1) % 3, out pb); | ||
140 | } | ||
141 | |||
142 | public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) | ||
143 | { | ||
144 | GetAabbSlow(t, out aabbMin, out aabbMax); | ||
145 | } | ||
146 | |||
147 | public override void GetVertex(int i, out Vector3 vtx) | ||
148 | { | ||
149 | vtx = _vertices[i]; | ||
150 | } | ||
151 | |||
152 | public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i) | ||
153 | { | ||
154 | GetPlaneEquation(i, out planeNormal, out planeSupport); | ||
155 | } | ||
156 | |||
157 | public override bool IsInside(Vector3 pt, float tolerance) | ||
158 | { | ||
159 | Vector3 normal; | ||
160 | CalculateNormal(out normal); | ||
161 | //distance to plane | ||
162 | float dist = Vector3.Dot(pt, normal); | ||
163 | float planeconst = Vector3.Dot(_vertices[0], normal); | ||
164 | dist -= planeconst; | ||
165 | if (dist >= -tolerance && dist <= tolerance) | ||
166 | { | ||
167 | //inside check on edge-planes | ||
168 | int i; | ||
169 | for (i = 0; i < 3; i++) | ||
170 | { | ||
171 | Vector3 pa, pb; | ||
172 | GetEdge(i, out pa, out pb); | ||
173 | Vector3 edge = pb - pa; | ||
174 | Vector3 edgeNormal = Vector3.Cross(edge, normal); | ||
175 | edgeNormal = Vector3.Normalize(edgeNormal); | ||
176 | float distance = Vector3.Dot(pt, edgeNormal); | ||
177 | float edgeConst = Vector3.Dot(pa, edgeNormal); | ||
178 | distance -= edgeConst; | ||
179 | if (distance < -tolerance) | ||
180 | return false; | ||
181 | } | ||
182 | return true; | ||
183 | } | ||
184 | return false; | ||
185 | } | ||
186 | } | ||
187 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs new file mode 100644 index 0000000..497f220 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs | |||
@@ -0,0 +1,199 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// ContinuousConvexCollision implements angular and linear time of impact for convex objects. | ||
31 | /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). | ||
32 | /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. | ||
33 | /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops | ||
34 | /// </summary> | ||
35 | public class ContinuousConvexCollision : IConvexCast | ||
36 | { | ||
37 | /// <summary> | ||
38 | /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. | ||
39 | /// You don't want your game ever to lock-up. | ||
40 | /// </summary> | ||
41 | private const int MaxIterations = 1000; | ||
42 | |||
43 | private ISimplexSolver _simplexSolver; | ||
44 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | ||
45 | private ConvexShape _convexA; | ||
46 | private ConvexShape _convexB; | ||
47 | |||
48 | public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB, | ||
49 | ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
50 | { | ||
51 | _simplexSolver = simplexSolver; | ||
52 | _penetrationDepthSolver = penetrationDepthSolver; | ||
53 | _convexA = convexA; | ||
54 | _convexB = convexB; | ||
55 | } | ||
56 | |||
57 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) | ||
58 | { | ||
59 | _simplexSolver.Reset(); | ||
60 | |||
61 | // compute linear and angular velocity for this interval, to interpolate | ||
62 | Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3(); | ||
63 | TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA); | ||
64 | TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB); | ||
65 | |||
66 | float boundingRadiusA = _convexA.GetAngularMotionDisc(); | ||
67 | float boundingRadiusB = _convexB.GetAngularMotionDisc(); | ||
68 | |||
69 | float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + | ||
70 | angVelB.Length() * boundingRadiusB; | ||
71 | |||
72 | float radius = 0.001f; | ||
73 | |||
74 | float lambda = 0f; | ||
75 | Vector3 v = new Vector3(1f, 0f, 0f); | ||
76 | |||
77 | int maxIter = MaxIterations; | ||
78 | |||
79 | Vector3 n = new Vector3(); | ||
80 | bool hasResult = false; | ||
81 | Vector3 c; | ||
82 | |||
83 | float lastLambda = lambda; | ||
84 | //float epsilon = 0.001f; | ||
85 | |||
86 | int numIter = 0; | ||
87 | //first solution, using GJK | ||
88 | |||
89 | |||
90 | Matrix identityTrans = Matrix.Identity; | ||
91 | |||
92 | SphereShape raySphere = new SphereShape(0f); | ||
93 | raySphere.Margin=0f; | ||
94 | |||
95 | |||
96 | //result.drawCoordSystem(sphereTr); | ||
97 | |||
98 | PointCollector pointCollector1 = new PointCollector(); | ||
99 | |||
100 | GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); | ||
101 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
102 | |||
103 | //we don't use margins during CCD | ||
104 | gjk.setIgnoreMargin(true); | ||
105 | |||
106 | input.TransformA = fromA; | ||
107 | input.TransformB = fromB; | ||
108 | |||
109 | DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; | ||
110 | gjk.GetClosestPoints(input, r, null); | ||
111 | |||
112 | hasResult = pointCollector1.HasResult; | ||
113 | c = pointCollector1.PointInWorld; | ||
114 | |||
115 | if (hasResult) | ||
116 | { | ||
117 | float dist; | ||
118 | dist = pointCollector1.Distance; | ||
119 | n = pointCollector1.NormalOnBInWorld; | ||
120 | |||
121 | //not close enough | ||
122 | while (dist > radius) | ||
123 | { | ||
124 | numIter++; | ||
125 | if (numIter > maxIter) | ||
126 | return false; //todo: report a failure | ||
127 | |||
128 | float dLambda = 0f; | ||
129 | |||
130 | //calculate safe moving fraction from distance / (linear+rotational velocity) | ||
131 | |||
132 | //float clippedDist = GEN_min(angularConservativeRadius,dist); | ||
133 | //float clippedDist = dist; | ||
134 | |||
135 | float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n); | ||
136 | |||
137 | dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); | ||
138 | |||
139 | lambda = lambda + dLambda; | ||
140 | |||
141 | if (lambda > 1f) return false; | ||
142 | if (lambda < 0f) return false; | ||
143 | |||
144 | //todo: next check with relative epsilon | ||
145 | if (lambda <= lastLambda) | ||
146 | break; | ||
147 | lastLambda = lambda; | ||
148 | |||
149 | |||
150 | //interpolate to next lambda | ||
151 | Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans; | ||
152 | |||
153 | TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA); | ||
154 | TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB); | ||
155 | |||
156 | relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA); | ||
157 | |||
158 | result.DebugDraw(lambda); | ||
159 | |||
160 | PointCollector pointCollector = new PointCollector(); | ||
161 | gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver); | ||
162 | input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
163 | input.TransformA = interpolatedTransA; | ||
164 | input.TransformB = interpolatedTransB; | ||
165 | |||
166 | // !!!!!!!!!! | ||
167 | r = (DiscreteCollisionDetectorInterface.Result)pointCollector1; | ||
168 | gjk.GetClosestPoints(input, r, null); | ||
169 | |||
170 | if (pointCollector.HasResult) | ||
171 | { | ||
172 | if (pointCollector.Distance < 0f) | ||
173 | { | ||
174 | //degenerate ?! | ||
175 | result.Fraction = lastLambda; | ||
176 | result.Normal = n; | ||
177 | return true; | ||
178 | } | ||
179 | c = pointCollector.PointInWorld; | ||
180 | |||
181 | dist = pointCollector.Distance; | ||
182 | } | ||
183 | else | ||
184 | { | ||
185 | //?? | ||
186 | return false; | ||
187 | } | ||
188 | |||
189 | } | ||
190 | |||
191 | result.Fraction = lambda; | ||
192 | result.Normal = n; | ||
193 | return true; | ||
194 | } | ||
195 | |||
196 | return false; | ||
197 | } | ||
198 | } | ||
199 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs new file mode 100644 index 0000000..9e88a7e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs | |||
@@ -0,0 +1,73 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | |||
30 | /// <summary> | ||
31 | /// CastResult stores the closest result | ||
32 | /// alternatively, add a callback method to decide about closest/all results | ||
33 | /// </summary> | ||
34 | public class CastResult | ||
35 | { | ||
36 | private Vector3 _normal; | ||
37 | private float _fraction; | ||
38 | private Matrix _hitTransformA; | ||
39 | private Matrix _hitTransformB; | ||
40 | private IDebugDraw _debugDrawer; | ||
41 | |||
42 | public CastResult() | ||
43 | { | ||
44 | _fraction = 1e30f; | ||
45 | } | ||
46 | |||
47 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | ||
48 | public float Fraction { get { return _fraction; } set { _fraction = value; } } | ||
49 | public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } } | ||
50 | public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } } | ||
51 | public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } } | ||
52 | |||
53 | public virtual void DebugDraw(float fraction) { } | ||
54 | public virtual void DrawCoordSystem(Matrix trans) { } | ||
55 | } | ||
56 | |||
57 | /// <summary> | ||
58 | /// ConvexCast is an interface for Casting | ||
59 | /// </summary> | ||
60 | public interface IConvexCast | ||
61 | { | ||
62 | /// <summary> | ||
63 | /// cast a convex against another convex object | ||
64 | /// </summary> | ||
65 | /// <param name="fromA"></param> | ||
66 | /// <param name="toA"></param> | ||
67 | /// <param name="fromB"></param> | ||
68 | /// <param name="toB"></param> | ||
69 | /// <param name="result"></param> | ||
70 | /// <returns></returns> | ||
71 | bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result); | ||
72 | } | ||
73 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs new file mode 100644 index 0000000..fae2557 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs | |||
@@ -0,0 +1,117 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class DiscreteCollisionDetectorInterface | ||
30 | { | ||
31 | public abstract class Result | ||
32 | { | ||
33 | public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB); | ||
34 | public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth); | ||
35 | } | ||
36 | |||
37 | public class ClosestPointInput | ||
38 | { | ||
39 | private float _maximumDistanceSquared; | ||
40 | private Matrix _transformA, _transformB; | ||
41 | |||
42 | #region Properties | ||
43 | public Matrix TransformB | ||
44 | { | ||
45 | get { return _transformB; } | ||
46 | set { _transformB = value; } | ||
47 | } | ||
48 | |||
49 | public Matrix TransformA | ||
50 | { | ||
51 | get { return _transformA; } | ||
52 | set { _transformA = value; } | ||
53 | } | ||
54 | |||
55 | public float MaximumDistanceSquared | ||
56 | { | ||
57 | get { return _maximumDistanceSquared; } | ||
58 | set { _maximumDistanceSquared = value; } | ||
59 | } | ||
60 | #endregion | ||
61 | |||
62 | public ClosestPointInput() | ||
63 | { | ||
64 | _maximumDistanceSquared = 1e30f; | ||
65 | } | ||
66 | } | ||
67 | |||
68 | public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw); | ||
69 | } | ||
70 | |||
71 | public class StorageResult : DiscreteCollisionDetectorInterface.Result | ||
72 | { | ||
73 | private Vector3 _closestPointInB; | ||
74 | private Vector3 _normalOnSurfaceB; | ||
75 | private float _distance; //negative means penetration ! | ||
76 | |||
77 | #region Properties | ||
78 | |||
79 | public float Distance | ||
80 | { | ||
81 | get { return _distance; } | ||
82 | set { _distance = value; } | ||
83 | } | ||
84 | public Vector3 NormalOnSurfaceB | ||
85 | { | ||
86 | get { return _normalOnSurfaceB; } | ||
87 | set { _normalOnSurfaceB = value; } | ||
88 | } | ||
89 | public Vector3 ClosestPointInB | ||
90 | { | ||
91 | get { return _closestPointInB; } | ||
92 | set { _closestPointInB = value; } | ||
93 | } | ||
94 | |||
95 | #endregion | ||
96 | |||
97 | public StorageResult() | ||
98 | { | ||
99 | _distance = 1e30f; | ||
100 | } | ||
101 | |||
102 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
103 | { | ||
104 | if (depth < _distance) | ||
105 | { | ||
106 | _normalOnSurfaceB = normalOnBInWorld; | ||
107 | _closestPointInB = pointInWorld; | ||
108 | _distance = depth; | ||
109 | } | ||
110 | } | ||
111 | |||
112 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) | ||
113 | { | ||
114 | throw new Exception("The method or operation is not implemented."); | ||
115 | } | ||
116 | } | ||
117 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs new file mode 100644 index 0000000..4ee9599 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs | |||
@@ -0,0 +1,176 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// GjkConvexCast performs a raycast on a convex object using support mapping. | ||
31 | /// </summary> | ||
32 | public class GjkConvexCast : IConvexCast | ||
33 | { | ||
34 | private VoronoiSimplexSolver _simplexSolver; | ||
35 | private ConvexShape _convexA, _convexB; | ||
36 | |||
37 | public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver) | ||
38 | { | ||
39 | _simplexSolver = solver; | ||
40 | |||
41 | _convexA = convexShapeA; | ||
42 | _convexB = convexShapeB; | ||
43 | } | ||
44 | |||
45 | #region IConvexCast Members | ||
46 | |||
47 | /// <summary> | ||
48 | /// cast a convex against another convex object | ||
49 | /// </summary> | ||
50 | /// <param name="fromA"></param> | ||
51 | /// <param name="toA"></param> | ||
52 | /// <param name="fromB"></param> | ||
53 | /// <param name="toB"></param> | ||
54 | /// <param name="result"></param> | ||
55 | /// <returns></returns> | ||
56 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) | ||
57 | { | ||
58 | MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB); | ||
59 | |||
60 | Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; | ||
61 | Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB; | ||
62 | |||
63 | Matrix transformA = fromA; | ||
64 | Matrix transformB = fromB; | ||
65 | |||
66 | transformA.Translation = new Vector3(0, 0, 0); | ||
67 | transformB.Translation = new Vector3(0, 0, 0); | ||
68 | |||
69 | combined.TransformA = transformA; | ||
70 | combined.TransformB = transformB; | ||
71 | |||
72 | float radius = 0.01f; | ||
73 | float lambda = 0; | ||
74 | |||
75 | Vector3 s = rayFromLocalA.Translation; | ||
76 | Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation; | ||
77 | Vector3 x = s; | ||
78 | Vector3 n = new Vector3(); | ||
79 | Vector3 c = new Vector3(); | ||
80 | |||
81 | bool hasResult = false; | ||
82 | float lastLambda = lambda; | ||
83 | |||
84 | IConvexPenetrationDepthSolver penSolver = null; | ||
85 | Matrix identityTransform = Matrix.Identity; | ||
86 | |||
87 | SphereShape raySphere = new SphereShape(0.0f); | ||
88 | raySphere.Margin=0.0f; | ||
89 | |||
90 | Matrix sphereTransform = Matrix.Identity; | ||
91 | sphereTransform.Translation = rayFromLocalA.Translation; | ||
92 | |||
93 | result.DrawCoordSystem(sphereTransform); | ||
94 | |||
95 | { | ||
96 | PointCollector pointCollector = new PointCollector(); | ||
97 | GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); | ||
98 | |||
99 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
100 | input.TransformA = sphereTransform; | ||
101 | input.TransformB = identityTransform; | ||
102 | |||
103 | gjk.GetClosestPoints(input, pointCollector, null); | ||
104 | |||
105 | hasResult = pointCollector.HasResult; | ||
106 | |||
107 | c = pointCollector.PointInWorld; | ||
108 | n = pointCollector.NormalOnBInWorld; | ||
109 | } | ||
110 | |||
111 | if (hasResult) | ||
112 | { | ||
113 | float dist = (c - x).Length(); | ||
114 | |||
115 | if (dist < radius) | ||
116 | { | ||
117 | lastLambda = 1.0f; | ||
118 | } | ||
119 | |||
120 | while (dist > radius) | ||
121 | { | ||
122 | n = x - c; | ||
123 | float dot = Vector3.Dot(n, r); | ||
124 | |||
125 | if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false; | ||
126 | |||
127 | lambda = lambda - Vector3.Distance(n, n) / dot; | ||
128 | if (lambda <= lastLambda) break; | ||
129 | |||
130 | lastLambda = lambda; | ||
131 | |||
132 | x = s + lambda * r; | ||
133 | |||
134 | sphereTransform.Translation = x; | ||
135 | result.DrawCoordSystem(sphereTransform); | ||
136 | PointCollector pointCollector = new PointCollector(); | ||
137 | |||
138 | GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver); | ||
139 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
140 | input.TransformA = sphereTransform; | ||
141 | input.TransformB = identityTransform; | ||
142 | |||
143 | gjk.GetClosestPoints(input, pointCollector, null); | ||
144 | |||
145 | if (pointCollector.HasResult) | ||
146 | { | ||
147 | if (pointCollector.Distance < 0.0f) | ||
148 | { | ||
149 | result.Fraction = lastLambda; | ||
150 | result.Normal = n; | ||
151 | return true; | ||
152 | } | ||
153 | |||
154 | c = pointCollector.PointInWorld; | ||
155 | dist = (c - x).Length(); | ||
156 | } | ||
157 | else | ||
158 | { | ||
159 | return false; | ||
160 | } | ||
161 | } | ||
162 | |||
163 | if (lastLambda < 1.0f) | ||
164 | { | ||
165 | result.Fraction = lastLambda; | ||
166 | result.Normal = n; | ||
167 | return true; | ||
168 | } | ||
169 | } | ||
170 | |||
171 | return false; | ||
172 | } | ||
173 | |||
174 | #endregion | ||
175 | } | ||
176 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs new file mode 100644 index 0000000..50ae7ab --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs | |||
@@ -0,0 +1,633 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// GJK-EPA collision solver by Nathanael Presson | ||
31 | /// Nov.2006 | ||
32 | /// </summary> | ||
33 | public class GjkEpa | ||
34 | { | ||
35 | //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/; | ||
36 | |||
37 | private static readonly float _infinity = MathHelper.Infinity; | ||
38 | //private static readonly float _pi = (float)Math.PI; | ||
39 | private static readonly float _twoPi = (float)(Math.PI * 2); | ||
40 | |||
41 | private static readonly int _gjkMaxIterations = 128; | ||
42 | private static readonly int _gjkHashSize = 1 << 6; | ||
43 | private static readonly int _gjkHashMask = _gjkHashSize - 1; | ||
44 | private static readonly float _gjkInSimplexEpsilon = 0.0001f; | ||
45 | private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon; | ||
46 | |||
47 | private static readonly int _epaMaxIterations = 256; | ||
48 | private static readonly float _epaInFaceEpsilon = 0.01f; | ||
49 | private static readonly float _epaAccuracy = 0.001f; | ||
50 | |||
51 | public static float EpaAccuracy { get { return _epaAccuracy; } } | ||
52 | |||
53 | private static float Abs(float v) { return (v < 0 ? -v : v); } | ||
54 | private static float Sign(float v) { return (v < 0 ? -1 : 1); } | ||
55 | |||
56 | static void Swap<T>(ref T a, ref T b) | ||
57 | { | ||
58 | T t = a; | ||
59 | a = b; | ||
60 | b = t; | ||
61 | } | ||
62 | |||
63 | public class Gjk | ||
64 | { | ||
65 | public class MinkowskiVertice | ||
66 | { | ||
67 | private Vector3 _vertice; /* Minkowski vertice */ | ||
68 | private Vector3 _ray; /* Ray */ | ||
69 | |||
70 | public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } } | ||
71 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | ||
72 | } | ||
73 | public class He | ||
74 | { | ||
75 | private Vector3 _ray; | ||
76 | private He _next; | ||
77 | |||
78 | public He Next { get { return _next; } set { _next = value; } } | ||
79 | public Vector3 Ray { get { return _ray; } set { _ray = value; } } | ||
80 | } | ||
81 | |||
82 | private He[] _table = new He[_gjkHashSize]; | ||
83 | private Matrix[] _wrotations = new Matrix[2]; | ||
84 | private Vector3[] _positions = new Vector3[2]; | ||
85 | private ConvexShape[] _shapes = new ConvexShape[2]; | ||
86 | private MinkowskiVertice[] _simplex = new MinkowskiVertice[5]; | ||
87 | private Vector3 _ray; | ||
88 | private int _order; | ||
89 | private int _iterations; | ||
90 | private float _margin; | ||
91 | private bool _failed; | ||
92 | |||
93 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | ||
94 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB) | ||
95 | : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { } | ||
96 | |||
97 | public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA, | ||
98 | Matrix wrotationB, Vector3 positionB, ConvexShape shapeB, | ||
99 | float pmargin) | ||
100 | { | ||
101 | for (int i = 0; i < _simplex.Length; i++) | ||
102 | _simplex[i] = new MinkowskiVertice(); | ||
103 | |||
104 | for (int i = 0; i < _wrotations.Length; i++) | ||
105 | _wrotations[i] = new Matrix(); | ||
106 | |||
107 | for (int i = 0; i < _positions.Length; i++) | ||
108 | _positions[i] = new Vector3(); | ||
109 | |||
110 | _wrotations[0] = wrotationA; _positions[0] = positionA; | ||
111 | _shapes[0] = shapeA; | ||
112 | _wrotations[0].Translation = Vector3.Zero; | ||
113 | _wrotations[1] = wrotationB; _positions[1] = positionB; | ||
114 | _shapes[1] = shapeB; | ||
115 | _wrotations[1].Translation = Vector3.Zero; | ||
116 | //sablock = sa->BeginBlock(); | ||
117 | _margin = pmargin; | ||
118 | _failed = false; | ||
119 | } | ||
120 | |||
121 | public bool Failed { get { return _failed; } } | ||
122 | public int Iterations { get { return _iterations; } } | ||
123 | public int Order { get { return _order; } } | ||
124 | public MinkowskiVertice[] Simplex { get { return _simplex; } } | ||
125 | |||
126 | public int Hash(Vector3 v) | ||
127 | { | ||
128 | int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473)); | ||
129 | return (h * 169639) & _gjkHashMask; | ||
130 | } | ||
131 | |||
132 | public bool FetchSupport() | ||
133 | { | ||
134 | int h = Hash(_ray); | ||
135 | He e = _table[h]; | ||
136 | while (e != null) | ||
137 | { | ||
138 | if (e.Ray == _ray) | ||
139 | { | ||
140 | --_order; | ||
141 | return (false); | ||
142 | } | ||
143 | else e = e.Next; | ||
144 | } | ||
145 | e = new He(); | ||
146 | e.Ray = _ray; | ||
147 | e.Next = _table[h]; | ||
148 | _table[h] = e; | ||
149 | Support(_ray, ref _simplex[++_order]); | ||
150 | return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0); | ||
151 | } | ||
152 | |||
153 | public Vector3 LocalSupport(Vector3 d, int i) | ||
154 | { | ||
155 | Matrix m = _wrotations[i]; | ||
156 | m.Translation = Vector3.Zero; | ||
157 | Vector3 vtx = Vector3.TransformNormal(d, m); | ||
158 | Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx)); | ||
159 | return (result + _positions[i]); | ||
160 | } | ||
161 | |||
162 | public void Support(Vector3 d, ref MinkowskiVertice v) | ||
163 | { | ||
164 | v.Ray = d; | ||
165 | v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin; | ||
166 | } | ||
167 | |||
168 | public bool SolveSimplex2(Vector3 ao, Vector3 ab) | ||
169 | { | ||
170 | if (Vector3.Dot(ab, ao) >= 0) | ||
171 | { | ||
172 | Vector3 cabo = Vector3.Cross(ab, ao); | ||
173 | if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon) | ||
174 | { _ray = Vector3.Cross(cabo, ab); } | ||
175 | else | ||
176 | { return true; } | ||
177 | } | ||
178 | else | ||
179 | { | ||
180 | _order = 0; | ||
181 | _simplex[0].Ray = _simplex[1].Ray; | ||
182 | _simplex[0].Vertice = _simplex[1].Vertice; | ||
183 | |||
184 | _ray = ao; | ||
185 | } | ||
186 | return false; | ||
187 | } | ||
188 | |||
189 | public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac) | ||
190 | { | ||
191 | return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac))); | ||
192 | } | ||
193 | |||
194 | public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc) | ||
195 | { | ||
196 | if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon) | ||
197 | { | ||
198 | _order = 1; | ||
199 | _simplex[0].Vertice = _simplex[1].Vertice; | ||
200 | _simplex[0].Ray = _simplex[1].Ray; | ||
201 | |||
202 | _simplex[1].Vertice = _simplex[2].Vertice; | ||
203 | _simplex[1].Ray = _simplex[2].Ray; | ||
204 | |||
205 | return (SolveSimplex2(ao, ab)); | ||
206 | } | ||
207 | else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon) | ||
208 | { | ||
209 | _order = 1; | ||
210 | _simplex[1].Vertice = _simplex[2].Vertice; | ||
211 | _simplex[1].Ray = _simplex[2].Ray; | ||
212 | |||
213 | return (SolveSimplex2(ao, ac)); | ||
214 | } | ||
215 | else | ||
216 | { | ||
217 | float d = Vector3.Dot(cabc, ao); | ||
218 | if (Abs(d) > _gjkInSimplexEpsilon) | ||
219 | { | ||
220 | if (d > 0) | ||
221 | { _ray = cabc; } | ||
222 | else | ||
223 | { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); } | ||
224 | return (false); | ||
225 | } | ||
226 | else return (true); | ||
227 | } | ||
228 | } | ||
229 | |||
230 | public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad) | ||
231 | { | ||
232 | Vector3 crs; | ||
233 | if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon) | ||
234 | { | ||
235 | _order = 2; | ||
236 | _simplex[0].Vertice = _simplex[1].Vertice; | ||
237 | _simplex[0].Ray = _simplex[1].Ray; | ||
238 | |||
239 | _simplex[1].Vertice = _simplex[2].Vertice; | ||
240 | _simplex[1].Ray = _simplex[2].Ray; | ||
241 | |||
242 | _simplex[2].Vertice = _simplex[3].Vertice; | ||
243 | _simplex[2].Ray = _simplex[3].Ray; | ||
244 | |||
245 | return (SolveSimplex3a(ao, ab, ac, crs)); | ||
246 | } | ||
247 | else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon) | ||
248 | { | ||
249 | _order = 2; | ||
250 | _simplex[2].Vertice = _simplex[3].Vertice; | ||
251 | _simplex[2].Ray = _simplex[3].Ray; | ||
252 | |||
253 | return (SolveSimplex3a(ao, ac, ad, crs)); | ||
254 | } | ||
255 | else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon) | ||
256 | { | ||
257 | _order = 2; | ||
258 | |||
259 | _simplex[1].Vertice = _simplex[0].Vertice; | ||
260 | _simplex[1].Ray = _simplex[0].Ray; | ||
261 | |||
262 | _simplex[0].Vertice = _simplex[2].Vertice; | ||
263 | _simplex[0].Ray = _simplex[2].Ray; | ||
264 | |||
265 | _simplex[2].Vertice = _simplex[3].Vertice; | ||
266 | _simplex[2].Ray = _simplex[3].Ray; | ||
267 | |||
268 | return (SolveSimplex3a(ao, ad, ab, crs)); | ||
269 | } | ||
270 | else return (true); | ||
271 | } | ||
272 | |||
273 | public bool SearchOrigin() | ||
274 | { | ||
275 | return SearchOrigin(new Vector3(1, 0, 0)); | ||
276 | } | ||
277 | |||
278 | public bool SearchOrigin(Vector3 initray) | ||
279 | { | ||
280 | _iterations = 0; | ||
281 | unchecked | ||
282 | { | ||
283 | _order = (int)(-1); | ||
284 | } | ||
285 | _failed = false; | ||
286 | _ray = Vector3.Normalize(initray); | ||
287 | |||
288 | //ClearMemory(table, sizeof(void*) * GJK_hashsize); | ||
289 | for (int i = 0; i < _table.Length; i++) | ||
290 | _table[i] = null; | ||
291 | FetchSupport(); | ||
292 | _ray = -_simplex[0].Vertice; | ||
293 | for (; _iterations < _gjkMaxIterations; ++_iterations) | ||
294 | { | ||
295 | float rl = _ray.Length(); | ||
296 | _ray /= rl > 0 ? rl : 1; | ||
297 | if (FetchSupport()) | ||
298 | { | ||
299 | bool found = (false); | ||
300 | switch (_order) | ||
301 | { | ||
302 | case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break; | ||
303 | case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break; | ||
304 | case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break; | ||
305 | } | ||
306 | if (found) return (true); | ||
307 | } | ||
308 | else return (false); | ||
309 | } | ||
310 | _failed = true; | ||
311 | return (false); | ||
312 | } | ||
313 | |||
314 | public bool EncloseOrigin() | ||
315 | { | ||
316 | switch (_order) | ||
317 | { | ||
318 | /* Point */ | ||
319 | case 0: break; | ||
320 | /* Line */ | ||
321 | case 1: | ||
322 | Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice; | ||
323 | Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)), | ||
324 | Vector3.Cross(ab, new Vector3(0, 1, 0)), | ||
325 | Vector3.Cross(ab, new Vector3(0, 0, 1)) }; | ||
326 | float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() }; | ||
327 | Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3)); | ||
328 | Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]; | ||
329 | Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r); | ||
330 | Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r); | ||
331 | Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r); | ||
332 | _order = 4; | ||
333 | return true; | ||
334 | /* Triangle */ | ||
335 | case 2: | ||
336 | Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice)); | ||
337 | Support(n, ref _simplex[3]); | ||
338 | Support(-n, ref _simplex[4]); | ||
339 | _order = 4; | ||
340 | return true; | ||
341 | /* Tetrahedron */ | ||
342 | case 3: return (true); | ||
343 | /* Hexahedron */ | ||
344 | case 4: return (true); | ||
345 | } | ||
346 | return (false); | ||
347 | } | ||
348 | } | ||
349 | |||
350 | public class Epa | ||
351 | { | ||
352 | public class Face | ||
353 | { | ||
354 | public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3]; | ||
355 | public Face[] _faces = new Face[3]; | ||
356 | public int[] _e = new int[3]; | ||
357 | public Vector3 _n; | ||
358 | public float _d; | ||
359 | public int _mark; | ||
360 | public Face _prev; | ||
361 | public Face _next; | ||
362 | } | ||
363 | |||
364 | private Gjk _gjk; | ||
365 | private Face _root; | ||
366 | private int _nfaces; | ||
367 | private int _iterations; | ||
368 | private Vector3[,] _features = new Vector3[2, 3]; | ||
369 | private Vector3[] _nearest = new Vector3[2]; | ||
370 | private Vector3 _normal; | ||
371 | private float _depth; | ||
372 | private bool _failed; | ||
373 | |||
374 | public Epa(Gjk gjk) | ||
375 | { | ||
376 | this._gjk = gjk; | ||
377 | } | ||
378 | |||
379 | public bool Failed { get { return _failed; } } | ||
380 | public int Iterations { get { return _iterations; } } | ||
381 | public Vector3 Normal { get { return _normal; } } | ||
382 | public Vector3[] Nearest { get { return _nearest; } } | ||
383 | |||
384 | public Vector3 GetCoordinates(Face face) | ||
385 | { | ||
386 | Vector3 o = face._n * -face._d; | ||
387 | float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(), | ||
388 | Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(), | ||
389 | Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()}; | ||
390 | float sm = a[0] + a[1] + a[2]; | ||
391 | return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1)); | ||
392 | } | ||
393 | |||
394 | public Face FindBest() | ||
395 | { | ||
396 | Face bf = null; | ||
397 | if (_root != null) | ||
398 | { | ||
399 | Face cf = _root; | ||
400 | float bd = _infinity; | ||
401 | do | ||
402 | { | ||
403 | if (cf._d < bd) { bd = cf._d; bf = cf; } | ||
404 | } while (null != (cf = cf._next)); | ||
405 | } | ||
406 | return bf; | ||
407 | } | ||
408 | |||
409 | public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | ||
410 | { | ||
411 | Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice); | ||
412 | float len = nrm.Length(); | ||
413 | bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon && | ||
414 | Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon && | ||
415 | Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon); | ||
416 | f._vertices[0] = a; | ||
417 | f._vertices[1] = b; | ||
418 | f._vertices[2] = c; | ||
419 | f._mark = 0; | ||
420 | f._n = nrm / (len > 0 ? len : _infinity); | ||
421 | f._d = Max(0, -Vector3.Dot(f._n, a.Vertice)); | ||
422 | return valid; | ||
423 | } | ||
424 | |||
425 | public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c) | ||
426 | { | ||
427 | Face pf = new Face(); | ||
428 | if (Set(ref pf, a, b, c)) | ||
429 | { | ||
430 | if (_root != null) _root._prev = pf; | ||
431 | pf._prev = null; | ||
432 | pf._next = _root; | ||
433 | _root = pf; | ||
434 | ++_nfaces; | ||
435 | } | ||
436 | else | ||
437 | { | ||
438 | pf._prev = pf._next = null; | ||
439 | } | ||
440 | return (pf); | ||
441 | } | ||
442 | |||
443 | public void Detach(ref Face face) | ||
444 | { | ||
445 | if (face._prev != null || face._next != null) | ||
446 | { | ||
447 | --_nfaces; | ||
448 | if (face == _root) | ||
449 | { | ||
450 | _root = face._next; | ||
451 | _root._prev = null; | ||
452 | } | ||
453 | else | ||
454 | { | ||
455 | if (face._next == null) | ||
456 | { | ||
457 | face._prev._next = null; | ||
458 | } | ||
459 | else | ||
460 | { | ||
461 | face._prev._next = face._next; | ||
462 | face._next._prev = face._prev; | ||
463 | } | ||
464 | } | ||
465 | face._prev = face._next = null; | ||
466 | } | ||
467 | } | ||
468 | |||
469 | public void Link(ref Face f0, int e0, ref Face f1, int e1) | ||
470 | { | ||
471 | f0._faces[e0] = f1; f1._e[e1] = e0; | ||
472 | f1._faces[e1] = f0; f0._e[e0] = e1; | ||
473 | } | ||
474 | |||
475 | public Gjk.MinkowskiVertice Support(Vector3 w) | ||
476 | { | ||
477 | Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice(); | ||
478 | _gjk.Support(w, ref v); | ||
479 | return v; | ||
480 | } | ||
481 | |||
482 | private static int[] mod3 ={ 0, 1, 2, 0, 1 }; | ||
483 | |||
484 | public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff) | ||
485 | { | ||
486 | int ne = (0); | ||
487 | if (f._mark != markid) | ||
488 | { | ||
489 | int e1 = (mod3[e + 1]); | ||
490 | if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0) | ||
491 | { | ||
492 | Face nf = NewFace(f._vertices[e1], f._vertices[e], w); | ||
493 | Link(ref nf, 0, ref f, e); | ||
494 | if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf; | ||
495 | cf = nf; ne = 1; | ||
496 | } | ||
497 | else | ||
498 | { | ||
499 | int e2 = (mod3[e + 2]); | ||
500 | Detach(ref f); | ||
501 | f._mark = markid; | ||
502 | ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff); | ||
503 | ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff); | ||
504 | } | ||
505 | } | ||
506 | return (ne); | ||
507 | } | ||
508 | |||
509 | public float EvaluatePD() | ||
510 | { | ||
511 | return EvaluatePD(_epaAccuracy); | ||
512 | } | ||
513 | |||
514 | private int[,] fidx; | ||
515 | private int[,] eidx; | ||
516 | |||
517 | public float EvaluatePD(float accuracy) | ||
518 | { | ||
519 | //Block* sablock = sa->BeginBlock(); | ||
520 | Face bestface = null; | ||
521 | int markid = 1; | ||
522 | _depth = -_infinity; | ||
523 | _normal = new Vector3(); | ||
524 | _root = null; | ||
525 | _nfaces = 0; | ||
526 | _iterations = 0; | ||
527 | _failed = false; | ||
528 | /* Prepare hull */ | ||
529 | if (_gjk.EncloseOrigin()) | ||
530 | { | ||
531 | int nfidx = 0; | ||
532 | int neidx = 0; | ||
533 | Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5]; | ||
534 | Face[] basefaces = new Face[6]; | ||
535 | switch (_gjk.Order) | ||
536 | { | ||
537 | /* Tetrahedron */ | ||
538 | case 3: | ||
539 | { | ||
540 | fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } }; | ||
541 | eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } }; | ||
542 | nfidx = 4; neidx = 6; | ||
543 | } break; | ||
544 | /* Hexahedron */ | ||
545 | case 4: | ||
546 | { | ||
547 | fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } }; | ||
548 | eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } }; | ||
549 | nfidx = 6; neidx = 9; | ||
550 | } break; | ||
551 | } | ||
552 | int i; | ||
553 | |||
554 | for (i = 0; i <= _gjk.Order; ++i) | ||
555 | { | ||
556 | //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); | ||
557 | basemkv[i] = new Gjk.MinkowskiVertice(); | ||
558 | basemkv[i].Vertice = _gjk.Simplex[i].Vertice; | ||
559 | basemkv[i].Ray = _gjk.Simplex[i].Ray; | ||
560 | } | ||
561 | for (i = 0; i < nfidx; ++i) | ||
562 | { | ||
563 | basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]); | ||
564 | } | ||
565 | for (i = 0; i < neidx; ++i) | ||
566 | { | ||
567 | Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]); | ||
568 | } | ||
569 | } | ||
570 | if (0 == _nfaces) | ||
571 | { | ||
572 | return _depth; | ||
573 | } | ||
574 | /* Expand hull */ | ||
575 | for (; _iterations < _epaMaxIterations; ++_iterations) | ||
576 | { | ||
577 | Face bf = FindBest(); | ||
578 | if (bf != null) | ||
579 | { | ||
580 | Gjk.MinkowskiVertice w = Support(-bf._n); | ||
581 | float d = Vector3.Dot(bf._n, w.Vertice) + bf._d; | ||
582 | bestface = bf; | ||
583 | if (d < -accuracy) | ||
584 | { | ||
585 | Face cf = null; | ||
586 | Face ff = null; | ||
587 | int nf = 0; | ||
588 | Detach(ref bf); | ||
589 | bf._mark = ++markid; | ||
590 | for (int i = 0; i < 3; ++i) | ||
591 | { | ||
592 | nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff); | ||
593 | } | ||
594 | if (nf <= 2) { break; } | ||
595 | Link(ref cf, 1, ref ff, 2); | ||
596 | } | ||
597 | else break; | ||
598 | } | ||
599 | else break; | ||
600 | } | ||
601 | /* Extract contact */ | ||
602 | if (bestface != null) | ||
603 | { | ||
604 | Vector3 b = GetCoordinates(bestface); | ||
605 | _normal = bestface._n; | ||
606 | _depth = Max(0, bestface._d); | ||
607 | for (int i = 0; i < 2; ++i) | ||
608 | { | ||
609 | float s = i != 0 ? -1 : 1; | ||
610 | for (int j = 0; j < 3; ++j) | ||
611 | { | ||
612 | _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i); | ||
613 | } | ||
614 | } | ||
615 | _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z; | ||
616 | _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z; | ||
617 | } | ||
618 | else _failed = true; | ||
619 | return _depth; | ||
620 | } | ||
621 | |||
622 | private float Max(float a, float b) | ||
623 | { | ||
624 | return (a > b ? a : b); | ||
625 | } | ||
626 | |||
627 | private float Min(float a, float b) | ||
628 | { | ||
629 | return (a < b ? a : b); | ||
630 | } | ||
631 | } | ||
632 | } | ||
633 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs new file mode 100644 index 0000000..739b4fc --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs | |||
@@ -0,0 +1,56 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to | ||
31 | /// calculate the penetration depth between two convex shapes. | ||
32 | /// </summary> | ||
33 | public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver | ||
34 | { | ||
35 | public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw) | ||
36 | { | ||
37 | float radialmargin = 0; | ||
38 | |||
39 | GjkEpaSolver.Results results; | ||
40 | if (GjkEpaSolver.Collide(convexA, transformA, | ||
41 | convexB, transformB, | ||
42 | radialmargin, out results)) | ||
43 | { | ||
44 | // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); | ||
45 | //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); | ||
46 | ptrA = results.Witnesses[0]; | ||
47 | ptrB = results.Witnesses[1]; | ||
48 | return true; | ||
49 | } | ||
50 | ptrA = new Vector3(); | ||
51 | ptrB = new Vector3(); | ||
52 | |||
53 | return false; | ||
54 | } | ||
55 | } | ||
56 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs new file mode 100644 index 0000000..aa9d61e --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs | |||
@@ -0,0 +1,101 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// GjkEpaSolver contributed under zlib by Nathanael Presson | ||
31 | /// </summary> | ||
32 | public class GjkEpaSolver | ||
33 | { | ||
34 | public struct Results | ||
35 | { | ||
36 | public enum Status | ||
37 | { | ||
38 | Separated, /* Shapes doesnt penetrate */ | ||
39 | Penetrating, /* Shapes are penetrating */ | ||
40 | GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ | ||
41 | EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ | ||
42 | } | ||
43 | |||
44 | private Vector3[] _witnesses; | ||
45 | private Vector3 _normal; | ||
46 | private float _depth; | ||
47 | private int _epaIterations; | ||
48 | private int _gjkIterations; | ||
49 | private Status _status; | ||
50 | |||
51 | public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } } | ||
52 | public Vector3 Normal { get { return _normal; } set { _normal = value; } } | ||
53 | public float Depth { get { return _depth; } set { _depth = value; } } | ||
54 | public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } } | ||
55 | public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } } | ||
56 | public Status ResultStatus { get { return _status; } set { _status = value; } } | ||
57 | } | ||
58 | |||
59 | public static bool Collide(ConvexShape shapeA, Matrix wtrsA, | ||
60 | ConvexShape shapeB, Matrix wtrsB, | ||
61 | float radialmargin, | ||
62 | out Results results) | ||
63 | { | ||
64 | /* Initialize */ | ||
65 | results = new Results(); | ||
66 | results.Witnesses = new Vector3[2]; | ||
67 | results.Witnesses[0] = | ||
68 | results.Witnesses[1] = | ||
69 | results.Normal = new Vector3(); | ||
70 | results.Depth = 0; | ||
71 | results.ResultStatus = Results.Status.Separated; | ||
72 | results.EpaIterations = 0; | ||
73 | results.GjkIterations = 0; | ||
74 | /* Use GJK to locate origin */ | ||
75 | GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA, | ||
76 | wtrsB, wtrsB.Translation, shapeB, | ||
77 | radialmargin + GjkEpa.EpaAccuracy); | ||
78 | bool collide = gjk.SearchOrigin(); | ||
79 | results.GjkIterations = gjk.Iterations + 1; | ||
80 | if (collide) | ||
81 | { | ||
82 | /* Then EPA for penetration depth */ | ||
83 | GjkEpa.Epa epa = new GjkEpa.Epa(gjk); | ||
84 | float pd = epa.EvaluatePD(); | ||
85 | results.EpaIterations = epa.Iterations + 1; | ||
86 | if (pd > 0) | ||
87 | { | ||
88 | results.ResultStatus = Results.Status.Penetrating; | ||
89 | results.Normal = epa.Normal; | ||
90 | results.Depth = pd; | ||
91 | results.Witnesses[0] = epa.Nearest[0]; | ||
92 | results.Witnesses[1] = epa.Nearest[1]; | ||
93 | return true; | ||
94 | } | ||
95 | else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; } | ||
96 | } | ||
97 | else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; } | ||
98 | return false; | ||
99 | } | ||
100 | } | ||
101 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs new file mode 100644 index 0000000..0831ff5 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs | |||
@@ -0,0 +1,343 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class GjkPairDetector : DiscreteCollisionDetectorInterface | ||
30 | { | ||
31 | private Vector3 _cachedSeparatingAxis; | ||
32 | private IConvexPenetrationDepthSolver _penetrationDepthSolver; | ||
33 | private ISimplexSolver _simplexSolver; | ||
34 | private ConvexShape _minkowskiA, _minkowskiB; | ||
35 | private bool _ignoreMargin; | ||
36 | |||
37 | private int _lastUsedMethod; | ||
38 | private int _currentIteration; | ||
39 | private int _degenerateSimplex; | ||
40 | private int _catchDegeneracies; | ||
41 | |||
42 | private static int _numDeepPenetrationChecks = 0; | ||
43 | private static int _numGjkChecks = 0; | ||
44 | |||
45 | private const float RelativeError2 = 1.0e-6f; | ||
46 | |||
47 | #region Properties | ||
48 | public int LastUsedMethod | ||
49 | { | ||
50 | get { return _lastUsedMethod; } | ||
51 | set { _lastUsedMethod = value; } | ||
52 | } | ||
53 | |||
54 | public int CurrentIteration | ||
55 | { | ||
56 | get { return _currentIteration; } | ||
57 | set { _currentIteration = value; } | ||
58 | } | ||
59 | |||
60 | public int DegenerateSimplex | ||
61 | { | ||
62 | get { return _degenerateSimplex; } | ||
63 | set { _degenerateSimplex = value; } | ||
64 | } | ||
65 | |||
66 | public int CatchDegeneracies | ||
67 | { | ||
68 | get { return _catchDegeneracies; } | ||
69 | set { _catchDegeneracies = value; } | ||
70 | } | ||
71 | |||
72 | public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } } | ||
73 | public static int GjkChecksCount { get { return _numGjkChecks; } } | ||
74 | #endregion | ||
75 | |||
76 | public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, | ||
77 | ISimplexSolver simplexSolver, | ||
78 | IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
79 | { | ||
80 | _cachedSeparatingAxis = new Vector3(0, 0, 1); | ||
81 | |||
82 | _penetrationDepthSolver = penetrationDepthSolver; | ||
83 | _simplexSolver = simplexSolver; | ||
84 | _minkowskiA = objectA; | ||
85 | _minkowskiB = objectB; | ||
86 | _ignoreMargin = false; | ||
87 | _lastUsedMethod = -1; | ||
88 | _catchDegeneracies = 1; | ||
89 | } | ||
90 | |||
91 | public void setMinkowskiA(ConvexShape minkA) | ||
92 | { | ||
93 | _minkowskiA = minkA; | ||
94 | } | ||
95 | |||
96 | public void setMinkowskiB(ConvexShape minkB) | ||
97 | { | ||
98 | _minkowskiB = minkB; | ||
99 | } | ||
100 | public void setCachedSeperatingAxis(Vector3 seperatingAxis) | ||
101 | { | ||
102 | _cachedSeparatingAxis = seperatingAxis; | ||
103 | } | ||
104 | |||
105 | public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver) | ||
106 | { | ||
107 | this._penetrationDepthSolver = penetrationDepthSolver; | ||
108 | } | ||
109 | |||
110 | public void setIgnoreMargin(bool ignoreMargin) | ||
111 | { | ||
112 | this._ignoreMargin = ignoreMargin; | ||
113 | } | ||
114 | |||
115 | public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw) | ||
116 | { | ||
117 | float distance = 0; | ||
118 | |||
119 | Vector3 normalInB = new Vector3(); | ||
120 | Vector3 pointOnA = new Vector3(), pointOnB = new Vector3(); | ||
121 | |||
122 | Matrix localTransA = input.TransformA; | ||
123 | Matrix localTransB = input.TransformB; | ||
124 | |||
125 | Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f; | ||
126 | localTransA.Translation -= positionOffset; | ||
127 | localTransB.Translation -= positionOffset; | ||
128 | |||
129 | float marginA = _minkowskiA.Margin; | ||
130 | float marginB = _minkowskiB.Margin; | ||
131 | |||
132 | _numGjkChecks++; | ||
133 | |||
134 | if (_ignoreMargin) | ||
135 | { | ||
136 | marginA = 0; | ||
137 | marginB = 0; | ||
138 | } | ||
139 | |||
140 | _currentIteration = 0; | ||
141 | |||
142 | int gjkMaxIter = 1000; | ||
143 | _cachedSeparatingAxis = new Vector3(0, 1, 0); | ||
144 | |||
145 | bool isValid = false; | ||
146 | bool checkSimplex = false; | ||
147 | bool checkPenetration = true; | ||
148 | _degenerateSimplex = 0; | ||
149 | |||
150 | _lastUsedMethod = -1; | ||
151 | |||
152 | { | ||
153 | float squaredDistance = MathHelper.Infinity; | ||
154 | float delta = 0; | ||
155 | |||
156 | float margin = marginA + marginB; | ||
157 | |||
158 | _simplexSolver.Reset(); | ||
159 | |||
160 | while (true) | ||
161 | { | ||
162 | Matrix transABasis = input.TransformA; | ||
163 | transABasis.Translation = Vector3.Zero; | ||
164 | |||
165 | Matrix transBBasis = input.TransformB; | ||
166 | transBBasis.Translation = Vector3.Zero; | ||
167 | |||
168 | Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis); | ||
169 | Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis); | ||
170 | |||
171 | Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); | ||
172 | Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); | ||
173 | Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA); | ||
174 | Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB); | ||
175 | |||
176 | Vector3 w = pWorld - qWorld; | ||
177 | delta = Vector3.Dot(_cachedSeparatingAxis, w); | ||
178 | |||
179 | if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared)) | ||
180 | { | ||
181 | checkPenetration = false; | ||
182 | break; | ||
183 | } | ||
184 | |||
185 | if (_simplexSolver.InSimplex(w)) | ||
186 | { | ||
187 | _degenerateSimplex = 1; | ||
188 | checkSimplex = true; | ||
189 | break; | ||
190 | } | ||
191 | |||
192 | float f0 = squaredDistance - delta; | ||
193 | float f1 = squaredDistance * RelativeError2; | ||
194 | |||
195 | if (f0 <= f1) | ||
196 | { | ||
197 | if (f0 <= 0.0f) | ||
198 | { | ||
199 | _degenerateSimplex = 2; | ||
200 | } | ||
201 | |||
202 | checkSimplex = true; | ||
203 | break; | ||
204 | } | ||
205 | |||
206 | _simplexSolver.AddVertex(w, pWorld, qWorld); | ||
207 | |||
208 | if (!_simplexSolver.Closest(out _cachedSeparatingAxis)) | ||
209 | { | ||
210 | _degenerateSimplex = 3; | ||
211 | checkSimplex = true; | ||
212 | break; | ||
213 | } | ||
214 | |||
215 | float previouseSquaredDistance = squaredDistance; | ||
216 | squaredDistance = _cachedSeparatingAxis.LengthSquared(); | ||
217 | |||
218 | if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance) | ||
219 | { | ||
220 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); | ||
221 | checkSimplex = true; | ||
222 | break; | ||
223 | } | ||
224 | |||
225 | if (_currentIteration++ > gjkMaxIter) | ||
226 | { | ||
227 | #if DEBUG | ||
228 | Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration); | ||
229 | Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}", | ||
230 | _cachedSeparatingAxis.X, | ||
231 | _cachedSeparatingAxis.Y, | ||
232 | _cachedSeparatingAxis.Z, | ||
233 | squaredDistance, | ||
234 | _minkowskiA.ShapeType, | ||
235 | _minkowskiB.ShapeType | ||
236 | ); | ||
237 | #endif | ||
238 | break; | ||
239 | } | ||
240 | |||
241 | bool check = (!_simplexSolver.FullSimplex); | ||
242 | |||
243 | if (!check) | ||
244 | { | ||
245 | _simplexSolver.BackupClosest(out _cachedSeparatingAxis); | ||
246 | break; | ||
247 | } | ||
248 | } | ||
249 | |||
250 | if (checkSimplex) | ||
251 | { | ||
252 | _simplexSolver.ComputePoints(out pointOnA, out pointOnB); | ||
253 | normalInB = pointOnA - pointOnB; | ||
254 | float lenSqr = _cachedSeparatingAxis.LengthSquared(); | ||
255 | |||
256 | if (lenSqr < 0.0001f) | ||
257 | { | ||
258 | _degenerateSimplex = 5; | ||
259 | } | ||
260 | |||
261 | if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon) | ||
262 | { | ||
263 | float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr); | ||
264 | normalInB *= rlen; | ||
265 | float s = (float)Math.Sqrt((float)squaredDistance); | ||
266 | |||
267 | BulletDebug.Assert(s > 0); | ||
268 | pointOnA -= _cachedSeparatingAxis * (marginA / s); | ||
269 | pointOnB += _cachedSeparatingAxis * (marginB / s); | ||
270 | distance = ((1 / rlen) - margin); | ||
271 | |||
272 | isValid = true; | ||
273 | |||
274 | _lastUsedMethod = 1; | ||
275 | } | ||
276 | else | ||
277 | { | ||
278 | _lastUsedMethod = 2; | ||
279 | } | ||
280 | } | ||
281 | |||
282 | bool catchDegeneratePenetrationCase = | ||
283 | (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f)); | ||
284 | |||
285 | if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) | ||
286 | { | ||
287 | #warning Check this | ||
288 | if (_penetrationDepthSolver != null) | ||
289 | { | ||
290 | Vector3 tmpPointOnA, tmpPointOnB; | ||
291 | |||
292 | _numDeepPenetrationChecks++; | ||
293 | |||
294 | bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth( | ||
295 | _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB, | ||
296 | _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB, | ||
297 | debugDraw | ||
298 | ); | ||
299 | |||
300 | if (isValid2) | ||
301 | { | ||
302 | Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; | ||
303 | float lengSqr = tmpNormalInB.LengthSquared(); | ||
304 | |||
305 | if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon)) | ||
306 | { | ||
307 | tmpNormalInB /= (float)Math.Sqrt((float)lengSqr); | ||
308 | float distance2 = -(tmpPointOnA - tmpPointOnB).Length(); | ||
309 | |||
310 | if (!isValid || (distance2 < distance)) | ||
311 | { | ||
312 | distance = distance2; | ||
313 | pointOnA = tmpPointOnA; | ||
314 | pointOnB = tmpPointOnB; | ||
315 | normalInB = tmpNormalInB; | ||
316 | isValid = true; | ||
317 | _lastUsedMethod = 3; | ||
318 | } | ||
319 | else | ||
320 | { | ||
321 | |||
322 | } | ||
323 | } | ||
324 | else | ||
325 | { | ||
326 | _lastUsedMethod = 4; | ||
327 | } | ||
328 | } | ||
329 | else | ||
330 | { | ||
331 | _lastUsedMethod = 5; | ||
332 | } | ||
333 | } | ||
334 | } | ||
335 | |||
336 | if (isValid) | ||
337 | { | ||
338 | output.AddContactPoint(normalInB, pointOnB + positionOffset, distance); | ||
339 | } | ||
340 | } | ||
341 | } | ||
342 | } | ||
343 | } \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs new file mode 100644 index 0000000..59fd486 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs | |||
@@ -0,0 +1,42 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation. | ||
31 | /// </summary> | ||
32 | public interface IConvexPenetrationDepthSolver | ||
33 | { | ||
34 | bool CalculatePenetrationDepth( | ||
35 | ISimplexSolver simplexSolver, | ||
36 | ConvexShape convexA, ConvexShape convexB, | ||
37 | Matrix transformA, Matrix transformB, | ||
38 | Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, | ||
39 | IDebugDraw debugDraw//, StackAlloc stackAlloc | ||
40 | ); | ||
41 | } | ||
42 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs new file mode 100644 index 0000000..71b1eaa --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs | |||
@@ -0,0 +1,48 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices | ||
30 | /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on | ||
31 | /// voronoi regions or barycentric coordinates | ||
32 | public interface ISimplexSolver | ||
33 | { | ||
34 | void Reset(); | ||
35 | void AddVertex(Vector3 w, Vector3 p, Vector3 q); | ||
36 | bool Closest(out Vector3 v); | ||
37 | |||
38 | int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf); | ||
39 | bool InSimplex(Vector3 w); | ||
40 | void BackupClosest(out Vector3 v); | ||
41 | void ComputePoints(out Vector3 pA, out Vector3 pB); | ||
42 | |||
43 | int NumVertices { get;} | ||
44 | bool EmptySimplex { get;} | ||
45 | float MaxVertex { get;} | ||
46 | bool FullSimplex { get;} | ||
47 | } | ||
48 | } \ No newline at end of file | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs new file mode 100644 index 0000000..751a9df --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs | |||
@@ -0,0 +1,78 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class ManifoldPoint | ||
30 | { | ||
31 | private Vector3 _localPointA; | ||
32 | private Vector3 _localPointB; | ||
33 | private Vector3 _positionWorldOnB; | ||
34 | private Vector3 _positionWorldOnA; | ||
35 | private Vector3 _normalWorldOnB; | ||
36 | |||
37 | private float _distance; | ||
38 | private float _combinedFriction; | ||
39 | private float _combinedRestitution; | ||
40 | |||
41 | private object _userPersistentData; | ||
42 | |||
43 | private int _lifeTime;//lifetime of the contactpoint in frames | ||
44 | |||
45 | public ManifoldPoint() | ||
46 | : this(new Vector3(), new Vector3(), new Vector3(), 0f) | ||
47 | { | ||
48 | } | ||
49 | |||
50 | public ManifoldPoint(Vector3 pointA, Vector3 pointB, | ||
51 | Vector3 normal, | ||
52 | float distance) | ||
53 | { | ||
54 | _localPointA = pointA; | ||
55 | _localPointB = pointB; | ||
56 | _normalWorldOnB = normal; | ||
57 | _distance = distance; | ||
58 | _positionWorldOnA = new Vector3(); | ||
59 | _positionWorldOnB = new Vector3(); | ||
60 | } | ||
61 | |||
62 | public float Distance { get { return _distance; } set { _distance = value; } } | ||
63 | public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } } | ||
64 | |||
65 | public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } } | ||
66 | public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } } | ||
67 | |||
68 | public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } } | ||
69 | public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } } | ||
70 | |||
71 | public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } } | ||
72 | |||
73 | public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } } | ||
74 | public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } } | ||
75 | |||
76 | public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } } | ||
77 | } | ||
78 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs new file mode 100644 index 0000000..81b05b8 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs | |||
@@ -0,0 +1,246 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. | ||
31 | /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. | ||
32 | /// </summary> | ||
33 | public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver | ||
34 | { | ||
35 | private const int UnitSpherePointsCount = 42; | ||
36 | |||
37 | private static Vector3[] penetrationDirections = | ||
38 | { | ||
39 | new Vector3(0.000000f , -0.000000f,-1.000000f), | ||
40 | new Vector3(0.723608f , -0.525725f,-0.447219f), | ||
41 | new Vector3(-0.276388f , -0.850649f,-0.447219f), | ||
42 | new Vector3(-0.894426f , -0.000000f,-0.447216f), | ||
43 | new Vector3(-0.276388f , 0.850649f,-0.447220f), | ||
44 | new Vector3(0.723608f , 0.525725f,-0.447219f), | ||
45 | new Vector3(0.276388f , -0.850649f,0.447220f), | ||
46 | new Vector3(-0.723608f , -0.525725f,0.447219f), | ||
47 | new Vector3(-0.723608f , 0.525725f,0.447219f), | ||
48 | new Vector3(0.276388f , 0.850649f,0.447219f), | ||
49 | new Vector3(0.894426f , 0.000000f,0.447216f), | ||
50 | new Vector3(-0.000000f , 0.000000f,1.000000f), | ||
51 | new Vector3(0.425323f , -0.309011f,-0.850654f), | ||
52 | new Vector3(-0.162456f , -0.499995f,-0.850654f), | ||
53 | new Vector3(0.262869f , -0.809012f,-0.525738f), | ||
54 | new Vector3(0.425323f , 0.309011f,-0.850654f), | ||
55 | new Vector3(0.850648f , -0.000000f,-0.525736f), | ||
56 | new Vector3(-0.525730f , -0.000000f,-0.850652f), | ||
57 | new Vector3(-0.688190f , -0.499997f,-0.525736f), | ||
58 | new Vector3(-0.162456f , 0.499995f,-0.850654f), | ||
59 | new Vector3(-0.688190f , 0.499997f,-0.525736f), | ||
60 | new Vector3(0.262869f , 0.809012f,-0.525738f), | ||
61 | new Vector3(0.951058f , 0.309013f,0.000000f), | ||
62 | new Vector3(0.951058f , -0.309013f,0.000000f), | ||
63 | new Vector3(0.587786f , -0.809017f,0.000000f), | ||
64 | new Vector3(0.000000f , -1.000000f,0.000000f), | ||
65 | new Vector3(-0.587786f , -0.809017f,0.000000f), | ||
66 | new Vector3(-0.951058f , -0.309013f,-0.000000f), | ||
67 | new Vector3(-0.951058f , 0.309013f,-0.000000f), | ||
68 | new Vector3(-0.587786f , 0.809017f,-0.000000f), | ||
69 | new Vector3(-0.000000f , 1.000000f,-0.000000f), | ||
70 | new Vector3(0.587786f , 0.809017f,-0.000000f), | ||
71 | new Vector3(0.688190f , -0.499997f,0.525736f), | ||
72 | new Vector3(-0.262869f , -0.809012f,0.525738f), | ||
73 | new Vector3(-0.850648f , 0.000000f,0.525736f), | ||
74 | new Vector3(-0.262869f , 0.809012f,0.525738f), | ||
75 | new Vector3(0.688190f , 0.499997f,0.525736f), | ||
76 | new Vector3(0.525730f , 0.000000f,0.850652f), | ||
77 | new Vector3(0.162456f , -0.499995f,0.850654f), | ||
78 | new Vector3(-0.425323f , -0.309011f,0.850654f), | ||
79 | new Vector3(-0.425323f , 0.309011f,0.850654f), | ||
80 | new Vector3(0.162456f , 0.499995f,0.850654f) | ||
81 | }; | ||
82 | |||
83 | private class IntermediateResult : DiscreteCollisionDetectorInterface.Result | ||
84 | { | ||
85 | private Vector3 _normalOnBInWorld; | ||
86 | private Vector3 _pointInWorld; | ||
87 | private float _depth; | ||
88 | private bool _hasResult; | ||
89 | |||
90 | public IntermediateResult() | ||
91 | { | ||
92 | _hasResult = false; | ||
93 | } | ||
94 | |||
95 | public bool HasResult { get { return _hasResult; } } | ||
96 | public float Depth { get { return _depth; } } | ||
97 | public Vector3 PointInWorld { get { return _pointInWorld; } } | ||
98 | |||
99 | public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1) | ||
100 | { | ||
101 | } | ||
102 | |||
103 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
104 | { | ||
105 | _normalOnBInWorld = normalOnBInWorld; | ||
106 | _pointInWorld = pointInWorld; | ||
107 | _depth = depth; | ||
108 | _hasResult = true; | ||
109 | } | ||
110 | } | ||
111 | |||
112 | #region IConvexPenetrationDepthSolver Members | ||
113 | public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, | ||
114 | ConvexShape convexA, ConvexShape convexB, | ||
115 | Matrix transformA, Matrix transformB, | ||
116 | Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw) | ||
117 | { | ||
118 | pa = new Vector3(); | ||
119 | pb = new Vector3(); | ||
120 | //just take fixed number of orientation, and sample the penetration depth in that direction | ||
121 | float minProj = 1e30f; | ||
122 | Vector3 minNorm = new Vector3(); | ||
123 | Vector3 minA = new Vector3(), minB = new Vector3(); | ||
124 | Vector3 seperatingAxisInA, seperatingAxisInB; | ||
125 | Vector3 pInA, qInB, pWorld, qWorld, w; | ||
126 | |||
127 | Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
128 | Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
129 | Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
130 | Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2]; | ||
131 | |||
132 | int numSampleDirections = UnitSpherePointsCount; | ||
133 | |||
134 | for (int i = 0; i < numSampleDirections; i++) | ||
135 | { | ||
136 | Vector3 norm = penetrationDirections[i]; | ||
137 | seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA); | ||
138 | seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB); | ||
139 | } | ||
140 | |||
141 | { | ||
142 | int numPDA = convexA.PreferredPenetrationDirectionsCount; | ||
143 | if (numPDA != 0) | ||
144 | { | ||
145 | for (int i = 0; i < numPDA; i++) | ||
146 | { | ||
147 | Vector3 norm; | ||
148 | convexA.GetPreferredPenetrationDirection(i, out norm); | ||
149 | norm = Vector3.TransformNormal(norm, transformA); | ||
150 | penetrationDirections[numSampleDirections] = norm; | ||
151 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); | ||
152 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); | ||
153 | numSampleDirections++; | ||
154 | } | ||
155 | } | ||
156 | } | ||
157 | |||
158 | { | ||
159 | int numPDB = convexB.PreferredPenetrationDirectionsCount; | ||
160 | if (numPDB != 0) | ||
161 | { | ||
162 | for (int i = 0; i < numPDB; i++) | ||
163 | { | ||
164 | Vector3 norm; | ||
165 | convexB.GetPreferredPenetrationDirection(i, out norm); | ||
166 | norm = Vector3.TransformNormal(norm, transformB); | ||
167 | penetrationDirections[numSampleDirections] = norm; | ||
168 | seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA); | ||
169 | seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB); | ||
170 | numSampleDirections++; | ||
171 | } | ||
172 | } | ||
173 | } | ||
174 | |||
175 | convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections); | ||
176 | convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections); | ||
177 | |||
178 | for (int i = 0; i < numSampleDirections; i++) | ||
179 | { | ||
180 | Vector3 norm = penetrationDirections[i]; | ||
181 | seperatingAxisInA = seperatingAxisInABatch[i]; | ||
182 | seperatingAxisInB = seperatingAxisInBBatch[i]; | ||
183 | |||
184 | pInA = supportVerticesABatch[i]; | ||
185 | qInB = supportVerticesBBatch[i]; | ||
186 | |||
187 | pWorld = MathHelper.MatrixToVector(transformA, pInA); | ||
188 | qWorld = MathHelper.MatrixToVector(transformB, qInB); | ||
189 | w = qWorld - pWorld; | ||
190 | float delta = Vector3.Dot(norm, w); | ||
191 | //find smallest delta | ||
192 | if (delta < minProj) | ||
193 | { | ||
194 | minProj = delta; | ||
195 | minNorm = norm; | ||
196 | minA = pWorld; | ||
197 | minB = qWorld; | ||
198 | } | ||
199 | } | ||
200 | |||
201 | //add the margins | ||
202 | minA += minNorm * convexA.Margin; | ||
203 | minB -= minNorm * convexB.Margin; | ||
204 | //no penetration | ||
205 | if (minProj < 0) | ||
206 | return false; | ||
207 | |||
208 | minProj += (convexA.Margin + convexB.Margin); | ||
209 | |||
210 | GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null); | ||
211 | |||
212 | float offsetDist = minProj; | ||
213 | Vector3 offset = minNorm * offsetDist; | ||
214 | |||
215 | GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput(); | ||
216 | |||
217 | Vector3 newOrg = transformA.Translation + offset; | ||
218 | |||
219 | Matrix displacedTrans = transformA; | ||
220 | displacedTrans.Translation = newOrg; | ||
221 | |||
222 | input.TransformA = displacedTrans; | ||
223 | input.TransformB = transformB; | ||
224 | input.MaximumDistanceSquared = 1e30f;//minProj; | ||
225 | |||
226 | IntermediateResult res = new IntermediateResult(); | ||
227 | gjkdet.GetClosestPoints(input, res, debugDraw); | ||
228 | |||
229 | float correctedMinNorm = minProj - res.Depth; | ||
230 | |||
231 | //the penetration depth is over-estimated, relax it | ||
232 | float penetration_relaxation = 1; | ||
233 | minNorm *= penetration_relaxation; | ||
234 | |||
235 | if (res.HasResult) | ||
236 | { | ||
237 | |||
238 | pa = res.PointInWorld - minNorm * correctedMinNorm; | ||
239 | pb = res.PointInWorld; | ||
240 | } | ||
241 | |||
242 | return res.HasResult; | ||
243 | } | ||
244 | #endregion | ||
245 | } | ||
246 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs new file mode 100644 index 0000000..47cfe9a --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs | |||
@@ -0,0 +1,272 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public delegate bool ContactDestroyedCallback(object userPersistentData); | ||
30 | |||
31 | public class PersistentManifold | ||
32 | { | ||
33 | private static ContactDestroyedCallback _contactDestroyedCallback = null; | ||
34 | private static float _contactBreakingThreshold = 0.02f; | ||
35 | |||
36 | private ManifoldPoint[] _pointCache = new ManifoldPoint[4]; | ||
37 | |||
38 | // this two body pointers can point to the physics rigidbody class. | ||
39 | // object will allow any rigidbody class | ||
40 | private object _bodyA; | ||
41 | private object _bodyB; | ||
42 | private int _cachedPoints; | ||
43 | |||
44 | public PersistentManifold(object bodyA, object bodyB) | ||
45 | { | ||
46 | _bodyA = bodyA; | ||
47 | _bodyB = bodyB; | ||
48 | _cachedPoints = 0; | ||
49 | } | ||
50 | |||
51 | public object BodyA { get { return _bodyA; } } | ||
52 | public object BodyB { get { return _bodyB; } } | ||
53 | |||
54 | public int ContactsCount { get { return _cachedPoints; } } | ||
55 | |||
56 | public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } } | ||
57 | public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } } | ||
58 | |||
59 | public void SetBodies(object bodyA, object bodyB) | ||
60 | { | ||
61 | _bodyA = bodyA; | ||
62 | _bodyB = bodyB; | ||
63 | } | ||
64 | |||
65 | public ManifoldPoint GetContactPoint(int index) | ||
66 | { | ||
67 | if (index >= _cachedPoints) | ||
68 | throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints"); | ||
69 | |||
70 | return _pointCache[index]; | ||
71 | } | ||
72 | |||
73 | public int GetCacheEntry(ManifoldPoint newPoint) | ||
74 | { | ||
75 | float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold; | ||
76 | int size = ContactsCount; | ||
77 | int nearestPoint = -1; | ||
78 | for (int i = 0; i < size; i++) | ||
79 | { | ||
80 | ManifoldPoint mp = _pointCache[i]; | ||
81 | |||
82 | Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA; | ||
83 | float distToManiPoint = Vector3.Dot(diffA, diffA); | ||
84 | if (distToManiPoint < shortestDist) | ||
85 | { | ||
86 | shortestDist = distToManiPoint; | ||
87 | nearestPoint = i; | ||
88 | } | ||
89 | } | ||
90 | return nearestPoint; | ||
91 | } | ||
92 | |||
93 | public void AddManifoldPoint(ManifoldPoint newPoint) | ||
94 | { | ||
95 | if (!ValidContactDistance(newPoint)) | ||
96 | throw new BulletException(); | ||
97 | |||
98 | int insertIndex = ContactsCount; | ||
99 | if (insertIndex == 4) | ||
100 | { | ||
101 | //sort cache so best points come first, based on area | ||
102 | insertIndex = SortCachedPoints(newPoint); | ||
103 | } | ||
104 | else | ||
105 | { | ||
106 | _cachedPoints++; | ||
107 | } | ||
108 | ReplaceContactPoint(newPoint, insertIndex); | ||
109 | } | ||
110 | |||
111 | public void RemoveContactPoint(int index) | ||
112 | { | ||
113 | ClearUserCache(_pointCache[index]); | ||
114 | |||
115 | int lastUsedIndex = ContactsCount - 1; | ||
116 | _pointCache[index] = _pointCache[lastUsedIndex]; | ||
117 | //get rid of duplicated userPersistentData pointer | ||
118 | _pointCache[lastUsedIndex].UserPersistentData = null; | ||
119 | _cachedPoints--; | ||
120 | } | ||
121 | |||
122 | public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex) | ||
123 | { | ||
124 | BulletDebug.Assert(ValidContactDistance(newPoint)); | ||
125 | |||
126 | if (_pointCache[insertIndex] != null) | ||
127 | { | ||
128 | int lifeTime = _pointCache[insertIndex].LifeTime; | ||
129 | BulletDebug.Assert(lifeTime >= 0); | ||
130 | object cache = _pointCache[insertIndex].UserPersistentData; | ||
131 | |||
132 | _pointCache[insertIndex] = newPoint; | ||
133 | |||
134 | _pointCache[insertIndex].UserPersistentData = cache; | ||
135 | _pointCache[insertIndex].LifeTime = lifeTime; | ||
136 | } | ||
137 | else | ||
138 | { | ||
139 | _pointCache[insertIndex] = newPoint; | ||
140 | } | ||
141 | |||
142 | //ClearUserCache(_pointCache[insertIndex]); | ||
143 | //_pointCache[insertIndex] = newPoint; | ||
144 | } | ||
145 | |||
146 | public bool ValidContactDistance(ManifoldPoint pt) | ||
147 | { | ||
148 | return pt.Distance <= ContactBreakingThreshold; | ||
149 | } | ||
150 | |||
151 | // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin | ||
152 | public void RefreshContactPoints(Matrix trA, Matrix trB) | ||
153 | { | ||
154 | // first refresh worldspace positions and distance | ||
155 | for (int i = ContactsCount - 1; i >= 0; i--) | ||
156 | { | ||
157 | ManifoldPoint manifoldPoint = _pointCache[i]; | ||
158 | manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA); | ||
159 | manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB); | ||
160 | manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB); | ||
161 | manifoldPoint.LifeTime++; | ||
162 | } | ||
163 | |||
164 | // then | ||
165 | float distance2d; | ||
166 | Vector3 projectedDifference, projectedPoint; | ||
167 | for (int i = ContactsCount - 1; i >= 0; i--) | ||
168 | { | ||
169 | |||
170 | ManifoldPoint manifoldPoint = _pointCache[i]; | ||
171 | //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) | ||
172 | if (!ValidContactDistance(manifoldPoint)) | ||
173 | { | ||
174 | RemoveContactPoint(i); | ||
175 | } | ||
176 | else | ||
177 | { | ||
178 | //contact also becomes invalid when relative movement orthogonal to normal exceeds margin | ||
179 | projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance; | ||
180 | projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint; | ||
181 | distance2d = Vector3.Dot(projectedDifference, projectedDifference); | ||
182 | if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold) | ||
183 | { | ||
184 | RemoveContactPoint(i); | ||
185 | } | ||
186 | } | ||
187 | } | ||
188 | } | ||
189 | |||
190 | public void ClearManifold() | ||
191 | { | ||
192 | for (int i = 0; i < _cachedPoints; i++) | ||
193 | { | ||
194 | ClearUserCache(_pointCache[i]); | ||
195 | } | ||
196 | _cachedPoints = 0; | ||
197 | } | ||
198 | |||
199 | private void ClearUserCache(ManifoldPoint pt) | ||
200 | { | ||
201 | if (pt != null) | ||
202 | { | ||
203 | object oldPtr = pt.UserPersistentData; | ||
204 | |||
205 | if (oldPtr != null) | ||
206 | { | ||
207 | if (pt.UserPersistentData != null && _contactDestroyedCallback != null) | ||
208 | { | ||
209 | _contactDestroyedCallback(pt.UserPersistentData); | ||
210 | pt.UserPersistentData = null; | ||
211 | } | ||
212 | } | ||
213 | } | ||
214 | } | ||
215 | |||
216 | // sort cached points so most isolated points come first | ||
217 | private int SortCachedPoints(ManifoldPoint pt) | ||
218 | { | ||
219 | //calculate 4 possible cases areas, and take biggest area | ||
220 | //also need to keep 'deepest' | ||
221 | |||
222 | int maxPenetrationIndex = -1; | ||
223 | float maxPenetration = pt.Distance; | ||
224 | for (int i = 0; i < 4; i++) | ||
225 | { | ||
226 | if (_pointCache[i].Distance < maxPenetration) | ||
227 | { | ||
228 | maxPenetrationIndex = i; | ||
229 | maxPenetration = _pointCache[i].Distance; | ||
230 | } | ||
231 | } | ||
232 | |||
233 | float res0 = 0, res1 = 0, res2 = 0, res3 = 0; | ||
234 | if (maxPenetrationIndex != 0) | ||
235 | { | ||
236 | Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA; | ||
237 | Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; | ||
238 | Vector3 cross = Vector3.Cross(a0, b0); | ||
239 | res0 = cross.LengthSquared(); | ||
240 | } | ||
241 | if (maxPenetrationIndex != 1) | ||
242 | { | ||
243 | Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA; | ||
244 | Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA; | ||
245 | Vector3 cross = Vector3.Cross(a1, b1); | ||
246 | res1 = cross.LengthSquared(); | ||
247 | } | ||
248 | |||
249 | if (maxPenetrationIndex != 2) | ||
250 | { | ||
251 | Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA; | ||
252 | Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA; | ||
253 | Vector3 cross = Vector3.Cross(a2, b2); | ||
254 | res2 = cross.LengthSquared(); | ||
255 | } | ||
256 | |||
257 | if (maxPenetrationIndex != 3) | ||
258 | { | ||
259 | Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA; | ||
260 | Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA; | ||
261 | Vector3 cross = Vector3.Cross(a3, b3); | ||
262 | res3 = cross.LengthSquared(); | ||
263 | } | ||
264 | |||
265 | Vector4 maxvec = new Vector4(res0, res1, res2, res3); | ||
266 | int biggestarea = MathHelper.ClosestAxis(maxvec); | ||
267 | return biggestarea; | ||
268 | } | ||
269 | |||
270 | private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; } | ||
271 | } | ||
272 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs new file mode 100644 index 0000000..4e0b0a0 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs | |||
@@ -0,0 +1,64 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class PointCollector : DiscreteCollisionDetectorInterface.Result | ||
30 | { | ||
31 | private Vector3 _normalOnBInWorld; | ||
32 | private Vector3 _pointInWorld; | ||
33 | private float _distance; //negative means penetration | ||
34 | private bool _hasResult; | ||
35 | |||
36 | public PointCollector() | ||
37 | { | ||
38 | _distance = 1e30f; | ||
39 | _hasResult = false; | ||
40 | } | ||
41 | |||
42 | public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } } | ||
43 | public Vector3 PointInWorld { get { return _pointInWorld; } } | ||
44 | public float Distance { get { return _distance; } } | ||
45 | public bool HasResult { get { return _hasResult; } } | ||
46 | |||
47 | public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB) | ||
48 | { | ||
49 | //?? | ||
50 | } | ||
51 | |||
52 | public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth) | ||
53 | { | ||
54 | if (depth < _distance) | ||
55 | { | ||
56 | _hasResult = true; | ||
57 | _normalOnBInWorld = normalOnBInWorld; | ||
58 | _pointInWorld = pointInWorld; | ||
59 | //negative means penetration | ||
60 | _distance = depth; | ||
61 | } | ||
62 | } | ||
63 | } | ||
64 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs new file mode 100644 index 0000000..6d320c4 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs | |||
@@ -0,0 +1,142 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// SubsimplexConvexCast implements Gino van den Bergens' paper | ||
31 | /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" | ||
32 | /// GJK based Ray Cast, optimized version | ||
33 | /// Objects should not start in overlap, otherwise results are not defined. | ||
34 | /// </summary> | ||
35 | public class SubsimplexConvexCast : IConvexCast | ||
36 | { | ||
37 | private ISimplexSolver _simplexSolver; | ||
38 | private ConvexShape _convexA; | ||
39 | private ConvexShape _convexB; | ||
40 | |||
41 | /// <summary> | ||
42 | /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. | ||
43 | /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 | ||
44 | /// </summary> | ||
45 | private const int MaxIterations = 32; | ||
46 | |||
47 | public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver) | ||
48 | { | ||
49 | _simplexSolver = simplexSolver; | ||
50 | _convexA = shapeA; | ||
51 | _convexB = shapeB; | ||
52 | } | ||
53 | |||
54 | #region IConvexCast Members | ||
55 | /// <summary> | ||
56 | /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. | ||
57 | /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector. | ||
58 | /// </summary> | ||
59 | /// <param name="fromA"></param> | ||
60 | /// <param name="toA"></param> | ||
61 | /// <param name="fromB"></param> | ||
62 | /// <param name="toB"></param> | ||
63 | /// <param name="result"></param> | ||
64 | /// <returns></returns> | ||
65 | public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result) | ||
66 | { | ||
67 | MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB); | ||
68 | |||
69 | Matrix rayFromLocalA; | ||
70 | Matrix rayToLocalA; | ||
71 | |||
72 | rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB; | ||
73 | rayToLocalA = MathHelper.InvertMatrix(toA) * toB; | ||
74 | |||
75 | _simplexSolver.Reset(); | ||
76 | |||
77 | convex.TransformB = rayFromLocalA; | ||
78 | |||
79 | float lambda = 0; | ||
80 | //todo: need to verify this: | ||
81 | //because of minkowski difference, we need the inverse direction | ||
82 | |||
83 | Vector3 s = -rayFromLocalA.Translation; | ||
84 | Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation); | ||
85 | Vector3 x = s; | ||
86 | Vector3 v; | ||
87 | Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r); | ||
88 | |||
89 | v = x - arbitraryPoint; | ||
90 | |||
91 | int maxIter = MaxIterations; | ||
92 | |||
93 | Vector3 n = new Vector3(); | ||
94 | float lastLambda = lambda; | ||
95 | |||
96 | float dist2 = v.LengthSquared(); | ||
97 | float epsilon = 0.0001f; | ||
98 | |||
99 | Vector3 w, p; | ||
100 | float VdotR; | ||
101 | |||
102 | while ((dist2 > epsilon) && (maxIter-- != 0)) | ||
103 | { | ||
104 | p = convex.LocalGetSupportingVertex(v); | ||
105 | w = x - p; | ||
106 | |||
107 | float VdotW = Vector3.Dot(v, w); | ||
108 | |||
109 | if (VdotW > 0) | ||
110 | { | ||
111 | VdotR = Vector3.Dot(v, r); | ||
112 | |||
113 | if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon)) | ||
114 | return false; | ||
115 | else | ||
116 | { | ||
117 | lambda = lambda - VdotW / VdotR; | ||
118 | x = s + lambda * r; | ||
119 | _simplexSolver.Reset(); | ||
120 | //check next line | ||
121 | w = x - p; | ||
122 | lastLambda = lambda; | ||
123 | n = v; | ||
124 | } | ||
125 | } | ||
126 | _simplexSolver.AddVertex(w, x, p); | ||
127 | if (_simplexSolver.Closest(out v)) | ||
128 | { | ||
129 | dist2 = v.LengthSquared(); | ||
130 | } | ||
131 | else | ||
132 | { | ||
133 | dist2 = 0f; | ||
134 | } | ||
135 | } | ||
136 | result.Fraction = lambda; | ||
137 | result.Normal = n; | ||
138 | return true; | ||
139 | } | ||
140 | #endregion | ||
141 | } | ||
142 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs new file mode 100644 index 0000000..c127460 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs | |||
@@ -0,0 +1,115 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public abstract class TriangleRaycastCallback : ITriangleCallback | ||
30 | { | ||
31 | private Vector3 _from; | ||
32 | private Vector3 _to; | ||
33 | private float _hitFraction; | ||
34 | |||
35 | public TriangleRaycastCallback(Vector3 from, Vector3 to) | ||
36 | { | ||
37 | _from = from; | ||
38 | _to = to; | ||
39 | _hitFraction = 1; | ||
40 | } | ||
41 | |||
42 | public Vector3 From { get { return _from; } set { _from = value; } } | ||
43 | public Vector3 To { get { return _to; } set { _to = value; } } | ||
44 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | ||
45 | |||
46 | public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex); | ||
47 | |||
48 | #region ITriangleCallback Members | ||
49 | |||
50 | public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex) | ||
51 | { | ||
52 | Vector3 vertA = triangle[0]; | ||
53 | Vector3 vertB = triangle[1]; | ||
54 | Vector3 vertC = triangle[2]; | ||
55 | |||
56 | Vector3 vBA = vertB - vertA; | ||
57 | Vector3 vCA = vertC - vertA; | ||
58 | |||
59 | Vector3 triangleNormal = Vector3.Cross(vBA, vCA); | ||
60 | |||
61 | float dist = Vector3.Dot(vertA, triangleNormal); | ||
62 | float distA = Vector3.Dot(triangleNormal, _from); | ||
63 | distA -= dist; | ||
64 | float distB = Vector3.Dot(triangleNormal, _to); | ||
65 | distB -= dist; | ||
66 | |||
67 | if (distA * distB >= 0.0f) | ||
68 | { | ||
69 | return; // same sign | ||
70 | } | ||
71 | |||
72 | float projLength = distA - distB; | ||
73 | float distance = (distA) / (projLength); | ||
74 | // Now we have the intersection point on the plane, we'll see if it's inside the triangle | ||
75 | // Add an epsilon as a tolerance for the raycast, | ||
76 | // in case the ray hits exacly on the edge of the triangle. | ||
77 | // It must be scaled for the triangle size. | ||
78 | |||
79 | if (distance < _hitFraction) | ||
80 | { | ||
81 | float edgeTolerance = triangleNormal.LengthSquared(); | ||
82 | edgeTolerance *= -0.0001f; | ||
83 | Vector3 point = new Vector3(); | ||
84 | MathHelper.SetInterpolate3(_from, _to, distance, ref point); | ||
85 | |||
86 | Vector3 vertexAPoint = vertA - point; | ||
87 | Vector3 vertexBPoint = vertB - point; | ||
88 | Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint); | ||
89 | |||
90 | if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance) | ||
91 | { | ||
92 | Vector3 vertexCPoint = vertC - point; | ||
93 | Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint); | ||
94 | if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance) | ||
95 | { | ||
96 | Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint); | ||
97 | |||
98 | if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance) | ||
99 | { | ||
100 | if (distA > 0) | ||
101 | { | ||
102 | _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex); | ||
103 | } | ||
104 | else | ||
105 | { | ||
106 | _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex); | ||
107 | } | ||
108 | } | ||
109 | } | ||
110 | } | ||
111 | } | ||
112 | } | ||
113 | #endregion | ||
114 | } | ||
115 | } | ||
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs new file mode 100644 index 0000000..16f3dab --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs | |||
@@ -0,0 +1,643 @@ | |||
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 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX | ||
28 | { | ||
29 | public class UsageBitfield | ||
30 | { | ||
31 | private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD; | ||
32 | |||
33 | public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } } | ||
34 | public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } } | ||
35 | public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } } | ||
36 | public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } } | ||
37 | |||
38 | public void Reset() | ||
39 | { | ||
40 | _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false; | ||
41 | } | ||
42 | } | ||
43 | |||
44 | public class SubSimplexClosestResult | ||
45 | { | ||
46 | private Vector3 _closestPointOnSimplex; | ||
47 | |||
48 | //MASK for m_usedVertices | ||
49 | //stores the simplex vertex-usage, using the MASK, | ||
50 | // if m_usedVertices & MASK then the related vertex is used | ||
51 | private UsageBitfield _usedVertices = new UsageBitfield(); | ||
52 | private float[] _barycentricCoords = new float[4]; | ||
53 | private bool _degenerate; | ||
54 | |||
55 | public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } } | ||
56 | public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } } | ||
57 | public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } } | ||
58 | public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } } | ||
59 | |||
60 | public void Reset() | ||
61 | { | ||
62 | _degenerate = false; | ||
63 | SetBarycentricCoordinates(); | ||
64 | _usedVertices.Reset(); | ||
65 | } | ||
66 | |||
67 | public bool IsValid | ||
68 | { | ||
69 | get | ||
70 | { | ||
71 | return (_barycentricCoords[0] >= 0f) && | ||
72 | (_barycentricCoords[1] >= 0f) && | ||
73 | (_barycentricCoords[2] >= 0f) && | ||
74 | (_barycentricCoords[3] >= 0f); | ||
75 | } | ||
76 | } | ||
77 | |||
78 | public void SetBarycentricCoordinates() | ||
79 | { | ||
80 | SetBarycentricCoordinates(0f, 0f, 0f, 0f); | ||
81 | } | ||
82 | |||
83 | public void SetBarycentricCoordinates(float a, float b, float c, float d) | ||
84 | { | ||
85 | _barycentricCoords[0] = a; | ||
86 | _barycentricCoords[1] = b; | ||
87 | _barycentricCoords[2] = c; | ||
88 | _barycentricCoords[3] = d; | ||
89 | } | ||
90 | } | ||
91 | |||
92 | /// VoronoiSimplexSolver is an implementation of the closest point distance | ||
93 | /// algorithm from a 1-4 points simplex to the origin. | ||
94 | /// Can be used with GJK, as an alternative to Johnson distance algorithm. | ||
95 | public class VoronoiSimplexSolver : ISimplexSolver | ||
96 | { | ||
97 | private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3; | ||
98 | |||
99 | private const int VoronoiSimplexMaxVerts = 5; | ||
100 | private const bool CatchDegenerateTetrahedron = true; | ||
101 | |||
102 | private int _numVertices; | ||
103 | |||
104 | private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts]; | ||
105 | private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts]; | ||
106 | private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts]; | ||
107 | |||
108 | private Vector3 _cachedPA; | ||
109 | private Vector3 _cachedPB; | ||
110 | private Vector3 _cachedV; | ||
111 | private Vector3 _lastW; | ||
112 | private bool _cachedValidClosest; | ||
113 | |||
114 | private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult(); | ||
115 | |||
116 | private bool _needsUpdate; | ||
117 | |||
118 | #region ISimplexSolver Members | ||
119 | |||
120 | public bool FullSimplex | ||
121 | { | ||
122 | get | ||
123 | { | ||
124 | return _numVertices == 4; | ||
125 | } | ||
126 | } | ||
127 | |||
128 | public int NumVertices | ||
129 | { | ||
130 | get | ||
131 | { | ||
132 | return _numVertices; | ||
133 | } | ||
134 | } | ||
135 | |||
136 | public void Reset() | ||
137 | { | ||
138 | _cachedValidClosest = false; | ||
139 | _numVertices = 0; | ||
140 | _needsUpdate = true; | ||
141 | _lastW = new Vector3(1e30f, 1e30f, 1e30f); | ||
142 | _cachedBC.Reset(); | ||
143 | } | ||
144 | |||
145 | public void AddVertex(Vector3 w, Vector3 p, Vector3 q) | ||
146 | { | ||
147 | _lastW = w; | ||
148 | _needsUpdate = true; | ||
149 | |||
150 | _simplexVectorW[_numVertices] = w; | ||
151 | _simplexPointsP[_numVertices] = p; | ||
152 | _simplexPointsQ[_numVertices] = q; | ||
153 | |||
154 | _numVertices++; | ||
155 | } | ||
156 | |||
157 | //return/calculate the closest vertex | ||
158 | public bool Closest(out Vector3 v) | ||
159 | { | ||
160 | bool succes = UpdateClosestVectorAndPoints(); | ||
161 | v = _cachedV; | ||
162 | return succes; | ||
163 | } | ||
164 | |||
165 | public float MaxVertex | ||
166 | { | ||
167 | get | ||
168 | { | ||
169 | int numverts = NumVertices; | ||
170 | float maxV = 0f, curLen2; | ||
171 | for (int i = 0; i < numverts; i++) | ||
172 | { | ||
173 | curLen2 = _simplexVectorW[i].LengthSquared(); | ||
174 | if (maxV < curLen2) maxV = curLen2; | ||
175 | } | ||
176 | return maxV; | ||
177 | } | ||
178 | } | ||
179 | |||
180 | //return the current simplex | ||
181 | public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf) | ||
182 | { | ||
183 | int numverts = NumVertices; | ||
184 | pBuf = new Vector3[numverts]; | ||
185 | qBuf = new Vector3[numverts]; | ||
186 | yBuf = new Vector3[numverts]; | ||
187 | for (int i = 0; i < numverts; i++) | ||
188 | { | ||
189 | yBuf[i] = _simplexVectorW[i]; | ||
190 | pBuf[i] = _simplexPointsP[i]; | ||
191 | qBuf[i] = _simplexPointsQ[i]; | ||
192 | } | ||
193 | return numverts; | ||
194 | } | ||
195 | |||
196 | public bool InSimplex(Vector3 w) | ||
197 | { | ||
198 | //check in case lastW is already removed | ||
199 | if (w == _lastW) return true; | ||
200 | |||
201 | //w is in the current (reduced) simplex | ||
202 | int numverts = NumVertices; | ||
203 | for (int i = 0; i < numverts; i++) | ||
204 | if (_simplexVectorW[i] == w) return true; | ||
205 | |||
206 | return false; | ||
207 | } | ||
208 | |||
209 | public void BackupClosest(out Vector3 v) | ||
210 | { | ||
211 | v = _cachedV; | ||
212 | } | ||
213 | |||
214 | public bool EmptySimplex | ||
215 | { | ||
216 | get | ||
217 | { | ||
218 | return NumVertices == 0; | ||
219 | } | ||
220 | } | ||
221 | |||
222 | public void ComputePoints(out Vector3 p1, out Vector3 p2) | ||
223 | { | ||
224 | UpdateClosestVectorAndPoints(); | ||
225 | p1 = _cachedPA; | ||
226 | p2 = _cachedPB; | ||
227 | } | ||
228 | |||
229 | #endregion | ||
230 | |||
231 | public void RemoveVertex(int index) | ||
232 | { | ||
233 | BulletDebug.Assert(_numVertices > 0); | ||
234 | _numVertices--; | ||
235 | _simplexVectorW[index] = _simplexVectorW[_numVertices]; | ||
236 | _simplexPointsP[index] = _simplexPointsP[_numVertices]; | ||
237 | _simplexPointsQ[index] = _simplexPointsQ[_numVertices]; | ||
238 | } | ||
239 | |||
240 | public void ReduceVertices(UsageBitfield usedVerts) | ||
241 | { | ||
242 | if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3); | ||
243 | if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2); | ||
244 | if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1); | ||
245 | if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0); | ||
246 | } | ||
247 | |||
248 | public bool UpdateClosestVectorAndPoints() | ||
249 | { | ||
250 | if (_needsUpdate) | ||
251 | { | ||
252 | _cachedBC.Reset(); | ||
253 | _needsUpdate = false; | ||
254 | |||
255 | Vector3 p, a, b, c, d; | ||
256 | switch (NumVertices) | ||
257 | { | ||
258 | case 0: | ||
259 | _cachedValidClosest = false; | ||
260 | break; | ||
261 | case 1: | ||
262 | _cachedPA = _simplexPointsP[0]; | ||
263 | _cachedPB = _simplexPointsQ[0]; | ||
264 | _cachedV = _cachedPA - _cachedPB; | ||
265 | _cachedBC.Reset(); | ||
266 | _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f); | ||
267 | _cachedValidClosest = _cachedBC.IsValid; | ||
268 | break; | ||
269 | case 2: | ||
270 | //closest point origin from line segment | ||
271 | Vector3 from = _simplexVectorW[0]; | ||
272 | Vector3 to = _simplexVectorW[1]; | ||
273 | Vector3 nearest; | ||
274 | |||
275 | Vector3 diff = -from; | ||
276 | Vector3 v = to - from; | ||
277 | float t = Vector3.Dot(v, diff); | ||
278 | |||
279 | if (t > 0) | ||
280 | { | ||
281 | float dotVV = v.LengthSquared(); | ||
282 | if (t < dotVV) | ||
283 | { | ||
284 | t /= dotVV; | ||
285 | diff -= t * v; | ||
286 | _cachedBC.UsedVertices.UsedVertexA = true; | ||
287 | _cachedBC.UsedVertices.UsedVertexB = true; | ||
288 | } | ||
289 | else | ||
290 | { | ||
291 | t = 1; | ||
292 | diff -= v; | ||
293 | //reduce to 1 point | ||
294 | _cachedBC.UsedVertices.UsedVertexB = true; | ||
295 | } | ||
296 | } | ||
297 | else | ||
298 | { | ||
299 | t = 0; | ||
300 | //reduce to 1 point | ||
301 | _cachedBC.UsedVertices.UsedVertexA = true; | ||
302 | } | ||
303 | |||
304 | _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0); | ||
305 | nearest = from + t * v; | ||
306 | |||
307 | _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); | ||
308 | _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); | ||
309 | _cachedV = _cachedPA - _cachedPB; | ||
310 | |||
311 | ReduceVertices(_cachedBC.UsedVertices); | ||
312 | |||
313 | _cachedValidClosest = _cachedBC.IsValid; | ||
314 | break; | ||
315 | case 3: | ||
316 | //closest point origin from triangle | ||
317 | p = new Vector3(); | ||
318 | a = _simplexVectorW[0]; | ||
319 | b = _simplexVectorW[1]; | ||
320 | c = _simplexVectorW[2]; | ||
321 | |||
322 | ClosestPtPointTriangle(p, a, b, c, ref _cachedBC); | ||
323 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | ||
324 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | ||
325 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | ||
326 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | ||
327 | |||
328 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | ||
329 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | ||
330 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | ||
331 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | ||
332 | |||
333 | _cachedV = _cachedPA - _cachedPB; | ||
334 | |||
335 | ReduceVertices(_cachedBC.UsedVertices); | ||
336 | _cachedValidClosest = _cachedBC.IsValid; | ||
337 | break; | ||
338 | case 4: | ||
339 | p = new Vector3(); | ||
340 | a = _simplexVectorW[0]; | ||
341 | b = _simplexVectorW[1]; | ||
342 | c = _simplexVectorW[2]; | ||
343 | d = _simplexVectorW[3]; | ||
344 | |||
345 | bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC); | ||
346 | |||
347 | if (hasSeperation) | ||
348 | { | ||
349 | _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] + | ||
350 | _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] + | ||
351 | _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] + | ||
352 | _simplexPointsP[3] * _cachedBC.BarycentricCoords[3]; | ||
353 | |||
354 | _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] + | ||
355 | _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] + | ||
356 | _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] + | ||
357 | _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3]; | ||
358 | |||
359 | _cachedV = _cachedPA - _cachedPB; | ||
360 | ReduceVertices(_cachedBC.UsedVertices); | ||
361 | } | ||
362 | else | ||
363 | { | ||
364 | if (_cachedBC.Degenerate) | ||
365 | { | ||
366 | _cachedValidClosest = false; | ||
367 | } | ||
368 | else | ||
369 | { | ||
370 | _cachedValidClosest = true; | ||
371 | //degenerate case == false, penetration = true + zero | ||
372 | _cachedV.X = _cachedV.Y = _cachedV.Z = 0f; | ||
373 | } | ||
374 | break; // !!!!!!!!!!!! proverit na vsakiy sluchai | ||
375 | } | ||
376 | |||
377 | _cachedValidClosest = _cachedBC.IsValid; | ||
378 | |||
379 | //closest point origin from tetrahedron | ||
380 | break; | ||
381 | default: | ||
382 | _cachedValidClosest = false; | ||
383 | break; | ||
384 | } | ||
385 | } | ||
386 | |||
387 | return _cachedValidClosest; | ||
388 | } | ||
389 | |||
390 | public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, | ||
391 | ref SubSimplexClosestResult result) | ||
392 | { | ||
393 | result.UsedVertices.Reset(); | ||
394 | |||
395 | float v, w; | ||
396 | |||
397 | // Check if P in vertex region outside A | ||
398 | Vector3 ab = b - a; | ||
399 | Vector3 ac = c - a; | ||
400 | Vector3 ap = p - a; | ||
401 | float d1 = Vector3.Dot(ab, ap); | ||
402 | float d2 = Vector3.Dot(ac, ap); | ||
403 | if (d1 <= 0f && d2 <= 0f) | ||
404 | { | ||
405 | result.ClosestPointOnSimplex = a; | ||
406 | result.UsedVertices.UsedVertexA = true; | ||
407 | result.SetBarycentricCoordinates(1, 0, 0, 0); | ||
408 | return true; // a; // barycentric coordinates (1,0,0) | ||
409 | } | ||
410 | |||
411 | // Check if P in vertex region outside B | ||
412 | Vector3 bp = p - b; | ||
413 | float d3 = Vector3.Dot(ab, bp); | ||
414 | float d4 = Vector3.Dot(ac, bp); | ||
415 | if (d3 >= 0f && d4 <= d3) | ||
416 | { | ||
417 | result.ClosestPointOnSimplex = b; | ||
418 | result.UsedVertices.UsedVertexB = true; | ||
419 | result.SetBarycentricCoordinates(0, 1, 0, 0); | ||
420 | |||
421 | return true; // b; // barycentric coordinates (0,1,0) | ||
422 | } | ||
423 | // Check if P in edge region of AB, if so return projection of P onto AB | ||
424 | float vc = d1 * d4 - d3 * d2; | ||
425 | if (vc <= 0f && d1 >= 0f && d3 <= 0f) | ||
426 | { | ||
427 | v = d1 / (d1 - d3); | ||
428 | result.ClosestPointOnSimplex = a + v * ab; | ||
429 | result.UsedVertices.UsedVertexA = true; | ||
430 | result.UsedVertices.UsedVertexB = true; | ||
431 | result.SetBarycentricCoordinates(1 - v, v, 0, 0); | ||
432 | return true; | ||
433 | //return a + v * ab; // barycentric coordinates (1-v,v,0) | ||
434 | } | ||
435 | |||
436 | // Check if P in vertex region outside C | ||
437 | Vector3 cp = p - c; | ||
438 | float d5 = Vector3.Dot(ab, cp); | ||
439 | float d6 = Vector3.Dot(ac, cp); | ||
440 | if (d6 >= 0f && d5 <= d6) | ||
441 | { | ||
442 | result.ClosestPointOnSimplex = c; | ||
443 | result.UsedVertices.UsedVertexC = true; | ||
444 | result.SetBarycentricCoordinates(0, 0, 1, 0); | ||
445 | return true;//c; // barycentric coordinates (0,0,1) | ||
446 | } | ||
447 | |||
448 | // Check if P in edge region of AC, if so return projection of P onto AC | ||
449 | float vb = d5 * d2 - d1 * d6; | ||
450 | if (vb <= 0f && d2 >= 0f && d6 <= 0f) | ||
451 | { | ||
452 | w = d2 / (d2 - d6); | ||
453 | result.ClosestPointOnSimplex = a + w * ac; | ||
454 | result.UsedVertices.UsedVertexA = true; | ||
455 | result.UsedVertices.UsedVertexC = true; | ||
456 | result.SetBarycentricCoordinates(1 - w, 0, w, 0); | ||
457 | return true; | ||
458 | //return a + w * ac; // barycentric coordinates (1-w,0,w) | ||
459 | } | ||
460 | |||
461 | // Check if P in edge region of BC, if so return projection of P onto BC | ||
462 | float va = d3 * d6 - d5 * d4; | ||
463 | if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) | ||
464 | { | ||
465 | w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); | ||
466 | |||
467 | result.ClosestPointOnSimplex = b + w * (c - b); | ||
468 | result.UsedVertices.UsedVertexB = true; | ||
469 | result.UsedVertices.UsedVertexC = true; | ||
470 | result.SetBarycentricCoordinates(0, 1 - w, w, 0); | ||
471 | return true; | ||
472 | // return b + w * (c - b); // barycentric coordinates (0,1-w,w) | ||
473 | } | ||
474 | |||
475 | // P inside face region. Compute Q through its barycentric coordinates (u,v,w) | ||
476 | float denom = 1.0f / (va + vb + vc); | ||
477 | v = vb * denom; | ||
478 | w = vc * denom; | ||
479 | |||
480 | result.ClosestPointOnSimplex = a + ab * v + ac * w; | ||
481 | result.UsedVertices.UsedVertexA = true; | ||
482 | result.UsedVertices.UsedVertexB = true; | ||
483 | result.UsedVertices.UsedVertexC = true; | ||
484 | result.SetBarycentricCoordinates(1 - v - w, v, w, 0); | ||
485 | |||
486 | return true; | ||
487 | } | ||
488 | |||
489 | /// Test if point p and d lie on opposite sides of plane through abc | ||
490 | public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d) | ||
491 | { | ||
492 | Vector3 normal = Vector3.Cross(b - a, c - a); | ||
493 | |||
494 | float signp = Vector3.Dot(p - a, normal); // [AP AB AC] | ||
495 | float signd = Vector3.Dot(d - a, normal); // [AD AB AC] | ||
496 | |||
497 | if (CatchDegenerateTetrahedron) | ||
498 | if (signd * signd < (1e-4f * 1e-4f)) return -1; | ||
499 | |||
500 | // Points on opposite sides if expression signs are opposite | ||
501 | return signp * signd < 0f ? 1 : 0; | ||
502 | } | ||
503 | |||
504 | public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d, | ||
505 | ref SubSimplexClosestResult finalResult) | ||
506 | { | ||
507 | SubSimplexClosestResult tempResult = new SubSimplexClosestResult(); | ||
508 | |||
509 | // Start out assuming point inside all halfspaces, so closest to itself | ||
510 | finalResult.ClosestPointOnSimplex = p; | ||
511 | finalResult.UsedVertices.Reset(); | ||
512 | finalResult.UsedVertices.UsedVertexA = true; | ||
513 | finalResult.UsedVertices.UsedVertexB = true; | ||
514 | finalResult.UsedVertices.UsedVertexC = true; | ||
515 | finalResult.UsedVertices.UsedVertexD = true; | ||
516 | |||
517 | int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); | ||
518 | int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); | ||
519 | int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); | ||
520 | int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); | ||
521 | |||
522 | if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) | ||
523 | { | ||
524 | finalResult.Degenerate = true; | ||
525 | return false; | ||
526 | } | ||
527 | |||
528 | if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) | ||
529 | return false; | ||
530 | |||
531 | float bestSqDist = float.MaxValue; | ||
532 | // If point outside face abc then compute closest point on abc | ||
533 | if (pointOutsideABC != 0) | ||
534 | { | ||
535 | ClosestPtPointTriangle(p, a, b, c, ref tempResult); | ||
536 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
537 | |||
538 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
539 | // Update best closest point if (squared) distance is less than current best | ||
540 | if (sqDist < bestSqDist) | ||
541 | { | ||
542 | bestSqDist = sqDist; | ||
543 | finalResult.ClosestPointOnSimplex = q; | ||
544 | //convert result bitmask! | ||
545 | finalResult.UsedVertices.Reset(); | ||
546 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
547 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB; | ||
548 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | ||
549 | finalResult.SetBarycentricCoordinates( | ||
550 | tempResult.BarycentricCoords[VertexA], | ||
551 | tempResult.BarycentricCoords[VertexB], | ||
552 | tempResult.BarycentricCoords[VertexC], | ||
553 | 0); | ||
554 | } | ||
555 | } | ||
556 | |||
557 | // Repeat test for face acd | ||
558 | if (pointOutsideACD != 0) | ||
559 | { | ||
560 | ClosestPtPointTriangle(p, a, c, d, ref tempResult); | ||
561 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
562 | //convert result bitmask! | ||
563 | |||
564 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
565 | if (sqDist < bestSqDist) | ||
566 | { | ||
567 | bestSqDist = sqDist; | ||
568 | finalResult.ClosestPointOnSimplex = q; | ||
569 | finalResult.UsedVertices.Reset(); | ||
570 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
571 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB; | ||
572 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC; | ||
573 | finalResult.SetBarycentricCoordinates( | ||
574 | tempResult.BarycentricCoords[VertexA], | ||
575 | 0, | ||
576 | tempResult.BarycentricCoords[VertexB], | ||
577 | tempResult.BarycentricCoords[VertexC]); | ||
578 | } | ||
579 | } | ||
580 | // Repeat test for face adb | ||
581 | |||
582 | if (pointOutsideADB != 0) | ||
583 | { | ||
584 | ClosestPtPointTriangle(p, a, d, b, ref tempResult); | ||
585 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
586 | //convert result bitmask! | ||
587 | |||
588 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
589 | if (sqDist < bestSqDist) | ||
590 | { | ||
591 | bestSqDist = sqDist; | ||
592 | finalResult.ClosestPointOnSimplex = q; | ||
593 | finalResult.UsedVertices.Reset(); | ||
594 | finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA; | ||
595 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | ||
596 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC; | ||
597 | finalResult.SetBarycentricCoordinates( | ||
598 | tempResult.BarycentricCoords[VertexA], | ||
599 | tempResult.BarycentricCoords[VertexC], | ||
600 | 0, | ||
601 | tempResult.BarycentricCoords[VertexB]); | ||
602 | |||
603 | } | ||
604 | } | ||
605 | // Repeat test for face bdc | ||
606 | |||
607 | if (pointOutsideBDC != 0) | ||
608 | { | ||
609 | ClosestPtPointTriangle(p, b, d, c, ref tempResult); | ||
610 | Vector3 q = tempResult.ClosestPointOnSimplex; | ||
611 | //convert result bitmask! | ||
612 | float sqDist = ((Vector3)(q - p)).LengthSquared(); | ||
613 | if (sqDist < bestSqDist) | ||
614 | { | ||
615 | bestSqDist = sqDist; | ||
616 | finalResult.ClosestPointOnSimplex = q; | ||
617 | finalResult.UsedVertices.Reset(); | ||
618 | finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA; | ||
619 | finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB; | ||
620 | finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC; | ||
621 | |||
622 | finalResult.SetBarycentricCoordinates( | ||
623 | 0, | ||
624 | tempResult.BarycentricCoords[VertexA], | ||
625 | tempResult.BarycentricCoords[VertexC], | ||
626 | tempResult.BarycentricCoords[VertexB]); | ||
627 | } | ||
628 | } | ||
629 | |||
630 | //help! we ended up full ! | ||
631 | |||
632 | if (finalResult.UsedVertices.UsedVertexA && | ||
633 | finalResult.UsedVertices.UsedVertexB && | ||
634 | finalResult.UsedVertices.UsedVertexC && | ||
635 | finalResult.UsedVertices.UsedVertexD) | ||
636 | { | ||
637 | return true; | ||
638 | } | ||
639 | |||
640 | return true; | ||
641 | } | ||
642 | } | ||
643 | } | ||