Back to index

supertuxkart  0.5+dfsg1
btOptimizedBvh.cpp
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 #include "btOptimizedBvh.h"
00017 #include "btStridingMeshInterface.h"
00018 #include "LinearMath/btAabbUtil2.h"
00019 #include "LinearMath/btIDebugDraw.h"
00020 
00021 
00022 btOptimizedBvh::btOptimizedBvh() : m_useQuantization(false), 
00023                                    //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
00024                                    m_traversalMode(TRAVERSAL_STACKLESS)
00025                                    //m_traversalMode(TRAVERSAL_RECURSIVE)
00026                                    ,m_subtreeHeaderCount(0) //PCK: add this line
00027 { 
00028 
00029 }
00030 
00031 
00032 void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax)
00033 {
00034        m_useQuantization = useQuantizedAabbCompression;
00035 
00036 
00037        // NodeArray  triangleNodes;
00038 
00039        struct NodeTriangleCallback : public btInternalTriangleIndexCallback
00040        {
00041 
00042               NodeArray&    m_triangleNodes;
00043 
00044               NodeTriangleCallback& operator=(NodeTriangleCallback& other)
00045               {
00046                      m_triangleNodes = other.m_triangleNodes;
00047                      return *this;
00048               }
00049               
00050               NodeTriangleCallback(NodeArray&    triangleNodes)
00051                      :m_triangleNodes(triangleNodes)
00052               {
00053               }
00054 
00055               virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
00056               {
00057                      btOptimizedBvhNode node;
00058                      btVector3     aabbMin,aabbMax;
00059                      aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
00060                      aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
00061                      aabbMin.setMin(triangle[0]);
00062                      aabbMax.setMax(triangle[0]);
00063                      aabbMin.setMin(triangle[1]);
00064                      aabbMax.setMax(triangle[1]);
00065                      aabbMin.setMin(triangle[2]);
00066                      aabbMax.setMax(triangle[2]);
00067 
00068                      //with quantization?
00069                      node.m_aabbMinOrg = aabbMin;
00070                      node.m_aabbMaxOrg = aabbMax;
00071 
00072                      node.m_escapeIndex = -1;
00073        
00074                      //for child nodes
00075                      node.m_subPart = partId;
00076                      node.m_triangleIndex = triangleIndex;
00077                      m_triangleNodes.push_back(node);
00078               }
00079        };
00080        struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
00081        {
00082               QuantizedNodeArray&  m_triangleNodes;
00083               const btOptimizedBvh* m_optimizedTree; // for quantization
00084 
00085               QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
00086               {
00087                      m_triangleNodes = other.m_triangleNodes;
00088                      m_optimizedTree = other.m_optimizedTree;
00089                      return *this;
00090               }
00091 
00092               QuantizedNodeTriangleCallback(QuantizedNodeArray&       triangleNodes,const btOptimizedBvh* tree)
00093                      :m_triangleNodes(triangleNodes),m_optimizedTree(tree)
00094               {
00095               }
00096 
00097               virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
00098               {
00099                      // The partId and triangle index must fit in the same (positive) integer
00100                      btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS));
00101                      btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS)));
00102                      //negative indices are reserved for escapeIndex
00103                      btAssert(triangleIndex>=0);
00104 
00105                      btQuantizedBvhNode node;
00106                      btVector3     aabbMin,aabbMax;
00107                      aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
00108                      aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
00109                      aabbMin.setMin(triangle[0]);
00110                      aabbMax.setMax(triangle[0]);
00111                      aabbMin.setMin(triangle[1]);
00112                      aabbMax.setMax(triangle[1]);
00113                      aabbMin.setMin(triangle[2]);
00114                      aabbMax.setMax(triangle[2]);
00115 
00116                      //PCK: add these checks for zero dimensions of aabb
00117                      const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
00118                      const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
00119                      if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
00120                      {
00121                             aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
00122                             aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
00123                      }
00124                      if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
00125                      {
00126                             aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
00127                             aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
00128                      }
00129                      if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
00130                      {
00131                             aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
00132                             aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
00133                      }
00134 
00135                      m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
00136                      m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
00137 
00138                      node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
00139 
00140                      m_triangleNodes.push_back(node);
00141               }
00142        };
00143        
00144 
00145 
00146        int numLeafNodes = 0;
00147 
00148        
00149        if (m_useQuantization)
00150        {
00151 
00152               //initialize quantization values
00153               setQuantizationValues(bvhAabbMin,bvhAabbMax);
00154 
00155               QuantizedNodeTriangleCallback      callback(m_quantizedLeafNodes,this);
00156 
00157        
00158               triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
00159 
00160               //now we have an array of leafnodes in m_leafNodes
00161               numLeafNodes = m_quantizedLeafNodes.size();
00162 
00163 
00164               m_quantizedContiguousNodes.resize(2*numLeafNodes);
00165 
00166 
00167        } else
00168        {
00169               NodeTriangleCallback callback(m_leafNodes);
00170 
00171               btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
00172               btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
00173 
00174               triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
00175 
00176               //now we have an array of leafnodes in m_leafNodes
00177               numLeafNodes = m_leafNodes.size();
00178 
00179               m_contiguousNodes.resize(2*numLeafNodes);
00180        }
00181 
00182        m_curNodeIndex = 0;
00183 
00184        buildTree(0,numLeafNodes);
00185 
00187        if(m_useQuantization && !m_SubtreeHeaders.size())
00188        {
00189               btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
00190               subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
00191               subtree.m_rootNodeIndex = 0;
00192               subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
00193        }
00194 
00195        //PCK: update the copy of the size
00196        m_subtreeHeaderCount = m_SubtreeHeaders.size();
00197 
00198        //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
00199        m_quantizedLeafNodes.clear();
00200        m_leafNodes.clear();
00201 }
00202 
00203 
00204 
00205 
00206 void btOptimizedBvh::buildInternal()
00207 {
00209        m_useQuantization = true;
00210        int numLeafNodes = 0;
00211        
00212        if (m_useQuantization)
00213        {
00214               //now we have an array of leafnodes in m_leafNodes
00215               numLeafNodes = m_quantizedLeafNodes.size();
00216 
00217               m_quantizedContiguousNodes.resize(2*numLeafNodes);
00218 
00219        }
00220 
00221        m_curNodeIndex = 0;
00222 
00223        buildTree(0,numLeafNodes);
00224 
00226        if(m_useQuantization && !m_SubtreeHeaders.size())
00227        {
00228               btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
00229               subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
00230               subtree.m_rootNodeIndex = 0;
00231               subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
00232        }
00233 
00234        //PCK: update the copy of the size
00235        m_subtreeHeaderCount = m_SubtreeHeaders.size();
00236 
00237        //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
00238        m_quantizedLeafNodes.clear();
00239        m_leafNodes.clear();
00240 }
00241 
00242 
00243 
00244 void   btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
00245 {
00246        //incrementally initialize quantization values
00247        btAssert(m_useQuantization);
00248 
00249        btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
00250        btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
00251        btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
00252 
00253        btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
00254        btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
00255        btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
00256 
00259        
00260        unsigned short       quantizedQueryAabbMin[3];
00261        unsigned short       quantizedQueryAabbMax[3];
00262 
00263        quantize(&quantizedQueryAabbMin[0],aabbMin,0);
00264        quantize(&quantizedQueryAabbMax[0],aabbMax,1);
00265 
00266        int i;
00267        for (i=0;i<this->m_SubtreeHeaders.size();i++)
00268        {
00269               btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
00270 
00271               //PCK: unsigned instead of bool
00272               unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
00273               if (overlap != 0)
00274               {
00275                      updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
00276 
00277                      subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
00278               }
00279        }
00280        
00281 }
00282 
00284 #ifdef DEBUG_PATCH_COLORS
00285 btVector3 color[4]=
00286 {
00287        btVector3(255,0,0),
00288        btVector3(0,255,0),
00289        btVector3(0,0,255),
00290        btVector3(0,255,255)
00291 };
00292 #endif //DEBUG_PATCH_COLORS
00293 
00294 
00295 void   btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
00296 {
00297        (void)index;
00298 
00299        btAssert(m_useQuantization);
00300 
00301        int curNodeSubPart=-1;
00302 
00303        //get access info to trianglemesh data
00304               const unsigned char *vertexbase;
00305               int numverts;
00306               PHY_ScalarType type;
00307               int stride;
00308               const unsigned char *indexbase;
00309               int indexstride;
00310               int numfaces;
00311               PHY_ScalarType indicestype;
00312 
00313               btVector3     triangleVerts[3];
00314               btVector3     aabbMin,aabbMax;
00315               const btVector3& meshScaling = meshInterface->getScaling();
00316               
00317               int i;
00318               for (i=endNode-1;i>=firstNode;i--)
00319               {
00320 
00321 
00322                      btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
00323                      if (curNode.isLeafNode())
00324                      {
00325                             //recalc aabb from triangle data
00326                             int nodeSubPart = curNode.getPartId();
00327                             int nodeTriangleIndex = curNode.getTriangleIndex();
00328                             if (nodeSubPart != curNodeSubPart)
00329                             {
00330                                    if (curNodeSubPart >= 0)
00331                                           meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
00332                                    meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
00333                                    btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
00334                             }
00335                             //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
00336 
00337                             int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride);
00338                             
00339                             
00340                             for (int j=2;j>=0;j--)
00341                             {
00342                                    
00343                                    int graphicsindex = indicestype==PHY_SHORT?((short*)gfxbase)[j]:gfxbase[j];
00344                                    btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
00345 #ifdef DEBUG_PATCH_COLORS
00346                                    btVector3 mycolor = color[index&3];
00347                                    graphicsbase[8] = mycolor.getX();
00348                                    graphicsbase[9] = mycolor.getY();
00349                                    graphicsbase[10] = mycolor.getZ();
00350 #endif //DEBUG_PATCH_COLORS
00351 
00352 
00353                                    triangleVerts[j] = btVector3(
00354                                           graphicsbase[0]*meshScaling.getX(),
00355                                           graphicsbase[1]*meshScaling.getY(),
00356                                           graphicsbase[2]*meshScaling.getZ());
00357                             }
00358 
00359 
00360                             
00361                             aabbMin.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));
00362                             aabbMax.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); 
00363                             aabbMin.setMin(triangleVerts[0]);
00364                             aabbMax.setMax(triangleVerts[0]);
00365                             aabbMin.setMin(triangleVerts[1]);
00366                             aabbMax.setMax(triangleVerts[1]);
00367                             aabbMin.setMin(triangleVerts[2]);
00368                             aabbMax.setMax(triangleVerts[2]);
00369 
00370                             quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
00371                             quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
00372                             
00373                      } else
00374                      {
00375                             //combine aabb from both children
00376 
00377                             btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
00378                             
00379                             btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
00380                                    &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
00381                             
00382 
00383                             {
00384                                    for (int i=0;i<3;i++)
00385                                    {
00386                                           curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
00387                                           if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
00388                                                  curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
00389 
00390                                           curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
00391                                           if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
00392                                                  curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
00393                                    }
00394                             }
00395                      }
00396 
00397               }
00398 
00399               if (curNodeSubPart >= 0)
00400                      meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
00401 
00402               
00403 }
00404 
00405 void   btOptimizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin)
00406 {
00407        //enlarge the AABB to avoid division by zero when initializing the quantization values
00408        btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
00409        m_bvhAabbMin = bvhAabbMin - clampValue;
00410        m_bvhAabbMax = bvhAabbMax + clampValue;
00411        btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin;
00412        m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize;
00413        m_useQuantization = true;
00414 }
00415 
00416 
00417 void   btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
00418 {
00419        if (m_useQuantization)
00420        {
00421 
00422               setQuantizationValues(aabbMin,aabbMax);
00423 
00424               updateBvhNodes(meshInterface,0,m_curNodeIndex,0);
00425 
00427 
00428               int i;
00429               for (i=0;i<m_SubtreeHeaders.size();i++)
00430               {
00431                      btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
00432                      subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
00433               }
00434 
00435        } else
00436        {
00437 
00438        }
00439 }
00440 
00441 
00442 
00443 btOptimizedBvh::~btOptimizedBvh()
00444 {
00445 }
00446 
00447 #ifdef DEBUG_TREE_BUILDING
00448 int gStackDepth = 0;
00449 int gMaxStackDepth = 0;
00450 #endif //DEBUG_TREE_BUILDING
00451 
00452 void   btOptimizedBvh::buildTree   (int startIndex,int endIndex)
00453 {
00454 #ifdef DEBUG_TREE_BUILDING
00455        gStackDepth++;
00456        if (gStackDepth > gMaxStackDepth)
00457               gMaxStackDepth = gStackDepth;
00458 #endif //DEBUG_TREE_BUILDING
00459 
00460 
00461        int splitAxis, splitIndex, i;
00462        int numIndices =endIndex-startIndex;
00463        int curIndex = m_curNodeIndex;
00464 
00465        assert(numIndices>0);
00466 
00467        if (numIndices==1)
00468        {
00469 #ifdef DEBUG_TREE_BUILDING
00470               gStackDepth--;
00471 #endif //DEBUG_TREE_BUILDING
00472               
00473               assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex);
00474 
00475               m_curNodeIndex++;
00476               return;       
00477        }
00478        //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
00479        
00480        splitAxis = calcSplittingAxis(startIndex,endIndex);
00481 
00482        splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis);
00483 
00484        int internalNodeIndex = m_curNodeIndex;
00485        
00486        setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);
00487        setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);
00488        
00489        for (i=startIndex;i<endIndex;i++)
00490        {
00491               mergeInternalNodeAabb(m_curNodeIndex,getAabbMin(i),getAabbMax(i));
00492        }
00493 
00494        m_curNodeIndex++;
00495        
00496 
00497        //internalNode->m_escapeIndex;
00498        
00499        int leftChildNodexIndex = m_curNodeIndex;
00500 
00501        //build left child tree
00502        buildTree(startIndex,splitIndex);
00503 
00504        int rightChildNodexIndex = m_curNodeIndex;
00505        //build right child tree
00506        buildTree(splitIndex,endIndex);
00507 
00508 #ifdef DEBUG_TREE_BUILDING
00509        gStackDepth--;
00510 #endif //DEBUG_TREE_BUILDING
00511 
00512        int escapeIndex = m_curNodeIndex - curIndex;
00513 
00514        if (m_useQuantization)
00515        {
00516               //escapeIndex is the number of nodes of this subtree
00517               const int sizeQuantizedNode =sizeof(btQuantizedBvhNode);
00518               const int treeSizeInBytes = escapeIndex * sizeQuantizedNode;
00519               if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES)
00520               {
00521                      updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
00522               }
00523        }
00524 
00525        setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex);
00526 
00527 }
00528 
00529 void   btOptimizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex)
00530 {
00531        btAssert(m_useQuantization);
00532 
00533        btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex];
00534        int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex();
00535        int leftSubTreeSizeInBytes =  leftSubTreeSize * sizeof(btQuantizedBvhNode);
00536        
00537        btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex];
00538        int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex();
00539        int rightSubTreeSizeInBytes =  rightSubTreeSize * sizeof(btQuantizedBvhNode);
00540 
00541        if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
00542        {
00543               btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
00544               subtree.setAabbFromQuantizeNode(leftChildNode);
00545               subtree.m_rootNodeIndex = leftChildNodexIndex;
00546               subtree.m_subtreeSize = leftSubTreeSize;
00547        }
00548 
00549        if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES)
00550        {
00551               btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
00552               subtree.setAabbFromQuantizeNode(rightChildNode);
00553               subtree.m_rootNodeIndex = rightChildNodexIndex;
00554               subtree.m_subtreeSize = rightSubTreeSize;
00555        }
00556 
00557        //PCK: update the copy of the size
00558        m_subtreeHeaderCount = m_SubtreeHeaders.size();
00559 }
00560 
00561 
00562 int    btOptimizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis)
00563 {
00564        int i;
00565        int splitIndex =startIndex;
00566        int numIndices = endIndex - startIndex;
00567        btScalar splitValue;
00568 
00569        btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
00570        for (i=startIndex;i<endIndex;i++)
00571        {
00572               btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
00573               means+=center;
00574        }
00575        means *= (btScalar(1.)/(btScalar)numIndices);
00576        
00577        splitValue = means[splitAxis];
00578        
00579        //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
00580        for (i=startIndex;i<endIndex;i++)
00581        {
00582               btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
00583               if (center[splitAxis] > splitValue)
00584               {
00585                      //swap
00586                      swapLeafNodes(i,splitIndex);
00587                      splitIndex++;
00588               }
00589        }
00590 
00591        //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
00592        //otherwise the tree-building might fail due to stack-overflows in certain cases.
00593        //unbalanced1 is unsafe: it can cause stack overflows
00594        //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
00595 
00596        //unbalanced2 should work too: always use center (perfect balanced trees)    
00597        //bool unbalanced2 = true;
00598 
00599        //this should be safe too:
00600        int rangeBalancedIndices = numIndices/3;
00601        bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
00602        
00603        if (unbalanced)
00604        {
00605               splitIndex = startIndex+ (numIndices>>1);
00606        }
00607 
00608        bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
00609        btAssert(!unbal);
00610 
00611        return splitIndex;
00612 }
00613 
00614 
00615 int    btOptimizedBvh::calcSplittingAxis(int startIndex,int endIndex)
00616 {
00617        int i;
00618 
00619        btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
00620        btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
00621        int numIndices = endIndex-startIndex;
00622 
00623        for (i=startIndex;i<endIndex;i++)
00624        {
00625               btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
00626               means+=center;
00627        }
00628        means *= (btScalar(1.)/(btScalar)numIndices);
00629               
00630        for (i=startIndex;i<endIndex;i++)
00631        {
00632               btVector3 center = btScalar(0.5)*(getAabbMax(i)+getAabbMin(i));
00633               btVector3 diff2 = center-means;
00634               diff2 = diff2 * diff2;
00635               variance += diff2;
00636        }
00637        variance *= (btScalar(1.)/  ((btScalar)numIndices-1)    );
00638        
00639        return variance.maxAxis();
00640 }
00641 
00642 
00643 
00644 void   btOptimizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
00645 {
00646        //either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
00647 
00648        if (m_useQuantization)
00649        {
00651               unsigned short int quantizedQueryAabbMin[3];
00652               unsigned short int quantizedQueryAabbMax[3];
00653               quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);
00654               quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);
00655 
00656               switch (m_traversalMode)
00657               {
00658               case TRAVERSAL_STACKLESS:
00659                             walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,0,m_curNodeIndex);
00660                      break;
00661               case TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
00662                             walkStacklessQuantizedTreeCacheFriendly(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
00663                      break;
00664               case TRAVERSAL_RECURSIVE:
00665                      {
00666                             const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[0];
00667                             walkRecursiveQuantizedTreeAgainstQueryAabb(rootNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
00668                      }
00669                      break;
00670               default:
00671                      //unsupported
00672                      btAssert(0);
00673               }
00674        } else
00675        {
00676               walkStacklessTree(nodeCallback,aabbMin,aabbMax);
00677        }
00678 }
00679 
00680 
00681 int maxIterations = 0;
00682 
00683 void   btOptimizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
00684 {
00685        btAssert(!m_useQuantization);
00686 
00687        const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
00688        int escapeIndex, curIndex = 0;
00689        int walkIterations = 0;
00690        bool isLeafNode;
00691        //PCK: unsigned instead of bool
00692        unsigned aabbOverlap;
00693 
00694        while (curIndex < m_curNodeIndex)
00695        {
00696               //catch bugs in tree data
00697               assert (walkIterations < m_curNodeIndex);
00698 
00699               walkIterations++;
00700               aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
00701               isLeafNode = rootNode->m_escapeIndex == -1;
00702               
00703               //PCK: unsigned instead of bool
00704               if (isLeafNode && (aabbOverlap != 0))
00705               {
00706                      nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
00707               } 
00708               
00709               //PCK: unsigned instead of bool
00710               if ((aabbOverlap != 0) || isLeafNode)
00711               {
00712                      rootNode++;
00713                      curIndex++;
00714               } else
00715               {
00716                      escapeIndex = rootNode->m_escapeIndex;
00717                      rootNode += escapeIndex;
00718                      curIndex += escapeIndex;
00719               }
00720        }
00721        if (maxIterations < walkIterations)
00722               maxIterations = walkIterations;
00723 
00724 }
00725 
00726 /*
00728 void   btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
00729 {
00730        bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
00731        if (aabbOverlap)
00732        {
00733               isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
00734               if (isLeafNode)
00735               {
00736                      nodeCallback->processNode(rootNode);
00737               } else
00738               {
00739                      walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
00740                      walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
00741               }
00742        }
00743 
00744 }
00745 */
00746 
00747 void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
00748 {
00749        btAssert(m_useQuantization);
00750        
00751        bool isLeafNode;
00752        //PCK: unsigned instead of bool
00753        unsigned aabbOverlap;
00754 
00755        //PCK: unsigned instead of bool
00756        aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax);
00757        isLeafNode = currentNode->isLeafNode();
00758               
00759        //PCK: unsigned instead of bool
00760        if (aabbOverlap != 0)
00761        {
00762               if (isLeafNode)
00763               {
00764                      nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex());
00765               } else
00766               {
00767                      //process left and right children
00768                      const btQuantizedBvhNode* leftChildNode = currentNode+1;
00769                      walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
00770 
00771                      const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex();
00772                      walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax);
00773               }
00774        }             
00775 }
00776 
00777 
00778 
00779 
00780 
00781 void   btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
00782 {
00783        btAssert(m_useQuantization);
00784        
00785        int curIndex = startNodeIndex;
00786        int walkIterations = 0;
00787        int subTreeSize = endNodeIndex - startNodeIndex;
00788 
00789        const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
00790        int escapeIndex;
00791        
00792        bool isLeafNode;
00793        //PCK: unsigned instead of bool
00794        unsigned boxBoxOverlap = 0;
00795        unsigned rayBoxOverlap = 0;
00796 
00797        btScalar lambda_max = 1.0;
00798 #define RAYAABB2
00799 #ifdef RAYAABB2
00800        btVector3 rayFrom = raySource;
00801        btVector3 rayDirection = (rayTarget-raySource);
00802        rayDirection.normalize ();
00803        lambda_max = rayDirection.dot(rayTarget-raySource);
00805        rayDirection[0] = btScalar(1.0) / rayDirection[0];
00806        rayDirection[1] = btScalar(1.0) / rayDirection[1];
00807        rayDirection[2] = btScalar(1.0) / rayDirection[2];
00808        unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0};
00809 #endif
00810 
00811        /* Quick pruning by quantized box */
00812        btVector3 rayAabbMin = raySource;
00813        btVector3 rayAabbMax = raySource;
00814        rayAabbMin.setMin(rayTarget);
00815        rayAabbMax.setMax(rayTarget);
00816 
00817        /* Add box cast extents to bounding box */
00818        rayAabbMin += aabbMin;
00819        rayAabbMax += aabbMax;
00820 
00821        unsigned short int quantizedQueryAabbMin[3];
00822        unsigned short int quantizedQueryAabbMax[3];
00823        quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0);
00824        quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1);
00825 
00826        while (curIndex < endNodeIndex)
00827        {
00828 
00829 //#define VISUALLY_ANALYZE_BVH 1
00830 #ifdef VISUALLY_ANALYZE_BVH
00831               //some code snippet to debugDraw aabb, to visually analyze bvh structure
00832               static int drawPatch = 0;
00833               //need some global access to a debugDrawer
00834               extern btIDebugDraw* debugDrawerPtr;
00835               if (curIndex==drawPatch)
00836               {
00837                      btVector3 aabbMin,aabbMax;
00838                      aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
00839                      aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
00840                      btVector3     color(1,0,0);
00841                      debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
00842               }
00843 #endif//VISUALLY_ANALYZE_BVH
00844 
00845               //catch bugs in tree data
00846               assert (walkIterations < subTreeSize);
00847 
00848               walkIterations++;
00849               //PCK: unsigned instead of bool
00850               // only interested if this is closer than any previous hit
00851               btScalar param = 1.0;
00852               rayBoxOverlap = 0;
00853               boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
00854               isLeafNode = rootNode->isLeafNode();
00855               if (boxBoxOverlap)
00856               {
00857                      btVector3 bounds[2];
00858                      bounds[0] = unQuantize(rootNode->m_quantizedAabbMin);
00859                      bounds[1] = unQuantize(rootNode->m_quantizedAabbMax);
00860                      /* Add box cast extents */
00861                      bounds[0] += aabbMin;
00862                      bounds[1] += aabbMax;
00863                      btVector3 normal;
00864 #if 0
00865                      bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max);
00866                      bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal);
00867                      if (ra2 != ra)
00868                      {
00869                             printf("functions don't match\n");
00870                      }
00871 #endif
00872 #ifdef RAYAABB2
00873 
00874 
00875 
00876 
00877                      rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
00878 #else
00879                      rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
00880 #endif
00881               }
00882               
00883               if (isLeafNode && rayBoxOverlap)
00884               {
00885                      nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
00886               }
00887               
00888               //PCK: unsigned instead of bool
00889               if ((rayBoxOverlap != 0) || isLeafNode)
00890               {
00891                      rootNode++;
00892                      curIndex++;
00893               } else
00894               {
00895                      escapeIndex = rootNode->getEscapeIndex();
00896                      rootNode += escapeIndex;
00897                      curIndex += escapeIndex;
00898               }
00899        }
00900        if (maxIterations < walkIterations)
00901               maxIterations = walkIterations;
00902 
00903 }
00904 
00905 void   btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const
00906 {
00907        btAssert(m_useQuantization);
00908        
00909        int curIndex = startNodeIndex;
00910        int walkIterations = 0;
00911        int subTreeSize = endNodeIndex - startNodeIndex;
00912 
00913        const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex];
00914        int escapeIndex;
00915        
00916        bool isLeafNode;
00917        //PCK: unsigned instead of bool
00918        unsigned aabbOverlap;
00919 
00920        while (curIndex < endNodeIndex)
00921        {
00922 
00923 //#define VISUALLY_ANALYZE_BVH 1
00924 #ifdef VISUALLY_ANALYZE_BVH
00925               //some code snippet to debugDraw aabb, to visually analyze bvh structure
00926               static int drawPatch = 0;
00927               //need some global access to a debugDrawer
00928               extern btIDebugDraw* debugDrawerPtr;
00929               if (curIndex==drawPatch)
00930               {
00931                      btVector3 aabbMin,aabbMax;
00932                      aabbMin = unQuantize(rootNode->m_quantizedAabbMin);
00933                      aabbMax = unQuantize(rootNode->m_quantizedAabbMax);
00934                      btVector3     color(1,0,0);
00935                      debugDrawerPtr->drawAabb(aabbMin,aabbMax,color);
00936               }
00937 #endif//VISUALLY_ANALYZE_BVH
00938 
00939               //catch bugs in tree data
00940               assert (walkIterations < subTreeSize);
00941 
00942               walkIterations++;
00943               //PCK: unsigned instead of bool
00944               aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
00945               isLeafNode = rootNode->isLeafNode();
00946               
00947               if (isLeafNode && aabbOverlap)
00948               {
00949                      nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
00950               } 
00951               
00952               //PCK: unsigned instead of bool
00953               if ((aabbOverlap != 0) || isLeafNode)
00954               {
00955                      rootNode++;
00956                      curIndex++;
00957               } else
00958               {
00959                      escapeIndex = rootNode->getEscapeIndex();
00960                      rootNode += escapeIndex;
00961                      curIndex += escapeIndex;
00962               }
00963        }
00964        if (maxIterations < walkIterations)
00965               maxIterations = walkIterations;
00966 
00967 }
00968 
00969 //This traversal can be called from Playstation 3 SPU
00970 void   btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const
00971 {
00972        btAssert(m_useQuantization);
00973 
00974        int i;
00975 
00976 
00977        for (i=0;i<this->m_SubtreeHeaders.size();i++)
00978        {
00979               const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
00980 
00981               //PCK: unsigned instead of bool
00982               unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
00983               if (overlap != 0)
00984               {
00985                      walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,
00986                             subtree.m_rootNodeIndex,
00987                             subtree.m_rootNodeIndex+subtree.m_subtreeSize);
00988               }
00989        }
00990 }
00991 
00992 
00993 void   btOptimizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
00994 {
00995        bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
00996        if (fast_path)
00997        {
00998               walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex);
00999        } else {
01000               /* Otherwise fallback to AABB overlap test */
01001               btVector3 aabbMin = raySource;
01002               btVector3 aabbMax = raySource;
01003               aabbMin.setMin(rayTarget);
01004               aabbMax.setMax(rayTarget);
01005               reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax);
01006        }
01007 }
01008 
01009 
01010 void   btOptimizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
01011 {
01012        bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
01013        if (fast_path)
01014        {
01015               walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
01016        } else {
01017               /* Slow path:
01018                  Construct the bounding box for the entire box cast and send that down the tree */
01019               btVector3 qaabbMin = raySource;
01020               btVector3 qaabbMax = raySource;
01021               qaabbMin.setMin(rayTarget);
01022               qaabbMax.setMax(rayTarget);
01023               qaabbMin += aabbMin;
01024               qaabbMax += aabbMax;
01025               reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
01026        }
01027 }
01028 
01029 
01030 void   btOptimizedBvh::swapLeafNodes(int i,int splitIndex)
01031 {
01032        if (m_useQuantization)
01033        {
01034                      btQuantizedBvhNode tmp = m_quantizedLeafNodes[i];
01035                      m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex];
01036                      m_quantizedLeafNodes[splitIndex] = tmp;
01037        } else
01038        {
01039                      btOptimizedBvhNode tmp = m_leafNodes[i];
01040                      m_leafNodes[i] = m_leafNodes[splitIndex];
01041                      m_leafNodes[splitIndex] = tmp;
01042        }
01043 }
01044 
01045 void   btOptimizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex)
01046 {
01047        if (m_useQuantization)
01048        {
01049               m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex];
01050        } else
01051        {
01052               m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex];
01053        }
01054 }
01055 
01056 //PCK: include
01057 #include <new>
01058 
01059 //PCK: consts
01060 static const unsigned BVH_ALIGNMENT = 16;
01061 static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1;
01062 
01063 static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
01064 
01065 
01066 
01067 unsigned int btOptimizedBvh::getAlignmentSerializationPadding()
01068 {
01069        return BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT;
01070 }
01071 
01072 unsigned btOptimizedBvh::calculateSerializeBufferSize()
01073 {
01074        unsigned baseSize = sizeof(btOptimizedBvh) + getAlignmentSerializationPadding();
01075        baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount;
01076        if (m_useQuantization)
01077        {
01078               return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode);
01079        }
01080        return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode);
01081 }
01082 
01083 bool btOptimizedBvh::serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian)
01084 {
01085        assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
01086        m_subtreeHeaderCount = m_SubtreeHeaders.size();
01087 
01088 /*     if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
01089        {
01091               btAssert(0);
01092               return false;
01093        }
01094 */
01095 
01096        btOptimizedBvh *targetBvh = (btOptimizedBvh *)o_alignedDataBuffer;
01097 
01098        // construct the class so the virtual function table, etc will be set up
01099        // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor
01100        new (targetBvh) btOptimizedBvh;
01101 
01102        if (i_swapEndian)
01103        {
01104               targetBvh->m_curNodeIndex = btSwapEndian(m_curNodeIndex);
01105 
01106 
01107               btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin);
01108               btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax);
01109               btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization);
01110 
01111               targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode);
01112               targetBvh->m_subtreeHeaderCount = btSwapEndian(m_subtreeHeaderCount);
01113        }
01114        else
01115        {
01116               targetBvh->m_curNodeIndex = m_curNodeIndex;
01117               targetBvh->m_bvhAabbMin = m_bvhAabbMin;
01118               targetBvh->m_bvhAabbMax = m_bvhAabbMax;
01119               targetBvh->m_bvhQuantization = m_bvhQuantization;
01120               targetBvh->m_traversalMode = m_traversalMode;
01121               targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount;
01122        }
01123 
01124        targetBvh->m_useQuantization = m_useQuantization;
01125 
01126        unsigned char *nodeData = (unsigned char *)targetBvh;
01127        nodeData += sizeof(btOptimizedBvh);
01128        
01129        unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
01130        nodeData += sizeToAdd;
01131        
01132        int nodeCount = m_curNodeIndex;
01133 
01134        if (m_useQuantization)
01135        {
01136               targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
01137 
01138               if (i_swapEndian)
01139               {
01140                      for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
01141                      {
01142                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]);
01143                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]);
01144                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]);
01145 
01146                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]);
01147                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]);
01148                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]);
01149 
01150                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex);
01151                      }
01152               }
01153               else
01154               {
01155                      for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
01156                      {
01157        
01158                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0];
01159                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1];
01160                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2];
01161 
01162                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0];
01163                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1];
01164                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2];
01165 
01166                             targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex;
01167 
01168 
01169                      }
01170               }
01171               nodeData += sizeof(btQuantizedBvhNode) * nodeCount;
01172        }
01173        else
01174        {
01175               targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
01176 
01177               if (i_swapEndian)
01178               {
01179                      for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
01180                      {
01181                             btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg);
01182                             btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg);
01183 
01184                             targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex);
01185                             targetBvh->m_contiguousNodes[nodeIndex].m_subPart = btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart);
01186                             targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex);
01187                      }
01188               }
01189               else
01190               {
01191                      for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
01192                      {
01193                             targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg;
01194                             targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg;
01195 
01196                             targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex;
01197                             targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart;
01198                             targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex;
01199                      }
01200               }
01201               nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
01202        }
01203 
01204        sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
01205        nodeData += sizeToAdd;
01206 
01207        // Now serialize the subtree headers
01208        targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount);
01209        if (i_swapEndian)
01210        {
01211               for (int i = 0; i < m_subtreeHeaderCount; i++)
01212               {
01213                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
01214                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
01215                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
01216 
01217                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
01218                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
01219                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
01220 
01221                      targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex);
01222                      targetBvh->m_SubtreeHeaders[i].m_subtreeSize = btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize);
01223               }
01224        }
01225        else
01226        {
01227               for (int i = 0; i < m_subtreeHeaderCount; i++)
01228               {
01229                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
01230                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
01231                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
01232 
01233                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
01234                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
01235                      targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
01236 
01237                      targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex);
01238                      targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize);
01239                      targetBvh->m_SubtreeHeaders[i] = m_SubtreeHeaders[i];
01240               }
01241        }
01242 
01243        nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount;
01244 
01245        return true;
01246 }
01247 
01248 btOptimizedBvh *btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
01249 {
01250 
01251        if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0))
01252        {
01253               return NULL;
01254        }
01255        btOptimizedBvh *bvh = (btOptimizedBvh *)i_alignedDataBuffer;
01256 
01257        if (i_swapEndian)
01258        {
01259               bvh->m_curNodeIndex = btSwapEndian(bvh->m_curNodeIndex);
01260 
01261               btUnSwapVector3Endian(bvh->m_bvhAabbMin);
01262               btUnSwapVector3Endian(bvh->m_bvhAabbMax);
01263               btUnSwapVector3Endian(bvh->m_bvhQuantization);
01264 
01265               bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode);
01266               bvh->m_subtreeHeaderCount = btSwapEndian(bvh->m_subtreeHeaderCount);
01267        }
01268 
01269        unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize();
01270        btAssert(calculatedBufSize <= i_dataBufferSize);
01271 
01272        if (calculatedBufSize > i_dataBufferSize)
01273        {
01274               return NULL;
01275        }
01276 
01277        unsigned char *nodeData = (unsigned char *)bvh;
01278        nodeData += sizeof(btOptimizedBvh);
01279        
01280        unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
01281        nodeData += sizeToAdd;
01282        
01283        int nodeCount = bvh->m_curNodeIndex;
01284 
01285        // Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor
01286        // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor
01287        new (bvh) btOptimizedBvh(*bvh, false);
01288 
01289        if (bvh->m_useQuantization)
01290        {
01291               bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
01292 
01293               if (i_swapEndian)
01294               {
01295                      for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
01296                      {
01297                             bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]);
01298                             bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]);
01299                             bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]);
01300 
01301                             bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]);
01302                             bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]);
01303                             bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]);
01304 
01305                             bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex);
01306                      }
01307               }
01308               nodeData += sizeof(btQuantizedBvhNode) * nodeCount;
01309        }
01310        else
01311        {
01312               bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount);
01313 
01314               if (i_swapEndian)
01315               {
01316                      for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++)
01317                      {
01318                             btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg);
01319                             btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg);
01320                             
01321                             bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex);
01322                             bvh->m_contiguousNodes[nodeIndex].m_subPart = btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart);
01323                             bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex);
01324                      }
01325               }
01326               nodeData += sizeof(btOptimizedBvhNode) * nodeCount;
01327        }
01328 
01329        sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK;
01330        nodeData += sizeToAdd;
01331 
01332        // Now serialize the subtree headers
01333        bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount);
01334        if (i_swapEndian)
01335        {
01336               for (int i = 0; i < bvh->m_subtreeHeaderCount; i++)
01337               {
01338                      bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]);
01339                      bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]);
01340                      bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]);
01341 
01342                      bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]);
01343                      bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]);
01344                      bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]);
01345 
01346                      bvh->m_SubtreeHeaders[i].m_rootNodeIndex = btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex);
01347                      bvh->m_SubtreeHeaders[i].m_subtreeSize = btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize);
01348               }
01349        }
01350 
01351        return bvh;
01352 }
01353 
01354 // Constructor that prevents btVector3's default constructor from being called
01355 btOptimizedBvh::btOptimizedBvh(btOptimizedBvh &self, bool ownsMemory) :
01356 m_bvhAabbMin(self.m_bvhAabbMin),
01357 m_bvhAabbMax(self.m_bvhAabbMax),
01358 m_bvhQuantization(self.m_bvhQuantization)
01359 {
01360 
01361 
01362 }
01363 
01364