btCompoundShape.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "btCompoundShape.h"
00017 #include "btCollisionShape.h"
00018 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
00019 #include "LinearMath/btSerializer.h"
00020 
00021 btCompoundShape::btCompoundShape(bool enableDynamicAabbTree)
00022 : m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)),
00023 m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)),
00024 m_dynamicAabbTree(0),
00025 m_updateRevision(1),
00026 m_collisionMargin(btScalar(0.)),
00027 m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
00028 {
00029         m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
00030 
00031         if (enableDynamicAabbTree)
00032         {
00033                 void* mem = btAlignedAlloc(sizeof(btDbvt),16);
00034                 m_dynamicAabbTree = new(mem) btDbvt();
00035                 btAssert(mem==m_dynamicAabbTree);
00036         }
00037 }
00038 
00039 
00040 btCompoundShape::~btCompoundShape()
00041 {
00042         if (m_dynamicAabbTree)
00043         {
00044                 m_dynamicAabbTree->~btDbvt();
00045                 btAlignedFree(m_dynamicAabbTree);
00046         }
00047 }
00048 
00049 void    btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
00050 {
00051         m_updateRevision++;
00052         //m_childTransforms.push_back(localTransform);
00053         //m_childShapes.push_back(shape);
00054         btCompoundShapeChild child;
00055         child.m_node = 0;
00056         child.m_transform = localTransform;
00057         child.m_childShape = shape;
00058         child.m_childShapeType = shape->getShapeType();
00059         child.m_childMargin = shape->getMargin();
00060 
00061         
00062         //extend the local aabbMin/aabbMax
00063         btVector3 localAabbMin,localAabbMax;
00064         shape->getAabb(localTransform,localAabbMin,localAabbMax);
00065         for (int i=0;i<3;i++)
00066         {
00067                 if (m_localAabbMin[i] > localAabbMin[i])
00068                 {
00069                         m_localAabbMin[i] = localAabbMin[i];
00070                 }
00071                 if (m_localAabbMax[i] < localAabbMax[i])
00072                 {
00073                         m_localAabbMax[i] = localAabbMax[i];
00074                 }
00075 
00076         }
00077         if (m_dynamicAabbTree)
00078         {
00079                 const btDbvtVolume      bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
00080                 int index = m_children.size();
00081                 child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
00082         }
00083 
00084         m_children.push_back(child);
00085 
00086 }
00087 
00088 void    btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb)
00089 {
00090         m_children[childIndex].m_transform = newChildTransform;
00091 
00092         if (m_dynamicAabbTree)
00093         {
00095                 btVector3 localAabbMin,localAabbMax;
00096                 m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
00097                 ATTRIBUTE_ALIGNED16(btDbvtVolume)       bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
00098                 //int index = m_children.size()-1;
00099                 m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
00100         }
00101 
00102         if (shouldRecalculateLocalAabb)
00103         {
00104                 recalculateLocalAabb();
00105         }
00106 }
00107 
00108 void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
00109 {
00110         m_updateRevision++;
00111         btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
00112         if (m_dynamicAabbTree)
00113         {
00114                 m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
00115         }
00116         m_children.swap(childShapeIndex,m_children.size()-1);
00117     if (m_dynamicAabbTree) 
00118                 m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex;
00119         m_children.pop_back();
00120 
00121 }
00122 
00123 
00124 
00125 void btCompoundShape::removeChildShape(btCollisionShape* shape)
00126 {
00127         m_updateRevision++;
00128         // Find the children containing the shape specified, and remove those children.
00129         //note: there might be multiple children using the same shape!
00130         for(int i = m_children.size()-1; i >= 0 ; i--)
00131         {
00132                 if(m_children[i].m_childShape == shape)
00133                 {
00134                         removeChildShapeByIndex(i);
00135                 }
00136         }
00137 
00138 
00139 
00140         recalculateLocalAabb();
00141 }
00142 
00143 void btCompoundShape::recalculateLocalAabb()
00144 {
00145         // Recalculate the local aabb
00146         // Brute force, it iterates over all the shapes left.
00147 
00148         m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00149         m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
00150 
00151         //extend the local aabbMin/aabbMax
00152         for (int j = 0; j < m_children.size(); j++)
00153         {
00154                 btVector3 localAabbMin,localAabbMax;
00155                 m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax);
00156                 for (int i=0;i<3;i++)
00157                 {
00158                         if (m_localAabbMin[i] > localAabbMin[i])
00159                                 m_localAabbMin[i] = localAabbMin[i];
00160                         if (m_localAabbMax[i] < localAabbMax[i])
00161                                 m_localAabbMax[i] = localAabbMax[i];
00162                 }
00163         }
00164 }
00165 
00167 void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
00168 {
00169         btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
00170         btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
00171         
00172         //avoid an illegal AABB when there are no children
00173         if (!m_children.size())
00174         {
00175                 localHalfExtents.setValue(0,0,0);
00176                 localCenter.setValue(0,0,0);
00177         }
00178         localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
00179                 
00180 
00181         btMatrix3x3 abs_b = trans.getBasis().absolute();  
00182 
00183         btVector3 center = trans(localCenter);
00184 
00185     btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
00186         aabbMin = center-extent;
00187         aabbMax = center+extent;
00188         
00189 }
00190 
00191 void    btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
00192 {
00193         //approximation: take the inertia from the aabb for now
00194         btTransform ident;
00195         ident.setIdentity();
00196         btVector3 aabbMin,aabbMax;
00197         getAabb(ident,aabbMin,aabbMax);
00198 
00199         btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
00200 
00201         btScalar lx=btScalar(2.)*(halfExtents.x());
00202         btScalar ly=btScalar(2.)*(halfExtents.y());
00203         btScalar lz=btScalar(2.)*(halfExtents.z());
00204 
00205         inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
00206         inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
00207         inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
00208 
00209 }
00210 
00211 
00212 
00213 
00214 void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
00215 {
00216         int n = m_children.size();
00217 
00218         btScalar totalMass = 0;
00219         btVector3 center(0, 0, 0);
00220         int k;
00221 
00222         for (k = 0; k < n; k++)
00223         {
00224                 btAssert(masses[k]>0);
00225                 center += m_children[k].m_transform.getOrigin() * masses[k];
00226                 totalMass += masses[k];
00227         }
00228 
00229         btAssert(totalMass>0);
00230 
00231         center /= totalMass;
00232         principal.setOrigin(center);
00233 
00234         btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
00235         for ( k = 0; k < n; k++)
00236         {
00237                 btVector3 i;
00238                 m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
00239 
00240                 const btTransform& t = m_children[k].m_transform;
00241                 btVector3 o = t.getOrigin() - center;
00242 
00243                 //compute inertia tensor in coordinate system of compound shape
00244                 btMatrix3x3 j = t.getBasis().transpose();
00245                 j[0] *= i[0];
00246                 j[1] *= i[1];
00247                 j[2] *= i[2];
00248                 j = t.getBasis() * j;
00249 
00250                 //add inertia tensor
00251                 tensor[0] += j[0];
00252                 tensor[1] += j[1];
00253                 tensor[2] += j[2];
00254 
00255                 //compute inertia tensor of pointmass at o
00256                 btScalar o2 = o.length2();
00257                 j[0].setValue(o2, 0, 0);
00258                 j[1].setValue(0, o2, 0);
00259                 j[2].setValue(0, 0, o2);
00260                 j[0] += o * -o.x(); 
00261                 j[1] += o * -o.y(); 
00262                 j[2] += o * -o.z();
00263 
00264                 //add inertia tensor of pointmass
00265                 tensor[0] += masses[k] * j[0];
00266                 tensor[1] += masses[k] * j[1];
00267                 tensor[2] += masses[k] * j[2];
00268         }
00269 
00270         tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
00271         inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
00272 }
00273 
00274 
00275 
00276 void btCompoundShape::setLocalScaling(const btVector3& scaling)
00277 {
00278 
00279         for(int i = 0; i < m_children.size(); i++)
00280         {
00281                 btTransform childTrans = getChildTransform(i);
00282                 btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
00283 //              childScale = childScale * (childTrans.getBasis() * scaling);
00284                 childScale = childScale * scaling / m_localScaling;
00285                 m_children[i].m_childShape->setLocalScaling(childScale);
00286                 childTrans.setOrigin((childTrans.getOrigin())*scaling);
00287                 updateChildTransform(i, childTrans,false);
00288         }
00289         
00290         m_localScaling = scaling;
00291         recalculateLocalAabb();
00292 
00293 }
00294 
00295 
00296 void btCompoundShape::createAabbTreeFromChildren()
00297 {
00298     if ( !m_dynamicAabbTree )
00299     {
00300         void* mem = btAlignedAlloc(sizeof(btDbvt),16);
00301         m_dynamicAabbTree = new(mem) btDbvt();
00302         btAssert(mem==m_dynamicAabbTree);
00303 
00304         for ( int index = 0; index < m_children.size(); index++ )
00305         {
00306             btCompoundShapeChild &child = m_children[index];
00307 
00308             //extend the local aabbMin/aabbMax
00309             btVector3 localAabbMin,localAabbMax;
00310             child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
00311 
00312             const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
00313             child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
00314         }
00315     }
00316 }
00317 
00318 
00320 const char*     btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
00321 {
00322 
00323         btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
00324         btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
00325 
00326         shapeData->m_collisionMargin = float(m_collisionMargin);
00327         shapeData->m_numChildShapes = m_children.size();
00328         shapeData->m_childShapePtr = 0;
00329         if (shapeData->m_numChildShapes)
00330         {
00331                 btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
00332                 btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
00333                 shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
00334 
00335                 for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
00336                 {
00337                         memPtr->m_childMargin = float(m_children[i].m_childMargin);
00338                         memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
00339                         //don't serialize shapes that already have been serialized
00340                         if (!serializer->findPointer(m_children[i].m_childShape))
00341                         {
00342                                 btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
00343                                 const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
00344                                 serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
00345                         } 
00346 
00347                         memPtr->m_childShapeType = m_children[i].m_childShapeType;
00348                         m_children[i].m_transform.serializeFloat(memPtr->m_transform);
00349                 }
00350                 serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
00351         }
00352         return "btCompoundShapeData";
00353 }
00354