Bullet Collision Detection & Physics Library
btCompoundShape.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 #include "btCompoundShape.h"
17 #include "btCollisionShape.h"
18 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
19 #include "LinearMath/btSerializer.h"
20 
21 btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
22 : m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
23 m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
24 m_dynamicAabbTree(0),
25 m_updateRevision(1),
26 m_collisionMargin(btScalar(0.)),
27 m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
28 {
29  m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
30 
31  if (enableDynamicAabbTree)
32  {
33  void* mem = btAlignedAlloc(sizeof(btDbvt),16);
34  m_dynamicAabbTree = new(mem) btDbvt();
35  btAssert(mem==m_dynamicAabbTree);
36  }
37 }
38 
39 
40 btCompoundShape::~btCompoundShape()
41 {
42  if (m_dynamicAabbTree)
43  {
44  m_dynamicAabbTree->~btDbvt();
45  btAlignedFree(m_dynamicAabbTree);
46  }
47 }
48 
49 void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
50 {
51  m_updateRevision++;
52  //m_childTransforms.push_back(localTransform);
53  //m_childShapes.push_back(shape);
54  btCompoundShapeChild child;
55  child.m_node = 0;
56  child.m_transform = localTransform;
57  child.m_childShape = shape;
58  child.m_childShapeType = shape->getShapeType();
59  child.m_childMargin = shape->getMargin();
60 
61 
62  //extend the local aabbMin/aabbMax
63  btVector3 localAabbMin,localAabbMax;
64  shape->getAabb(localTransform,localAabbMin,localAabbMax);
65  for (int i=0;i<3;i++)
66  {
67  if (m_localAabbMin[i] > localAabbMin[i])
68  {
69  m_localAabbMin[i] = localAabbMin[i];
70  }
71  if (m_localAabbMax[i] < localAabbMax[i])
72  {
73  m_localAabbMax[i] = localAabbMax[i];
74  }
75 
76  }
77  if (m_dynamicAabbTree)
78  {
79  const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
80  int index = m_children.size();
81  child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
82  }
83 
84  m_children.push_back(child);
85 
86 }
87 
88 void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb)
89 {
90  m_children[childIndex].m_transform = newChildTransform;
91 
92  if (m_dynamicAabbTree)
93  {
95  btVector3 localAabbMin,localAabbMax;
96  m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
97  ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
98  //int index = m_children.size()-1;
99  m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
100  }
101 
102  if (shouldRecalculateLocalAabb)
103  {
104  recalculateLocalAabb();
105  }
106 }
107 
108 void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
109 {
110  m_updateRevision++;
111  btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
112  if (m_dynamicAabbTree)
113  {
114  m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
115  }
116  m_children.swap(childShapeIndex,m_children.size()-1);
117  if (m_dynamicAabbTree)
118  m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
119  m_children.pop_back();
120 
121 }
122 
123 
124 
125 void btCompoundShape::removeChildShape(btCollisionShape* shape)
126 {
127  m_updateRevision++;
128  // Find the children containing the shape specified, and remove those children.
129  //note: there might be multiple children using the same shape!
130  for(int i = m_children.size()-1; i >= 0 ; i--)
131  {
132  if(m_children[i].m_childShape == shape)
133  {
134  removeChildShapeByIndex(i);
135  }
136  }
137 
138 
139 
140  recalculateLocalAabb();
141 }
142 
143 void btCompoundShape::recalculateLocalAabb()
144 {
145  // Recalculate the local aabb
146  // Brute force, it iterates over all the shapes left.
147 
148  m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
149  m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
150 
151  //extend the local aabbMin/aabbMax
152  for (int j = 0; j < m_children.size(); j++)
153  {
154  btVector3 localAabbMin,localAabbMax;
155  m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
156  for (int i=0;i<3;i++)
157  {
158  if (m_localAabbMin[i] > localAabbMin[i])
159  m_localAabbMin[i] = localAabbMin[i];
160  if (m_localAabbMax[i] < localAabbMax[i])
161  m_localAabbMax[i] = localAabbMax[i];
162  }
163  }
164 }
165 
167 void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
168 {
169  btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
170  btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
171 
172  //avoid an illegal AABB when there are no children
173  if (!m_children.size())
174  {
175  localHalfExtents.setValue(0,0,0);
176  localCenter.setValue(0,0,0);
177  }
178  localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
179 
180 
181  btMatrix3x3 abs_b = trans.getBasis().absolute();
182 
183  btVector3 center = trans(localCenter);
184 
185  btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
186  aabbMin = center-extent;
187  aabbMax = center+extent;
188 
189 }
190 
191 void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
192 {
193  //approximation: take the inertia from the aabb for now
194  btTransform ident;
195  ident.setIdentity();
196  btVector3 aabbMin,aabbMax;
197  getAabb(ident,aabbMin,aabbMax);
198 
199  btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
200 
201  btScalar lx=btScalar(2.)*(halfExtents.x());
202  btScalar ly=btScalar(2.)*(halfExtents.y());
203  btScalar lz=btScalar(2.)*(halfExtents.z());
204 
205  inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
206  inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
207  inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
208 
209 }
210 
211 
212 
213 
214 void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
215 {
216  int n = m_children.size();
217 
218  btScalar totalMass = 0;
219  btVector3 center(0, 0, 0);
220  int k;
221 
222  for (k = 0; k < n; k++)
223  {
224  btAssert(masses[k]>0);
225  center += m_children[k].m_transform.getOrigin() * masses[k];
226  totalMass += masses[k];
227  }
228 
229  btAssert(totalMass>0);
230 
231  center /= totalMass;
232  principal.setOrigin(center);
233 
234  btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
235  for ( k = 0; k < n; k++)
236  {
237  btVector3 i;
238  m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
239 
240  const btTransform& t = m_children[k].m_transform;
241  btVector3 o = t.getOrigin() - center;
242 
243  //compute inertia tensor in coordinate system of compound shape
244  btMatrix3x3 j = t.getBasis().transpose();
245  j[0] *= i[0];
246  j[1] *= i[1];
247  j[2] *= i[2];
248  j = t.getBasis() * j;
249 
250  //add inertia tensor
251  tensor[0] += j[0];
252  tensor[1] += j[1];
253  tensor[2] += j[2];
254 
255  //compute inertia tensor of pointmass at o
256  btScalar o2 = o.length2();
257  j[0].setValue(o2, 0, 0);
258  j[1].setValue(0, o2, 0);
259  j[2].setValue(0, 0, o2);
260  j[0] += o * -o.x();
261  j[1] += o * -o.y();
262  j[2] += o * -o.z();
263 
264  //add inertia tensor of pointmass
265  tensor[0] += masses[k] * j[0];
266  tensor[1] += masses[k] * j[1];
267  tensor[2] += masses[k] * j[2];
268  }
269 
270  tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
271  inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
272 }
273 
274 
275 
276 
277 
278 void btCompoundShape::setLocalScaling(const btVector3& scaling)
279 {
280 
281  for(int i = 0; i < m_children.size(); i++)
282  {
283  btTransform childTrans = getChildTransform(i);
284  btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
285 // childScale = childScale * (childTrans.getBasis() * scaling);
286  childScale = childScale * scaling / m_localScaling;
287  m_children[i].m_childShape->setLocalScaling(childScale);
288  childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
289  updateChildTransform(i, childTrans,false);
290  }
291 
292  m_localScaling = scaling;
293  recalculateLocalAabb();
294 
295 }
296 
297 
298 void btCompoundShape::createAabbTreeFromChildren()
299 {
300  if ( !m_dynamicAabbTree )
301  {
302  void* mem = btAlignedAlloc(sizeof(btDbvt),16);
303  m_dynamicAabbTree = new(mem) btDbvt();
304  btAssert(mem==m_dynamicAabbTree);
305 
306  for ( int index = 0; index < m_children.size(); index++ )
307  {
308  btCompoundShapeChild &child = m_children[index];
309 
310  //extend the local aabbMin/aabbMax
311  btVector3 localAabbMin,localAabbMax;
312  child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
313 
314  const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
315  child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
316  }
317  }
318 }
319 
320 
322 const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
323 {
324 
325  btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
326  btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
327 
328  shapeData->m_collisionMargin = float(m_collisionMargin);
329  shapeData->m_numChildShapes = m_children.size();
330  shapeData->m_childShapePtr = 0;
331  if (shapeData->m_numChildShapes)
332  {
333  btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
334  btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
335  shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
336 
337  for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
338  {
339  memPtr->m_childMargin = float(m_children[i].m_childMargin);
340  memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
341  //don't serialize shapes that already have been serialized
342  if (!serializer->findPointer(m_children[i].m_childShape))
343  {
344  btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
345  const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
346  serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
347  }
348 
349  memPtr->m_childShapeType = m_children[i].m_childShapeType;
350  m_children[i].m_transform.serializeFloat(memPtr->m_transform);
351  }
352  serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
353  }
354  return "btCompoundShapeData";
355 }
356 
Generated on Wed Apr 9 2014 02:38:22 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2