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