Blender  V3.3
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  BP_FP_INT_TYPE m_bpHandleMask;
40  BP_FP_INT_TYPE m_handleSentinel;
41 
42 public:
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 
54 public:
55  class Handle : public btBroadphaseProxy
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 
70 protected:
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)
83  void* m_pEdgesRawPtr[3];
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 
117 public:
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 
154  {
155  return m_pairCache;
156  }
158  {
159  return m_pairCache;
160  }
161 
163  {
164  m_userPairCallback = pairCallback;
165  }
167  {
168  return m_userPairCallback;
169  }
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 
194 template <typename BP_FP_INT_TYPE>
195 void 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 
216 template <typename BP_FP_INT_TYPE>
217 btBroadphaseProxy* 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 
224  if (m_raycastAccelerator)
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 
232 template <typename BP_FP_INT_TYPE>
234 {
235  Handle* handle = static_cast<Handle*>(proxy);
236  if (m_raycastAccelerator)
237  m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy, dispatcher);
238  removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
239 }
240 
241 template <typename BP_FP_INT_TYPE>
242 void btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* dispatcher)
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);
248  if (m_raycastAccelerator)
249  m_raycastAccelerator->setAabb(handle->m_dbvtProxy, aabbMin, aabbMax, dispatcher);
250 }
251 
252 template <typename BP_FP_INT_TYPE>
253 void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
254 {
255  if (m_raycastAccelerator)
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 
274 template <typename BP_FP_INT_TYPE>
276 {
277  if (m_raycastAccelerator)
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 
300 template <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 
308 template <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 
330 template <typename BP_FP_INT_TYPE>
331 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)
332  : m_bpHandleMask(handleMask),
333  m_handleSentinel(handleSentinel),
334  m_pairCache(pairCache),
335  m_userPairCallback(0),
336  m_ownsPairCache(false),
337  m_invalidPair(0),
338  m_raycastAccelerator(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  {
351  m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache), 16)) btNullPairCache();
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
375  m_firstFreeHandle = 1;
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 
411 template <typename BP_FP_INT_TYPE>
413 {
414  if (m_raycastAccelerator)
415  {
416  m_nullPairCache->~btOverlappingPairCache();
417  btAlignedFree(m_nullPairCache);
418  m_raycastAccelerator->~btDbvtBroadphase();
419  btAlignedFree(m_raycastAccelerator);
420  }
421 
422  for (int i = 2; i >= 0; i--)
423  {
424  btAlignedFree(m_pEdgesRawPtr[i]);
425  }
426  delete[] m_pHandles;
427 
428  if (m_ownsPairCache)
429  {
430  m_pairCache->~btOverlappingPairCache();
431  btAlignedFree(m_pairCache);
432  }
433 }
434 
435 template <typename BP_FP_INT_TYPE>
436 void 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 
456 template <typename BP_FP_INT_TYPE>
458 {
459  btAssert(m_firstFreeHandle);
460 
461  BP_FP_INT_TYPE handle = m_firstFreeHandle;
462  m_firstFreeHandle = getHandle(handle)->GetNextFree();
463  m_numHandles++;
464 
465  return handle;
466 }
467 
468 template <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 
479 template <typename BP_FP_INT_TYPE>
480 BP_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 
529 template <typename BP_FP_INT_TYPE>
530 void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle, btDispatcher* dispatcher)
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 
578 template <typename BP_FP_INT_TYPE>
580 {
581  if (m_numHandles == 0)
582  {
583  m_firstFreeHandle = 1;
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 
594 template <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;
654  m_invalidPair++;
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 
673 template <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 
692 template <typename BP_FP_INT_TYPE>
693 bool 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 
707 template <typename BP_FP_INT_TYPE>
708 void 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
753 template <typename BP_FP_INT_TYPE>
754 void 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);
772  if (m_userPairCallback)
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
802 template <typename BP_FP_INT_TYPE>
803 void 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);
828  if (m_userPairCallback)
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
852 template <typename BP_FP_INT_TYPE>
853 void 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);
880  if (m_userPairCallback)
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
909 template <typename BP_FP_INT_TYPE>
910 void 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);
931  if (m_userPairCallback)
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
void swap(T &a, T &b)
Definition: Common.h:19
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a point
ATTR_WARN_UNUSED_RESULT const BMVert * v
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
btBroadphasePair
btBroadphaseProxy
btHashedOverlappingPairCache()
SIMD_FORCE_INLINE void quantize(unsigned short *out, const btVector3 &point, int isMax) const
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:280
#define btAssert(x)
Definition: btScalar.h:295
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
BP_FP_INT_TYPE IsMax() const
SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next)
SIMD_FORCE_INLINE 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))
BP_FP_INT_TYPE m_bpHandleMask
BP_FP_INT_TYPE allocHandle()
bool testOverlap2D(const Handle *pHandleA, const Handle *pHandleB, int axis0, int axis1)
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)
btOverlappingPairCache * getOverlappingPairCache()
const btOverlappingPairCache * getOverlappingPairCache() const
virtual btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
SIMD_FORCE_INLINE Handle * getHandle(BP_FP_INT_TYPE index) const
void quantize(BP_FP_INT_TYPE *out, const btVector3 &point, int isMax) const
BP_FP_INT_TYPE getNumHandles() const
btDbvtBroadphase * m_raycastAccelerator
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
btOverlappingPairCallback * m_userPairCallback
btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pai...
const btOverlappingPairCallback * getOverlappingPairUserCallback() const
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)
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
virtual bool hasDeferredRemoval()=0
virtual btBroadphasePairArray & getOverlappingPairArray()=0
The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/rem...
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy0, btDispatcher *dispatcher)=0
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
DEGForeachIDComponentCallback callback
SyclQueue void void size_t num_bytes void
static ulong * next
static const pxr::TfToken out("out", pxr::TfToken::Immortal)
#define min(a, b)
Definition: sort.c:35
virtual bool process(const btBroadphaseProxy *proxy)=0
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)
float max
PointerRNA * ptr
Definition: wm_files.c:3480