Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp @ 7983

Last change on this file since 7983 was 7983, checked in by rgrieder, 14 years ago

Updated Bullet Physics Engine from v2.74 to v2.77

  • Property svn:eol-style set to native
File size: 10.7 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
21btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
22: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
23m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
24m_dynamicAabbTree(0),
25m_updateRevision(1),
26m_collisionMargin(btScalar(0.)),
27m_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
40btCompoundShape::~btCompoundShape()
41{
42        if (m_dynamicAabbTree)
43        {
44                m_dynamicAabbTree->~btDbvt();
45                btAlignedFree(m_dynamicAabbTree);
46        }
47}
48
49void    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
88void    btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform)
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        recalculateLocalAabb();
103}
104
105void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
106{
107        m_updateRevision++;
108        btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
109        if (m_dynamicAabbTree)
110        {
111                m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
112        }
113        m_children.swap(childShapeIndex,m_children.size()-1);
114    if (m_dynamicAabbTree) 
115                m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
116        m_children.pop_back();
117
118}
119
120
121
122void btCompoundShape::removeChildShape(btCollisionShape* shape)
123{
124        m_updateRevision++;
125        // Find the children containing the shape specified, and remove those children.
126        //note: there might be multiple children using the same shape!
127        for(int i = m_children.size()-1; i >= 0 ; i--)
128        {
129                if(m_children[i].m_childShape == shape)
130                {
131                        removeChildShapeByIndex(i);
132                }
133        }
134
135
136
137        recalculateLocalAabb();
138}
139
140void btCompoundShape::recalculateLocalAabb()
141{
142        // Recalculate the local aabb
143        // Brute force, it iterates over all the shapes left.
144
145        m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
146        m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
147
148        //extend the local aabbMin/aabbMax
149        for (int j = 0; j < m_children.size(); j++)
150        {
151                btVector3 localAabbMin,localAabbMax;
152                m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
153                for (int i=0;i<3;i++)
154                {
155                        if (m_localAabbMin[i] > localAabbMin[i])
156                                m_localAabbMin[i] = localAabbMin[i];
157                        if (m_localAabbMax[i] < localAabbMax[i])
158                                m_localAabbMax[i] = localAabbMax[i];
159                }
160        }
161}
162
163///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
164void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
165{
166        btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
167        btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
168       
169        //avoid an illegal AABB when there are no children
170        if (!m_children.size())
171        {
172                localHalfExtents.setValue(0,0,0);
173                localCenter.setValue(0,0,0);
174        }
175        localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
176               
177
178        btMatrix3x3 abs_b = trans.getBasis().absolute(); 
179
180        btVector3 center = trans(localCenter);
181
182        btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
183                abs_b[1].dot(localHalfExtents),
184                abs_b[2].dot(localHalfExtents));
185        aabbMin = center-extent;
186        aabbMax = center+extent;
187       
188}
189
190void    btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
191{
192        //approximation: take the inertia from the aabb for now
193        btTransform ident;
194        ident.setIdentity();
195        btVector3 aabbMin,aabbMax;
196        getAabb(ident,aabbMin,aabbMax);
197
198        btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
199
200        btScalar lx=btScalar(2.)*(halfExtents.x());
201        btScalar ly=btScalar(2.)*(halfExtents.y());
202        btScalar lz=btScalar(2.)*(halfExtents.z());
203
204        inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
205        inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
206        inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
207
208}
209
210
211
212
213void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
214{
215        int n = m_children.size();
216
217        btScalar totalMass = 0;
218        btVector3 center(0, 0, 0);
219        int k;
220
221        for (k = 0; k < n; k++)
222        {
223                btAssert(masses[k]>0);
224                center += m_children[k].m_transform.getOrigin() * masses[k];
225                totalMass += masses[k];
226        }
227
228        btAssert(totalMass>0);
229
230        center /= totalMass;
231        principal.setOrigin(center);
232
233        btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
234        for ( k = 0; k < n; k++)
235        {
236                btVector3 i;
237                m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
238
239                const btTransform& t = m_children[k].m_transform;
240                btVector3 o = t.getOrigin() - center;
241
242                //compute inertia tensor in coordinate system of compound shape
243                btMatrix3x3 j = t.getBasis().transpose();
244                j[0] *= i[0];
245                j[1] *= i[1];
246                j[2] *= i[2];
247                j = t.getBasis() * j;
248
249                //add inertia tensor
250                tensor[0] += j[0];
251                tensor[1] += j[1];
252                tensor[2] += j[2];
253
254                //compute inertia tensor of pointmass at o
255                btScalar o2 = o.length2();
256                j[0].setValue(o2, 0, 0);
257                j[1].setValue(0, o2, 0);
258                j[2].setValue(0, 0, o2);
259                j[0] += o * -o.x(); 
260                j[1] += o * -o.y(); 
261                j[2] += o * -o.z();
262
263                //add inertia tensor of pointmass
264                tensor[0] += masses[k] * j[0];
265                tensor[1] += masses[k] * j[1];
266                tensor[2] += masses[k] * j[2];
267        }
268
269        tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
270        inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
271}
272
273
274
275void btCompoundShape::setLocalScaling(const btVector3& scaling)
276{
277
278        for(int i = 0; i < m_children.size(); i++)
279        {
280                btTransform childTrans = getChildTransform(i);
281                btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
282//              childScale = childScale * (childTrans.getBasis() * scaling);
283                childScale = childScale * scaling / m_localScaling;
284                m_children[i].m_childShape->setLocalScaling(childScale);
285                childTrans.setOrigin((childTrans.getOrigin())*scaling);
286                updateChildTransform(i, childTrans);
287                recalculateLocalAabb();
288        }
289        m_localScaling = scaling;
290}
291
292
293void btCompoundShape::createAabbTreeFromChildren()
294{
295    if ( !m_dynamicAabbTree )
296    {
297        void* mem = btAlignedAlloc(sizeof(btDbvt),16);
298        m_dynamicAabbTree = new(mem) btDbvt();
299        btAssert(mem==m_dynamicAabbTree);
300
301        for ( int index = 0; index < m_children.size(); index++ )
302        {
303            btCompoundShapeChild &child = m_children[index];
304
305            //extend the local aabbMin/aabbMax
306            btVector3 localAabbMin,localAabbMax;
307            child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
308
309            const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
310            child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
311        }
312    }
313}
314
315
316///fills the dataBuffer and returns the struct name (and 0 on failure)
317const char*     btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
318{
319
320        btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
321        btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
322
323        shapeData->m_collisionMargin = float(m_collisionMargin);
324        shapeData->m_numChildShapes = m_children.size();
325        shapeData->m_childShapePtr = 0;
326        if (shapeData->m_numChildShapes)
327        {
328                btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
329                btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
330                shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
331
332                for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
333                {
334                        memPtr->m_childMargin = float(m_children[i].m_childMargin);
335                        memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
336                        //don't serialize shapes that already have been serialized
337                        if (!serializer->findPointer(m_children[i].m_childShape))
338                        {
339                                btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
340                                const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
341                                serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
342                        } 
343
344                        memPtr->m_childShapeType = m_children[i].m_childShapeType;
345                        m_children[i].m_transform.serializeFloat(memPtr->m_transform);
346                }
347                serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
348        }
349        return "btCompoundShapeData";
350}
351
Note: See TracBrowser for help on using the repository browser.