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 }
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
@ COMPOUND_SHAPE_PROXYTYPE
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:299
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
#define btAssert(x)
Definition: btScalar.h:153
#define BT_SHAPE_CODE
Definition: btSerializer.h:117
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
int size() const
return the number of elements in the array
void swap(int index0, int index1)
void push_back(const T &_Val)
void * m_oldPtr
Definition: btSerializer.h:52
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
int getShapeType() const
virtual btScalar getMargin() const =0
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.
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
virtual void setLocalScaling(const btVector3 &scaling)
btCompoundShape(bool enableDynamicAabbTree=true, const int initialChildCapacity=0)
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
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
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void createAabbTreeFromChildren()
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)
virtual ~btCompoundShape()
btAlignedObjectArray< btCompoundShapeChild > m_children
virtual void removeChildShape(btCollisionShape *shape)
Remove all children shapes that contain the specified shape.
btTransform & getChildTransform(int index)
void removeChildShapeByIndex(int childShapeindex)
btVector3 m_localScaling
btScalar m_collisionMargin
int m_updateRevision
increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be up...
void addChildShape(const btTransform &localTransform, btCollisionShape *shape)
btDbvt * m_dynamicAabbTree
btVector3 m_localAabbMin
virtual btScalar getMargin() const
virtual void recalculateLocalAabb()
Re-calculate the local Aabb.
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
void diagonalize(btMatrix3x3 &rot, btScalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: btMatrix3x3.h:716
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:204
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:1028
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void * findPointer(void *oldPtr)=0
virtual void * getUniquePointer(void *oldPtr)=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
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 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:720
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
btTransformFloatData m_transform
btCollisionShapeData * m_childShape
struct btDbvtNode * m_node
btCollisionShape * m_childShape
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
btCompoundShapeChildData * m_childShapePtr
btCollisionShapeData m_collisionShapeData
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
Definition: btDbvt.h:229
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:535
~btDbvt()
Definition: btDbvt.cpp:471
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:544
void remove(btDbvtNode *leaf)
Definition: btDbvt.cpp:611