| 1 | /* | 
|---|
| 2 | Bullet Continuous Collision Detection and Physics Library | 
|---|
| 3 | Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/ | 
|---|
| 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 BT_QUANTIZED_BVH_H | 
|---|
| 17 | #define BT_QUANTIZED_BVH_H | 
|---|
| 18 |  | 
|---|
| 19 | class btSerializer; | 
|---|
| 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 "LinearMath/btVector3.h" | 
|---|
| 32 | #include "LinearMath/btAlignedAllocator.h" | 
|---|
| 33 |  | 
|---|
| 34 | #ifdef BT_USE_DOUBLE_PRECISION | 
|---|
| 35 | #define btQuantizedBvhData btQuantizedBvhDoubleData | 
|---|
| 36 | #define btOptimizedBvhNodeData btOptimizedBvhNodeDoubleData | 
|---|
| 37 | #define btQuantizedBvhDataName "btQuantizedBvhDoubleData" | 
|---|
| 38 | #else | 
|---|
| 39 | #define btQuantizedBvhData btQuantizedBvhFloatData | 
|---|
| 40 | #define btOptimizedBvhNodeData btOptimizedBvhNodeFloatData | 
|---|
| 41 | #define btQuantizedBvhDataName "btQuantizedBvhFloatData" | 
|---|
| 42 | #endif | 
|---|
| 43 |  | 
|---|
| 44 |  | 
|---|
| 45 |  | 
|---|
| 46 | //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp | 
|---|
| 47 |  | 
|---|
| 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 | ///btQuantizedBvhNode 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 | ATTRIBUTE_ALIGNED16     (struct) btQuantizedBvhNode | 
|---|
| 59 | { | 
|---|
| 60 |         BT_DECLARE_ALIGNED_ALLOCATOR(); | 
|---|
| 61 |  | 
|---|
| 62 |         //12 bytes | 
|---|
| 63 |         unsigned short int      m_quantizedAabbMin[3]; | 
|---|
| 64 |         unsigned short int      m_quantizedAabbMax[3]; | 
|---|
| 65 |         //4 bytes | 
|---|
| 66 |         int     m_escapeIndexOrTriangleIndex; | 
|---|
| 67 |  | 
|---|
| 68 |         bool isLeafNode() const | 
|---|
| 69 |         { | 
|---|
| 70 |                 //skipindex is negative (internal node), triangleindex >=0 (leafnode) | 
|---|
| 71 |                 return (m_escapeIndexOrTriangleIndex >= 0); | 
|---|
| 72 |         } | 
|---|
| 73 |         int getEscapeIndex() const | 
|---|
| 74 |         { | 
|---|
| 75 |                 btAssert(!isLeafNode()); | 
|---|
| 76 |                 return -m_escapeIndexOrTriangleIndex; | 
|---|
| 77 |         } | 
|---|
| 78 |         int     getTriangleIndex() const | 
|---|
| 79 |         { | 
|---|
| 80 |                 btAssert(isLeafNode()); | 
|---|
| 81 |                 // Get only the lower bits where the triangle index is stored | 
|---|
| 82 |                 return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); | 
|---|
| 83 |         } | 
|---|
| 84 |         int     getPartId() const | 
|---|
| 85 |         { | 
|---|
| 86 |                 btAssert(isLeafNode()); | 
|---|
| 87 |                 // Get only the highest bits where the part index is stored | 
|---|
| 88 |                 return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); | 
|---|
| 89 |         } | 
|---|
| 90 | } | 
|---|
| 91 | ; | 
|---|
| 92 |  | 
|---|
| 93 | /// btOptimizedBvhNode contains both internal and leaf node information. | 
|---|
| 94 | /// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. | 
|---|
| 95 | ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode | 
|---|
| 96 | { | 
|---|
| 97 |         BT_DECLARE_ALIGNED_ALLOCATOR(); | 
|---|
| 98 |  | 
|---|
| 99 |         //32 bytes | 
|---|
| 100 |         btVector3       m_aabbMinOrg; | 
|---|
| 101 |         btVector3       m_aabbMaxOrg; | 
|---|
| 102 |  | 
|---|
| 103 |         //4 | 
|---|
| 104 |         int     m_escapeIndex; | 
|---|
| 105 |  | 
|---|
| 106 |         //8 | 
|---|
| 107 |         //for child nodes | 
|---|
| 108 |         int     m_subPart; | 
|---|
| 109 |         int     m_triangleIndex; | 
|---|
| 110 |         int     m_padding[5];//bad, due to alignment | 
|---|
| 111 |  | 
|---|
| 112 |  | 
|---|
| 113 | }; | 
|---|
| 114 |  | 
|---|
| 115 |  | 
|---|
| 116 | ///btBvhSubtreeInfo provides info to gather a subtree of limited size | 
|---|
| 117 | ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo | 
|---|
| 118 | { | 
|---|
| 119 | public: | 
|---|
| 120 |         BT_DECLARE_ALIGNED_ALLOCATOR(); | 
|---|
| 121 |  | 
|---|
| 122 |         //12 bytes | 
|---|
| 123 |         unsigned short int      m_quantizedAabbMin[3]; | 
|---|
| 124 |         unsigned short int      m_quantizedAabbMax[3]; | 
|---|
| 125 |         //4 bytes, points to the root of the subtree | 
|---|
| 126 |         int                     m_rootNodeIndex; | 
|---|
| 127 |         //4 bytes | 
|---|
| 128 |         int                     m_subtreeSize; | 
|---|
| 129 |         int                     m_padding[3]; | 
|---|
| 130 |  | 
|---|
| 131 |         btBvhSubtreeInfo() | 
|---|
| 132 |         { | 
|---|
| 133 |                 //memset(&m_padding[0], 0, sizeof(m_padding)); | 
|---|
| 134 |         } | 
|---|
| 135 |  | 
|---|
| 136 |  | 
|---|
| 137 |         void    setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) | 
|---|
| 138 |         { | 
|---|
| 139 |                 m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; | 
|---|
| 140 |                 m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; | 
|---|
| 141 |                 m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; | 
|---|
| 142 |                 m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; | 
|---|
| 143 |                 m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; | 
|---|
| 144 |                 m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; | 
|---|
| 145 |         } | 
|---|
| 146 | } | 
|---|
| 147 | ; | 
|---|
| 148 |  | 
|---|
| 149 |  | 
|---|
| 150 | class btNodeOverlapCallback | 
|---|
| 151 | { | 
|---|
| 152 | public: | 
|---|
| 153 |         virtual ~btNodeOverlapCallback() {}; | 
|---|
| 154 |  | 
|---|
| 155 |         virtual void processNode(int subPart, int triangleIndex) = 0; | 
|---|
| 156 | }; | 
|---|
| 157 |  | 
|---|
| 158 | #include "LinearMath/btAlignedAllocator.h" | 
|---|
| 159 | #include "LinearMath/btAlignedObjectArray.h" | 
|---|
| 160 |  | 
|---|
| 161 |  | 
|---|
| 162 |  | 
|---|
| 163 | ///for code readability: | 
|---|
| 164 | typedef btAlignedObjectArray<btOptimizedBvhNode>        NodeArray; | 
|---|
| 165 | typedef btAlignedObjectArray<btQuantizedBvhNode>        QuantizedNodeArray; | 
|---|
| 166 | typedef btAlignedObjectArray<btBvhSubtreeInfo>          BvhSubtreeInfoArray; | 
|---|
| 167 |  | 
|---|
| 168 |  | 
|---|
| 169 | ///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. | 
|---|
| 170 | ///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase. | 
|---|
| 171 | ///It is recommended to use quantization for better performance and lower memory requirements. | 
|---|
| 172 | ATTRIBUTE_ALIGNED16(class) btQuantizedBvh | 
|---|
| 173 | { | 
|---|
| 174 | public: | 
|---|
| 175 |         enum btTraversalMode | 
|---|
| 176 |         { | 
|---|
| 177 |                 TRAVERSAL_STACKLESS = 0, | 
|---|
| 178 |                 TRAVERSAL_STACKLESS_CACHE_FRIENDLY, | 
|---|
| 179 |                 TRAVERSAL_RECURSIVE | 
|---|
| 180 |         }; | 
|---|
| 181 |  | 
|---|
| 182 | protected: | 
|---|
| 183 |  | 
|---|
| 184 |  | 
|---|
| 185 |         btVector3                       m_bvhAabbMin; | 
|---|
| 186 |         btVector3                       m_bvhAabbMax; | 
|---|
| 187 |         btVector3                       m_bvhQuantization; | 
|---|
| 188 |  | 
|---|
| 189 |         int                                     m_bulletVersion;        //for serialization versioning. It could also be used to detect endianess. | 
|---|
| 190 |  | 
|---|
| 191 |         int                                     m_curNodeIndex; | 
|---|
| 192 |         //quantization data | 
|---|
| 193 |         bool                            m_useQuantization; | 
|---|
| 194 |  | 
|---|
| 195 |  | 
|---|
| 196 |  | 
|---|
| 197 |         NodeArray                       m_leafNodes; | 
|---|
| 198 |         NodeArray                       m_contiguousNodes; | 
|---|
| 199 |         QuantizedNodeArray      m_quantizedLeafNodes; | 
|---|
| 200 |         QuantizedNodeArray      m_quantizedContiguousNodes; | 
|---|
| 201 |          | 
|---|
| 202 |         btTraversalMode m_traversalMode; | 
|---|
| 203 |         BvhSubtreeInfoArray             m_SubtreeHeaders; | 
|---|
| 204 |  | 
|---|
| 205 |         //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray | 
|---|
| 206 |         mutable int m_subtreeHeaderCount; | 
|---|
| 207 |  | 
|---|
| 208 |          | 
|---|
| 209 |  | 
|---|
| 210 |  | 
|---|
| 211 |  | 
|---|
| 212 |         ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) | 
|---|
| 213 |         ///this might be refactored into a virtual, it is usually not calculated at run-time | 
|---|
| 214 |         void    setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) | 
|---|
| 215 |         { | 
|---|
| 216 |                 if (m_useQuantization) | 
|---|
| 217 |                 { | 
|---|
| 218 |                         quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); | 
|---|
| 219 |                 } else | 
|---|
| 220 |                 { | 
|---|
| 221 |                         m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; | 
|---|
| 222 |  | 
|---|
| 223 |                 } | 
|---|
| 224 |         } | 
|---|
| 225 |         void    setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) | 
|---|
| 226 |         { | 
|---|
| 227 |                 if (m_useQuantization) | 
|---|
| 228 |                 { | 
|---|
| 229 |                         quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); | 
|---|
| 230 |                 } else | 
|---|
| 231 |                 { | 
|---|
| 232 |                         m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; | 
|---|
| 233 |                 } | 
|---|
| 234 |         } | 
|---|
| 235 |  | 
|---|
| 236 |         btVector3 getAabbMin(int nodeIndex) const | 
|---|
| 237 |         { | 
|---|
| 238 |                 if (m_useQuantization) | 
|---|
| 239 |                 { | 
|---|
| 240 |                         return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); | 
|---|
| 241 |                 } | 
|---|
| 242 |                 //non-quantized | 
|---|
| 243 |                 return m_leafNodes[nodeIndex].m_aabbMinOrg; | 
|---|
| 244 |  | 
|---|
| 245 |         } | 
|---|
| 246 |         btVector3 getAabbMax(int nodeIndex) const | 
|---|
| 247 |         { | 
|---|
| 248 |                 if (m_useQuantization) | 
|---|
| 249 |                 { | 
|---|
| 250 |                         return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); | 
|---|
| 251 |                 }  | 
|---|
| 252 |                 //non-quantized | 
|---|
| 253 |                 return m_leafNodes[nodeIndex].m_aabbMaxOrg; | 
|---|
| 254 |                  | 
|---|
| 255 |         } | 
|---|
| 256 |  | 
|---|
| 257 |          | 
|---|
| 258 |         void    setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) | 
|---|
| 259 |         { | 
|---|
| 260 |                 if (m_useQuantization) | 
|---|
| 261 |                 { | 
|---|
| 262 |                         m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; | 
|---|
| 263 |                 }  | 
|---|
| 264 |                 else | 
|---|
| 265 |                 { | 
|---|
| 266 |                         m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; | 
|---|
| 267 |                 } | 
|---|
| 268 |  | 
|---|
| 269 |         } | 
|---|
| 270 |  | 
|---|
| 271 |         void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax)  | 
|---|
| 272 |         { | 
|---|
| 273 |                 if (m_useQuantization) | 
|---|
| 274 |                 { | 
|---|
| 275 |                         unsigned short int quantizedAabbMin[3]; | 
|---|
| 276 |                         unsigned short int quantizedAabbMax[3]; | 
|---|
| 277 |                         quantize(quantizedAabbMin,newAabbMin,0); | 
|---|
| 278 |                         quantize(quantizedAabbMax,newAabbMax,1); | 
|---|
| 279 |                         for (int i=0;i<3;i++) | 
|---|
| 280 |                         { | 
|---|
| 281 |                                 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) | 
|---|
| 282 |                                         m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i]; | 
|---|
| 283 |  | 
|---|
| 284 |                                 if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i]) | 
|---|
| 285 |                                         m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; | 
|---|
| 286 |  | 
|---|
| 287 |                         } | 
|---|
| 288 |                 } else | 
|---|
| 289 |                 { | 
|---|
| 290 |                         //non-quantized | 
|---|
| 291 |                         m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); | 
|---|
| 292 |                         m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);            | 
|---|
| 293 |                 } | 
|---|
| 294 |         } | 
|---|
| 295 |  | 
|---|
| 296 |         void    swapLeafNodes(int firstIndex,int secondIndex); | 
|---|
| 297 |  | 
|---|
| 298 |         void    assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); | 
|---|
| 299 |  | 
|---|
| 300 | protected: | 
|---|
| 301 |  | 
|---|
| 302 |          | 
|---|
| 303 |  | 
|---|
| 304 |         void    buildTree       (int startIndex,int endIndex); | 
|---|
| 305 |  | 
|---|
| 306 |         int     calcSplittingAxis(int startIndex,int endIndex); | 
|---|
| 307 |  | 
|---|
| 308 |         int     sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); | 
|---|
| 309 |          | 
|---|
| 310 |         void    walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; | 
|---|
| 311 |  | 
|---|
| 312 |         void    walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; | 
|---|
| 313 |         void    walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; | 
|---|
| 314 |         void    walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; | 
|---|
| 315 |  | 
|---|
| 316 |         ///tree traversal designed for small-memory processors like PS3 SPU | 
|---|
| 317 |         void    walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; | 
|---|
| 318 |  | 
|---|
| 319 |         ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal | 
|---|
| 320 |         void    walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; | 
|---|
| 321 |  | 
|---|
| 322 |         ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal | 
|---|
| 323 |         void    walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; | 
|---|
| 324 |          | 
|---|
| 325 |  | 
|---|
| 326 |  | 
|---|
| 327 |  | 
|---|
| 328 |         void    updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); | 
|---|
| 329 |  | 
|---|
| 330 | public: | 
|---|
| 331 |          | 
|---|
| 332 |         BT_DECLARE_ALIGNED_ALLOCATOR(); | 
|---|
| 333 |  | 
|---|
| 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 |  | 
|---|
| 461 |         /////Calculate space needed to store BVH for serialization | 
|---|
| 462 |         unsigned calculateSerializeBufferSize() const; | 
|---|
| 463 |  | 
|---|
| 464 |         /// Data buffer MUST be 16 byte aligned | 
|---|
| 465 |         virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const; | 
|---|
| 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 |  | 
|---|
| 473 |          | 
|---|
| 474 |         virtual int     calculateSerializeBufferSizeNew() const; | 
|---|
| 475 |  | 
|---|
| 476 |         ///fills the dataBuffer and returns the struct name (and 0 on failure) | 
|---|
| 477 |         virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const; | 
|---|
| 478 |  | 
|---|
| 479 |         virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData); | 
|---|
| 480 |  | 
|---|
| 481 |         virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData); | 
|---|
| 482 |  | 
|---|
| 483 |  | 
|---|
| 484 | //////////////////////////////////////////////////////////////////// | 
|---|
| 485 |  | 
|---|
| 486 |         SIMD_FORCE_INLINE bool isQuantized() | 
|---|
| 487 |         { | 
|---|
| 488 |                 return m_useQuantization; | 
|---|
| 489 |         } | 
|---|
| 490 |  | 
|---|
| 491 | private: | 
|---|
| 492 |         // Special "copy" constructor that allows for in-place deserialization | 
|---|
| 493 |         // Prevents btVector3's default constructor from being called, but doesn't inialize much else | 
|---|
| 494 |         // 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) | 
|---|
| 495 |         btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory); | 
|---|
| 496 |  | 
|---|
| 497 | } | 
|---|
| 498 | ; | 
|---|
| 499 |  | 
|---|
| 500 |  | 
|---|
| 501 | struct  btBvhSubtreeInfoData | 
|---|
| 502 | { | 
|---|
| 503 |         int                     m_rootNodeIndex; | 
|---|
| 504 |         int                     m_subtreeSize; | 
|---|
| 505 |         unsigned short m_quantizedAabbMin[3]; | 
|---|
| 506 |         unsigned short m_quantizedAabbMax[3]; | 
|---|
| 507 | }; | 
|---|
| 508 |  | 
|---|
| 509 | struct btOptimizedBvhNodeFloatData | 
|---|
| 510 | { | 
|---|
| 511 |         btVector3FloatData      m_aabbMinOrg; | 
|---|
| 512 |         btVector3FloatData      m_aabbMaxOrg; | 
|---|
| 513 |         int     m_escapeIndex; | 
|---|
| 514 |         int     m_subPart; | 
|---|
| 515 |         int     m_triangleIndex; | 
|---|
| 516 |         char m_pad[4]; | 
|---|
| 517 | }; | 
|---|
| 518 |  | 
|---|
| 519 | struct btOptimizedBvhNodeDoubleData | 
|---|
| 520 | { | 
|---|
| 521 |         btVector3DoubleData     m_aabbMinOrg; | 
|---|
| 522 |         btVector3DoubleData     m_aabbMaxOrg; | 
|---|
| 523 |         int     m_escapeIndex; | 
|---|
| 524 |         int     m_subPart; | 
|---|
| 525 |         int     m_triangleIndex; | 
|---|
| 526 |         char    m_pad[4]; | 
|---|
| 527 | }; | 
|---|
| 528 |  | 
|---|
| 529 |  | 
|---|
| 530 | struct btQuantizedBvhNodeData | 
|---|
| 531 | { | 
|---|
| 532 |         unsigned short m_quantizedAabbMin[3]; | 
|---|
| 533 |         unsigned short m_quantizedAabbMax[3]; | 
|---|
| 534 |         int     m_escapeIndexOrTriangleIndex; | 
|---|
| 535 | }; | 
|---|
| 536 |  | 
|---|
| 537 | struct  btQuantizedBvhFloatData | 
|---|
| 538 | { | 
|---|
| 539 |         btVector3FloatData                      m_bvhAabbMin; | 
|---|
| 540 |         btVector3FloatData                      m_bvhAabbMax; | 
|---|
| 541 |         btVector3FloatData                      m_bvhQuantization; | 
|---|
| 542 |         int                                     m_curNodeIndex; | 
|---|
| 543 |         int                                     m_useQuantization; | 
|---|
| 544 |         int                                     m_numContiguousLeafNodes; | 
|---|
| 545 |         int                                     m_numQuantizedContiguousNodes; | 
|---|
| 546 |         btOptimizedBvhNodeFloatData     *m_contiguousNodesPtr; | 
|---|
| 547 |         btQuantizedBvhNodeData          *m_quantizedContiguousNodesPtr; | 
|---|
| 548 |         btBvhSubtreeInfoData    *m_subTreeInfoPtr; | 
|---|
| 549 |         int                                     m_traversalMode; | 
|---|
| 550 |         int                                     m_numSubtreeHeaders; | 
|---|
| 551 |          | 
|---|
| 552 | }; | 
|---|
| 553 |  | 
|---|
| 554 | struct  btQuantizedBvhDoubleData | 
|---|
| 555 | { | 
|---|
| 556 |         btVector3DoubleData                     m_bvhAabbMin; | 
|---|
| 557 |         btVector3DoubleData                     m_bvhAabbMax; | 
|---|
| 558 |         btVector3DoubleData                     m_bvhQuantization; | 
|---|
| 559 |         int                                                     m_curNodeIndex; | 
|---|
| 560 |         int                                                     m_useQuantization; | 
|---|
| 561 |         int                                                     m_numContiguousLeafNodes; | 
|---|
| 562 |         int                                                     m_numQuantizedContiguousNodes; | 
|---|
| 563 |         btOptimizedBvhNodeDoubleData    *m_contiguousNodesPtr; | 
|---|
| 564 |         btQuantizedBvhNodeData                  *m_quantizedContiguousNodesPtr; | 
|---|
| 565 |  | 
|---|
| 566 |         int                                                     m_traversalMode; | 
|---|
| 567 |         int                                                     m_numSubtreeHeaders; | 
|---|
| 568 |         btBvhSubtreeInfoData            *m_subTreeInfoPtr; | 
|---|
| 569 | }; | 
|---|
| 570 |  | 
|---|
| 571 |  | 
|---|
| 572 | SIMD_FORCE_INLINE       int     btQuantizedBvh::calculateSerializeBufferSizeNew() const | 
|---|
| 573 | { | 
|---|
| 574 |         return sizeof(btQuantizedBvhData); | 
|---|
| 575 | } | 
|---|
| 576 |  | 
|---|
| 577 |  | 
|---|
| 578 |  | 
|---|
| 579 | #endif //BT_QUANTIZED_BVH_H | 
|---|