Bullet Collision Detection & Physics Library
btAxisSweep3Internal.h
Go to the documentation of this file.
1//Bullet Continuous Collision Detection and Physics Library
2//Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
3
4//
5// btAxisSweep3.h
6//
7// Copyright (c) 2006 Simon Hobbs
8//
9// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
10//
11// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
12//
13// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
14//
15// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
16//
17// 3. This notice may not be removed or altered from any source distribution.
18
19#ifndef BT_AXIS_SWEEP_3_INTERNAL_H
20#define BT_AXIS_SWEEP_3_INTERNAL_H
21
25#include "btBroadphaseProxy.h"
27#include "btDbvtBroadphase.h"
28
29//#define DEBUG_BROADPHASE 1
30#define USE_OVERLAP_TEST_ON_REMOVES 1
31
35template <typename BP_FP_INT_TYPE>
37{
38protected:
39 BP_FP_INT_TYPE m_bpHandleMask;
40 BP_FP_INT_TYPE m_handleSentinel;
41
42public:
44
45 class Edge
46 {
47 public:
48 BP_FP_INT_TYPE m_pos; // low bit is min/max
49 BP_FP_INT_TYPE m_handle;
50
51 BP_FP_INT_TYPE IsMax() const { return static_cast<BP_FP_INT_TYPE>(m_pos & 1); }
52 };
53
54public:
56 {
57 public:
59
60 // indexes into the edge arrays
61 BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12
62 // BP_FP_INT_TYPE m_uniqueId;
63 btBroadphaseProxy* m_dbvtProxy; //for faster raycast
64 //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
65
66 SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) { m_minEdges[0] = next; }
67 SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const { return m_minEdges[0]; }
68 }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry
69
70protected:
71 btVector3 m_worldAabbMin; // overall system bounds
72 btVector3 m_worldAabbMax; // overall system bounds
73
74 btVector3 m_quantize; // scaling factor for quantization
75
76 BP_FP_INT_TYPE m_numHandles; // number of active handles
77 BP_FP_INT_TYPE m_maxHandles; // max number of handles
78 Handle* m_pHandles; // handles pool
79
80 BP_FP_INT_TYPE m_firstFreeHandle; // free handles list
81
82 Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
84
86
89
91
93
98
99 // allocation/deallocation
100 BP_FP_INT_TYPE allocHandle();
101 void freeHandle(BP_FP_INT_TYPE handle);
102
103 bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1);
104
105#ifdef DEBUG_BROADPHASE
106 void debugPrintAxis(int axis, bool checkCardinality = true);
107#endif //DEBUG_BROADPHASE
108
109 //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
110 //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
111
112 void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
113 void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
114 void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
115 void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps);
116
117public:
118 btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false);
119
121
122 BP_FP_INT_TYPE getNumHandles() const
123 {
124 return m_numHandles;
125 }
126
127 virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
128
129 BP_FP_INT_TYPE addHandle(const btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
130 void removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher);
131 void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
132 SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const { return m_pHandles + index; }
133
134 virtual void resetPool(btDispatcher* dispatcher);
135
137
138 //Broadphase Interface
139 virtual btBroadphaseProxy* createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher);
140 virtual void destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher);
141 virtual void setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher);
142 virtual void getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
143
144 virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin = btVector3(0, 0, 0), const btVector3& aabbMax = btVector3(0, 0, 0));
145 virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
146
147 void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
149 void unQuantize(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const;
150
152
158 {
159 return m_pairCache;
160 }
161
163 {
164 m_userPairCallback = pairCallback;
165 }
170
173 virtual void getBroadphaseAabb(btVector3& aabbMin, btVector3& aabbMax) const
174 {
175 aabbMin = m_worldAabbMin;
176 aabbMax = m_worldAabbMax;
177 }
178
179 virtual void printStats()
180 {
181 /* printf("btAxisSweep3.h\n");
182 printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
183 printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
184 m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
185 */
186 }
187};
188
190
191#ifdef DEBUG_BROADPHASE
192#include <stdio.h>
193
194template <typename BP_FP_INT_TYPE>
195void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
196{
197 int numEdges = m_pHandles[0].m_maxEdges[axis];
198 printf("SAP Axis %d, numEdges=%d\n", axis, numEdges);
199
200 int i;
201 for (i = 0; i < numEdges + 1; i++)
202 {
203 Edge* pEdge = m_pEdges[axis] + i;
204 Handle* pHandlePrev = getHandle(pEdge->m_handle);
205 int handleIndex = pEdge->IsMax() ? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
206 char beginOrEnd;
207 beginOrEnd = pEdge->IsMax() ? 'E' : 'B';
208 printf(" [%c,h=%d,p=%x,i=%d]\n", beginOrEnd, pEdge->m_handle, pEdge->m_pos, handleIndex);
209 }
210
211 if (checkCardinality)
212 btAssert(numEdges == m_numHandles * 2 + 1);
213}
214#endif //DEBUG_BROADPHASE
215
216template <typename BP_FP_INT_TYPE>
217btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher)
218{
219 (void)shapeType;
220 BP_FP_INT_TYPE handleId = addHandle(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
221
222 Handle* handle = getHandle(handleId);
223
225 {
226 btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
227 handle->m_dbvtProxy = rayProxy;
228 }
229 return handle;
230}
231
232template <typename BP_FP_INT_TYPE>
234{
235 Handle* handle = static_cast<Handle*>(proxy);
237 m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy, dispatcher);
238 removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
239}
240
241template <typename BP_FP_INT_TYPE>
243{
244 Handle* handle = static_cast<Handle*>(proxy);
245 handle->m_aabbMin = aabbMin;
246 handle->m_aabbMax = aabbMax;
247 updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax, dispatcher);
249 m_raycastAccelerator->setAabb(handle->m_dbvtProxy, aabbMin, aabbMax, dispatcher);
250}
251
252template <typename BP_FP_INT_TYPE>
253void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
254{
256 {
257 m_raycastAccelerator->rayTest(rayFrom, rayTo, rayCallback, aabbMin, aabbMax);
258 }
259 else
260 {
261 //choose axis?
262 BP_FP_INT_TYPE axis = 0;
263 //for each proxy
264 for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
265 {
266 if (m_pEdges[axis][i].IsMax())
267 {
268 rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
269 }
270 }
271 }
272}
273
274template <typename BP_FP_INT_TYPE>
276{
278 {
279 m_raycastAccelerator->aabbTest(aabbMin, aabbMax, callback);
280 }
281 else
282 {
283 //choose axis?
284 BP_FP_INT_TYPE axis = 0;
285 //for each proxy
286 for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
287 {
288 if (m_pEdges[axis][i].IsMax())
289 {
290 Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
291 if (TestAabbAgainstAabb2(aabbMin, aabbMax, handle->m_aabbMin, handle->m_aabbMax))
292 {
293 callback.process(handle);
294 }
295 }
296 }
297 }
298}
299
300template <typename BP_FP_INT_TYPE>
302{
303 Handle* pHandle = static_cast<Handle*>(proxy);
304 aabbMin = pHandle->m_aabbMin;
305 aabbMax = pHandle->m_aabbMax;
306}
307
308template <typename BP_FP_INT_TYPE>
310{
311 Handle* pHandle = static_cast<Handle*>(proxy);
312
313 unsigned short vecInMin[3];
314 unsigned short vecInMax[3];
315
316 vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos;
317 vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos + 1;
318 vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos;
319 vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos + 1;
320 vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos;
321 vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos + 1;
322
323 aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()), (btScalar)(vecInMin[1]) / (m_quantize.getY()), (btScalar)(vecInMin[2]) / (m_quantize.getZ()));
324 aabbMin += m_worldAabbMin;
325
326 aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()), (btScalar)(vecInMax[1]) / (m_quantize.getY()), (btScalar)(vecInMax[2]) / (m_quantize.getZ()));
327 aabbMax += m_worldAabbMin;
328}
329
330template <typename BP_FP_INT_TYPE>
331btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin, const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
332 : m_bpHandleMask(handleMask),
333 m_handleSentinel(handleSentinel),
334 m_pairCache(pairCache),
336 m_ownsPairCache(false),
337 m_invalidPair(0),
339{
340 BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles + 1); //need to add one sentinel handle
341
342 if (!m_pairCache)
343 {
344 void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16);
346 m_ownsPairCache = true;
347 }
348
349 if (!disableRaycastAccelerator)
350 {
353 m_raycastAccelerator->m_deferedcollide = true; //don't add/remove pairs
354 }
355
356 //btAssert(bounds.HasVolume());
357
358 // init bounds
359 m_worldAabbMin = worldAabbMin;
360 m_worldAabbMax = worldAabbMax;
361
363
364 BP_FP_INT_TYPE maxInt = m_handleSentinel;
365
366 m_quantize = btVector3(btScalar(maxInt), btScalar(maxInt), btScalar(maxInt)) / aabbSize;
367
368 // allocate handles buffer, using btAlignedAlloc, and put all handles on free list
369 m_pHandles = new Handle[maxHandles];
370
371 m_maxHandles = maxHandles;
372 m_numHandles = 0;
373
374 // handle 0 is reserved as the null index, and is also used as the sentinel
376 {
377 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
378 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
379 m_pHandles[maxHandles - 1].SetNextFree(0);
380 }
381
382 {
383 // allocate edge buffers
384 for (int i = 0; i < 3; i++)
385 {
386 m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge) * maxHandles * 2, 16);
387 m_pEdges[i] = new (m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
388 }
389 }
390 //removed overlap management
391
392 // make boundary sentinels
393
394 m_pHandles[0].m_clientObject = 0;
395
396 for (int axis = 0; axis < 3; axis++)
397 {
398 m_pHandles[0].m_minEdges[axis] = 0;
399 m_pHandles[0].m_maxEdges[axis] = 1;
400
401 m_pEdges[axis][0].m_pos = 0;
402 m_pEdges[axis][0].m_handle = 0;
403 m_pEdges[axis][1].m_pos = m_handleSentinel;
404 m_pEdges[axis][1].m_handle = 0;
405#ifdef DEBUG_BROADPHASE
406 debugPrintAxis(axis);
407#endif //DEBUG_BROADPHASE
408 }
409}
410
411template <typename BP_FP_INT_TYPE>
413{
415 {
416 m_nullPairCache->~btOverlappingPairCache();
418 m_raycastAccelerator->~btDbvtBroadphase();
420 }
421
422 for (int i = 2; i >= 0; i--)
423 {
425 }
426 delete[] m_pHandles;
427
428 if (m_ownsPairCache)
429 {
430 m_pairCache->~btOverlappingPairCache();
432 }
433}
434
435template <typename BP_FP_INT_TYPE>
436void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
437{
438#ifdef OLD_CLAMPING_METHOD
441 btVector3 clampedPoint(point);
442 clampedPoint.setMax(m_worldAabbMin);
443 clampedPoint.setMin(m_worldAabbMax);
444 btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
445 out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
446 out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
447 out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
448#else
449 btVector3 v = (point - m_worldAabbMin) * m_quantize;
450 out[0] = (v[0] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[0] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0] & m_bpHandleMask) | isMax);
451 out[1] = (v[1] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[1] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1] & m_bpHandleMask) | isMax);
452 out[2] = (v[2] <= 0) ? (BP_FP_INT_TYPE)isMax : (v[2] >= m_handleSentinel) ? (BP_FP_INT_TYPE)((m_handleSentinel & m_bpHandleMask) | isMax) : (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2] & m_bpHandleMask) | isMax);
453#endif //OLD_CLAMPING_METHOD
454}
455
456template <typename BP_FP_INT_TYPE>
458{
460
461 BP_FP_INT_TYPE handle = m_firstFreeHandle;
462 m_firstFreeHandle = getHandle(handle)->GetNextFree();
463 m_numHandles++;
464
465 return handle;
466}
467
468template <typename BP_FP_INT_TYPE>
470{
471 btAssert(handle > 0 && handle < m_maxHandles);
472
473 getHandle(handle)->SetNextFree(m_firstFreeHandle);
474 m_firstFreeHandle = handle;
475
476 m_numHandles--;
477}
478
479template <typename BP_FP_INT_TYPE>
480BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin, const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher)
481{
482 // quantize the bounds
483 BP_FP_INT_TYPE min[3], max[3];
484 quantize(min, aabbMin, 0);
485 quantize(max, aabbMax, 1);
486
487 // allocate a handle
488 BP_FP_INT_TYPE handle = allocHandle();
489
490 Handle* pHandle = getHandle(handle);
491
492 pHandle->m_uniqueId = static_cast<int>(handle);
493 //pHandle->m_pOverlaps = 0;
494 pHandle->m_clientObject = pOwner;
495 pHandle->m_collisionFilterGroup = collisionFilterGroup;
496 pHandle->m_collisionFilterMask = collisionFilterMask;
497
498 // compute current limit of edge arrays
499 BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
500
501 // insert new edges just inside the max boundary edge
502 for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
503 {
504 m_pHandles[0].m_maxEdges[axis] += 2;
505
506 m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
507
508 m_pEdges[axis][limit - 1].m_pos = min[axis];
509 m_pEdges[axis][limit - 1].m_handle = handle;
510
511 m_pEdges[axis][limit].m_pos = max[axis];
512 m_pEdges[axis][limit].m_handle = handle;
513
514 pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
515 pHandle->m_maxEdges[axis] = limit;
516 }
517
518 // now sort the new edges to their correct position
519 sortMinDown(0, pHandle->m_minEdges[0], dispatcher, false);
520 sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher, false);
521 sortMinDown(1, pHandle->m_minEdges[1], dispatcher, false);
522 sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher, false);
523 sortMinDown(2, pHandle->m_minEdges[2], dispatcher, true);
524 sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher, true);
525
526 return handle;
527}
528
529template <typename BP_FP_INT_TYPE>
531{
532 Handle* pHandle = getHandle(handle);
533
534 //explicitly remove the pairs containing the proxy
535 //we could do it also in the sortMinUp (passing true)
537 if (!m_pairCache->hasDeferredRemoval())
538 {
539 m_pairCache->removeOverlappingPairsContainingProxy(pHandle, dispatcher);
540 }
541
542 // compute current limit of edge arrays
543 int limit = static_cast<int>(m_numHandles * 2);
544
545 int axis;
546
547 for (axis = 0; axis < 3; axis++)
548 {
549 m_pHandles[0].m_maxEdges[axis] -= 2;
550 }
551
552 // remove the edges by sorting them up to the end of the list
553 for (axis = 0; axis < 3; axis++)
554 {
555 Edge* pEdges = m_pEdges[axis];
556 BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
557 pEdges[max].m_pos = m_handleSentinel;
558
559 sortMaxUp(axis, max, dispatcher, false);
560
561 BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
562 pEdges[i].m_pos = m_handleSentinel;
563
564 sortMinUp(axis, i, dispatcher, false);
565
566 pEdges[limit - 1].m_handle = 0;
567 pEdges[limit - 1].m_pos = m_handleSentinel;
568
569#ifdef DEBUG_BROADPHASE
570 debugPrintAxis(axis, false);
571#endif //DEBUG_BROADPHASE
572 }
573
574 // free the handle
575 freeHandle(handle);
576}
577
578template <typename BP_FP_INT_TYPE>
580{
581 if (m_numHandles == 0)
582 {
584 {
585 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
586 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
587 m_pHandles[m_maxHandles - 1].SetNextFree(0);
588 }
589 }
590}
591
592//#include <stdio.h>
593
594template <typename BP_FP_INT_TYPE>
596{
597 if (m_pairCache->hasDeferredRemoval())
598 {
599 btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
600
601 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
602 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
603
604 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
605 m_invalidPair = 0;
606
607 int i;
608
609 btBroadphasePair previousPair;
610 previousPair.m_pProxy0 = 0;
611 previousPair.m_pProxy1 = 0;
612 previousPair.m_algorithm = 0;
613
614 for (i = 0; i < overlappingPairArray.size(); i++)
615 {
616 btBroadphasePair& pair = overlappingPairArray[i];
617
618 bool isDuplicate = (pair == previousPair);
619
620 previousPair = pair;
621
622 bool needsRemoval = false;
623
624 if (!isDuplicate)
625 {
627 bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
628
629 if (hasOverlap)
630 {
631 needsRemoval = false; //callback->processOverlap(pair);
632 }
633 else
634 {
635 needsRemoval = true;
636 }
637 }
638 else
639 {
640 //remove duplicate
641 needsRemoval = true;
642 //should have no algorithm
643 btAssert(!pair.m_algorithm);
644 }
645
646 if (needsRemoval)
647 {
648 m_pairCache->cleanOverlappingPair(pair, dispatcher);
649
650 // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
651 // m_overlappingPairArray.pop_back();
652 pair.m_pProxy0 = 0;
653 pair.m_pProxy1 = 0;
655 }
656 }
657
659#define CLEAN_INVALID_PAIRS 1
660#ifdef CLEAN_INVALID_PAIRS
661
662 //perform a sort, to sort 'invalid' pairs to the end
663 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
664
665 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
666 m_invalidPair = 0;
667#endif //CLEAN_INVALID_PAIRS
668
669 //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
670 }
671}
672
673template <typename BP_FP_INT_TYPE>
675{
676 const Handle* pHandleA = static_cast<Handle*>(proxy0);
677 const Handle* pHandleB = static_cast<Handle*>(proxy1);
678
679 //optimization 1: check the array index (memory address), instead of the m_pos
680
681 for (int axis = 0; axis < 3; axis++)
682 {
683 if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
684 pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
685 {
686 return false;
687 }
688 }
689 return true;
690}
691
692template <typename BP_FP_INT_TYPE>
693bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB, int axis0, int axis1)
694{
695 //optimization 1: check the array index (memory address), instead of the m_pos
696
697 if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] ||
698 pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
699 pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
700 pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1])
701 {
702 return false;
703 }
704 return true;
705}
706
707template <typename BP_FP_INT_TYPE>
708void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher)
709{
710 // btAssert(bounds.IsFinite());
711 //btAssert(bounds.HasVolume());
712
713 Handle* pHandle = getHandle(handle);
714
715 // quantize the new bounds
716 BP_FP_INT_TYPE min[3], max[3];
717 quantize(min, aabbMin, 0);
718 quantize(max, aabbMax, 1);
719
720 // update changed edges
721 for (int axis = 0; axis < 3; axis++)
722 {
723 BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
724 BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
725
726 int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
727 int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
728
729 m_pEdges[axis][emin].m_pos = min[axis];
730 m_pEdges[axis][emax].m_pos = max[axis];
731
732 // expand (only adds overlaps)
733 if (dmin < 0)
734 sortMinDown(axis, emin, dispatcher, true);
735
736 if (dmax > 0)
737 sortMaxUp(axis, emax, dispatcher, true);
738
739 // shrink (only removes overlaps)
740 if (dmin > 0)
741 sortMinUp(axis, emin, dispatcher, true);
742
743 if (dmax < 0)
744 sortMaxDown(axis, emax, dispatcher, true);
745
746#ifdef DEBUG_BROADPHASE
747 debugPrintAxis(axis);
748#endif //DEBUG_BROADPHASE
749 }
750}
751
752// sorting a min edge downwards can only ever *add* overlaps
753template <typename BP_FP_INT_TYPE>
754void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
755{
756 Edge* pEdge = m_pEdges[axis] + edge;
757 Edge* pPrev = pEdge - 1;
758 Handle* pHandleEdge = getHandle(pEdge->m_handle);
759
760 while (pEdge->m_pos < pPrev->m_pos)
761 {
762 Handle* pHandlePrev = getHandle(pPrev->m_handle);
763
764 if (pPrev->IsMax())
765 {
766 // if previous edge is a maximum check the bounds and add an overlap if necessary
767 const int axis1 = (1 << axis) & 3;
768 const int axis2 = (1 << axis1) & 3;
769 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev, axis1, axis2))
770 {
771 m_pairCache->addOverlappingPair(pHandleEdge, pHandlePrev);
773 m_userPairCallback->addOverlappingPair(pHandleEdge, pHandlePrev);
774
775 //AddOverlap(pEdge->m_handle, pPrev->m_handle);
776 }
777
778 // update edge reference in other handle
779 pHandlePrev->m_maxEdges[axis]++;
780 }
781 else
782 pHandlePrev->m_minEdges[axis]++;
783
784 pHandleEdge->m_minEdges[axis]--;
785
786 // swap the edges
787 Edge swap = *pEdge;
788 *pEdge = *pPrev;
789 *pPrev = swap;
790
791 // decrement
792 pEdge--;
793 pPrev--;
794 }
795
796#ifdef DEBUG_BROADPHASE
797 debugPrintAxis(axis);
798#endif //DEBUG_BROADPHASE
799}
800
801// sorting a min edge upwards can only ever *remove* overlaps
802template <typename BP_FP_INT_TYPE>
803void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
804{
805 Edge* pEdge = m_pEdges[axis] + edge;
806 Edge* pNext = pEdge + 1;
807 Handle* pHandleEdge = getHandle(pEdge->m_handle);
808
809 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
810 {
811 Handle* pHandleNext = getHandle(pNext->m_handle);
812
813 if (pNext->IsMax())
814 {
815 Handle* handle0 = getHandle(pEdge->m_handle);
816 Handle* handle1 = getHandle(pNext->m_handle);
817 const int axis1 = (1 << axis) & 3;
818 const int axis2 = (1 << axis1) & 3;
819
820 // if next edge is maximum remove any overlap between the two handles
821 if (updateOverlaps
823 && testOverlap2D(handle0, handle1, axis1, axis2)
824#endif //USE_OVERLAP_TEST_ON_REMOVES
825 )
826 {
827 m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
829 m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher);
830 }
831
832 // update edge reference in other handle
833 pHandleNext->m_maxEdges[axis]--;
834 }
835 else
836 pHandleNext->m_minEdges[axis]--;
837
838 pHandleEdge->m_minEdges[axis]++;
839
840 // swap the edges
841 Edge swap = *pEdge;
842 *pEdge = *pNext;
843 *pNext = swap;
844
845 // increment
846 pEdge++;
847 pNext++;
848 }
849}
850
851// sorting a max edge downwards can only ever *remove* overlaps
852template <typename BP_FP_INT_TYPE>
853void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
854{
855 Edge* pEdge = m_pEdges[axis] + edge;
856 Edge* pPrev = pEdge - 1;
857 Handle* pHandleEdge = getHandle(pEdge->m_handle);
858
859 while (pEdge->m_pos < pPrev->m_pos)
860 {
861 Handle* pHandlePrev = getHandle(pPrev->m_handle);
862
863 if (!pPrev->IsMax())
864 {
865 // if previous edge was a minimum remove any overlap between the two handles
866 Handle* handle0 = getHandle(pEdge->m_handle);
867 Handle* handle1 = getHandle(pPrev->m_handle);
868 const int axis1 = (1 << axis) & 3;
869 const int axis2 = (1 << axis1) & 3;
870
871 if (updateOverlaps
873 && testOverlap2D(handle0, handle1, axis1, axis2)
874#endif //USE_OVERLAP_TEST_ON_REMOVES
875 )
876 {
877 //this is done during the overlappingpairarray iteration/narrowphase collision
878
879 m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
881 m_userPairCallback->removeOverlappingPair(handle0, handle1, dispatcher);
882 }
883
884 // update edge reference in other handle
885 pHandlePrev->m_minEdges[axis]++;
886 ;
887 }
888 else
889 pHandlePrev->m_maxEdges[axis]++;
890
891 pHandleEdge->m_maxEdges[axis]--;
892
893 // swap the edges
894 Edge swap = *pEdge;
895 *pEdge = *pPrev;
896 *pPrev = swap;
897
898 // decrement
899 pEdge--;
900 pPrev--;
901 }
902
903#ifdef DEBUG_BROADPHASE
904 debugPrintAxis(axis);
905#endif //DEBUG_BROADPHASE
906}
907
908// sorting a max edge upwards can only ever *add* overlaps
909template <typename BP_FP_INT_TYPE>
910void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
911{
912 Edge* pEdge = m_pEdges[axis] + edge;
913 Edge* pNext = pEdge + 1;
914 Handle* pHandleEdge = getHandle(pEdge->m_handle);
915
916 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
917 {
918 Handle* pHandleNext = getHandle(pNext->m_handle);
919
920 const int axis1 = (1 << axis) & 3;
921 const int axis2 = (1 << axis1) & 3;
922
923 if (!pNext->IsMax())
924 {
925 // if next edge is a minimum check the bounds and add an overlap if necessary
926 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext, axis1, axis2))
927 {
928 Handle* handle0 = getHandle(pEdge->m_handle);
929 Handle* handle1 = getHandle(pNext->m_handle);
930 m_pairCache->addOverlappingPair(handle0, handle1);
932 m_userPairCallback->addOverlappingPair(handle0, handle1);
933 }
934
935 // update edge reference in other handle
936 pHandleNext->m_minEdges[axis]--;
937 }
938 else
939 pHandleNext->m_maxEdges[axis]--;
940
941 pHandleEdge->m_maxEdges[axis]++;
942
943 // swap the edges
944 Edge swap = *pEdge;
945 *pEdge = *pNext;
946 *pNext = swap;
947
948 // increment
949 pEdge++;
950 pNext++;
951 }
952}
953
954#endif
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2)
conservative test for overlap between two aabbs
Definition btAabbUtil2.h:43
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
#define USE_OVERLAP_TEST_ON_REMOVES
btAlignedObjectArray< btBroadphasePair > btBroadphasePairArray
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define SIMD_FORCE_INLINE
Definition btScalar.h:98
#define btAssert(x)
Definition btScalar.h:153
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
BP_FP_INT_TYPE IsMax() const
void SetNextFree(BP_FP_INT_TYPE next)
BP_FP_INT_TYPE GetNextFree() const
virtual void getAabb(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
btOverlappingPairCache * m_pairCache
BP_FP_INT_TYPE m_handleSentinel
bool testAabbOverlap(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
void unQuantize(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
virtual void rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, btBroadphaseRayCallback &rayCallback, const btVector3 &aabbMin=btVector3(0, 0, 0), const btVector3 &aabbMax=btVector3(0, 0, 0))
bool testOverlap2D(const Handle *pHandleA, const Handle *pHandleB, int axis0, int axis1)
const btOverlappingPairCallback * getOverlappingPairUserCallback() const
BP_FP_INT_TYPE addHandle(const btVector3 &aabbMin, const btVector3 &aabbMax, void *pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
void removeHandle(BP_FP_INT_TYPE handle, btDispatcher *dispatcher)
virtual btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
Handle * getHandle(BP_FP_INT_TYPE index) const
const btOverlappingPairCache * getOverlappingPairCache() const
void quantize(BP_FP_INT_TYPE *out, const btVector3 &point, int isMax) const
BP_FP_INT_TYPE getNumHandles() const
btDbvtBroadphase * m_raycastAccelerator
additional dynamic aabb structure, used to accelerate ray cast queries.
btOverlappingPairCache * getOverlappingPairCache()
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
virtual void calculateOverlappingPairs(btDispatcher *dispatcher)
calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during th...
void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
void freeHandle(BP_FP_INT_TYPE handle)
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
void updateHandle(BP_FP_INT_TYPE handle, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
btAxisSweep3Internal(const btVector3 &worldAabbMin, const btVector3 &worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles=16384, btOverlappingPairCache *pairCache=0, bool disableRaycastAccelerator=false)
virtual void getBroadphaseAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb returns the axis aligned bounding box in the 'global' coordinate frame will add some transfor...
btOverlappingPairCallback * m_userPairCallback
btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pai...
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
void processAllOverlappingPairs(btOverlapCallback *callback)
BP_FP_INT_TYPE m_firstFreeHandle
void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
void setOverlappingPairUserCallback(btOverlappingPairCallback *pairCallback)
virtual void resetPool(btDispatcher *dispatcher)
reset broadphase internal structures, to ensure determinism/reproducability
btOverlappingPairCache * m_nullPairCache
void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman,...
btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/rem...
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
const btScalar & getZ() const
Return the z value.
Definition btVector3.h:565
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition btVector3.h:609
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition btVector3.h:640
const btScalar & getY() const
Return the y value.
Definition btVector3.h:563
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition btVector3.h:626
const btScalar & getX() const
Return the x value.
Definition btVector3.h:561
virtual bool process(const btBroadphaseProxy *proxy)=0
The btBroadphasePair class contains a pair of aabb-overlapping objects.
btBroadphaseProxy * m_pProxy1
btBroadphaseProxy * m_pProxy0
btCollisionAlgorithm * m_algorithm
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees...