btOptimizedBvh.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
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 
00017 #include "btOptimizedBvh.h"
00018 #include "btStridingMeshInterface.h"
00019 #include "LinearMath/btAabbUtil2.h"
00020 #include "LinearMath/btIDebugDraw.h"
00021 
00022 
00023 btOptimizedBvh::btOptimizedBvh()
00024 { 
00025 }
00026 
00027 btOptimizedBvh::~btOptimizedBvh()
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.copyFromArray(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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00060                         aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
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 btQuantizedBvh* m_optimizedTree; // for quantization
00084 
00085                 QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
00086                 {
00087                         m_triangleNodes.copyFromArray(other.m_triangleNodes);
00088                         m_optimizedTree = other.m_optimizedTree;
00089                         return *this;
00090                 }
00091 
00092                 QuantizedNodeTriangleCallback(QuantizedNodeArray&       triangleNodes,const btQuantizedBvh* 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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00108                         aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
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(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
00172                 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
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::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
00207 {
00208         if (m_useQuantization)
00209         {
00210 
00211                 setQuantizationValues(aabbMin,aabbMax);
00212 
00213                 updateBvhNodes(meshInterface,0,m_curNodeIndex,0);
00214 
00216 
00217                 int i;
00218                 for (i=0;i<m_SubtreeHeaders.size();i++)
00219                 {
00220                         btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
00221                         subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
00222                 }
00223 
00224         } else
00225         {
00226 
00227         }
00228 }
00229 
00230 
00231 
00232 
00233 void    btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
00234 {
00235         //incrementally initialize quantization values
00236         btAssert(m_useQuantization);
00237 
00238         btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
00239         btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
00240         btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
00241 
00242         btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
00243         btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
00244         btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
00245 
00248         
00249         unsigned short  quantizedQueryAabbMin[3];
00250         unsigned short  quantizedQueryAabbMax[3];
00251 
00252         quantize(&quantizedQueryAabbMin[0],aabbMin,0);
00253         quantize(&quantizedQueryAabbMax[0],aabbMax,1);
00254 
00255         int i;
00256         for (i=0;i<this->m_SubtreeHeaders.size();i++)
00257         {
00258                 btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
00259 
00260                 //PCK: unsigned instead of bool
00261                 unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
00262                 if (overlap != 0)
00263                 {
00264                         updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
00265 
00266                         subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
00267                 }
00268         }
00269         
00270 }
00271 
00272 void    btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
00273 {
00274         (void)index;
00275 
00276         btAssert(m_useQuantization);
00277 
00278         int curNodeSubPart=-1;
00279 
00280         //get access info to trianglemesh data
00281                 const unsigned char *vertexbase = 0;
00282                 int numverts = 0;
00283                 PHY_ScalarType type = PHY_INTEGER;
00284                 int stride = 0;
00285                 const unsigned char *indexbase = 0;
00286                 int indexstride = 0;
00287                 int numfaces = 0;
00288                 PHY_ScalarType indicestype = PHY_INTEGER;
00289 
00290                 btVector3       triangleVerts[3];
00291                 btVector3       aabbMin,aabbMax;
00292                 const btVector3& meshScaling = meshInterface->getScaling();
00293                 
00294                 int i;
00295                 for (i=endNode-1;i>=firstNode;i--)
00296                 {
00297 
00298 
00299                         btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
00300                         if (curNode.isLeafNode())
00301                         {
00302                                 //recalc aabb from triangle data
00303                                 int nodeSubPart = curNode.getPartId();
00304                                 int nodeTriangleIndex = curNode.getTriangleIndex();
00305                                 if (nodeSubPart != curNodeSubPart)
00306                                 {
00307                                         if (curNodeSubPart >= 0)
00308                                                 meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
00309                                         meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,   type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
00310 
00311                                         curNodeSubPart = nodeSubPart;
00312                                         btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
00313                                 }
00314                                 //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
00315 
00316                                 unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
00317                                 
00318                                 
00319                                 for (int j=2;j>=0;j--)
00320                                 {
00321                                         
00322                                         int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
00323                                         if (type == PHY_FLOAT)
00324                                         {
00325                                                 float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
00326                                                 triangleVerts[j] = btVector3(
00327                                                         graphicsbase[0]*meshScaling.getX(),
00328                                                         graphicsbase[1]*meshScaling.getY(),
00329                                                         graphicsbase[2]*meshScaling.getZ());
00330                                         }
00331                                         else
00332                                         {
00333                                                 double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
00334                                                 triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
00335                                         }
00336                                 }
00337 
00338 
00339                                 
00340                                 aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00341                                 aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); 
00342                                 aabbMin.setMin(triangleVerts[0]);
00343                                 aabbMax.setMax(triangleVerts[0]);
00344                                 aabbMin.setMin(triangleVerts[1]);
00345                                 aabbMax.setMax(triangleVerts[1]);
00346                                 aabbMin.setMin(triangleVerts[2]);
00347                                 aabbMax.setMax(triangleVerts[2]);
00348 
00349                                 quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
00350                                 quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
00351                                 
00352                         } else
00353                         {
00354                                 //combine aabb from both children
00355 
00356                                 btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
00357                                 
00358                                 btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
00359                                         &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
00360                                 
00361 
00362                                 {
00363                                         for (int i=0;i<3;i++)
00364                                         {
00365                                                 curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
00366                                                 if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
00367                                                         curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
00368 
00369                                                 curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
00370                                                 if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
00371                                                         curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
00372                                         }
00373                                 }
00374                         }
00375 
00376                 }
00377 
00378                 if (curNodeSubPart >= 0)
00379                         meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
00380 
00381                 
00382 }
00383 
00385 btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
00386 {
00387         btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
00388         
00389         //we don't add additional data so just do a static upcast
00390         return static_cast<btOptimizedBvh*>(bvh);
00391 }