src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16
  17 #include "btOptimizedBvh.h"
  18 #include "btStridingMeshInterface.h"
  19 #include "LinearMath/btAabbUtil2.h"
  20 #include "LinearMath/btIDebugDraw.h"
  21
  22
  23 btOptimizedBvh::btOptimizedBvh()
  24 {
  25 }
  26
  27 btOptimizedBvh::~btOptimizedBvh()
  28 {
  29 }
  30
  31
  32 void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax)
  33 {
  34         m_useQuantization = useQuantizedAabbCompression;
  35
  36
  37         // NodeArray    triangleNodes;
  38
  39         struct  NodeTriangleCallback : public btInternalTriangleIndexCallback
  40         {
  41
  42                 NodeArray&      m_triangleNodes;
  43
  44                 NodeTriangleCallback& operator=(NodeTriangleCallback& other)
  45                 {
  46                         m_triangleNodes.copyFromArray(other.m_triangleNodes);
  47                         return *this;
  48                 }
  49
  50                 NodeTriangleCallback(NodeArray& triangleNodes)
  51                         :m_triangleNodes(triangleNodes)
  52                 {
  53                 }
  54
  55                 virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
  56                 {
  57                         btOptimizedBvhNode node;
  58                         btVector3       aabbMin,aabbMax;
  59                         aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
  60                         aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
  61                         aabbMin.setMin(triangle[0]);
  62                         aabbMax.setMax(triangle[0]);
  63                         aabbMin.setMin(triangle[1]);
  64                         aabbMax.setMax(triangle[1]);
  65                         aabbMin.setMin(triangle[2]);
  66                         aabbMax.setMax(triangle[2]);
  67
  68                         //with quantization?
  69                         node.m_aabbMinOrg = aabbMin;
  70                         node.m_aabbMaxOrg = aabbMax;
  71
  72                         node.m_escapeIndex = -1;
  73
  74                         //for child nodes
  75                         node.m_subPart = partId;
  76                         node.m_triangleIndex = triangleIndex;
  77                         m_triangleNodes.push_back(node);
  78                 }
  79         };
  80         struct  QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
  81         {
  82                 QuantizedNodeArray&     m_triangleNodes;
  83                 const btQuantizedBvh* m_optimizedTree; // for quantization
  84
  85                 QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
  86                 {
  87                         m_triangleNodes.copyFromArray(other.m_triangleNodes);
  88                         m_optimizedTree = other.m_optimizedTree;
  89                         return *this;
  90                 }
  91
  92                 QuantizedNodeTriangleCallback(QuantizedNodeArray&       triangleNodes,const btQuantizedBvh* tree)
  93                         :m_triangleNodes(triangleNodes),m_optimizedTree(tree)
  94                 {
  95                 }
  96
  97                 virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
  98                 {
  99                         // The partId and triangle index must fit in the same (positive) integer
 100                         btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS));
 101                         btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS)));
 102                         //negative indices are reserved for escapeIndex
 103                         btAssert(triangleIndex>=0);
 104
 105                         btQuantizedBvhNode node;
 106                         btVector3       aabbMin,aabbMax;
 107                         aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
 108                         aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
 109                         aabbMin.setMin(triangle[0]);
 110                         aabbMax.setMax(triangle[0]);
 111                         aabbMin.setMin(triangle[1]);
 112                         aabbMax.setMax(triangle[1]);
 113                         aabbMin.setMin(triangle[2]);
 114                         aabbMax.setMax(triangle[2]);
 115
 116                         //PCK: add these checks for zero dimensions of aabb
 117                         const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
 118                         const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
 119                         if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
 120                         {
 121                                 aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
 122                                 aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
 123                         }
 124                         if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
 125                         {
 126                                 aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
 127                                 aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
 128                         }
 129                         if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
 130                         {
 131                                 aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
 132                                 aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
 133                         }
 134
 135                         m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
 136                         m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
 137
 138                         node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
 139
 140                         m_triangleNodes.push_back(node);
 141                 }
 142         };
 143
 144
 145
 146         int numLeafNodes = 0;
 147
 148
 149         if (m_useQuantization)
 150         {
 151
 152                 //initialize quantization values
 153                 setQuantizationValues(bvhAabbMin,bvhAabbMax);
 154
 155                 QuantizedNodeTriangleCallback   callback(m_quantizedLeafNodes,this);
 156
 157
 158                 triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
 159
 160                 //now we have an array of leafnodes in m_leafNodes
 161                 numLeafNodes = m_quantizedLeafNodes.size();
 162
 163
 164                 m_quantizedContiguousNodes.resize(2*numLeafNodes);
 165
 166
 167         } else
 168         {
 169                 NodeTriangleCallback    callback(m_leafNodes);
 170
 171                 btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
 172                 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
 173
 174                 triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
 175
 176                 //now we have an array of leafnodes in m_leafNodes
 177                 numLeafNodes = m_leafNodes.size();
 178
 179                 m_contiguousNodes.resize(2*numLeafNodes);
 180         }
 181
 182         m_curNodeIndex = 0;
 183
 184         buildTree(0,numLeafNodes);
 185
 186         ///if the entire tree is small then subtree size, we need to create a header info for the tree
 187         if(m_useQuantization && !m_SubtreeHeaders.size())
 188         {
 189                 btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
 190                 subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
 191                 subtree.m_rootNodeIndex = 0;
 192                 subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
 193         }
 194
 195         //PCK: update the copy of the size
 196         m_subtreeHeaderCount = m_SubtreeHeaders.size();
 197
 198         //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
 199         m_quantizedLeafNodes.clear();
 200         m_leafNodes.clear();
 201 }
 202
 203
 204
 205
 206 void    btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
 207 {
 208         if (m_useQuantization)
 209         {
 210
 211                 setQuantizationValues(aabbMin,aabbMax);
 212
 213                 updateBvhNodes(meshInterface,0,m_curNodeIndex,0);
 214
 215                 ///now update all subtree headers
 216
 217                 int i;
 218                 for (i=0;i<m_SubtreeHeaders.size();i++)
 219                 {
 220                         btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
 221                         subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
 222                 }
 223
 224         } else
 225         {
 226
 227         }
 228 }
 229
 230
 231
 232
 233 void    btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
 234 {
 235         //incrementally initialize quantization values
 236         btAssert(m_useQuantization);
 237
 238         btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
 239         btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
 240         btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
 241
 242         btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
 243         btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
 244         btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
 245
 246         ///we should update all quantization values, using updateBvhNodes(meshInterface);
 247         ///but we only update chunks that overlap the given aabb
 248
 249         unsigned short  quantizedQueryAabbMin[3];
 250         unsigned short  quantizedQueryAabbMax[3];
 251
 252         quantize(&quantizedQueryAabbMin[0],aabbMin,0);
 253         quantize(&quantizedQueryAabbMax[0],aabbMax,1);
 254
 255         int i;
 256         for (i=0;i<this->m_SubtreeHeaders.size();i++)
 257         {
 258                 btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
 259
 260                 //PCK: unsigned instead of bool
 261                 unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
 262                 if (overlap != 0)
 263                 {
 264                         updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
 265
 266                         subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
 267                 }
 268         }
 269
 270 }
 271
 272 void    btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
 273 {
 274         (void)index;
 275
 276         btAssert(m_useQuantization);
 277
 278         int curNodeSubPart=-1;
 279
 280         //get access info to trianglemesh data
 281                 const unsigned char *vertexbase = 0;
 282                 int numverts = 0;
 283                 PHY_ScalarType type = PHY_INTEGER;
 284                 int stride = 0;
 285                 const unsigned char *indexbase = 0;
 286                 int indexstride = 0;
 287                 int numfaces = 0;
 288                 PHY_ScalarType indicestype = PHY_INTEGER;
 289
 290                 btVector3       triangleVerts[3];
 291                 btVector3       aabbMin,aabbMax;
 292                 const btVector3& meshScaling = meshInterface->getScaling();
 293
 294                 int i;
 295                 for (i=endNode-1;i>=firstNode;i--)
 296                 {
 297
 298
 299                         btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
 300                         if (curNode.isLeafNode())
 301                         {
 302                                 //recalc aabb from triangle data
 303                                 int nodeSubPart = curNode.getPartId();
 304                                 int nodeTriangleIndex = curNode.getTriangleIndex();
 305                                 if (nodeSubPart != curNodeSubPart)
 306                                 {
 307                                         if (curNodeSubPart >= 0)
 308                                                 meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
 309                                         meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,   type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
 310
 311                                         curNodeSubPart = nodeSubPart;
 312                                         btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
 313                                 }
 314                                 //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
 315
 316                                 unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
 317
 318
 319                                 for (int j=2;j>=0;j--)
 320                                 {
 321
 322                                         int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
 323                                         if (type == PHY_FLOAT)
 324                                         {
 325                                                 float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
 326                                                 triangleVerts[j] = btVector3(
 327                                                         graphicsbase[0]*meshScaling.getX(),
 328                                                         graphicsbase[1]*meshScaling.getY(),
 329                                                         graphicsbase[2]*meshScaling.getZ());
 330                                         }
 331                                         else
 332                                         {
 333                                                 double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
 334                                                 triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
 335                                         }
 336                                 }
 337
 338
 339
 340                                 aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
 341                                 aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
 342                                 aabbMin.setMin(triangleVerts[0]);
 343                                 aabbMax.setMax(triangleVerts[0]);
 344                                 aabbMin.setMin(triangleVerts[1]);
 345                                 aabbMax.setMax(triangleVerts[1]);
 346                                 aabbMin.setMin(triangleVerts[2]);
 347                                 aabbMax.setMax(triangleVerts[2]);
 348
 349                                 quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
 350                                 quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
 351
 352                         } else
 353                         {
 354                                 //combine aabb from both children
 355
 356                                 btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
 357
 358                                 btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
 359                                         &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
 360
 361
 362                                 {
 363                                         for (int i=0;i<3;i++)
 364                                         {
 365                                                 curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
 366                                                 if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
 367                                                         curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
 368
 369                                                 curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
 370                                                 if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
 371                                                         curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
 372                                         }
 373                                 }
 374                         }
 375
 376                 }
 377
 378                 if (curNodeSubPart >= 0)
 379                         meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
 380
 381
 382 }
 383
 384 ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
 385 btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
 386 {
 387         btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian);
 388
 389         //we don't add additional data so just do a static upcast
 390         return static_cast<btOptimizedBvh*>(bvh);
 391 }