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"
20 
21 btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity)
24  m_dynamicAabbTree(0),
25  m_updateRevision(1),
26  m_collisionMargin(btScalar(0.)),
27  m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.))
28 {
30 
31  if (enableDynamicAabbTree)
32  {
33  void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
34  m_dynamicAabbTree = new (mem) btDbvt();
36  }
37 
38  m_children.reserve(initialChildCapacity);
39 }
40 
42 {
44  {
47  }
48 }
49 
50 void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape)
51 {
53  //m_childTransforms.push_back(localTransform);
54  //m_childShapes.push_back(shape);
56  child.m_node = 0;
57  child.m_transform = localTransform;
58  child.m_childShape = shape;
59  child.m_childShapeType = shape->getShapeType();
60  child.m_childMargin = shape->getMargin();
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  }
77  {
78  const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
79  size_t index = m_children.size();
80  child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index));
81  }
82 
83  m_children.push_back(child);
84 }
85 
86 void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb)
87 {
88  m_children[childIndex].m_transform = newChildTransform;
89 
91  {
93  btVector3 localAabbMin, localAabbMax;
94  m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax);
96  bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
97  //int index = m_children.size()-1;
98  m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds);
99  }
100 
101  if (shouldRecalculateLocalAabb)
102  {
104  }
105 }
106 
108 {
110  btAssert(childShapeIndex >= 0 && childShapeIndex < m_children.size());
111  if (m_dynamicAabbTree)
112  {
113  m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
114  }
115  m_children.swap(childShapeIndex, m_children.size() - 1);
116  if (m_dynamicAabbTree)
117  m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
119 }
120 
122 {
124  // Find the children containing the shape specified, and remove those children.
125  //note: there might be multiple children using the same shape!
126  for (int i = m_children.size() - 1; i >= 0; i--)
127  {
128  if (m_children[i].m_childShape == shape)
129  {
131  }
132  }
133 
135 }
136 
138 {
139  // Recalculate the local aabb
140  // Brute force, it iterates over all the shapes left.
141 
144 
145  //extend the local aabbMin/aabbMax
146  for (int j = 0; j < m_children.size(); j++)
147  {
148  btVector3 localAabbMin, localAabbMax;
149  m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
150  for (int i = 0; i < 3; i++)
151  {
152  if (m_localAabbMin[i] > localAabbMin[i])
153  m_localAabbMin[i] = localAabbMin[i];
154  if (m_localAabbMax[i] < localAabbMax[i])
155  m_localAabbMax[i] = localAabbMax[i];
156  }
157  }
158 }
159 
161 void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const
162 {
163  btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin);
164  btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin);
165 
166  //avoid an illegal AABB when there are no children
167  if (!m_children.size())
168  {
169  localHalfExtents.setValue(0, 0, 0);
170  localCenter.setValue(0, 0, 0);
171  }
172  localHalfExtents += btVector3(getMargin(), getMargin(), getMargin());
173 
174  btMatrix3x3 abs_b = trans.getBasis().absolute();
175 
176  btVector3 center = trans(localCenter);
177 
178  btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
179  aabbMin = center - extent;
180  aabbMax = center + extent;
181 }
182 
184 {
185  //approximation: take the inertia from the aabb for now
186  btTransform ident;
187  ident.setIdentity();
188  btVector3 aabbMin, aabbMax;
189  getAabb(ident, aabbMin, aabbMax);
190 
191  btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
192 
193  btScalar lx = btScalar(2.) * (halfExtents.x());
194  btScalar ly = btScalar(2.) * (halfExtents.y());
195  btScalar lz = btScalar(2.) * (halfExtents.z());
196 
197  inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz);
198  inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz);
199  inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly);
200 }
201 
203 {
204  int n = m_children.size();
205 
206  btScalar totalMass = 0;
207  btVector3 center(0, 0, 0);
208  int k;
209 
210  for (k = 0; k < n; k++)
211  {
212  btAssert(masses[k] > 0);
213  center += m_children[k].m_transform.getOrigin() * masses[k];
214  totalMass += masses[k];
215  }
216 
217  btAssert(totalMass > 0);
218 
219  center /= totalMass;
220  principal.setOrigin(center);
221 
222  btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
223  for (k = 0; k < n; k++)
224  {
225  btVector3 i;
226  m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
227 
228  const btTransform& t = m_children[k].m_transform;
229  btVector3 o = t.getOrigin() - center;
230 
231  //compute inertia tensor in coordinate system of compound shape
232  btMatrix3x3 j = t.getBasis().transpose();
233  j[0] *= i[0];
234  j[1] *= i[1];
235  j[2] *= i[2];
236  j = t.getBasis() * j;
237 
238  //add inertia tensor
239  tensor[0] += j[0];
240  tensor[1] += j[1];
241  tensor[2] += j[2];
242 
243  //compute inertia tensor of pointmass at o
244  btScalar o2 = o.length2();
245  j[0].setValue(o2, 0, 0);
246  j[1].setValue(0, o2, 0);
247  j[2].setValue(0, 0, o2);
248  j[0] += o * -o.x();
249  j[1] += o * -o.y();
250  j[2] += o * -o.z();
251 
252  //add inertia tensor of pointmass
253  tensor[0] += masses[k] * j[0];
254  tensor[1] += masses[k] * j[1];
255  tensor[2] += masses[k] * j[2];
256  }
257 
258  tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
259  inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
260 }
261 
263 {
264  for (int i = 0; i < m_children.size(); i++)
265  {
266  btTransform childTrans = getChildTransform(i);
267  btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
268  // childScale = childScale * (childTrans.getBasis() * scaling);
269  childScale = childScale * scaling / m_localScaling;
270  m_children[i].m_childShape->setLocalScaling(childScale);
271  childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
272  updateChildTransform(i, childTrans, false);
273  }
274 
275  m_localScaling = scaling;
277 }
278 
280 {
281  if (!m_dynamicAabbTree)
282  {
283  void* mem = btAlignedAlloc(sizeof(btDbvt), 16);
284  m_dynamicAabbTree = new (mem) btDbvt();
285  btAssert(mem == m_dynamicAabbTree);
286 
287  for (int index = 0; index < m_children.size(); index++)
288  {
289  btCompoundShapeChild& child = m_children[index];
290 
291  //extend the local aabbMin/aabbMax
292  btVector3 localAabbMin, localAabbMax;
293  child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax);
294 
295  const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
296  size_t index2 = index;
297  child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2));
298  }
299  }
300 }
301 
303 const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
304 {
305  btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer;
306  btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
307 
308  shapeData->m_collisionMargin = float(m_collisionMargin);
309  shapeData->m_numChildShapes = m_children.size();
310  shapeData->m_childShapePtr = 0;
311  if (shapeData->m_numChildShapes)
312  {
313  btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes);
315  shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
316 
317  for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++)
318  {
319  memPtr->m_childMargin = float(m_children[i].m_childMargin);
320  memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
321  //don't serialize shapes that already have been serialized
322  if (!serializer->findPointer(m_children[i].m_childShape))
323  {
324  btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1);
325  const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer);
326  serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape);
327  }
328 
329  memPtr->m_childShapeType = m_children[i].m_childShapeType;
330  m_children[i].m_transform.serializeFloat(memPtr->m_transform);
331  }
332  serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr);
333  }
334  return "btCompoundShapeData";
335 }
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
void calculatePrincipalAxisTransform(const btScalar *masses, btTransform &principal, btVector3 &inertia) const
computes the exact moment of inertia and the transform from the coordinate system defined by the prin...
btVector3 m_localAabbMax
void push_back(const T &_Val)
virtual void recalculateLocalAabb()
Re-calculate the local Aabb.
btCompoundShapeChildData * m_childShapePtr
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
~btDbvt()
Definition: btDbvt.cpp:470
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btDbvt * m_dynamicAabbTree
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:720
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb&#39;s default implementation is brute force, expected derived classes to implement a fast dedicat...
btAlignedObjectArray< btCompoundShapeChild > m_children
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
virtual void * getUniquePointer(void *oldPtr)=0
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
#define btAssert(x)
Definition: btScalar.h:133
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
Definition: btDbvt.h:198
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:534
btCollisionShape * m_childShape
void addChildShape(const btTransform &localTransform, btCollisionShape *shape)
btCompoundShape(bool enableDynamicAabbTree=true, const int initialChildCapacity=0)
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btCollisionShapeData m_collisionShapeData
struct btDbvtNode * m_node
void swap(int index0, int index1)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t...
int m_updateRevision
increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be up...
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1026
btVector3 m_localAabbMin
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:543
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void diagonalize(btMatrix3x3 &rot, btScalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: btMatrix3x3.h:693
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btTransform & getChildTransform(int index)
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:434
btTransformFloatData m_transform
#define btAlignedFree(ptr)
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3 m_localScaling
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
Definition: btMatrix3x3.h:195
btScalar m_collisionMargin
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:84
int size() const
return the number of elements in the array
virtual ~btCompoundShape()
void updateChildTransform(int childIndex, const btTransform &newChildTransform, bool shouldRecalculateLocalAabb=true)
set a new transform for a child, and update internal data structures (local aabb and dynamic tree) ...
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
void createAabbTreeFromChildren()
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:1005
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
virtual btScalar getMargin() const =0
virtual void setLocalScaling(const btVector3 &scaling)
#define btAlignedAlloc(size, alignment)
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:298
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void removeChildShapeByIndex(int childShapeindex)
int getShapeType() const
virtual void removeChildShape(btCollisionShape *shape)
Remove all children shapes that contain the specified shape.
virtual void * findPointer(void *oldPtr)=0
btCollisionShapeData * m_childShape
#define BT_SHAPE_CODE
Definition: btSerializer.h:117
virtual btScalar getMargin() const
void * m_oldPtr
Definition: btSerializer.h:52
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void remove(btDbvtNode *leaf)
Definition: btDbvt.cpp:610
virtual btChunk * allocate(size_t size, int numElements)=0
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294