dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.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 //#define DISABLE_BVH
  17
  18 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
  19 #include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
  20 #include "LinearMath/btSerializer.h"
  21
  22 ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
  23 ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
  24 btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh)
  25         : btTriangleMeshShape(meshInterface),
  26           m_bvh(0),
  27           m_triangleInfoMap(0),
  28           m_useQuantizedAabbCompression(useQuantizedAabbCompression),
  29           m_ownsBvh(false)
  30 {
  31         m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
  32         //construct bvh from meshInterface
  33 #ifndef DISABLE_BVH
  34
  35         if (buildBvh)
  36         {
  37                 buildOptimizedBvh();
  38         }
  39
  40 #endif  //DISABLE_BVH
  41 }
  42
  43 btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh)
  44         : btTriangleMeshShape(meshInterface),
  45           m_bvh(0),
  46           m_triangleInfoMap(0),
  47           m_useQuantizedAabbCompression(useQuantizedAabbCompression),
  48           m_ownsBvh(false)
  49 {
  50         m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE;
  51         //construct bvh from meshInterface
  52 #ifndef DISABLE_BVH
  53
  54         if (buildBvh)
  55         {
  56                 void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16);
  57                 m_bvh = new (mem) btOptimizedBvh();
  58
  59                 m_bvh->build(meshInterface, m_useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax);
  60                 m_ownsBvh = true;
  61         }
  62
  63 #endif  //DISABLE_BVH
  64 }
  65
  66 void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax)
  67 {
  68         m_bvh->refitPartial(m_meshInterface, aabbMin, aabbMax);
  69
  70         m_localAabbMin.setMin(aabbMin);
  71         m_localAabbMax.setMax(aabbMax);
  72 }
  73
  74 void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin, const btVector3& aabbMax)
  75 {
  76         m_bvh->refit(m_meshInterface, aabbMin, aabbMax);
  77
  78         recalcLocalAabb();
  79 }
  80
  81 btBvhTriangleMeshShape::~btBvhTriangleMeshShape()
  82 {
  83         if (m_ownsBvh)
  84         {
  85                 m_bvh->~btOptimizedBvh();
  86                 btAlignedFree(m_bvh);
  87         }
  88 }
  89
  90 void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget)
  91 {
  92         struct MyNodeOverlapCallback : public btNodeOverlapCallback
  93         {
  94                 btStridingMeshInterface* m_meshInterface;
  95                 btTriangleCallback* m_callback;
  96
  97                 MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
  98                         : m_meshInterface(meshInterface),
  99                           m_callback(callback)
 100                 {
 101                 }
 102
 103                 virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 104                 {
 105                         btVector3 m_triangle[3];
 106                         const unsigned char* vertexbase;
 107                         int numverts;
 108                         PHY_ScalarType type;
 109                         int stride;
 110                         const unsigned char* indexbase;
 111                         int indexstride;
 112                         int numfaces;
 113                         PHY_ScalarType indicestype;
 114
 115                         m_meshInterface->getLockedReadOnlyVertexIndexBase(
 116                                 &vertexbase,
 117                                 numverts,
 118                                 type,
 119                                 stride,
 120                                 &indexbase,
 121                                 indexstride,
 122                                 numfaces,
 123                                 indicestype,
 124                                 nodeSubPart);
 125
 126                         unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
 127
 128                         const btVector3& meshScaling = m_meshInterface->getScaling();
 129                         for (int j = 2; j >= 0; j--)
 130                         {
 131                                 int graphicsindex;
 132                                 switch (indicestype) {
 133                                         case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
 134                                         case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
 135                                         case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
 136                                         default: btAssert(0);
 137                                 }
 138
 139                                 if (type == PHY_FLOAT)
 140                                 {
 141                                         float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
 142
 143                                         m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
 144                                 }
 145                                 else
 146                                 {
 147                                         double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
 148
 149                                         m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ());
 150                                 }
 151                         }
 152
 153                         /* Perform ray vs. triangle collision here */
 154                         m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
 155                         m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 156                 }
 157         };
 158
 159         MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
 160
 161         m_bvh->reportRayOverlappingNodex(&myNodeCallback, raySource, rayTarget);
 162 }
 163
 164 void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax)
 165 {
 166         struct MyNodeOverlapCallback : public btNodeOverlapCallback
 167         {
 168                 btStridingMeshInterface* m_meshInterface;
 169                 btTriangleCallback* m_callback;
 170
 171                 MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
 172                         : m_meshInterface(meshInterface),
 173                           m_callback(callback)
 174                 {
 175                 }
 176
 177                 virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 178                 {
 179                         btVector3 m_triangle[3];
 180                         const unsigned char* vertexbase;
 181                         int numverts;
 182                         PHY_ScalarType type;
 183                         int stride;
 184                         const unsigned char* indexbase;
 185                         int indexstride;
 186                         int numfaces;
 187                         PHY_ScalarType indicestype;
 188
 189                         m_meshInterface->getLockedReadOnlyVertexIndexBase(
 190                                 &vertexbase,
 191                                 numverts,
 192                                 type,
 193                                 stride,
 194                                 &indexbase,
 195                                 indexstride,
 196                                 numfaces,
 197                                 indicestype,
 198                                 nodeSubPart);
 199
 200                         unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
 201
 202                         const btVector3& meshScaling = m_meshInterface->getScaling();
 203                         for (int j = 2; j >= 0; j--)
 204                         {
 205                                 int graphicsindex;
 206                                 switch (indicestype) {
 207                                         case PHY_INTEGER: graphicsindex = gfxbase[j]; break;
 208                                         case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break;
 209                                         case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break;
 210                                         default: btAssert(0);
 211                                 }
 212
 213                                 if (type == PHY_FLOAT)
 214                                 {
 215                                         float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
 216
 217                                         m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ());
 218                                 }
 219                                 else
 220                                 {
 221                                         double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
 222
 223                                         m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ());
 224                                 }
 225                         }
 226
 227                         /* Perform ray vs. triangle collision here */
 228                         m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
 229                         m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 230                 }
 231         };
 232
 233         MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
 234
 235         m_bvh->reportBoxCastOverlappingNodex(&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
 236 }
 237
 238 //perform bvh tree traversal and report overlapping triangles to 'callback'
 239 void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 240 {
 241 #ifdef DISABLE_BVH
 242         //brute force traverse all triangles
 243         btTriangleMeshShape::processAllTriangles(callback, aabbMin, aabbMax);
 244 #else
 245
 246         //first get all the nodes
 247
 248         struct MyNodeOverlapCallback : public btNodeOverlapCallback
 249         {
 250                 btStridingMeshInterface* m_meshInterface;
 251                 btTriangleCallback* m_callback;
 252                 btVector3 m_triangle[3];
 253                 int m_numOverlap;
 254
 255                 MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface)
 256                         : m_meshInterface(meshInterface),
 257                           m_callback(callback),
 258                           m_numOverlap(0)
 259                 {
 260                 }
 261
 262                 virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 263                 {
 264                         m_numOverlap++;
 265                         const unsigned char* vertexbase;
 266                         int numverts;
 267                         PHY_ScalarType type;
 268                         int stride;
 269                         const unsigned char* indexbase;
 270                         int indexstride;
 271                         int numfaces;
 272                         PHY_ScalarType indicestype;
 273
 274                         m_meshInterface->getLockedReadOnlyVertexIndexBase(
 275                                 &vertexbase,
 276                                 numverts,
 277                                 type,
 278                                 stride,
 279                                 &indexbase,
 280                                 indexstride,
 281                                 numfaces,
 282                                 indicestype,
 283                                 nodeSubPart);
 284
 285                         unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
 286                         btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT || indicestype == PHY_UCHAR);
 287
 288                         const btVector3& meshScaling = m_meshInterface->getScaling();
 289                         for (int j = 2; j >= 0; j--)
 290                         {
 291                                 int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : indicestype == PHY_INTEGER ? gfxbase[j] : ((unsigned char*)gfxbase)[j];
 292
 293 #ifdef DEBUG_TRIANGLE_MESH
 294                                 printf("%d ,", graphicsindex);
 295 #endif  //DEBUG_TRIANGLE_MESH
 296                                 if (type == PHY_FLOAT)
 297                                 {
 298                                         float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
 299
 300                                         m_triangle[j] = btVector3(
 301                                                 graphicsbase[0] * meshScaling.getX(),
 302                                                 graphicsbase[1] * meshScaling.getY(),
 303                                                 graphicsbase[2] * meshScaling.getZ());
 304                                 }
 305                                 else
 306                                 {
 307                                         double* graphicsbase = (double*)(vertexbase + graphicsindex * stride);
 308
 309                                         m_triangle[j] = btVector3(
 310                                                 btScalar(graphicsbase[0]) * meshScaling.getX(),
 311                                                 btScalar(graphicsbase[1]) * meshScaling.getY(),
 312                                                 btScalar(graphicsbase[2]) * meshScaling.getZ());
 313                                 }
 314 #ifdef DEBUG_TRIANGLE_MESH
 315                                 printf("triangle vertices:%f,%f,%f\n", triangle[j].x(), triangle[j].y(), triangle[j].z());
 316 #endif  //DEBUG_TRIANGLE_MESH
 317                         }
 318
 319                         m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex);
 320                         m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart);
 321                 }
 322         };
 323
 324         MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface);
 325
 326         m_bvh->reportAabbOverlappingNodex(&myNodeCallback, aabbMin, aabbMax);
 327
 328 #endif  //DISABLE_BVH
 329 }
 330
 331 void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 332 {
 333         if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON)
 334         {
 335                 btTriangleMeshShape::setLocalScaling(scaling);
 336                 buildOptimizedBvh();
 337         }
 338 }
 339
 340 void btBvhTriangleMeshShape::buildOptimizedBvh()
 341 {
 342         if (m_ownsBvh)
 343         {
 344                 m_bvh->~btOptimizedBvh();
 345                 btAlignedFree(m_bvh);
 346         }
 347         ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
 348         void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16);
 349         m_bvh = new (mem) btOptimizedBvh();
 350         //rebuild the bvh...
 351         m_bvh->build(m_meshInterface, m_useQuantizedAabbCompression, m_localAabbMin, m_localAabbMax);
 352         m_ownsBvh = true;
 353 }
 354
 355 void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling)
 356 {
 357         btAssert(!m_bvh);
 358         btAssert(!m_ownsBvh);
 359
 360         m_bvh = bvh;
 361         m_ownsBvh = false;
 362         // update the scaling without rebuilding the bvh
 363         if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON)
 364         {
 365                 btTriangleMeshShape::setLocalScaling(scaling);
 366         }
 367 }
 368
 369 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 370 const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
 371 {
 372         btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*)dataBuffer;
 373
 374         btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer);
 375
 376         m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer);
 377
 378         trimeshData->m_collisionMargin = float(m_collisionMargin);
 379
 380         if (m_bvh && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_BVH))
 381         {
 382                 void* chunk = serializer->findPointer(m_bvh);
 383                 if (chunk)
 384                 {
 385 #ifdef BT_USE_DOUBLE_PRECISION
 386                         trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk;
 387                         trimeshData->m_quantizedFloatBvh = 0;
 388 #else
 389                         trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk;
 390                         trimeshData->m_quantizedDoubleBvh = 0;
 391 #endif  //BT_USE_DOUBLE_PRECISION
 392                 }
 393                 else
 394                 {
 395 #ifdef BT_USE_DOUBLE_PRECISION
 396                         trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
 397                         trimeshData->m_quantizedFloatBvh = 0;
 398 #else
 399                         trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh);
 400                         trimeshData->m_quantizedDoubleBvh = 0;
 401 #endif  //BT_USE_DOUBLE_PRECISION
 402
 403                         int sz = m_bvh->calculateSerializeBufferSizeNew();
 404                         btChunk* chunk = serializer->allocate(sz, 1);
 405                         const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
 406                         serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, m_bvh);
 407                 }
 408         }
 409         else
 410         {
 411                 trimeshData->m_quantizedFloatBvh = 0;
 412                 trimeshData->m_quantizedDoubleBvh = 0;
 413         }
 414
 415         if (m_triangleInfoMap && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_TRIANGLEINFOMAP))
 416         {
 417                 void* chunk = serializer->findPointer(m_triangleInfoMap);
 418                 if (chunk)
 419                 {
 420                         trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk;
 421                 }
 422                 else
 423                 {
 424                         trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap);
 425                         int sz = m_triangleInfoMap->calculateSerializeBufferSize();
 426                         btChunk* chunk = serializer->allocate(sz, 1);
 427                         const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
 428                         serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, m_triangleInfoMap);
 429                 }
 430         }
 431         else
 432         {
 433                 trimeshData->m_triangleInfoMap = 0;
 434         }
 435
 436         // Fill padding with zeros to appease msan.
 437         memset(trimeshData->m_pad3, 0, sizeof(trimeshData->m_pad3));
 438
 439         return "btTriangleMeshShapeData";
 440 }
 441
 442 void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const
 443 {
 444         if (m_bvh)
 445         {
 446                 int len = m_bvh->calculateSerializeBufferSizeNew();  //make sure not to use calculateSerializeBufferSize because it is used for in-place
 447                 btChunk* chunk = serializer->allocate(len, 1);
 448                 const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer);
 449                 serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, (void*)m_bvh);
 450         }
 451 }
 452
 453 void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const
 454 {
 455         if (m_triangleInfoMap)
 456         {
 457                 int len = m_triangleInfoMap->calculateSerializeBufferSize();
 458                 btChunk* chunk = serializer->allocate(len, 1);
 459                 const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer);
 460                 serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, (void*)m_triangleInfoMap);
 461         }
 462 }