Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp

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

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 10.8 KB
RevLine 
[1963]1/*
2Bullet Continuous Collision Detection and Physics Library
[8351]3Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
[1963]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"
[8351]19#include "LinearMath/btSerializer.h"
[1963]20
[2430]21btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
[8351]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),
[1963]26m_collisionMargin(btScalar(0.)),
[8351]27m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
[1963]28{
29        m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
[2430]30
31        if (enableDynamicAabbTree)
32        {
33                void* mem = btAlignedAlloc(sizeof(btDbvt),16);
34                m_dynamicAabbTree = new(mem) btDbvt();
35                btAssert(mem==m_dynamicAabbTree);
36        }
[1963]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{
[2882]51        m_updateRevision++;
[1963]52        //m_childTransforms.push_back(localTransform);
53        //m_childShapes.push_back(shape);
54        btCompoundShapeChild child;
[8351]55        child.m_node = 0;
[1963]56        child.m_transform = localTransform;
57        child.m_childShape = shape;
58        child.m_childShapeType = shape->getShapeType();
59        child.m_childMargin = shape->getMargin();
60
[2882]61       
[1963]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);
[2882]80                int index = m_children.size();
[1963]81                child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
82        }
83
[2882]84        m_children.push_back(child);
85
[1963]86}
87
[8393]88void    btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb)
[2430]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);
[8351]98                //int index = m_children.size()-1;
[2430]99                m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
100        }
101
[8393]102        if (shouldRecalculateLocalAabb)
103        {
104                recalculateLocalAabb();
105        }
[2430]106}
107
[1963]108void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
109{
[2882]110        m_updateRevision++;
[1963]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);
[8351]117    if (m_dynamicAabbTree) 
118                m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
[1963]119        m_children.pop_back();
120
121}
122
[2430]123
124
[1963]125void btCompoundShape::removeChildShape(btCollisionShape* shape)
126{
[2882]127        m_updateRevision++;
[1963]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                {
[8351]134                        removeChildShapeByIndex(i);
[1963]135                }
136        }
137
138
139
140        recalculateLocalAabb();
141}
142
143void btCompoundShape::recalculateLocalAabb()
144{
145        // Recalculate the local aabb
146        // Brute force, it iterates over all the shapes left.
[2882]147
[8351]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));
[1963]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
167void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
168{
169        btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
[2882]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        }
[1963]178        localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
[2882]179               
[1963]180
181        btMatrix3x3 abs_b = trans.getBasis().absolute(); 
182
[2430]183        btVector3 center = trans(localCenter);
[1963]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;
[2882]190       
[1963]191}
192
193void    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
216void 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);
[2430]222        int k;
223
224        for (k = 0; k < n; k++)
[1963]225        {
[8351]226                btAssert(masses[k]>0);
[1963]227                center += m_children[k].m_transform.getOrigin() * masses[k];
228                totalMass += masses[k];
229        }
[8351]230
231        btAssert(totalMass>0);
232
[1963]233        center /= totalMass;
234        principal.setOrigin(center);
235
236        btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
[2430]237        for ( k = 0; k < n; k++)
[1963]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
[8351]278void 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);
[8393]289                updateChildTransform(i, childTrans,false);
[8351]290        }
[8393]291       
[8351]292        m_localScaling = scaling;
[8393]293        recalculateLocalAabb();
294
[8351]295}
296
297
298void 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)
322const 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
Note: See TracBrowser for help on using the repository browser.