Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 16 years ago

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 16.2 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#ifndef QUANTIZED_BVH_H
17#define QUANTIZED_BVH_H
18
19//#define DEBUG_CHECK_DEQUANTIZATION 1
20#ifdef DEBUG_CHECK_DEQUANTIZATION
21#ifdef __SPU__
22#define printf spu_printf
23#endif //__SPU__
24
25#include <stdio.h>
26#include <stdlib.h>
27#endif //DEBUG_CHECK_DEQUANTIZATION
28
29#include "LinearMath/btVector3.h"
30#include "LinearMath/btAlignedAllocator.h"
31
32
33//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
34
35
36//Note: currently we have 16 bytes per quantized node
37#define MAX_SUBTREE_SIZE_IN_BYTES  2048
38
39// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
40// actually) triangles each (since the sign bit is reserved
41#define MAX_NUM_PARTS_IN_BITS 10
42
43///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
44///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
45ATTRIBUTE_ALIGNED16     (struct) btQuantizedBvhNode
46{
47        BT_DECLARE_ALIGNED_ALLOCATOR();
48
49        //12 bytes
50        unsigned short int      m_quantizedAabbMin[3];
51        unsigned short int      m_quantizedAabbMax[3];
52        //4 bytes
53        int     m_escapeIndexOrTriangleIndex;
54
55        bool isLeafNode() const
56        {
57                //skipindex is negative (internal node), triangleindex >=0 (leafnode)
58                return (m_escapeIndexOrTriangleIndex >= 0);
59        }
60        int getEscapeIndex() const
61        {
62                btAssert(!isLeafNode());
63                return -m_escapeIndexOrTriangleIndex;
64        }
65        int     getTriangleIndex() const
66        {
67                btAssert(isLeafNode());
68                // Get only the lower bits where the triangle index is stored
69                return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS)));
70        }
71        int     getPartId() const
72        {
73                btAssert(isLeafNode());
74                // Get only the highest bits where the part index is stored
75                return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
76        }
77}
78;
79
80/// btOptimizedBvhNode contains both internal and leaf node information.
81/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
82ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
83{
84        BT_DECLARE_ALIGNED_ALLOCATOR();
85
86        //32 bytes
87        btVector3       m_aabbMinOrg;
88        btVector3       m_aabbMaxOrg;
89
90        //4
91        int     m_escapeIndex;
92
93        //8
94        //for child nodes
95        int     m_subPart;
96        int     m_triangleIndex;
97        int     m_padding[5];//bad, due to alignment
98
99
100};
101
102
103///btBvhSubtreeInfo provides info to gather a subtree of limited size
104ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
105{
106public:
107        BT_DECLARE_ALIGNED_ALLOCATOR();
108
109        //12 bytes
110        unsigned short int      m_quantizedAabbMin[3];
111        unsigned short int      m_quantizedAabbMax[3];
112        //4 bytes, points to the root of the subtree
113        int                     m_rootNodeIndex;
114        //4 bytes
115        int                     m_subtreeSize;
116        int                     m_padding[3];
117
118        btBvhSubtreeInfo()
119        {
120                //memset(&m_padding[0], 0, sizeof(m_padding));
121        }
122
123
124        void    setAabbFromQuantizeNode(const btQuantizedBvhNode& 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
136
137class btNodeOverlapCallback
138{
139public:
140        virtual ~btNodeOverlapCallback() {};
141
142        virtual void processNode(int subPart, int triangleIndex) = 0;
143};
144
145#include "LinearMath/btAlignedAllocator.h"
146#include "LinearMath/btAlignedObjectArray.h"
147
148
149
150///for code readability:
151typedef btAlignedObjectArray<btOptimizedBvhNode>        NodeArray;
152typedef btAlignedObjectArray<btQuantizedBvhNode>        QuantizedNodeArray;
153typedef btAlignedObjectArray<btBvhSubtreeInfo>          BvhSubtreeInfoArray;
154
155
156///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU.
157///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase.
158///It is recommended to use quantization for better performance and lower memory requirements.
159ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
160{
161protected:
162
163        NodeArray                       m_leafNodes;
164        NodeArray                       m_contiguousNodes;
165
166        QuantizedNodeArray      m_quantizedLeafNodes;
167       
168        QuantizedNodeArray      m_quantizedContiguousNodes;
169       
170        int                                     m_curNodeIndex;
171
172
173        //quantization data
174        bool                            m_useQuantization;
175        btVector3                       m_bvhAabbMin;
176        btVector3                       m_bvhAabbMax;
177        btVector3                       m_bvhQuantization;
178public:
179        BT_DECLARE_ALIGNED_ALLOCATOR();
180
181        enum btTraversalMode
182        {
183                TRAVERSAL_STACKLESS = 0,
184                TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
185                TRAVERSAL_RECURSIVE
186        };
187protected:
188
189        btTraversalMode m_traversalMode;
190       
191        BvhSubtreeInfoArray             m_SubtreeHeaders;
192
193        //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
194        int m_subtreeHeaderCount;
195
196
197        ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
198        ///this might be refactored into a virtual, it is usually not calculated at run-time
199        void    setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
200        {
201                if (m_useQuantization)
202                {
203                        quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
204                } else
205                {
206                        m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
207
208                }
209        }
210        void    setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
211        {
212                if (m_useQuantization)
213                {
214                        quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
215                } else
216                {
217                        m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
218                }
219        }
220
221        btVector3 getAabbMin(int nodeIndex) const
222        {
223                if (m_useQuantization)
224                {
225                        return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
226                }
227                //non-quantized
228                return m_leafNodes[nodeIndex].m_aabbMinOrg;
229
230        }
231        btVector3 getAabbMax(int nodeIndex) const
232        {
233                if (m_useQuantization)
234                {
235                        return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
236                } 
237                //non-quantized
238                return m_leafNodes[nodeIndex].m_aabbMaxOrg;
239               
240        }
241
242       
243        void    setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
244        {
245                if (m_useQuantization)
246                {
247                        m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
248                } 
249                else
250                {
251                        m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
252                }
253
254        }
255
256        void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) 
257        {
258                if (m_useQuantization)
259                {
260                        unsigned short int quantizedAabbMin[3];
261                        unsigned short int quantizedAabbMax[3];
262                        quantize(quantizedAabbMin,newAabbMin,0);
263                        quantize(quantizedAabbMax,newAabbMax,1);
264                        for (int i=0;i<3;i++)
265                        {
266                                if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
267                                        m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
268
269                                if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
270                                        m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
271
272                        }
273                } else
274                {
275                        //non-quantized
276                        m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
277                        m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);           
278                }
279        }
280
281        void    swapLeafNodes(int firstIndex,int secondIndex);
282
283        void    assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
284
285protected:
286
287       
288
289        void    buildTree       (int startIndex,int endIndex);
290
291        int     calcSplittingAxis(int startIndex,int endIndex);
292
293        int     sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
294       
295        void    walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
296
297        void    walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
298        void    walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
299        void    walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
300
301        ///tree traversal designed for small-memory processors like PS3 SPU
302        void    walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
303
304        ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
305        void    walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
306
307        ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
308        void    walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
309       
310
311#define USE_BANCHLESS 1
312#ifdef USE_BANCHLESS
313        //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
314        SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
315        {               
316                return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
317                        & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
318                        & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
319                        1, 0));
320        }
321#else
322        SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
323        {
324                bool overlap = true;
325                overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
326                overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
327                overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
328                return overlap;
329        }
330#endif //USE_BANCHLESS
331
332        void    updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
333
334public:
335        btQuantizedBvh();
336
337        virtual ~btQuantizedBvh();
338
339       
340        ///***************************************** expert/internal use only *************************
341        void    setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
342        QuantizedNodeArray&     getLeafNodeArray() {                    return  m_quantizedLeafNodes;   }
343        ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
344        void    buildInternal();
345        ///***************************************** expert/internal use only *************************
346
347        void    reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
348        void    reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
349        void    reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
350
351                SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
352        {
353
354                btAssert(m_useQuantization);
355
356                btAssert(point.getX() <= m_bvhAabbMax.getX());
357                btAssert(point.getY() <= m_bvhAabbMax.getY());
358                btAssert(point.getZ() <= m_bvhAabbMax.getZ());
359
360                btAssert(point.getX() >= m_bvhAabbMin.getX());
361                btAssert(point.getY() >= m_bvhAabbMin.getY());
362                btAssert(point.getZ() >= m_bvhAabbMin.getZ());
363
364                btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
365                ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
366                ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
367                ///todo: double-check this
368                if (isMax)
369                {
370                        out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
371                        out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
372                        out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
373                } else
374                {
375                        out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
376                        out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
377                        out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
378                }
379
380
381#ifdef DEBUG_CHECK_DEQUANTIZATION
382                btVector3 newPoint = unQuantize(out);
383                if (isMax)
384                {
385                        if (newPoint.getX() < point.getX())
386                        {
387                                printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
388                        }
389                        if (newPoint.getY() < point.getY())
390                        {
391                                printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
392                        }
393                        if (newPoint.getZ() < point.getZ())
394                        {
395
396                                printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
397                        }
398                } else
399                {
400                        if (newPoint.getX() > point.getX())
401                        {
402                                printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
403                        }
404                        if (newPoint.getY() > point.getY())
405                        {
406                                printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
407                        }
408                        if (newPoint.getZ() > point.getZ())
409                        {
410                                printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
411                        }
412                }
413#endif //DEBUG_CHECK_DEQUANTIZATION
414
415        }
416
417
418        SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
419        {
420
421                btAssert(m_useQuantization);
422
423                btVector3 clampedPoint(point2);
424                clampedPoint.setMax(m_bvhAabbMin);
425                clampedPoint.setMin(m_bvhAabbMax);
426
427                quantize(out,clampedPoint,isMax);
428
429        }
430       
431        SIMD_FORCE_INLINE btVector3     unQuantize(const unsigned short* vecIn) const
432        {
433                        btVector3       vecOut;
434                        vecOut.setValue(
435                        (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
436                        (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
437                        (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
438                        vecOut += m_bvhAabbMin;
439                        return vecOut;
440        }
441
442        ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
443        void    setTraversalMode(btTraversalMode        traversalMode)
444        {
445                m_traversalMode = traversalMode;
446        }
447
448
449        SIMD_FORCE_INLINE QuantizedNodeArray&   getQuantizedNodeArray()
450        {       
451                return  m_quantizedContiguousNodes;
452        }
453
454
455        SIMD_FORCE_INLINE BvhSubtreeInfoArray&  getSubtreeInfoArray()
456        {
457                return m_SubtreeHeaders;
458        }
459
460
461        /////Calculate space needed to store BVH for serialization
462        unsigned calculateSerializeBufferSize();
463
464        /// Data buffer MUST be 16 byte aligned
465        virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian);
466
467        ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
468        static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
469
470        static unsigned int getAlignmentSerializationPadding();
471
472        SIMD_FORCE_INLINE bool isQuantized()
473        {
474                return m_useQuantization;
475        }
476
477private:
478        // Special "copy" constructor that allows for in-place deserialization
479        // Prevents btVector3's default constructor from being called, but doesn't inialize much else
480        // 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)
481        btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
482
483}
484;
485
486
487#endif //QUANTIZED_BVH_H
Note: See TracBrowser for help on using the repository browser.