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
b3OptimizedBvh()20 b3OptimizedBvh::b3OptimizedBvh()
21 {
22 }
23
~b3OptimizedBvh()24 b3OptimizedBvh::~b3OptimizedBvh()
25 {
26 }
27
build(b3StridingMeshInterface * triangles,bool useQuantizedAabbCompression,const b3Vector3 & bvhAabbMin,const b3Vector3 & bvhAabbMax)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
refit(b3StridingMeshInterface * meshInterface,const b3Vector3 & aabbMin,const b3Vector3 & aabbMax)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
refitPartial(b3StridingMeshInterface * meshInterface,const b3Vector3 & aabbMin,const b3Vector3 & aabbMax)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
updateBvhNodes(b3StridingMeshInterface * meshInterface,int firstNode,int endNode,int index)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 b3Assert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT);
289 }
290 //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
291
292 unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride);
293
294 for (int j = 2; j >= 0; j--)
295 {
296 int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j];
297 if (type == PHY_FLOAT)
298 {
299 float* graphicsbase = (float*)(vertexbase + graphicsindex * stride);
300 triangleVerts[j] = b3MakeVector3(
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 triangleVerts[j] = b3MakeVector3(b3Scalar(graphicsbase[0] * meshScaling.getX()), b3Scalar(graphicsbase[1] * meshScaling.getY()), b3Scalar(graphicsbase[2] * meshScaling.getZ()));
309 }
310 }
311
312 aabbMin.setValue(b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT));
313 aabbMax.setValue(b3Scalar(-B3_LARGE_FLOAT), b3Scalar(-B3_LARGE_FLOAT), b3Scalar(-B3_LARGE_FLOAT));
314 aabbMin.setMin(triangleVerts[0]);
315 aabbMax.setMax(triangleVerts[0]);
316 aabbMin.setMin(triangleVerts[1]);
317 aabbMax.setMax(triangleVerts[1]);
318 aabbMin.setMin(triangleVerts[2]);
319 aabbMax.setMax(triangleVerts[2]);
320
321 quantize(&curNode.m_quantizedAabbMin[0], aabbMin, 0);
322 quantize(&curNode.m_quantizedAabbMax[0], aabbMax, 1);
323 }
324 else
325 {
326 //combine aabb from both children
327
328 b3QuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i + 1];
329
330 b3QuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i + 2] : &m_quantizedContiguousNodes[i + 1 + leftChildNode->getEscapeIndex()];
331
332 {
333 for (int i = 0; i < 3; i++)
334 {
335 curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
336 if (curNode.m_quantizedAabbMin[i] > rightChildNode->m_quantizedAabbMin[i])
337 curNode.m_quantizedAabbMin[i] = rightChildNode->m_quantizedAabbMin[i];
338
339 curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
340 if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
341 curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
342 }
343 }
344 }
345 }
346
347 if (curNodeSubPart >= 0)
348 meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
349 }
350
351 ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place'
deSerializeInPlace(void * i_alignedDataBuffer,unsigned int i_dataBufferSize,bool i_swapEndian)352 b3OptimizedBvh* b3OptimizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian)
353 {
354 b3QuantizedBvh* bvh = b3QuantizedBvh::deSerializeInPlace(i_alignedDataBuffer, i_dataBufferSize, i_swapEndian);
355
356 //we don't add additional data so just do a static upcast
357 return static_cast<b3OptimizedBvh*>(bvh);
358 }
359