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