[dali_2.3.21] Merge branch 'devel/master'
[platform/core/uifw/dali-toolkit.git] / dali-physics / third-party / bullet3 / src / Bullet3OpenCL / NarrowphaseCollision / b3QuantizedBvh.h
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).
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.
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
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:
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.
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
506 B3_FORCE_INLINE int b3QuantizedBvh::calculateSerializeBufferSizeNew() const
507 {
508         return sizeof(b3QuantizedBvhData);
509 }
510
511 #endif  //B3_QUANTIZED_BVH_H