[1963] | 1 | /* |
---|
| 2 | Bullet Continuous Collision Detection and Physics Library |
---|
[8351] | 3 | Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org |
---|
[1963] | 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 | |
---|
[8351] | 16 | |
---|
[1963] | 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 = 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; |
---|
[8351] | 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)); |
---|
[1963] | 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 = 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; |
---|
[8351] | 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)); |
---|
[1963] | 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 | |
---|
[8351] | 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)); |
---|
[1963] | 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]; |
---|
[2430] | 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 | } |
---|
[1963] | 336 | } |
---|
| 337 | |
---|
| 338 | |
---|
| 339 | |
---|
[8351] | 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)); |
---|
[1963] | 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 | } |
---|