btQuantizedBvh.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #ifndef BT_QUANTIZED_BVH_H
00017 #define BT_QUANTIZED_BVH_H
00018 
00019 class btSerializer;
00020 
00021 //#define DEBUG_CHECK_DEQUANTIZATION 1
00022 #ifdef DEBUG_CHECK_DEQUANTIZATION
00023 #ifdef __SPU__
00024 #define printf spu_printf
00025 #endif //__SPU__
00026 
00027 #include <stdio.h>
00028 #include <stdlib.h>
00029 #endif //DEBUG_CHECK_DEQUANTIZATION
00030 
00031 #include "LinearMath/btVector3.h"
00032 #include "LinearMath/btAlignedAllocator.h"
00033 
00034 #ifdef BT_USE_DOUBLE_PRECISION
00035 #define btQuantizedBvhData btQuantizedBvhDoubleData
00036 #define btOptimizedBvhNodeData btOptimizedBvhNodeDoubleData
00037 #define btQuantizedBvhDataName "btQuantizedBvhDoubleData"
00038 #else
00039 #define btQuantizedBvhData btQuantizedBvhFloatData
00040 #define btOptimizedBvhNodeData btOptimizedBvhNodeFloatData
00041 #define btQuantizedBvhDataName "btQuantizedBvhFloatData"
00042 #endif
00043 
00044 
00045 
00046 //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
00047 
00048 
00049 //Note: currently we have 16 bytes per quantized node
00050 #define MAX_SUBTREE_SIZE_IN_BYTES  2048
00051 
00052 // 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
00053 // actually) triangles each (since the sign bit is reserved
00054 #define MAX_NUM_PARTS_IN_BITS 10
00055 
00058 ATTRIBUTE_ALIGNED16     (struct) btQuantizedBvhNode
00059 {
00060         BT_DECLARE_ALIGNED_ALLOCATOR();
00061 
00062         //12 bytes
00063         unsigned short int      m_quantizedAabbMin[3];
00064         unsigned short int      m_quantizedAabbMax[3];
00065         //4 bytes
00066         int     m_escapeIndexOrTriangleIndex;
00067 
00068         bool isLeafNode() const
00069         {
00070                 //skipindex is negative (internal node), triangleindex >=0 (leafnode)
00071                 return (m_escapeIndexOrTriangleIndex >= 0);
00072         }
00073         int getEscapeIndex() const
00074         {
00075                 btAssert(!isLeafNode());
00076                 return -m_escapeIndexOrTriangleIndex;
00077         }
00078         int     getTriangleIndex() const
00079         {
00080                 btAssert(isLeafNode());
00081                 unsigned int x=0;
00082                 unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
00083                 // Get only the lower bits where the triangle index is stored
00084                 return (m_escapeIndexOrTriangleIndex&~(y));
00085         }
00086         int     getPartId() const
00087         {
00088                 btAssert(isLeafNode());
00089                 // Get only the highest bits where the part index is stored
00090                 return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
00091         }
00092 }
00093 ;
00094 
00097 ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
00098 {
00099         BT_DECLARE_ALIGNED_ALLOCATOR();
00100 
00101         //32 bytes
00102         btVector3       m_aabbMinOrg;
00103         btVector3       m_aabbMaxOrg;
00104 
00105         //4
00106         int     m_escapeIndex;
00107 
00108         //8
00109         //for child nodes
00110         int     m_subPart;
00111         int     m_triangleIndex;
00112 
00113 //pad the size to 64 bytes
00114         char    m_padding[20];
00115 };
00116 
00117 
00119 ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
00120 {
00121 public:
00122         BT_DECLARE_ALIGNED_ALLOCATOR();
00123 
00124         //12 bytes
00125         unsigned short int      m_quantizedAabbMin[3];
00126         unsigned short int      m_quantizedAabbMax[3];
00127         //4 bytes, points to the root of the subtree
00128         int                     m_rootNodeIndex;
00129         //4 bytes
00130         int                     m_subtreeSize;
00131         int                     m_padding[3];
00132 
00133         btBvhSubtreeInfo()
00134         {
00135                 //memset(&m_padding[0], 0, sizeof(m_padding));
00136         }
00137 
00138 
00139         void    setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
00140         {
00141                 m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
00142                 m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
00143                 m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2];
00144                 m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0];
00145                 m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
00146                 m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
00147         }
00148 }
00149 ;
00150 
00151 
00152 class btNodeOverlapCallback
00153 {
00154 public:
00155         virtual ~btNodeOverlapCallback() {};
00156 
00157         virtual void processNode(int subPart, int triangleIndex) = 0;
00158 };
00159 
00160 #include "LinearMath/btAlignedAllocator.h"
00161 #include "LinearMath/btAlignedObjectArray.h"
00162 
00163 
00164 
00166 typedef btAlignedObjectArray<btOptimizedBvhNode>        NodeArray;
00167 typedef btAlignedObjectArray<btQuantizedBvhNode>        QuantizedNodeArray;
00168 typedef btAlignedObjectArray<btBvhSubtreeInfo>          BvhSubtreeInfoArray;
00169 
00170 
00174 ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
00175 {
00176 public:
00177         enum btTraversalMode
00178         {
00179                 TRAVERSAL_STACKLESS = 0,
00180                 TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
00181                 TRAVERSAL_RECURSIVE
00182         };
00183 
00184 protected:
00185 
00186 
00187         btVector3                       m_bvhAabbMin;
00188         btVector3                       m_bvhAabbMax;
00189         btVector3                       m_bvhQuantization;
00190 
00191         int                                     m_bulletVersion;        //for serialization versioning. It could also be used to detect endianess.
00192 
00193         int                                     m_curNodeIndex;
00194         //quantization data
00195         bool                            m_useQuantization;
00196 
00197 
00198 
00199         NodeArray                       m_leafNodes;
00200         NodeArray                       m_contiguousNodes;
00201         QuantizedNodeArray      m_quantizedLeafNodes;
00202         QuantizedNodeArray      m_quantizedContiguousNodes;
00203         
00204         btTraversalMode m_traversalMode;
00205         BvhSubtreeInfoArray             m_SubtreeHeaders;
00206 
00207         //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
00208         mutable int m_subtreeHeaderCount;
00209 
00210         
00211 
00212 
00213 
00216         void    setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
00217         {
00218                 if (m_useQuantization)
00219                 {
00220                         quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
00221                 } else
00222                 {
00223                         m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
00224 
00225                 }
00226         }
00227         void    setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
00228         {
00229                 if (m_useQuantization)
00230                 {
00231                         quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
00232                 } else
00233                 {
00234                         m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
00235                 }
00236         }
00237 
00238         btVector3 getAabbMin(int nodeIndex) const
00239         {
00240                 if (m_useQuantization)
00241                 {
00242                         return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
00243                 }
00244                 //non-quantized
00245                 return m_leafNodes[nodeIndex].m_aabbMinOrg;
00246 
00247         }
00248         btVector3 getAabbMax(int nodeIndex) const
00249         {
00250                 if (m_useQuantization)
00251                 {
00252                         return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
00253                 } 
00254                 //non-quantized
00255                 return m_leafNodes[nodeIndex].m_aabbMaxOrg;
00256                 
00257         }
00258 
00259         
00260         void    setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
00261         {
00262                 if (m_useQuantization)
00263                 {
00264                         m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
00265                 } 
00266                 else
00267                 {
00268                         m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
00269                 }
00270 
00271         }
00272 
00273         void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) 
00274         {
00275                 if (m_useQuantization)
00276                 {
00277                         unsigned short int quantizedAabbMin[3];
00278                         unsigned short int quantizedAabbMax[3];
00279                         quantize(quantizedAabbMin,newAabbMin,0);
00280                         quantize(quantizedAabbMax,newAabbMax,1);
00281                         for (int i=0;i<3;i++)
00282                         {
00283                                 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
00284                                         m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i];
00285 
00286                                 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
00287                                         m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
00288 
00289                         }
00290                 } else
00291                 {
00292                         //non-quantized
00293                         m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
00294                         m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);           
00295                 }
00296         }
00297 
00298         void    swapLeafNodes(int firstIndex,int secondIndex);
00299 
00300         void    assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
00301 
00302 protected:
00303 
00304         
00305 
00306         void    buildTree       (int startIndex,int endIndex);
00307 
00308         int     calcSplittingAxis(int startIndex,int endIndex);
00309 
00310         int     sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
00311         
00312         void    walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
00313 
00314         void    walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
00315         void    walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const;
00316         void    walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const;
00317 
00319         void    walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
00320 
00322         void    walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const;
00323 
00325         void    walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
00326         
00327 
00328 
00329 
00330         void    updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
00331 
00332 public:
00333         
00334         BT_DECLARE_ALIGNED_ALLOCATOR();
00335 
00336         btQuantizedBvh();
00337 
00338         virtual ~btQuantizedBvh();
00339 
00340         
00342         void    setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
00343         QuantizedNodeArray&     getLeafNodeArray() {                    return  m_quantizedLeafNodes;   }
00345         void    buildInternal();
00347 
00348         void    reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
00349         void    reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const;
00350         void    reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const;
00351 
00352                 SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
00353         {
00354 
00355                 btAssert(m_useQuantization);
00356 
00357                 btAssert(point.getX() <= m_bvhAabbMax.getX());
00358                 btAssert(point.getY() <= m_bvhAabbMax.getY());
00359                 btAssert(point.getZ() <= m_bvhAabbMax.getZ());
00360 
00361                 btAssert(point.getX() >= m_bvhAabbMin.getX());
00362                 btAssert(point.getY() >= m_bvhAabbMin.getY());
00363                 btAssert(point.getZ() >= m_bvhAabbMin.getZ());
00364 
00365                 btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
00369                 if (isMax)
00370                 {
00371                         out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
00372                         out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1));
00373                         out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
00374                 } else
00375                 {
00376                         out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
00377                         out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
00378                         out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
00379                 }
00380 
00381 
00382 #ifdef DEBUG_CHECK_DEQUANTIZATION
00383                 btVector3 newPoint = unQuantize(out);
00384                 if (isMax)
00385                 {
00386                         if (newPoint.getX() < point.getX())
00387                         {
00388                                 printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
00389                         }
00390                         if (newPoint.getY() < point.getY())
00391                         {
00392                                 printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
00393                         }
00394                         if (newPoint.getZ() < point.getZ())
00395                         {
00396 
00397                                 printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
00398                         }
00399                 } else
00400                 {
00401                         if (newPoint.getX() > point.getX())
00402                         {
00403                                 printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX());
00404                         }
00405                         if (newPoint.getY() > point.getY())
00406                         {
00407                                 printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY());
00408                         }
00409                         if (newPoint.getZ() > point.getZ())
00410                         {
00411                                 printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
00412                         }
00413                 }
00414 #endif //DEBUG_CHECK_DEQUANTIZATION
00415 
00416         }
00417 
00418 
00419         SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
00420         {
00421 
00422                 btAssert(m_useQuantization);
00423 
00424                 btVector3 clampedPoint(point2);
00425                 clampedPoint.setMax(m_bvhAabbMin);
00426                 clampedPoint.setMin(m_bvhAabbMax);
00427 
00428                 quantize(out,clampedPoint,isMax);
00429 
00430         }
00431         
00432         SIMD_FORCE_INLINE btVector3     unQuantize(const unsigned short* vecIn) const
00433         {
00434                         btVector3       vecOut;
00435                         vecOut.setValue(
00436                         (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
00437                         (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
00438                         (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
00439                         vecOut += m_bvhAabbMin;
00440                         return vecOut;
00441         }
00442 
00444         void    setTraversalMode(btTraversalMode        traversalMode)
00445         {
00446                 m_traversalMode = traversalMode;
00447         }
00448 
00449 
00450         SIMD_FORCE_INLINE QuantizedNodeArray&   getQuantizedNodeArray()
00451         {       
00452                 return  m_quantizedContiguousNodes;
00453         }
00454 
00455 
00456         SIMD_FORCE_INLINE BvhSubtreeInfoArray&  getSubtreeInfoArray()
00457         {
00458                 return m_SubtreeHeaders;
00459         }
00460 
00462 
00464         unsigned calculateSerializeBufferSize() const;
00465 
00467         virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const;
00468 
00470         static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
00471 
00472         static unsigned int getAlignmentSerializationPadding();
00474 
00475         
00476         virtual int     calculateSerializeBufferSizeNew() const;
00477 
00479         virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
00480 
00481         virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData);
00482 
00483         virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData);
00484 
00485 
00487 
00488         SIMD_FORCE_INLINE bool isQuantized()
00489         {
00490                 return m_useQuantization;
00491         }
00492 
00493 private:
00494         // Special "copy" constructor that allows for in-place deserialization
00495         // Prevents btVector3's default constructor from being called, but doesn't inialize much else
00496         // 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)
00497         btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
00498 
00499 }
00500 ;
00501 
00502 
00503 struct  btBvhSubtreeInfoData
00504 {
00505         int                     m_rootNodeIndex;
00506         int                     m_subtreeSize;
00507         unsigned short m_quantizedAabbMin[3];
00508         unsigned short m_quantizedAabbMax[3];
00509 };
00510 
00511 struct btOptimizedBvhNodeFloatData
00512 {
00513         btVector3FloatData      m_aabbMinOrg;
00514         btVector3FloatData      m_aabbMaxOrg;
00515         int     m_escapeIndex;
00516         int     m_subPart;
00517         int     m_triangleIndex;
00518         char m_pad[4];
00519 };
00520 
00521 struct btOptimizedBvhNodeDoubleData
00522 {
00523         btVector3DoubleData     m_aabbMinOrg;
00524         btVector3DoubleData     m_aabbMaxOrg;
00525         int     m_escapeIndex;
00526         int     m_subPart;
00527         int     m_triangleIndex;
00528         char    m_pad[4];
00529 };
00530 
00531 
00532 struct btQuantizedBvhNodeData
00533 {
00534         unsigned short m_quantizedAabbMin[3];
00535         unsigned short m_quantizedAabbMax[3];
00536         int     m_escapeIndexOrTriangleIndex;
00537 };
00538 
00539 struct  btQuantizedBvhFloatData
00540 {
00541         btVector3FloatData                      m_bvhAabbMin;
00542         btVector3FloatData                      m_bvhAabbMax;
00543         btVector3FloatData                      m_bvhQuantization;
00544         int                                     m_curNodeIndex;
00545         int                                     m_useQuantization;
00546         int                                     m_numContiguousLeafNodes;
00547         int                                     m_numQuantizedContiguousNodes;
00548         btOptimizedBvhNodeFloatData     *m_contiguousNodesPtr;
00549         btQuantizedBvhNodeData          *m_quantizedContiguousNodesPtr;
00550         btBvhSubtreeInfoData    *m_subTreeInfoPtr;
00551         int                                     m_traversalMode;
00552         int                                     m_numSubtreeHeaders;
00553         
00554 };
00555 
00556 struct  btQuantizedBvhDoubleData
00557 {
00558         btVector3DoubleData                     m_bvhAabbMin;
00559         btVector3DoubleData                     m_bvhAabbMax;
00560         btVector3DoubleData                     m_bvhQuantization;
00561         int                                                     m_curNodeIndex;
00562         int                                                     m_useQuantization;
00563         int                                                     m_numContiguousLeafNodes;
00564         int                                                     m_numQuantizedContiguousNodes;
00565         btOptimizedBvhNodeDoubleData    *m_contiguousNodesPtr;
00566         btQuantizedBvhNodeData                  *m_quantizedContiguousNodesPtr;
00567 
00568         int                                                     m_traversalMode;
00569         int                                                     m_numSubtreeHeaders;
00570         btBvhSubtreeInfoData            *m_subTreeInfoPtr;
00571 };
00572 
00573 
00574 SIMD_FORCE_INLINE       int     btQuantizedBvh::calculateSerializeBufferSizeNew() const
00575 {
00576         return sizeof(btQuantizedBvhData);
00577 }
00578 
00579 
00580 
00581 #endif //BT_QUANTIZED_BVH_H