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 } 392
