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