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 http://continuousphysics.com/Bullet/
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
40 BP_FP_INT_TYPE m_bpHandleMask;
41 BP_FP_INT_TYPE m_handleSentinel;
42
43public:
44
46
47 class Edge
48 {
49 public:
50 BP_FP_INT_TYPE m_pos; // low bit is min/max
51 BP_FP_INT_TYPE m_handle;
52
53 BP_FP_INT_TYPE IsMax() const {return static_cast<BP_FP_INT_TYPE>(m_pos & 1);}
54 };
55
56public:
58 {
59 public:
61
62 // indexes into the edge arrays
63 BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12
64// BP_FP_INT_TYPE m_uniqueId;
65 btBroadphaseProxy* m_dbvtProxy;//for faster raycast
66 //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject
67
68 SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;}
69 SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];}
70 }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry
71
72
73protected:
74 btVector3 m_worldAabbMin; // overall system bounds
75 btVector3 m_worldAabbMax; // overall system bounds
76
77 btVector3 m_quantize; // scaling factor for quantization
78
79 BP_FP_INT_TYPE m_numHandles; // number of active handles
80 BP_FP_INT_TYPE m_maxHandles; // max number of handles
81 Handle* m_pHandles; // handles pool
82
83 BP_FP_INT_TYPE m_firstFreeHandle; // free handles list
84
85 Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
87
89
92
94
96
101
102
103 // allocation/deallocation
104 BP_FP_INT_TYPE allocHandle();
105 void freeHandle(BP_FP_INT_TYPE handle);
106
107
108 bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1);
109
110#ifdef DEBUG_BROADPHASE
111 void debugPrintAxis(int axis,bool checkCardinality=true);
112#endif //DEBUG_BROADPHASE
113
114 //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
115 //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB);
116
117
118
119 void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
120 void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
121 void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
122 void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps );
123
124public:
125
126 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);
127
129
130 BP_FP_INT_TYPE getNumHandles() const
131 {
132 return m_numHandles;
133 }
134
135 virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
136
137 BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher);
138 void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher);
139 void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
140 SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
141
142 virtual void resetPool(btDispatcher* dispatcher);
143
145
146 //Broadphase Interface
147 virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher);
148 virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
149 virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
150 virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
151
152 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));
153 virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback);
154
155
156 void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const;
158 void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
159
161
163 {
164 return m_pairCache;
165 }
167 {
168 return m_pairCache;
169 }
170
172 {
173 m_userPairCallback = pairCallback;
174 }
176 {
177 return m_userPairCallback;
178 }
179
182 virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
183 {
184 aabbMin = m_worldAabbMin;
185 aabbMax = m_worldAabbMax;
186 }
187
188 virtual void printStats()
189 {
190/* printf("btAxisSweep3.h\n");
191 printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles);
192 printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(),
193 m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ());
194 */
195
196 }
197
198};
199
201
202
203
204
205#ifdef DEBUG_BROADPHASE
206#include <stdio.h>
207
208template <typename BP_FP_INT_TYPE>
209void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
210{
211 int numEdges = m_pHandles[0].m_maxEdges[axis];
212 printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
213
214 int i;
215 for (i=0;i<numEdges+1;i++)
216 {
217 Edge* pEdge = m_pEdges[axis] + i;
218 Handle* pHandlePrev = getHandle(pEdge->m_handle);
219 int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
220 char beginOrEnd;
221 beginOrEnd=pEdge->IsMax()?'E':'B';
222 printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
223 }
224
225 if (checkCardinality)
226 btAssert(numEdges == m_numHandles*2+1);
227}
228#endif //DEBUG_BROADPHASE
229
230template <typename BP_FP_INT_TYPE>
231btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher)
232{
233 (void)shapeType;
234 BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
235
236 Handle* handle = getHandle(handleId);
237
238 if (m_raycastAccelerator)
239 {
240 btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
241 handle->m_dbvtProxy = rayProxy;
242 }
243 return handle;
244}
245
246
247
248template <typename BP_FP_INT_TYPE>
250{
251 Handle* handle = static_cast<Handle*>(proxy);
252 if (m_raycastAccelerator)
253 m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
254 removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
255}
256
257template <typename BP_FP_INT_TYPE>
259{
260 Handle* handle = static_cast<Handle*>(proxy);
261 handle->m_aabbMin = aabbMin;
262 handle->m_aabbMax = aabbMax;
263 updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher);
264 if (m_raycastAccelerator)
265 m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
266
267}
268
269template <typename BP_FP_INT_TYPE>
270void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
271{
272 if (m_raycastAccelerator)
273 {
274 m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax);
275 } else
276 {
277 //choose axis?
278 BP_FP_INT_TYPE axis = 0;
279 //for each proxy
280 for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
281 {
282 if (m_pEdges[axis][i].IsMax())
283 {
284 rayCallback.process(getHandle(m_pEdges[axis][i].m_handle));
285 }
286 }
287 }
288}
289
290template <typename BP_FP_INT_TYPE>
292{
293 if (m_raycastAccelerator)
294 {
295 m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback);
296 } else
297 {
298 //choose axis?
299 BP_FP_INT_TYPE axis = 0;
300 //for each proxy
301 for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
302 {
303 if (m_pEdges[axis][i].IsMax())
304 {
305 Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
306 if (TestAabbAgainstAabb2(aabbMin,aabbMax,handle->m_aabbMin,handle->m_aabbMax))
307 {
308 callback.process(handle);
309 }
310 }
311 }
312 }
313}
314
315
316
317template <typename BP_FP_INT_TYPE>
319{
320 Handle* pHandle = static_cast<Handle*>(proxy);
321 aabbMin = pHandle->m_aabbMin;
322 aabbMax = pHandle->m_aabbMax;
323}
324
325
326template <typename BP_FP_INT_TYPE>
328{
329 Handle* pHandle = static_cast<Handle*>(proxy);
330
331 unsigned short vecInMin[3];
332 unsigned short vecInMax[3];
333
334 vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ;
335 vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ;
336 vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ;
337 vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ;
338 vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ;
339 vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ;
340
341 aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ()));
342 aabbMin += m_worldAabbMin;
343
344 aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ()));
345 aabbMax += m_worldAabbMin;
346}
347
348
349
350
351template <typename BP_FP_INT_TYPE>
352btAxisSweep3Internal<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)
353:m_bpHandleMask(handleMask),
354m_handleSentinel(handleSentinel),
355m_pairCache(pairCache),
356m_userPairCallback(0),
357m_ownsPairCache(false),
358m_invalidPair(0),
359m_raycastAccelerator(0)
360{
361 BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle
362
363 if (!m_pairCache)
364 {
365 void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16);
367 m_ownsPairCache = true;
368 }
369
370 if (!disableRaycastAccelerator)
371 {
374 m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
375 }
376
377 //btAssert(bounds.HasVolume());
378
379 // init bounds
380 m_worldAabbMin = worldAabbMin;
381 m_worldAabbMax = worldAabbMax;
382
384
385 BP_FP_INT_TYPE maxInt = m_handleSentinel;
386
387 m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
388
389 // allocate handles buffer, using btAlignedAlloc, and put all handles on free list
390 m_pHandles = new Handle[maxHandles];
391
392 m_maxHandles = maxHandles;
393 m_numHandles = 0;
394
395 // handle 0 is reserved as the null index, and is also used as the sentinel
397 {
398 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
399 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
400 m_pHandles[maxHandles - 1].SetNextFree(0);
401 }
402
403 {
404 // allocate edge buffers
405 for (int i = 0; i < 3; i++)
406 {
407 m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16);
408 m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2];
409 }
410 }
411 //removed overlap management
412
413 // make boundary sentinels
414
416
417 for (int axis = 0; axis < 3; axis++)
418 {
419 m_pHandles[0].m_minEdges[axis] = 0;
420 m_pHandles[0].m_maxEdges[axis] = 1;
421
422 m_pEdges[axis][0].m_pos = 0;
423 m_pEdges[axis][0].m_handle = 0;
425 m_pEdges[axis][1].m_handle = 0;
426#ifdef DEBUG_BROADPHASE
427 debugPrintAxis(axis);
428#endif //DEBUG_BROADPHASE
429
430 }
431
432}
433
434template <typename BP_FP_INT_TYPE>
436{
437 if (m_raycastAccelerator)
438 {
439 m_nullPairCache->~btOverlappingPairCache();
440 btAlignedFree(m_nullPairCache);
441 m_raycastAccelerator->~btDbvtBroadphase();
442 btAlignedFree (m_raycastAccelerator);
443 }
444
445 for (int i = 2; i >= 0; i--)
446 {
447 btAlignedFree(m_pEdgesRawPtr[i]);
448 }
449 delete [] m_pHandles;
450
451 if (m_ownsPairCache)
452 {
453 m_pairCache->~btOverlappingPairCache();
454 btAlignedFree(m_pairCache);
455 }
456}
457
458template <typename BP_FP_INT_TYPE>
459void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
460{
461#ifdef OLD_CLAMPING_METHOD
464 btVector3 clampedPoint(point);
465 clampedPoint.setMax(m_worldAabbMin);
466 clampedPoint.setMin(m_worldAabbMax);
467 btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
468 out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
469 out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
470 out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
471#else
472 btVector3 v = (point - m_worldAabbMin) * m_quantize;
473 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);
474 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);
475 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);
476#endif //OLD_CLAMPING_METHOD
477}
478
479
480template <typename BP_FP_INT_TYPE>
482{
483 btAssert(m_firstFreeHandle);
484
485 BP_FP_INT_TYPE handle = m_firstFreeHandle;
486 m_firstFreeHandle = getHandle(handle)->GetNextFree();
487 m_numHandles++;
488
489 return handle;
490}
491
492template <typename BP_FP_INT_TYPE>
494{
495 btAssert(handle > 0 && handle < m_maxHandles);
496
497 getHandle(handle)->SetNextFree(m_firstFreeHandle);
498 m_firstFreeHandle = handle;
499
500 m_numHandles--;
501}
502
503
504template <typename BP_FP_INT_TYPE>
505BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher)
506{
507 // quantize the bounds
508 BP_FP_INT_TYPE min[3], max[3];
509 quantize(min, aabbMin, 0);
510 quantize(max, aabbMax, 1);
511
512 // allocate a handle
513 BP_FP_INT_TYPE handle = allocHandle();
514
515
516 Handle* pHandle = getHandle(handle);
517
518 pHandle->m_uniqueId = static_cast<int>(handle);
519 //pHandle->m_pOverlaps = 0;
520 pHandle->m_clientObject = pOwner;
521 pHandle->m_collisionFilterGroup = collisionFilterGroup;
522 pHandle->m_collisionFilterMask = collisionFilterMask;
523
524 // compute current limit of edge arrays
525 BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2);
526
527
528 // insert new edges just inside the max boundary edge
529 for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
530 {
531
532 m_pHandles[0].m_maxEdges[axis] += 2;
533
534 m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
535
536 m_pEdges[axis][limit - 1].m_pos = min[axis];
537 m_pEdges[axis][limit - 1].m_handle = handle;
538
539 m_pEdges[axis][limit].m_pos = max[axis];
540 m_pEdges[axis][limit].m_handle = handle;
541
542 pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1);
543 pHandle->m_maxEdges[axis] = limit;
544 }
545
546 // now sort the new edges to their correct position
547 sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false);
548 sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false);
549 sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false);
550 sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false);
551 sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true);
552 sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true);
553
554
555 return handle;
556}
557
558
559template <typename BP_FP_INT_TYPE>
561{
562
563 Handle* pHandle = getHandle(handle);
564
565 //explicitly remove the pairs containing the proxy
566 //we could do it also in the sortMinUp (passing true)
568 if (!m_pairCache->hasDeferredRemoval())
569 {
570 m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
571 }
572
573 // compute current limit of edge arrays
574 int limit = static_cast<int>(m_numHandles * 2);
575
576 int axis;
577
578 for (axis = 0;axis<3;axis++)
579 {
580 m_pHandles[0].m_maxEdges[axis] -= 2;
581 }
582
583 // remove the edges by sorting them up to the end of the list
584 for ( axis = 0; axis < 3; axis++)
585 {
586 Edge* pEdges = m_pEdges[axis];
587 BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
588 pEdges[max].m_pos = m_handleSentinel;
589
590 sortMaxUp(axis,max,dispatcher,false);
591
592
593 BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
594 pEdges[i].m_pos = m_handleSentinel;
595
596
597 sortMinUp(axis,i,dispatcher,false);
598
599 pEdges[limit-1].m_handle = 0;
600 pEdges[limit-1].m_pos = m_handleSentinel;
601
602#ifdef DEBUG_BROADPHASE
603 debugPrintAxis(axis,false);
604#endif //DEBUG_BROADPHASE
605
606
607 }
608
609
610 // free the handle
611 freeHandle(handle);
612
613
614}
615
616template <typename BP_FP_INT_TYPE>
618{
619 if (m_numHandles == 0)
620 {
621 m_firstFreeHandle = 1;
622 {
623 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
624 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
625 m_pHandles[m_maxHandles - 1].SetNextFree(0);
626 }
627 }
628}
629
630
631extern int gOverlappingPairs;
632//#include <stdio.h>
633
634template <typename BP_FP_INT_TYPE>
636{
637
638 if (m_pairCache->hasDeferredRemoval())
639 {
640
641 btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
642
643 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
644 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
645
646 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
647 m_invalidPair = 0;
648
649
650 int i;
651
652 btBroadphasePair previousPair;
653 previousPair.m_pProxy0 = 0;
654 previousPair.m_pProxy1 = 0;
655 previousPair.m_algorithm = 0;
656
657
658 for (i=0;i<overlappingPairArray.size();i++)
659 {
660
661 btBroadphasePair& pair = overlappingPairArray[i];
662
663 bool isDuplicate = (pair == previousPair);
664
665 previousPair = pair;
666
667 bool needsRemoval = false;
668
669 if (!isDuplicate)
670 {
672 bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
673
674 if (hasOverlap)
675 {
676 needsRemoval = false;//callback->processOverlap(pair);
677 } else
678 {
679 needsRemoval = true;
680 }
681 } else
682 {
683 //remove duplicate
684 needsRemoval = true;
685 //should have no algorithm
686 btAssert(!pair.m_algorithm);
687 }
688
689 if (needsRemoval)
690 {
691 m_pairCache->cleanOverlappingPair(pair,dispatcher);
692
693 // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
694 // m_overlappingPairArray.pop_back();
695 pair.m_pProxy0 = 0;
696 pair.m_pProxy1 = 0;
697 m_invalidPair++;
699 }
700
701 }
702
704 #define CLEAN_INVALID_PAIRS 1
705 #ifdef CLEAN_INVALID_PAIRS
706
707 //perform a sort, to sort 'invalid' pairs to the end
708 overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
709
710 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
711 m_invalidPair = 0;
712 #endif//CLEAN_INVALID_PAIRS
713
714 //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
715 }
716
717}
718
719
720template <typename BP_FP_INT_TYPE>
722{
723 const Handle* pHandleA = static_cast<Handle*>(proxy0);
724 const Handle* pHandleB = static_cast<Handle*>(proxy1);
725
726 //optimization 1: check the array index (memory address), instead of the m_pos
727
728 for (int axis = 0; axis < 3; axis++)
729 {
730 if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
731 pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
732 {
733 return false;
734 }
735 }
736 return true;
737}
738
739template <typename BP_FP_INT_TYPE>
740bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1)
741{
742 //optimization 1: check the array index (memory address), instead of the m_pos
743
744 if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] ||
745 pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
746 pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
747 pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1])
748 {
749 return false;
750 }
751 return true;
752}
753
754template <typename BP_FP_INT_TYPE>
755void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
756{
757// btAssert(bounds.IsFinite());
758 //btAssert(bounds.HasVolume());
759
760 Handle* pHandle = getHandle(handle);
761
762 // quantize the new bounds
763 BP_FP_INT_TYPE min[3], max[3];
764 quantize(min, aabbMin, 0);
765 quantize(max, aabbMax, 1);
766
767 // update changed edges
768 for (int axis = 0; axis < 3; axis++)
769 {
770 BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
771 BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
772
773 int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
774 int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
775
776 m_pEdges[axis][emin].m_pos = min[axis];
777 m_pEdges[axis][emax].m_pos = max[axis];
778
779 // expand (only adds overlaps)
780 if (dmin < 0)
781 sortMinDown(axis, emin,dispatcher,true);
782
783 if (dmax > 0)
784 sortMaxUp(axis, emax,dispatcher,true);
785
786 // shrink (only removes overlaps)
787 if (dmin > 0)
788 sortMinUp(axis, emin,dispatcher,true);
789
790 if (dmax < 0)
791 sortMaxDown(axis, emax,dispatcher,true);
792
793#ifdef DEBUG_BROADPHASE
794 debugPrintAxis(axis);
795#endif //DEBUG_BROADPHASE
796 }
797
798
799}
800
801
802
803
804// sorting a min edge downwards can only ever *add* overlaps
805template <typename BP_FP_INT_TYPE>
806void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
807{
808
809 Edge* pEdge = m_pEdges[axis] + edge;
810 Edge* pPrev = pEdge - 1;
811 Handle* pHandleEdge = getHandle(pEdge->m_handle);
812
813 while (pEdge->m_pos < pPrev->m_pos)
814 {
815 Handle* pHandlePrev = getHandle(pPrev->m_handle);
816
817 if (pPrev->IsMax())
818 {
819 // if previous edge is a maximum check the bounds and add an overlap if necessary
820 const int axis1 = (1 << axis) & 3;
821 const int axis2 = (1 << axis1) & 3;
822 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2))
823 {
824 m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
825 if (m_userPairCallback)
826 m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev);
827
828 //AddOverlap(pEdge->m_handle, pPrev->m_handle);
829
830 }
831
832 // update edge reference in other handle
833 pHandlePrev->m_maxEdges[axis]++;
834 }
835 else
836 pHandlePrev->m_minEdges[axis]++;
837
838 pHandleEdge->m_minEdges[axis]--;
839
840 // swap the edges
841 Edge swap = *pEdge;
842 *pEdge = *pPrev;
843 *pPrev = swap;
844
845 // decrement
846 pEdge--;
847 pPrev--;
848 }
849
850#ifdef DEBUG_BROADPHASE
851 debugPrintAxis(axis);
852#endif //DEBUG_BROADPHASE
853
854}
855
856// sorting a min edge upwards can only ever *remove* overlaps
857template <typename BP_FP_INT_TYPE>
858void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
859{
860 Edge* pEdge = m_pEdges[axis] + edge;
861 Edge* pNext = pEdge + 1;
862 Handle* pHandleEdge = getHandle(pEdge->m_handle);
863
864 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
865 {
866 Handle* pHandleNext = getHandle(pNext->m_handle);
867
868 if (pNext->IsMax())
869 {
870 Handle* handle0 = getHandle(pEdge->m_handle);
871 Handle* handle1 = getHandle(pNext->m_handle);
872 const int axis1 = (1 << axis) & 3;
873 const int axis2 = (1 << axis1) & 3;
874
875 // if next edge is maximum remove any overlap between the two handles
876 if (updateOverlaps
878 && testOverlap2D(handle0,handle1,axis1,axis2)
879#endif //USE_OVERLAP_TEST_ON_REMOVES
880 )
881 {
882
883
884 m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
885 if (m_userPairCallback)
886 m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
887
888 }
889
890
891 // update edge reference in other handle
892 pHandleNext->m_maxEdges[axis]--;
893 }
894 else
895 pHandleNext->m_minEdges[axis]--;
896
897 pHandleEdge->m_minEdges[axis]++;
898
899 // swap the edges
900 Edge swap = *pEdge;
901 *pEdge = *pNext;
902 *pNext = swap;
903
904 // increment
905 pEdge++;
906 pNext++;
907 }
908
909
910}
911
912// sorting a max edge downwards can only ever *remove* overlaps
913template <typename BP_FP_INT_TYPE>
914void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps)
915{
916
917 Edge* pEdge = m_pEdges[axis] + edge;
918 Edge* pPrev = pEdge - 1;
919 Handle* pHandleEdge = getHandle(pEdge->m_handle);
920
921 while (pEdge->m_pos < pPrev->m_pos)
922 {
923 Handle* pHandlePrev = getHandle(pPrev->m_handle);
924
925 if (!pPrev->IsMax())
926 {
927 // if previous edge was a minimum remove any overlap between the two handles
928 Handle* handle0 = getHandle(pEdge->m_handle);
929 Handle* handle1 = getHandle(pPrev->m_handle);
930 const int axis1 = (1 << axis) & 3;
931 const int axis2 = (1 << axis1) & 3;
932
933 if (updateOverlaps
935 && testOverlap2D(handle0,handle1,axis1,axis2)
936#endif //USE_OVERLAP_TEST_ON_REMOVES
937 )
938 {
939 //this is done during the overlappingpairarray iteration/narrowphase collision
940
941
942 m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
943 if (m_userPairCallback)
944 m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
945
946
947
948 }
949
950 // update edge reference in other handle
951 pHandlePrev->m_minEdges[axis]++;;
952 }
953 else
954 pHandlePrev->m_maxEdges[axis]++;
955
956 pHandleEdge->m_maxEdges[axis]--;
957
958 // swap the edges
959 Edge swap = *pEdge;
960 *pEdge = *pPrev;
961 *pPrev = swap;
962
963 // decrement
964 pEdge--;
965 pPrev--;
966 }
967
968
969#ifdef DEBUG_BROADPHASE
970 debugPrintAxis(axis);
971#endif //DEBUG_BROADPHASE
972
973}
974
975// sorting a max edge upwards can only ever *add* overlaps
976template <typename BP_FP_INT_TYPE>
977void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps)
978{
979 Edge* pEdge = m_pEdges[axis] + edge;
980 Edge* pNext = pEdge + 1;
981 Handle* pHandleEdge = getHandle(pEdge->m_handle);
982
983 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
984 {
985 Handle* pHandleNext = getHandle(pNext->m_handle);
986
987 const int axis1 = (1 << axis) & 3;
988 const int axis2 = (1 << axis1) & 3;
989
990 if (!pNext->IsMax())
991 {
992 // if next edge is a minimum check the bounds and add an overlap if necessary
993 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2))
994 {
995 Handle* handle0 = getHandle(pEdge->m_handle);
996 Handle* handle1 = getHandle(pNext->m_handle);
997 m_pairCache->addOverlappingPair(handle0,handle1);
998 if (m_userPairCallback)
999 m_userPairCallback->addOverlappingPair(handle0,handle1);
1000 }
1001
1002 // update edge reference in other handle
1003 pHandleNext->m_minEdges[axis]--;
1004 }
1005 else
1006 pHandleNext->m_maxEdges[axis]--;
1007
1008 pHandleEdge->m_maxEdges[axis]++;
1009
1010 // swap the edges
1011 Edge swap = *pEdge;
1012 *pEdge = *pNext;
1013 *pNext = swap;
1014
1015 // increment
1016 pEdge++;
1017 pNext++;
1018 }
1019
1020}
1021
1022#endif
bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2)
conservative test for overlap between two aabbs
Definition: btAabbUtil2.h:48
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
int gOverlappingPairs
#define USE_OVERLAP_TEST_ON_REMOVES
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
#define btAssert(x)
Definition: btScalar.h:131
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
The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
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))
BP_FP_INT_TYPE m_bpHandleMask
BP_FP_INT_TYPE allocHandle()
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.
Definition: btAxisSweep3.h:34
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 ...
Definition: btDispatcher.h:76
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,...
virtual btBroadphasePairArray & getOverlappingPairArray()=0
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
virtual bool hasDeferredRemoval()=0
The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/rem...
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy0, btDispatcher *dispatcher)=0
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:621
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:638
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
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...
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
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))
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)