| 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 | 
|---|