| /* | |
| Bullet Continuous Collision Detection and Physics Library | |
| Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 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. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #include "BulletCollision/CollisionShapes/btOptimizedBvh.h" | |
| #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" | |
| #include "LinearMath/btAabbUtil2.h" | |
| #include "LinearMath/btIDebugDraw.h" | |
| btOptimizedBvh::btOptimizedBvh() | |
| { | |
| } | |
| btOptimizedBvh::~btOptimizedBvh() | |
| { | |
| } | |
| void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax) | |
| { | |
| m_useQuantization = useQuantizedAabbCompression; | |
| // NodeArray triangleNodes; | |
| struct NodeTriangleCallback : public btInternalTriangleIndexCallback | |
| { | |
| NodeArray& m_triangleNodes; | |
| NodeTriangleCallback& operator=(NodeTriangleCallback& other) | |
| { | |
| m_triangleNodes = other.m_triangleNodes; | |
| return *this; | |
| } | |
| NodeTriangleCallback(NodeArray& triangleNodes) | |
| :m_triangleNodes(triangleNodes) | |
| { | |
| } | |
| virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) | |
| { | |
| btOptimizedBvhNode node; | |
| btVector3 aabbMin,aabbMax; | |
| aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); | |
| aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); | |
| aabbMin.setMin(triangle[0]); | |
| aabbMax.setMax(triangle[0]); | |
| aabbMin.setMin(triangle[1]); | |
| aabbMax.setMax(triangle[1]); | |
| aabbMin.setMin(triangle[2]); | |
| aabbMax.setMax(triangle[2]); | |
| //with quantization? | |
| node.m_aabbMinOrg = aabbMin; | |
| node.m_aabbMaxOrg = aabbMax; | |
| node.m_escapeIndex = -1; | |
| //for child nodes | |
| node.m_subPart = partId; | |
| node.m_triangleIndex = triangleIndex; | |
| m_triangleNodes.push_back(node); | |
| } | |
| }; | |
| struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback | |
| { | |
| QuantizedNodeArray& m_triangleNodes; | |
| const btQuantizedBvh* m_optimizedTree; // for quantization | |
| QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) | |
| { | |
| m_triangleNodes = other.m_triangleNodes; | |
| m_optimizedTree = other.m_optimizedTree; | |
| return *this; | |
| } | |
| QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree) | |
| :m_triangleNodes(triangleNodes),m_optimizedTree(tree) | |
| { | |
| } | |
| virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) | |
| { | |
| // The partId and triangle index must fit in the same (positive) integer | |
| btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS)); | |
| btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS))); | |
| //negative indices are reserved for escapeIndex | |
| btAssert(triangleIndex>=0); | |
| btQuantizedBvhNode node; | |
| btVector3 aabbMin,aabbMax; | |
| aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); | |
| aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); | |
| aabbMin.setMin(triangle[0]); | |
| aabbMax.setMax(triangle[0]); | |
| aabbMin.setMin(triangle[1]); | |
| aabbMax.setMax(triangle[1]); | |
| aabbMin.setMin(triangle[2]); | |
| aabbMax.setMax(triangle[2]); | |
| //PCK: add these checks for zero dimensions of aabb | |
| const btScalar MIN_AABB_DIMENSION = btScalar(0.002); | |
| const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001); | |
| if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION) | |
| { | |
| aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION); | |
| aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION); | |
| } | |
| if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION) | |
| { | |
| aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION); | |
| aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION); | |
| } | |
| if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION) | |
| { | |
| aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION); | |
| aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION); | |
| } | |
| m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); | |
| m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); | |
| node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; | |
| m_triangleNodes.push_back(node); | |
| } | |
| }; | |
| int numLeafNodes = 0; | |
| if (m_useQuantization) | |
| { | |
| //initialize quantization values | |
| setQuantizationValues(bvhAabbMin,bvhAabbMax); | |
| QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); | |
| triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); | |
| //now we have an array of leafnodes in m_leafNodes | |
| numLeafNodes = m_quantizedLeafNodes.size(); | |
| m_quantizedContiguousNodes.resize(2*numLeafNodes); | |
| } else | |
| { | |
| NodeTriangleCallback callback(m_leafNodes); | |
| btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); | |
| btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); | |
| triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); | |
| //now we have an array of leafnodes in m_leafNodes | |
| numLeafNodes = m_leafNodes.size(); | |
| m_contiguousNodes.resize(2*numLeafNodes); | |
| } | |
| m_curNodeIndex = 0; | |
| buildTree(0,numLeafNodes); | |
| ///if the entire tree is small then subtree size, we need to create a header info for the tree | |
| if(m_useQuantization && !m_SubtreeHeaders.size()) | |
| { | |
| btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); | |
| subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); | |
| subtree.m_rootNodeIndex = 0; | |
| subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); | |
| } | |
| //PCK: update the copy of the size | |
| m_subtreeHeaderCount = m_SubtreeHeaders.size(); | |
| //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary | |
| m_quantizedLeafNodes.clear(); | |
| m_leafNodes.clear(); | |
| } | |
| void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) | |
| { | |
| if (m_useQuantization) | |
| { | |
| setQuantizationValues(aabbMin,aabbMax); | |
| updateBvhNodes(meshInterface,0,m_curNodeIndex,0); | |
| ///now update all subtree headers | |
| int i; | |
| for (i=0;i<m_SubtreeHeaders.size();i++) | |
| { | |
| btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; | |
| subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); | |
| } | |
| } else | |
| { | |
| } | |
| } | |
| void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) | |
| { | |
| //incrementally initialize quantization values | |
| btAssert(m_useQuantization); | |
| btAssert(aabbMin.getX() > m_bvhAabbMin.getX()); | |
| btAssert(aabbMin.getY() > m_bvhAabbMin.getY()); | |
| btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ()); | |
| btAssert(aabbMax.getX() < m_bvhAabbMax.getX()); | |
| btAssert(aabbMax.getY() < m_bvhAabbMax.getY()); | |
| btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ()); | |
| ///we should update all quantization values, using updateBvhNodes(meshInterface); | |
| ///but we only update chunks that overlap the given aabb | |
| unsigned short quantizedQueryAabbMin[3]; | |
| unsigned short quantizedQueryAabbMax[3]; | |
| quantize(&quantizedQueryAabbMin[0],aabbMin,0); | |
| quantize(&quantizedQueryAabbMax[0],aabbMax,1); | |
| int i; | |
| for (i=0;i<this->m_SubtreeHeaders.size();i++) | |
| { | |
| btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; | |
| //PCK: unsigned instead of bool | |
| unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); | |
| if (overlap != 0) | |
| { | |
| updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); | |
| subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); | |
| } | |
| } | |
| } | |
| void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) | |
| { | |
| (void)index; | |
| btAssert(m_useQuantization); | |
| int curNodeSubPart=-1; | |
| //get access info to trianglemesh data | |
| const unsigned char *vertexbase = 0; | |
| int numverts = 0; | |
| PHY_ScalarType type = PHY_INTEGER; | |
| int stride = 0; | |
| const unsigned char *indexbase = 0; | |
| int indexstride = 0; | |
| int numfaces = 0; | |
| PHY_ScalarType indicestype = PHY_INTEGER; | |
| btVector3 triangleVerts[3]; | |
| btVector3 aabbMin,aabbMax; | |
| const btVector3& meshScaling = meshInterface->getScaling(); | |
| int i; | |
| for (i=endNode-1;i>=firstNode;i--) | |
| { | |
| btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; | |
| if (curNode.isLeafNode()) | |
| { | |
| //recalc aabb from triangle data | |
| int nodeSubPart = curNode.getPartId(); | |
| int nodeTriangleIndex = curNode.getTriangleIndex(); | |
| if (nodeSubPart != curNodeSubPart) | |
| { | |
| if (curNodeSubPart >= 0) | |
| meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); | |
| meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); | |
| curNodeSubPart = nodeSubPart; | |
| btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); | |
| } | |
| //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, | |
| unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); | |
| for (int j=2;j>=0;j--) | |
| { | |
| int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; | |
| if (type == PHY_FLOAT) | |
| { | |
| float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); | |
| triangleVerts[j] = btVector3( | |
| graphicsbase[0]*meshScaling.getX(), | |
| graphicsbase[1]*meshScaling.getY(), | |
| graphicsbase[2]*meshScaling.getZ()); | |
| } | |
| else | |
| { | |
| double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); | |
| triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); | |
| } | |
| } | |
| aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); | |
| aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); | |
| aabbMin.setMin(triangleVerts[0]); | |
| aabbMax.setMax(triangleVerts[0]); | |
| aabbMin.setMin(triangleVerts[1]); | |
| aabbMax.setMax(triangleVerts[1]); | |
| aabbMin.setMin(triangleVerts[2]); | |
| aabbMax.setMax(triangleVerts[2]); | |
| quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); | |
| quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1); | |
| } else | |
| { | |
| //combine aabb from both children | |
| btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; | |
| btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : | |
| &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; | |
| { | |
| for (int i=0;i<3;i++) | |
| { | |
| curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; | |
| if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i]) | |
| curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i]; | |
| curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; | |
| if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) | |
| curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; | |
| } | |
| } | |
| } | |
| } | |
| if (curNodeSubPart >= 0) | |
| meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); | |
| } | |
| ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' | |
| btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) | |
| { | |
| btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian); | |
| //we don't add additional data so just do a static upcast | |
| return static_cast<btOptimizedBvh*>(bvh); | |
| } |