== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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_transform = localTransform;
56         child.m_childShape = shape;
57         child.m_childShapeType = shape->getShapeType();
58         child.m_childMargin = shape->getMargin();
59
60         
61         //extend the local aabbMin/aabbMax
62         btVector3 localAabbMin,localAabbMax;
63         shape->getAabb(localTransform,localAabbMin,localAabbMax);
64         for (int i=0;i<3;i++)
65         {
66                 if (m_localAabbMin[i] > localAabbMin[i])
67                 {
68                         m_localAabbMin[i] = localAabbMin[i];
69                 }
70                 if (m_localAabbMax[i] < localAabbMax[i])
71                 {
72                         m_localAabbMax[i] = localAabbMax[i];
73                 }
74
75         }
76         if (m_dynamicAabbTree)
77         {
78                 const btDbvtVolume      bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
79                 int index = m_children.size();
80                 child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
81         }
82
83         m_children.push_back(child);
84
85 }
86
87 void    btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform)
88 {
89         m_children[childIndex].m_transform = newChildTransform;
90
91         if (m_dynamicAabbTree)
92         {
93                 ///update the dynamic aabb tree
94                 btVector3 localAabbMin,localAabbMax;
95                 m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
96                 ATTRIBUTE_ALIGNED16(btDbvtVolume)       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         recalculateLocalAabb();
102 }
103
104 void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
105 {
106         m_updateRevision++;
107         btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
108         if (m_dynamicAabbTree)
109         {
110                 m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
111         }
112         m_children.swap(childShapeIndex,m_children.size()-1);
113         m_children.pop_back();
114
115 }
116
117
118
119 void btCompoundShape::removeChildShape(btCollisionShape* shape)
120 {
121         m_updateRevision++;
122         // Find the children containing the shape specified, and remove those children.
123         //note: there might be multiple children using the same shape!
124         for(int i = m_children.size()-1; i >= 0 ; i--)
125         {
126                 if(m_children[i].m_childShape == shape)
127                 {
128                         removeChildShapeByIndex(i);
129                 }
130         }
131
132
133
134         recalculateLocalAabb();
135 }
136
137 void btCompoundShape::recalculateLocalAabb()
138 {
139         // Recalculate the local aabb
140         // Brute force, it iterates over all the shapes left.
141
142         m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
143         m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
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
160 ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
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
175         btMatrix3x3 abs_b = trans.getBasis().absolute();  
176
177         btVector3 center = trans(localCenter);
178
179         btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
180                 abs_b[1].dot(localHalfExtents),
181                 abs_b[2].dot(localHalfExtents));
182         aabbMin = center-extent;
183         aabbMax = center+extent;
184         
185 }
186
187 void    btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
188 {
189         //approximation: take the inertia from the aabb for now
190         btTransform ident;
191         ident.setIdentity();
192         btVector3 aabbMin,aabbMax;
193         getAabb(ident,aabbMin,aabbMax);
194
195         btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
196
197         btScalar lx=btScalar(2.)*(halfExtents.x());
198         btScalar ly=btScalar(2.)*(halfExtents.y());
199         btScalar lz=btScalar(2.)*(halfExtents.z());
200
201         inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
202         inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
203         inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
204
205 }
206
207
208
209
210 void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
211 {
212         int n = m_children.size();
213
214         btScalar totalMass = 0;
215         btVector3 center(0, 0, 0);
216         int k;
217
218         for (k = 0; k < n; k++)
219         {
220                 center += m_children[k].m_transform.getOrigin() * masses[k];
221                 totalMass += masses[k];
222         }
223         center /= totalMass;
224         principal.setOrigin(center);
225
226         btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
227         for ( k = 0; k < n; k++)
228         {
229                 btVector3 i;
230                 m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
231
232                 const btTransform& t = m_children[k].m_transform;
233                 btVector3 o = t.getOrigin() - center;
234
235                 //compute inertia tensor in coordinate system of compound shape
236                 btMatrix3x3 j = t.getBasis().transpose();
237                 j[0] *= i[0];
238                 j[1] *= i[1];
239                 j[2] *= i[2];
240                 j = t.getBasis() * j;
241
242                 //add inertia tensor
243                 tensor[0] += j[0];
244                 tensor[1] += j[1];
245                 tensor[2] += j[2];
246
247                 //compute inertia tensor of pointmass at o
248                 btScalar o2 = o.length2();
249                 j[0].setValue(o2, 0, 0);
250                 j[1].setValue(0, o2, 0);
251                 j[2].setValue(0, 0, o2);
252                 j[0] += o * -o.x(); 
253                 j[1] += o * -o.y(); 
254                 j[2] += o * -o.z();
255
256                 //add inertia tensor of pointmass
257                 tensor[0] += masses[k] * j[0];
258                 tensor[1] += masses[k] * j[1];
259                 tensor[2] += masses[k] * j[2];
260         }
261
262         tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
263         inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
264 }
265
266
267
268 void btCompoundShape::setLocalScaling(const btVector3& scaling)
269 {
270
271         for(int i = 0; i < m_children.size(); i++)
272         {
273                 btTransform childTrans = getChildTransform(i);
274                 btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
275 //              childScale = childScale * (childTrans.getBasis() * scaling);
276                 childScale = childScale * scaling / m_localScaling;
277                 m_children[i].m_childShape->setLocalScaling(childScale);
278                 childTrans.setOrigin((childTrans.getOrigin())*scaling);
279                 updateChildTransform(i, childTrans);
280                 recalculateLocalAabb();
281         }
282         m_localScaling = scaling;
283 }
284
285
286
287
288 ///fills the dataBuffer and returns the struct name (and 0 on failure)
289 const char*     btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
290 {
291
292         btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
293         btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
294
295         shapeData->m_collisionMargin = float(m_collisionMargin);
296         shapeData->m_numChildShapes = m_children.size();
297         shapeData->m_childShapePtr = 0;
298         if (shapeData->m_numChildShapes)
299         {
300                 btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
301                 btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
302                 shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
303
304                 for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
305                 {
306                         memPtr->m_childMargin = float(m_children[i].m_childMargin);
307                         memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
308                         //don't serialize shapes that already have been serialized
309                         if (!serializer->findPointer(m_children[i].m_childShape))
310                         {
311                                 btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
312                                 const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
313                                 serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
314                         } 
315
316                         memPtr->m_childShapeType = m_children[i].m_childShapeType;
317                         m_children[i].m_transform.serializeFloat(memPtr->m_transform);
318                 }
319                 serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
320         }
321         return "btCompoundShapeData";
322 }
323