4eb860c57f18c939cda562c6e5c7640670465f35
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionShapes / btCompoundShape.cpp
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         {
94                 ///update the dynamic aabb tree
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
166 ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
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 = btVector3(abs_b[0].dot(localHalfExtents),
186                 abs_b[1].dot(localHalfExtents),
187                 abs_b[2].dot(localHalfExtents));
188         aabbMin = center-extent;
189         aabbMax = center+extent;
190         
191 }
192
193 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
216 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 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);
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
321 ///fills the dataBuffer and returns the struct name (and 0 on failure)
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