Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 7306 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 7.8 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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
20btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
21: m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)),
22m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)),
23m_collisionMargin(btScalar(0.)),
24m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
25m_dynamicAabbTree(0),
26m_updateRevision(1)
27{
28        m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
29
30        if (enableDynamicAabbTree)
31        {
32                void* mem = btAlignedAlloc(sizeof(btDbvt),16);
33                m_dynamicAabbTree = new(mem) btDbvt();
34                btAssert(mem==m_dynamicAabbTree);
35        }
36}
37
38
39btCompoundShape::~btCompoundShape()
40{
41        if (m_dynamicAabbTree)
42        {
43                m_dynamicAabbTree->~btDbvt();
44                btAlignedFree(m_dynamicAabbTree);
45        }
46}
47
48void    btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
49{
50        m_updateRevision++;
51        //m_childTransforms.push_back(localTransform);
52        //m_childShapes.push_back(shape);
53        btCompoundShapeChild child;
54        child.m_transform = localTransform;
55        child.m_childShape = shape;
56        child.m_childShapeType = shape->getShapeType();
57        child.m_childMargin = shape->getMargin();
58
59       
60        //extend the local aabbMin/aabbMax
61        btVector3 localAabbMin,localAabbMax;
62        shape->getAabb(localTransform,localAabbMin,localAabbMax);
63        for (int i=0;i<3;i++)
64        {
65                if (m_localAabbMin[i] > localAabbMin[i])
66                {
67                        m_localAabbMin[i] = localAabbMin[i];
68                }
69                if (m_localAabbMax[i] < localAabbMax[i])
70                {
71                        m_localAabbMax[i] = localAabbMax[i];
72                }
73
74        }
75        if (m_dynamicAabbTree)
76        {
77                const btDbvtVolume      bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
78                int index = m_children.size();
79                child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
80        }
81
82        m_children.push_back(child);
83
84}
85
86void    btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform)
87{
88        m_children[childIndex].m_transform = newChildTransform;
89
90        if (m_dynamicAabbTree)
91        {
92                ///update the dynamic aabb tree
93                btVector3 localAabbMin,localAabbMax;
94                m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
95                ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
96                int index = m_children.size()-1;
97                m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
98        }
99
100        recalculateLocalAabb();
101}
102
103void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
104{
105        m_updateRevision++;
106        btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
107        if (m_dynamicAabbTree)
108        {
109                m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
110        }
111        m_children.swap(childShapeIndex,m_children.size()-1);
112        m_children.pop_back();
113
114}
115
116
117
118void btCompoundShape::removeChildShape(btCollisionShape* shape)
119{
120        m_updateRevision++;
121        // Find the children containing the shape specified, and remove those children.
122        //note: there might be multiple children using the same shape!
123        for(int i = m_children.size()-1; i >= 0 ; i--)
124        {
125                if(m_children[i].m_childShape == shape)
126                {
127                        m_children.swap(i,m_children.size()-1);
128                        m_children.pop_back();
129                        //remove it from the m_dynamicAabbTree too
130                        //@todo: this leads to problems due to caching in the btCompoundCollisionAlgorithm
131                        //so effectively, removeChildShape is broken at the moment
132                        //m_dynamicAabbTree->remove(m_aabbProxies[i]);
133                        //m_aabbProxies.swap(i,m_children.size()-1);
134                        //m_aabbProxies.pop_back();
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.
147
148        m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
149        m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
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);
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
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);
222        int k;
223
224        for (k = 0; k < n; k++)
225        {
226                center += m_children[k].m_transform.getOrigin() * masses[k];
227                totalMass += masses[k];
228        }
229        center /= totalMass;
230        principal.setOrigin(center);
231
232        btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
233        for ( k = 0; k < n; k++)
234        {
235                btVector3 i;
236                m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
237
238                const btTransform& t = m_children[k].m_transform;
239                btVector3 o = t.getOrigin() - center;
240
241                //compute inertia tensor in coordinate system of compound shape
242                btMatrix3x3 j = t.getBasis().transpose();
243                j[0] *= i[0];
244                j[1] *= i[1];
245                j[2] *= i[2];
246                j = t.getBasis() * j;
247
248                //add inertia tensor
249                tensor[0] += j[0];
250                tensor[1] += j[1];
251                tensor[2] += j[2];
252
253                //compute inertia tensor of pointmass at o
254                btScalar o2 = o.length2();
255                j[0].setValue(o2, 0, 0);
256                j[1].setValue(0, o2, 0);
257                j[2].setValue(0, 0, o2);
258                j[0] += o * -o.x(); 
259                j[1] += o * -o.y(); 
260                j[2] += o * -o.z();
261
262                //add inertia tensor of pointmass
263                tensor[0] += masses[k] * j[0];
264                tensor[1] += masses[k] * j[1];
265                tensor[2] += masses[k] * j[2];
266        }
267
268        tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
269        inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
270}
271
272
273
Note: See TracBrowser for help on using the repository browser.