2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
16 #ifndef B3_QUANTIZED_BVH_H
17 #define B3_QUANTIZED_BVH_H
21 //#define DEBUG_CHECK_DEQUANTIZATION 1
22 #ifdef DEBUG_CHECK_DEQUANTIZATION
24 #define printf spu_printf
29 #endif //DEBUG_CHECK_DEQUANTIZATION
31 #include "Bullet3Common/b3Vector3.h"
32 #include "Bullet3Common/b3AlignedAllocator.h"
34 #ifdef B3_USE_DOUBLE_PRECISION
35 #define b3QuantizedBvhData b3QuantizedBvhDoubleData
36 #define b3OptimizedBvhNodeData b3OptimizedBvhNodeDoubleData
37 #define b3QuantizedBvhDataName "b3QuantizedBvhDoubleData"
39 #define b3QuantizedBvhData b3QuantizedBvhFloatData
40 #define b3OptimizedBvhNodeData b3OptimizedBvhNodeFloatData
41 #define b3QuantizedBvhDataName "b3QuantizedBvhFloatData"
44 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
45 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
47 //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
49 //Note: currently we have 16 bytes per quantized node
50 #define MAX_SUBTREE_SIZE_IN_BYTES 2048
52 // 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
53 // actually) triangles each (since the sign bit is reserved
54 #define MAX_NUM_PARTS_IN_BITS 10
56 ///b3QuantizedBvhNode is a compressed aabb node, 16 bytes.
57 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
58 B3_ATTRIBUTE_ALIGNED16(struct)
59 b3QuantizedBvhNode : public b3QuantizedBvhNodeData
61 B3_DECLARE_ALIGNED_ALLOCATOR();
63 bool isLeafNode() const
65 //skipindex is negative (internal node), triangleindex >=0 (leafnode)
66 return (m_escapeIndexOrTriangleIndex >= 0);
68 int getEscapeIndex() const
70 b3Assert(!isLeafNode());
71 return -m_escapeIndexOrTriangleIndex;
73 int getTriangleIndex() const
75 b3Assert(isLeafNode());
77 unsigned int y = (~(x & 0)) << (31 - MAX_NUM_PARTS_IN_BITS);
78 // Get only the lower bits where the triangle index is stored
79 return (m_escapeIndexOrTriangleIndex & ~(y));
83 b3Assert(isLeafNode());
84 // Get only the highest bits where the part index is stored
85 return (m_escapeIndexOrTriangleIndex >> (31 - MAX_NUM_PARTS_IN_BITS));
89 /// b3OptimizedBvhNode contains both internal and leaf node information.
90 /// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
91 B3_ATTRIBUTE_ALIGNED16(struct)
94 B3_DECLARE_ALIGNED_ALLOCATOR();
97 b3Vector3 m_aabbMinOrg;
98 b3Vector3 m_aabbMaxOrg;
108 //pad the size to 64 bytes
112 ///b3BvhSubtreeInfo provides info to gather a subtree of limited size
113 B3_ATTRIBUTE_ALIGNED16(class)
114 b3BvhSubtreeInfo : public b3BvhSubtreeInfoData
117 B3_DECLARE_ALIGNED_ALLOCATOR();
121 //memset(&m_padding[0], 0, sizeof(m_padding));
124 void setAabbFromQuantizeNode(const b3QuantizedBvhNode& quantizedNode)
126 m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
127 m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
128 m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2];
129 m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0];
130 m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
131 m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
135 class b3NodeOverlapCallback
138 virtual ~b3NodeOverlapCallback(){};
140 virtual void processNode(int subPart, int triangleIndex) = 0;
143 #include "Bullet3Common/b3AlignedAllocator.h"
144 #include "Bullet3Common/b3AlignedObjectArray.h"
146 ///for code readability:
147 typedef b3AlignedObjectArray<b3OptimizedBvhNode> NodeArray;
148 typedef b3AlignedObjectArray<b3QuantizedBvhNode> QuantizedNodeArray;
149 typedef b3AlignedObjectArray<b3BvhSubtreeInfo> BvhSubtreeInfoArray;
151 ///The b3QuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU.
152 ///It is used by the b3BvhTriangleMeshShape as midphase
153 ///It is recommended to use quantization for better performance and lower memory requirements.
154 B3_ATTRIBUTE_ALIGNED16(class)
160 TRAVERSAL_STACKLESS = 0,
161 TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
165 b3Vector3 m_bvhAabbMin;
166 b3Vector3 m_bvhAabbMax;
167 b3Vector3 m_bvhQuantization;
170 int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess.
174 bool m_useQuantization;
176 NodeArray m_leafNodes;
177 NodeArray m_contiguousNodes;
178 QuantizedNodeArray m_quantizedLeafNodes;
179 QuantizedNodeArray m_quantizedContiguousNodes;
181 b3TraversalMode m_traversalMode;
182 BvhSubtreeInfoArray m_SubtreeHeaders;
184 //This is only used for serialization so we don't have to add serialization directly to b3AlignedObjectArray
185 mutable int m_subtreeHeaderCount;
187 ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
188 ///this might be refactored into a virtual, it is usually not calculated at run-time
189 void setInternalNodeAabbMin(int nodeIndex, const b3Vector3& aabbMin)
191 if (m_useQuantization)
193 quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0], aabbMin, 0);
197 m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
200 void setInternalNodeAabbMax(int nodeIndex, const b3Vector3& aabbMax)
202 if (m_useQuantization)
204 quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0], aabbMax, 1);
208 m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
212 b3Vector3 getAabbMin(int nodeIndex) const
214 if (m_useQuantization)
216 return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
219 return m_leafNodes[nodeIndex].m_aabbMinOrg;
221 b3Vector3 getAabbMax(int nodeIndex) const
223 if (m_useQuantization)
225 return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
228 return m_leafNodes[nodeIndex].m_aabbMaxOrg;
231 void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
233 if (m_useQuantization)
235 m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
239 m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
243 void mergeInternalNodeAabb(int nodeIndex, const b3Vector3& newAabbMin, const b3Vector3& newAabbMax)
245 if (m_useQuantization)
247 unsigned short int quantizedAabbMin[3];
248 unsigned short int quantizedAabbMax[3];
249 quantize(quantizedAabbMin, newAabbMin, 0);
250 quantize(quantizedAabbMax, newAabbMax, 1);
251 for (int i = 0; i < 3; i++)
253 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
254 m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
256 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
257 m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
263 m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
264 m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
268 void swapLeafNodes(int firstIndex, int secondIndex);
270 void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex);
273 void buildTree(int startIndex, int endIndex);
275 int calcSplittingAxis(int startIndex, int endIndex);
277 int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis);
279 void walkStacklessTree(b3NodeOverlapCallback * nodeCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax) const;
281 void walkStacklessQuantizedTreeAgainstRay(b3NodeOverlapCallback * nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin, const b3Vector3& aabbMax, int startNodeIndex, int endNodeIndex) const;
282 void walkStacklessQuantizedTree(b3NodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const;
283 void walkStacklessTreeAgainstRay(b3NodeOverlapCallback * nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin, const b3Vector3& aabbMax, int startNodeIndex, int endNodeIndex) const;
285 ///tree traversal designed for small-memory processors like PS3 SPU
286 void walkStacklessQuantizedTreeCacheFriendly(b3NodeOverlapCallback * nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const;
288 ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
289 void walkRecursiveQuantizedTreeAgainstQueryAabb(const b3QuantizedBvhNode* currentNode, b3NodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax) const;
291 ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
292 void walkRecursiveQuantizedTreeAgainstQuantizedTree(const b3QuantizedBvhNode* treeNodeA, const b3QuantizedBvhNode* treeNodeB, b3NodeOverlapCallback* nodeCallback) const;
294 void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex);
297 B3_DECLARE_ALIGNED_ALLOCATOR();
301 virtual ~b3QuantizedBvh();
303 ///***************************************** expert/internal use only *************************
304 void setQuantizationValues(const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax, b3Scalar quantizationMargin = b3Scalar(1.0));
305 QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; }
306 ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
307 void buildInternal();
308 ///***************************************** expert/internal use only *************************
310 void reportAabbOverlappingNodex(b3NodeOverlapCallback * nodeCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax) const;
311 void reportRayOverlappingNodex(b3NodeOverlapCallback * nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget) const;
312 void reportBoxCastOverlappingNodex(b3NodeOverlapCallback * nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin, const b3Vector3& aabbMax) const;
314 B3_FORCE_INLINE void quantize(unsigned short* out, const b3Vector3& point, int isMax) const
316 b3Assert(m_useQuantization);
318 b3Assert(point.getX() <= m_bvhAabbMax.getX());
319 b3Assert(point.getY() <= m_bvhAabbMax.getY());
320 b3Assert(point.getZ() <= m_bvhAabbMax.getZ());
322 b3Assert(point.getX() >= m_bvhAabbMin.getX());
323 b3Assert(point.getY() >= m_bvhAabbMin.getY());
324 b3Assert(point.getZ() >= m_bvhAabbMin.getZ());
326 b3Vector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
327 ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
328 ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
329 ///@todo: double-check this
332 out[0] = (unsigned short)(((unsigned short)(v.getX() + b3Scalar(1.)) | 1));
333 out[1] = (unsigned short)(((unsigned short)(v.getY() + b3Scalar(1.)) | 1));
334 out[2] = (unsigned short)(((unsigned short)(v.getZ() + b3Scalar(1.)) | 1));
338 out[0] = (unsigned short)(((unsigned short)(v.getX()) & 0xfffe));
339 out[1] = (unsigned short)(((unsigned short)(v.getY()) & 0xfffe));
340 out[2] = (unsigned short)(((unsigned short)(v.getZ()) & 0xfffe));
343 #ifdef DEBUG_CHECK_DEQUANTIZATION
344 b3Vector3 newPoint = unQuantize(out);
347 if (newPoint.getX() < point.getX())
349 printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
351 if (newPoint.getY() < point.getY())
353 printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
355 if (newPoint.getZ() < point.getZ())
357 printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
362 if (newPoint.getX() > point.getX())
364 printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
366 if (newPoint.getY() > point.getY())
368 printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
370 if (newPoint.getZ() > point.getZ())
372 printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
375 #endif //DEBUG_CHECK_DEQUANTIZATION
378 B3_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const b3Vector3& point2, int isMax) const
380 b3Assert(m_useQuantization);
382 b3Vector3 clampedPoint(point2);
383 clampedPoint.setMax(m_bvhAabbMin);
384 clampedPoint.setMin(m_bvhAabbMax);
386 quantize(out, clampedPoint, isMax);
389 B3_FORCE_INLINE b3Vector3 unQuantize(const unsigned short* vecIn) const
393 (b3Scalar)(vecIn[0]) / (m_bvhQuantization.getX()),
394 (b3Scalar)(vecIn[1]) / (m_bvhQuantization.getY()),
395 (b3Scalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
396 vecOut += m_bvhAabbMin;
400 ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
401 void setTraversalMode(b3TraversalMode traversalMode)
403 m_traversalMode = traversalMode;
406 B3_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray()
408 return m_quantizedContiguousNodes;
411 B3_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray()
413 return m_SubtreeHeaders;
416 ////////////////////////////////////////////////////////////////////
418 /////Calculate space needed to store BVH for serialization
419 unsigned calculateSerializeBufferSize() const;
421 /// Data buffer MUST be 16 byte aligned
422 virtual bool serialize(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
424 ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
425 static b3QuantizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
427 static unsigned int getAlignmentSerializationPadding();
428 //////////////////////////////////////////////////////////////////////
430 virtual int calculateSerializeBufferSizeNew() const;
432 ///fills the dataBuffer and returns the struct name (and 0 on failure)
433 virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
435 virtual void deSerializeFloat(struct b3QuantizedBvhFloatData & quantizedBvhFloatData);
437 virtual void deSerializeDouble(struct b3QuantizedBvhDoubleData & quantizedBvhDoubleData);
439 ////////////////////////////////////////////////////////////////////
441 B3_FORCE_INLINE bool isQuantized()
443 return m_useQuantization;
447 // Special "copy" constructor that allows for in-place deserialization
448 // Prevents b3Vector3's default constructor from being called, but doesn't inialize much else
449 // ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need)
450 b3QuantizedBvh(b3QuantizedBvh & other, bool ownsMemory);
453 struct b3OptimizedBvhNodeFloatData
455 b3Vector3FloatData m_aabbMinOrg;
456 b3Vector3FloatData m_aabbMaxOrg;
463 struct b3OptimizedBvhNodeDoubleData
465 b3Vector3DoubleData m_aabbMinOrg;
466 b3Vector3DoubleData m_aabbMaxOrg;
473 struct b3QuantizedBvhFloatData
475 b3Vector3FloatData m_bvhAabbMin;
476 b3Vector3FloatData m_bvhAabbMax;
477 b3Vector3FloatData m_bvhQuantization;
479 int m_useQuantization;
480 int m_numContiguousLeafNodes;
481 int m_numQuantizedContiguousNodes;
482 b3OptimizedBvhNodeFloatData* m_contiguousNodesPtr;
483 b3QuantizedBvhNodeData* m_quantizedContiguousNodesPtr;
484 b3BvhSubtreeInfoData* m_subTreeInfoPtr;
486 int m_numSubtreeHeaders;
489 struct b3QuantizedBvhDoubleData
491 b3Vector3DoubleData m_bvhAabbMin;
492 b3Vector3DoubleData m_bvhAabbMax;
493 b3Vector3DoubleData m_bvhQuantization;
495 int m_useQuantization;
496 int m_numContiguousLeafNodes;
497 int m_numQuantizedContiguousNodes;
498 b3OptimizedBvhNodeDoubleData* m_contiguousNodesPtr;
499 b3QuantizedBvhNodeData* m_quantizedContiguousNodesPtr;
502 int m_numSubtreeHeaders;
503 b3BvhSubtreeInfoData* m_subTreeInfoPtr;
506 B3_FORCE_INLINE int b3QuantizedBvh::calculateSerializeBufferSizeNew() const
508 return sizeof(b3QuantizedBvhData);
511 #endif //B3_QUANTIZED_BVH_H