Bullet Collision Detection & Physics Library
btOptimizedBvh.cpp
Go to the documentation of this file.
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 
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 
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 
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 
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 }
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4