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 }
btCollisionShapeData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btCollisionShape.h:151
btCompoundShapeChildData::m_childShape
btCollisionShapeData * m_childShape
Definition: btCompoundShape.h:182
btAlignedFree
#define btAlignedFree(ptr)
Definition: btAlignedAllocator.h:47
btCollisionShape.h
btCollisionShape::getShapeType
int getShapeType() const
Definition: btCollisionShape.h:106
btCompoundShape::recalculateLocalAabb
virtual void recalculateLocalAabb()
Re-calculate the local Aabb.
Definition: btCompoundShape.cpp:137
btCompoundShape::btCompoundShape
btCompoundShape(bool enableDynamicAabbTree=true, const int initialChildCapacity=0)
Definition: btCompoundShape.cpp:21
btVector3::dot3
btVector3 dot3(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:720
btCompoundShape::getChildTransform
btTransform & getChildTransform(int index)
Definition: btCompoundShape.h:100
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btCompoundShapeChild::m_node
struct btDbvtNode * m_node
Definition: btCompoundShape.h:39
btCompoundShape.h
btCompoundShape::getMargin
virtual btScalar getMargin() const
Definition: btCompoundShape.h:137
btCompoundShapeChildData::m_transform
btTransformFloatData m_transform
Definition: btCompoundShape.h:181
btCompoundShape::removeChildShapeByIndex
void removeChildShapeByIndex(int childShapeindex)
Definition: btCompoundShape.cpp:107
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
btCompoundShape::m_updateRevision
int m_updateRevision
increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be up...
Definition: btCompoundShape.h:66
btChunk
Definition: btSerializer.h:47
btMatrix3x3::diagonalize
void diagonalize(btMatrix3x3 &rot, btScalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: btMatrix3x3.h:693
btCompoundShape::removeChildShape
virtual void removeChildShape(btCollisionShape *shape)
Remove all children shapes that contain the specified shape.
Definition: btCompoundShape.cpp:121
BT_SHAPE_CODE
#define BT_SHAPE_CODE
Definition: btSerializer.h:117
btCompoundShapeChild::m_transform
btTransform m_transform
Definition: btCompoundShape.h:35
btAlignedAlloc
#define btAlignedAlloc(size, alignment)
Definition: btAlignedAllocator.h:46
btCompoundShape::~btCompoundShape
virtual ~btCompoundShape()
Definition: btCompoundShape.cpp:41
btDbvt::~btDbvt
~btDbvt()
Definition: btDbvt.cpp:470
btCompoundShape::getAabb
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...
Definition: btCompoundShape.cpp:161
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btCompoundShape::calculateLocalInertia
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
Definition: btCompoundShape.cpp:183
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btCollisionShape::m_shapeType
int m_shapeType
Definition: btCollisionShape.h:30
btCompoundShape::serialize
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btCompoundShape.cpp:303
btDbvt::update
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:543
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
Definition: btCollisionShape.h:26
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btCollisionShape::getMargin
virtual btScalar getMargin() const =0
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:296
btAlignedObjectArray::swap
void swap(int index0, int index1)
Definition: btAlignedObjectArray.h:389
btDbvt
The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes ...
Definition: btDbvt.h:198
btCompoundShapeChild::m_childShapeType
int m_childShapeType
Definition: btCompoundShape.h:37
btCompoundShape::createAabbTreeFromChildren
void createAabbTreeFromChildren()
Definition: btCompoundShape.cpp:279
bounds
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:298
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btCompoundShapeData::m_collisionMargin
float m_collisionMargin
Definition: btCompoundShape.h:196
btCompoundShape::m_localAabbMax
btVector3 m_localAabbMax
Definition: btCompoundShape.h:61
btAlignedObjectArray::pop_back
void pop_back()
Definition: btAlignedObjectArray.h:192
btCollisionShape::getAabb
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.
btCompoundShapeData::m_childShapePtr
btCompoundShapeChildData * m_childShapePtr
Definition: btCompoundShape.h:192
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1026
btCompoundShape::calculatePrincipalAxisTransform
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...
Definition: btCompoundShape.cpp:202
btSerializer.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btCompoundShape::setLocalScaling
virtual void setLocalScaling(const btVector3 &scaling)
Definition: btCompoundShape.cpp:262
btCompoundShapeData::m_numChildShapes
int m_numChildShapes
Definition: btCompoundShape.h:194
btCompoundShape::m_children
btAlignedObjectArray< btCompoundShapeChild > m_children
Definition: btCompoundShape.h:59
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btCompoundShape::m_localScaling
btVector3 m_localScaling
Definition: btCompoundShape.h:70
btDbvtAabbMm::FromMM
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:434
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:52
btCompoundShape::m_dynamicAabbTree
btDbvt * m_dynamicAabbTree
Definition: btCompoundShape.h:63
ATTRIBUTE_ALIGNED16
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:84
btCollisionShape::serialize
virtual const char * serialize(void *dataBuffer, btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btCollisionShape.cpp:96
btMatrix3x3::setValue
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
btCompoundShapeChild::m_childShape
btCollisionShape * m_childShape
Definition: btCompoundShape.h:36
btCompoundShapeChildData::m_childShapeType
int m_childShapeType
Definition: btCompoundShape.h:183
btSerializer
Definition: btSerializer.h:65
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btCompoundShape::updateChildTransform
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)
Definition: btCompoundShape.cpp:86
BT_ARRAY_CODE
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
btCompoundShape::addChildShape
void addChildShape(const btTransform &localTransform, btCollisionShape *shape)
Definition: btCompoundShape.cpp:50
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition: btAlignedObjectArray.h:287
btCompoundShape::m_collisionMargin
btScalar m_collisionMargin
Definition: btCompoundShape.h:68
btCompoundShapeChild::m_childMargin
btScalar m_childMargin
Definition: btCompoundShape.h:38
btCompoundShape::m_localAabbMin
btVector3 m_localAabbMin
Definition: btCompoundShape.h:60
btCompoundShapeChild
Definition: btCompoundShape.h:30
btDbvtAabbMm
Definition: btDbvt.h:132
btMatrix3x3::absolute
btMatrix3x3 absolute() const
Return the matrix with all values non negative.
Definition: btMatrix3x3.h:1005
btDbvt::insert
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:534
btSerializer::findPointer
virtual void * findPointer(void *oldPtr)=0
btCompoundShapeData::m_collisionShapeData
btCollisionShapeData m_collisionShapeData
Definition: btCompoundShape.h:190
btCompoundShapeChildData::m_childMargin
float m_childMargin
Definition: btCompoundShape.h:184
btCompoundShapeData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btCompoundShape.h:188
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:264
btDbvt::remove
void remove(btDbvtNode *leaf)
Definition: btDbvt.cpp:610
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btDbvt.h
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btCompoundShapeChildData
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btCompoundShape.h:179
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
COMPOUND_SHAPE_PROXYTYPE
Definition: btBroadphaseProxy.h:71