Upgrade Bullet to version 2.83.
[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, 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
42 btCompoundShape::~btCompoundShape()
43 {
44         if (m_dynamicAabbTree)
45         {
46                 m_dynamicAabbTree->~btDbvt();
47                 btAlignedFree(m_dynamicAabbTree);
48         }
49 }
50
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
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
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
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
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
169 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
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
279
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
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)
325 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