Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_new/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @ 2119

Last change on this file since 2119 was 2119, checked in by rgrieder, 15 years ago

Merged physics branch into physics_new branch.
  • Property svn:eol-style set to native
File size: 16.0 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
300        ///tree traversal designed for small-memory processors like PS3 SPU
301        void    walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
302
303        ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
304        void    walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
305
306        ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal
307        void    walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
308       
309
310#define USE_BANCHLESS 1
311#ifdef USE_BANCHLESS
312        //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)
313        SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
314        {               
315                return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
316                        & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
317                        & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
318                        1, 0));
319        }
320#else
321        SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const
322        {
323                bool overlap = true;
324                overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
325                overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
326                overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
327                return overlap;
328        }
329#endif //USE_BANCHLESS
330
331        void    updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
332
333public:
334        btQuantizedBvh();
335
336        virtual ~btQuantizedBvh();
337
338       
339        ///***************************************** expert/internal use only *************************
340        void    setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
341        QuantizedNodeArray&     getLeafNodeArray() {                    return  m_quantizedLeafNodes;   }
342        ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
343        void    buildInternal();
344        ///***************************************** expert/internal use only *************************
345
346        void    reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
347        void    reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
348        void    reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
349
350                SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
351        {
352
353                btAssert(m_useQuantization);
354
355                btAssert(point.getX() <= m_bvhAabbMax.getX());
356                btAssert(point.getY() <= m_bvhAabbMax.getY());
357                btAssert(point.getZ() <= m_bvhAabbMax.getZ());
358
359                btAssert(point.getX() >= m_bvhAabbMin.getX());
360                btAssert(point.getY() >= m_bvhAabbMin.getY());
361                btAssert(point.getZ() >= m_bvhAabbMin.getZ());
362
363                btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
364                ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative
365                ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
366                ///todo: double-check this
367                if (isMax)
368                {
369                        out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
370                        out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
371                        out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
372                } else
373                {
374                        out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
375                        out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
376                        out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
377                }
378
379
380#ifdef DEBUG_CHECK_DEQUANTIZATION
381                btVector3 newPoint = unQuantize(out);
382                if (isMax)
383                {
384                        if (newPoint.getX() < point.getX())
385                        {
386                                printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
387                        }
388                        if (newPoint.getY() < point.getY())
389                        {
390                                printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
391                        }
392                        if (newPoint.getZ() < point.getZ())
393                        {
394
395                                printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
396                        }
397                } else
398                {
399                        if (newPoint.getX() > point.getX())
400                        {
401                                printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
402                        }
403                        if (newPoint.getY() > point.getY())
404                        {
405                                printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
406                        }
407                        if (newPoint.getZ() > point.getZ())
408                        {
409                                printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
410                        }
411                }
412#endif //DEBUG_CHECK_DEQUANTIZATION
413
414        }
415
416
417        SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
418        {
419
420                btAssert(m_useQuantization);
421
422                btVector3 clampedPoint(point2);
423                clampedPoint.setMax(m_bvhAabbMin);
424                clampedPoint.setMin(m_bvhAabbMax);
425
426                quantize(out,clampedPoint,isMax);
427
428        }
429       
430        SIMD_FORCE_INLINE btVector3     unQuantize(const unsigned short* vecIn) const
431        {
432                        btVector3       vecOut;
433                        vecOut.setValue(
434                        (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
435                        (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
436                        (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
437                        vecOut += m_bvhAabbMin;
438                        return vecOut;
439        }
440
441        ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees.
442        void    setTraversalMode(btTraversalMode        traversalMode)
443        {
444                m_traversalMode = traversalMode;
445        }
446
447
448        SIMD_FORCE_INLINE QuantizedNodeArray&   getQuantizedNodeArray()
449        {       
450                return  m_quantizedContiguousNodes;
451        }
452
453
454        SIMD_FORCE_INLINE BvhSubtreeInfoArray&  getSubtreeInfoArray()
455        {
456                return m_SubtreeHeaders;
457        }
458
459
460        /////Calculate space needed to store BVH for serialization
461        unsigned calculateSerializeBufferSize();
462
463        /// Data buffer MUST be 16 byte aligned
464        virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian);
465
466        ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
467        static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
468
469        static unsigned int getAlignmentSerializationPadding();
470
471        SIMD_FORCE_INLINE bool isQuantized()
472        {
473                return m_useQuantization;
474        }
475
476private:
477        // Special "copy" constructor that allows for in-place deserialization
478        // Prevents btVector3's default constructor from being called, but doesn't inialize much else
479        // 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)
480        btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
481
482}
483;
484
485
486#endif //QUANTIZED_BVH_H
Note: See TracBrowser for help on using the repository browser.