00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #ifndef BT_QUANTIZED_BVH_H
00017 #define BT_QUANTIZED_BVH_H
00018
00019 class btSerializer;
00020
00021
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
00047
00048
00049
00050 #define MAX_SUBTREE_SIZE_IN_BYTES 2048
00051
00052
00053
00054 #define MAX_NUM_PARTS_IN_BITS 10
00055
00058 ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode
00059 {
00060 BT_DECLARE_ALIGNED_ALLOCATOR();
00061
00062
00063 unsigned short int m_quantizedAabbMin[3];
00064 unsigned short int m_quantizedAabbMax[3];
00065
00066 int m_escapeIndexOrTriangleIndex;
00067
00068 bool isLeafNode() const
00069 {
00070
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
00084 return (m_escapeIndexOrTriangleIndex&~(y));
00085 }
00086 int getPartId() const
00087 {
00088 btAssert(isLeafNode());
00089
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
00102 btVector3 m_aabbMinOrg;
00103 btVector3 m_aabbMaxOrg;
00104
00105
00106 int m_escapeIndex;
00107
00108
00109
00110 int m_subPart;
00111 int m_triangleIndex;
00112
00113
00114 char m_padding[20];
00115 };
00116
00117
00119 ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
00120 {
00121 public:
00122 BT_DECLARE_ALIGNED_ALLOCATOR();
00123
00124
00125 unsigned short int m_quantizedAabbMin[3];
00126 unsigned short int m_quantizedAabbMax[3];
00127
00128 int m_rootNodeIndex;
00129
00130 int m_subtreeSize;
00131 int m_padding[3];
00132
00133 btBvhSubtreeInfo()
00134 {
00135
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;
00192
00193 int m_curNodeIndex;
00194
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
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
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
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
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
00495
00496
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