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 
22 #include "LinearMath/btVector3.h"
23 #include "btOverlappingPairCache.h"
24 #include "btBroadphaseInterface.h"
25 #include "btBroadphaseProxy.h"
27 #include "btDbvtBroadphase.h"
28 
29 //#define DEBUG_BROADPHASE 1
30 #define USE_OVERLAP_TEST_ON_REMOVES 1
31 
35 template <typename BP_FP_INT_TYPE>
37 {
38 protected:
39 
40  BP_FP_INT_TYPE m_bpHandleMask;
41  BP_FP_INT_TYPE m_handleSentinel;
42 
43 public:
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 
56 public:
57  class Handle : public btBroadphaseProxy
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 
73 protected:
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)
86  void* m_pEdgesRawPtr[3];
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 
124 public:
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 
128  virtual ~btAxisSweep3Internal();
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 
208 template <typename BP_FP_INT_TYPE>
209 void 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 
230 template <typename BP_FP_INT_TYPE>
231 btBroadphaseProxy* 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 
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 
248 template <typename BP_FP_INT_TYPE>
250 {
251  Handle* handle = static_cast<Handle*>(proxy);
253  m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher);
254  removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
255 }
256 
257 template <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);
265  m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher);
266 
267 }
268 
269 template <typename BP_FP_INT_TYPE>
270 void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
271 {
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 
290 template <typename BP_FP_INT_TYPE>
292 {
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 
317 template <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 
326 template <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 
351 template <typename BP_FP_INT_TYPE>
352 btAxisSweep3Internal<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),
354 m_handleSentinel(handleSentinel),
355 m_pairCache(pairCache),
357 m_ownsPairCache(false),
358 m_invalidPair(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
396  m_firstFreeHandle = 1;
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 
415  m_pHandles[0].m_clientObject = 0;
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;
424  m_pEdges[axis][1].m_pos = m_handleSentinel;
425  m_pEdges[axis][1].m_handle = 0;
426 #ifdef DEBUG_BROADPHASE
427  debugPrintAxis(axis);
428 #endif //DEBUG_BROADPHASE
429 
430  }
431 
432 }
433 
434 template <typename BP_FP_INT_TYPE>
436 {
438  {
443  }
444 
445  for (int i = 2; i >= 0; i--)
446  {
448  }
449  delete [] m_pHandles;
450 
451  if (m_ownsPairCache)
452  {
455  }
456 }
457 
458 template <typename BP_FP_INT_TYPE>
459 void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const
460 {
461 #ifdef OLD_CLAMPING_METHOD
462  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 
480 template <typename BP_FP_INT_TYPE>
482 {
484 
485  BP_FP_INT_TYPE handle = m_firstFreeHandle;
487  m_numHandles++;
488 
489  return handle;
490 }
491 
492 template <typename BP_FP_INT_TYPE>
494 {
495  btAssert(handle > 0 && handle < m_maxHandles);
496 
498  m_firstFreeHandle = handle;
499 
500  m_numHandles--;
501 }
502 
503 
504 template <typename BP_FP_INT_TYPE>
505 BP_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 
559 template <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)
569  {
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 
616 template <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 
631 extern int gOverlappingPairs;
632 //#include <stdio.h>
633 
634 template <typename BP_FP_INT_TYPE>
636 {
637 
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 
720 template <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 
739 template <typename BP_FP_INT_TYPE>
740 bool 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 
754 template <typename BP_FP_INT_TYPE>
755 void 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
805 template <typename BP_FP_INT_TYPE>
806 void 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
857 template <typename BP_FP_INT_TYPE>
858 void 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
913 template <typename BP_FP_INT_TYPE>
914 void 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
976 template <typename BP_FP_INT_TYPE>
977 void 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
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
virtual void resetPool(btDispatcher *dispatcher)
reset broadphase internal structures, to ensure determinism/reproducability
void quantize(BP_FP_INT_TYPE *out, const btVector3 &point, int isMax) const
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 cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
#define USE_OVERLAP_TEST_ON_REMOVES
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
Definition: btAxisSweep3.h:33
virtual bool hasDeferredRemoval()=0
btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing...
void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
virtual void getAabb(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
void freeHandle(BP_FP_INT_TYPE handle)
#define btAssert(x)
Definition: btScalar.h:131
BP_FP_INT_TYPE IsMax() const
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees...
BP_FP_INT_TYPE m_handleSentinel
void processAllOverlappingPairs(btOverlapCallback *callback)
BP_FP_INT_TYPE m_firstFreeHandle
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
void removeHandle(BP_FP_INT_TYPE handle, btDispatcher *dispatcher)
BP_FP_INT_TYPE GetNextFree() const
The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
bool testAabbOverlap(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
BP_FP_INT_TYPE getNumHandles() const
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
int gOverlappingPairs
virtual btBroadphasePairArray & getOverlappingPairArray()=0
virtual void calculateOverlappingPairs(btDispatcher *dispatcher)
calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during th...
BP_FP_INT_TYPE addHandle(const btVector3 &aabbMin, const btVector3 &aabbMax, void *pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
virtual void destroyProxy(btBroadphaseProxy *proxy, 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))
void updateHandle(BP_FP_INT_TYPE handle, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
#define btAlignedFree(ptr)
void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
void setOverlappingPairUserCallback(btOverlappingPairCallback *pairCallback)
bool testOverlap2D(const Handle *pHandleA, const Handle *pHandleB, int axis0, int axis1)
BP_FP_INT_TYPE m_bpHandleMask
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy0, btDispatcher *dispatcher)=0
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs...
const btOverlappingPairCache * getOverlappingPairCache() const
Handle * getHandle(BP_FP_INT_TYPE index) const
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
btBroadphaseProxy * m_pProxy1
void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
btCollisionAlgorithm * m_algorithm
btOverlappingPairCache * m_nullPairCache
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
virtual bool process(const btBroadphaseProxy *proxy)=0
int size() const
return the number of elements in the array
btOverlappingPairCache * m_pairCache
btBroadphaseProxy * m_pProxy0
const btOverlappingPairCallback * getOverlappingPairUserCallback() const
btDbvtBroadphase * m_raycastAccelerator
additional dynamic aabb structure, used to accelerate ray cast queries.
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
BP_FP_INT_TYPE allocHandle()
void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
void resize(int newsize, const T &fillData=T())
The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
btOverlappingPairCache * getOverlappingPairCache()
void SetNextFree(BP_FP_INT_TYPE next)
#define btAlignedAlloc(size, alignment)
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 unQuantize(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
unQuantize should be conservative: aabbMin/aabbMax should be larger then &#39;getAabb&#39; result ...
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:75
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 &#39;global&#39; coordinate frame will add some transfor...
btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman...
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/rem...
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
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
void quickSort(const L &CompareFunc)
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
virtual btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
btOverlappingPairCallback * m_userPairCallback
btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pai...
The btBroadphasePair class contains a pair of aabb-overlapping objects.