1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
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:
10
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.
14 */
15
16 #ifndef B3_QUANTIZED_BVH_H
17 #define B3_QUANTIZED_BVH_H
18
19 class b3Serializer;
20
21 //#define DEBUG_CHECK_DEQUANTIZATION 1
22 #ifdef DEBUG_CHECK_DEQUANTIZATION
23 #ifdef __SPU__
24 #define printf spu_printf
25 #endif //__SPU__
26
27 #include <stdio.h>
28 #include <stdlib.h>
29 #endif //DEBUG_CHECK_DEQUANTIZATION
30
31 #include "Bullet3Common/b3Vector3.h"
32 #include "Bullet3Common/b3AlignedAllocator.h"
33
34 #ifdef B3_USE_DOUBLE_PRECISION
35 #define b3QuantizedBvhData b3QuantizedBvhDoubleData
36 #define b3OptimizedBvhNodeData b3OptimizedBvhNodeDoubleData
37 #define b3QuantizedBvhDataName "b3QuantizedBvhDoubleData"
38 #else
39 #define b3QuantizedBvhData b3QuantizedBvhFloatData
40 #define b3OptimizedBvhNodeData b3OptimizedBvhNodeFloatData
41 #define b3QuantizedBvhDataName "b3QuantizedBvhFloatData"
42 #endif
43
44 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
45 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
46
47 //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
48
49 //Note: currently we have 16 bytes per quantized node
50 #define MAX_SUBTREE_SIZE_IN_BYTES 2048
51
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
55
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).
B3_ATTRIBUTE_ALIGNED16(struct)58 B3_ATTRIBUTE_ALIGNED16(struct)
59 b3QuantizedBvhNode : public b3QuantizedBvhNodeData
60 {
61 B3_DECLARE_ALIGNED_ALLOCATOR();
62
63 bool isLeafNode() const
64 {
65 //skipindex is negative (internal node), triangleindex >=0 (leafnode)
66 return (m_escapeIndexOrTriangleIndex >= 0);
67 }
68 int getEscapeIndex() const
69 {
70 b3Assert(!isLeafNode());
71 return -m_escapeIndexOrTriangleIndex;
72 }
73 int getTriangleIndex() const
74 {
75 b3Assert(isLeafNode());
76 unsigned int x = 0;
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));
80 }
81 int getPartId() const
82 {
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));
86 }
87 };
88
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.
B3_ATTRIBUTE_ALIGNED16(struct)91 B3_ATTRIBUTE_ALIGNED16(struct)
92 b3OptimizedBvhNode
93 {
94 B3_DECLARE_ALIGNED_ALLOCATOR();
95
96 //32 bytes
97 b3Vector3 m_aabbMinOrg;
98 b3Vector3 m_aabbMaxOrg;
99
100 //4
101 int m_escapeIndex;
102
103 //8
104 //for child nodes
105 int m_subPart;
106 int m_triangleIndex;
107
108 //pad the size to 64 bytes
109 char m_padding[20];
110 };
111
112 ///b3BvhSubtreeInfo provides info to gather a subtree of limited size
B3_ATTRIBUTE_ALIGNED16(class)113 B3_ATTRIBUTE_ALIGNED16(class)
114 b3BvhSubtreeInfo : public b3BvhSubtreeInfoData
115 {
116 public:
117 B3_DECLARE_ALIGNED_ALLOCATOR();
118
119 b3BvhSubtreeInfo()
120 {
121 //memset(&m_padding[0], 0, sizeof(m_padding));
122 }
123
124 void setAabbFromQuantizeNode(const b3QuantizedBvhNode& quantizedNode)
125 {
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];
132 }
133 };
134
135 class b3NodeOverlapCallback
136 {
137 public:
~b3NodeOverlapCallback()138 virtual ~b3NodeOverlapCallback(){};
139
140 virtual void processNode(int subPart, int triangleIndex) = 0;
141 };
142
143 #include "Bullet3Common/b3AlignedAllocator.h"
144 #include "Bullet3Common/b3AlignedObjectArray.h"
145
146 ///for code readability:
147 typedef b3AlignedObjectArray<b3OptimizedBvhNode> NodeArray;
148 typedef b3AlignedObjectArray<b3QuantizedBvhNode> QuantizedNodeArray;
149 typedef b3AlignedObjectArray<b3BvhSubtreeInfo> BvhSubtreeInfoArray;
150
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.
B3_ATTRIBUTE_ALIGNED16(class)154 B3_ATTRIBUTE_ALIGNED16(class)
155 b3QuantizedBvh
156 {
157 public:
158 enum b3TraversalMode
159 {
160 TRAVERSAL_STACKLESS = 0,
161 TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
162 TRAVERSAL_RECURSIVE
163 };
164
165 b3Vector3 m_bvhAabbMin;
166 b3Vector3 m_bvhAabbMax;
167 b3Vector3 m_bvhQuantization;
168
169 protected:
170 int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess.
171
172 int m_curNodeIndex;
173 //quantization data
174 bool m_useQuantization;
175
176 NodeArray m_leafNodes;
177 NodeArray m_contiguousNodes;
178 QuantizedNodeArray m_quantizedLeafNodes;
179 QuantizedNodeArray m_quantizedContiguousNodes;
180
181 b3TraversalMode m_traversalMode;
182 BvhSubtreeInfoArray m_SubtreeHeaders;
183
184 //This is only used for serialization so we don't have to add serialization directly to b3AlignedObjectArray
185 mutable int m_subtreeHeaderCount;
186
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)
190 {
191 if (m_useQuantization)
192 {
193 quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0], aabbMin, 0);
194 }
195 else
196 {
197 m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
198 }
199 }
200 void setInternalNodeAabbMax(int nodeIndex, const b3Vector3& aabbMax)
201 {
202 if (m_useQuantization)
203 {
204 quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0], aabbMax, 1);
205 }
206 else
207 {
208 m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
209 }
210 }
211
212 b3Vector3 getAabbMin(int nodeIndex) const
213 {
214 if (m_useQuantization)
215 {
216 return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
217 }
218 //non-quantized
219 return m_leafNodes[nodeIndex].m_aabbMinOrg;
220 }
221 b3Vector3 getAabbMax(int nodeIndex) const
222 {
223 if (m_useQuantization)
224 {
225 return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
226 }
227 //non-quantized
228 return m_leafNodes[nodeIndex].m_aabbMaxOrg;
229 }
230
231 void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
232 {
233 if (m_useQuantization)
234 {
235 m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
236 }
237 else
238 {
239 m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
240 }
241 }
242
243 void mergeInternalNodeAabb(int nodeIndex, const b3Vector3& newAabbMin, const b3Vector3& newAabbMax)
244 {
245 if (m_useQuantization)
246 {
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++)
252 {
253 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
254 m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
255
256 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
257 m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
258 }
259 }
260 else
261 {
262 //non-quantized
263 m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
264 m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
265 }
266 }
267
268 void swapLeafNodes(int firstIndex, int secondIndex);
269
270 void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex);
271
272 protected:
273 void buildTree(int startIndex, int endIndex);
274
275 int calcSplittingAxis(int startIndex, int endIndex);
276
277 int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis);
278
279 void walkStacklessTree(b3NodeOverlapCallback * nodeCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax) const;
280
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;
284
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;
287
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;
290
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;
293
294 void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex);
295
296 public:
297 B3_DECLARE_ALIGNED_ALLOCATOR();
298
299 b3QuantizedBvh();
300
301 virtual ~b3QuantizedBvh();
302
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 *************************
309
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;
313
314 B3_FORCE_INLINE void quantize(unsigned short* out, const b3Vector3& point, int isMax) const
315 {
316 b3Assert(m_useQuantization);
317
318 b3Assert(point.getX() <= m_bvhAabbMax.getX());
319 b3Assert(point.getY() <= m_bvhAabbMax.getY());
320 b3Assert(point.getZ() <= m_bvhAabbMax.getZ());
321
322 b3Assert(point.getX() >= m_bvhAabbMin.getX());
323 b3Assert(point.getY() >= m_bvhAabbMin.getY());
324 b3Assert(point.getZ() >= m_bvhAabbMin.getZ());
325
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
330 if (isMax)
331 {
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));
335 }
336 else
337 {
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));
341 }
342
343 #ifdef DEBUG_CHECK_DEQUANTIZATION
344 b3Vector3 newPoint = unQuantize(out);
345 if (isMax)
346 {
347 if (newPoint.getX() < point.getX())
348 {
349 printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
350 }
351 if (newPoint.getY() < point.getY())
352 {
353 printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
354 }
355 if (newPoint.getZ() < point.getZ())
356 {
357 printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
358 }
359 }
360 else
361 {
362 if (newPoint.getX() > point.getX())
363 {
364 printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n", newPoint.getX() - point.getX(), newPoint.getX(), point.getX());
365 }
366 if (newPoint.getY() > point.getY())
367 {
368 printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n", newPoint.getY() - point.getY(), newPoint.getY(), point.getY());
369 }
370 if (newPoint.getZ() > point.getZ())
371 {
372 printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n", newPoint.getZ() - point.getZ(), newPoint.getZ(), point.getZ());
373 }
374 }
375 #endif //DEBUG_CHECK_DEQUANTIZATION
376 }
377
378 B3_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const b3Vector3& point2, int isMax) const
379 {
380 b3Assert(m_useQuantization);
381
382 b3Vector3 clampedPoint(point2);
383 clampedPoint.setMax(m_bvhAabbMin);
384 clampedPoint.setMin(m_bvhAabbMax);
385
386 quantize(out, clampedPoint, isMax);
387 }
388
389 B3_FORCE_INLINE b3Vector3 unQuantize(const unsigned short* vecIn) const
390 {
391 b3Vector3 vecOut;
392 vecOut.setValue(
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;
397 return vecOut;
398 }
399
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)
402 {
403 m_traversalMode = traversalMode;
404 }
405
406 B3_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray()
407 {
408 return m_quantizedContiguousNodes;
409 }
410
411 B3_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray()
412 {
413 return m_SubtreeHeaders;
414 }
415
416 ////////////////////////////////////////////////////////////////////
417
418 /////Calculate space needed to store BVH for serialization
419 unsigned calculateSerializeBufferSize() const;
420
421 /// Data buffer MUST be 16 byte aligned
422 virtual bool serialize(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
423
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);
426
427 static unsigned int getAlignmentSerializationPadding();
428 //////////////////////////////////////////////////////////////////////
429
430 virtual int calculateSerializeBufferSizeNew() const;
431
432 ///fills the dataBuffer and returns the struct name (and 0 on failure)
433 virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
434
435 virtual void deSerializeFloat(struct b3QuantizedBvhFloatData & quantizedBvhFloatData);
436
437 virtual void deSerializeDouble(struct b3QuantizedBvhDoubleData & quantizedBvhDoubleData);
438
439 ////////////////////////////////////////////////////////////////////
440
441 B3_FORCE_INLINE bool isQuantized()
442 {
443 return m_useQuantization;
444 }
445
446 private:
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);
451 };
452
453 struct b3OptimizedBvhNodeFloatData
454 {
455 b3Vector3FloatData m_aabbMinOrg;
456 b3Vector3FloatData m_aabbMaxOrg;
457 int m_escapeIndex;
458 int m_subPart;
459 int m_triangleIndex;
460 char m_pad[4];
461 };
462
463 struct b3OptimizedBvhNodeDoubleData
464 {
465 b3Vector3DoubleData m_aabbMinOrg;
466 b3Vector3DoubleData m_aabbMaxOrg;
467 int m_escapeIndex;
468 int m_subPart;
469 int m_triangleIndex;
470 char m_pad[4];
471 };
472
473 struct b3QuantizedBvhFloatData
474 {
475 b3Vector3FloatData m_bvhAabbMin;
476 b3Vector3FloatData m_bvhAabbMax;
477 b3Vector3FloatData m_bvhQuantization;
478 int m_curNodeIndex;
479 int m_useQuantization;
480 int m_numContiguousLeafNodes;
481 int m_numQuantizedContiguousNodes;
482 b3OptimizedBvhNodeFloatData* m_contiguousNodesPtr;
483 b3QuantizedBvhNodeData* m_quantizedContiguousNodesPtr;
484 b3BvhSubtreeInfoData* m_subTreeInfoPtr;
485 int m_traversalMode;
486 int m_numSubtreeHeaders;
487 };
488
489 struct b3QuantizedBvhDoubleData
490 {
491 b3Vector3DoubleData m_bvhAabbMin;
492 b3Vector3DoubleData m_bvhAabbMax;
493 b3Vector3DoubleData m_bvhQuantization;
494 int m_curNodeIndex;
495 int m_useQuantization;
496 int m_numContiguousLeafNodes;
497 int m_numQuantizedContiguousNodes;
498 b3OptimizedBvhNodeDoubleData* m_contiguousNodesPtr;
499 b3QuantizedBvhNodeData* m_quantizedContiguousNodesPtr;
500
501 int m_traversalMode;
502 int m_numSubtreeHeaders;
503 b3BvhSubtreeInfoData* m_subTreeInfoPtr;
504 };
505
calculateSerializeBufferSizeNew()506 B3_FORCE_INLINE int b3QuantizedBvh::calculateSerializeBufferSizeNew() const
507 {
508 return sizeof(b3QuantizedBvhData);
509 }
510
511 #endif //B3_QUANTIZED_BVH_H
512