Blender V2.61 - r43446

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 = btVector3(abs_b[0].dot(localHalfExtents),
00186         abs_b[1].dot(localHalfExtents),
00187         abs_b[2].dot(localHalfExtents));
00188     aabbMin = center-extent;
00189     aabbMax = center+extent;
00190     
00191 }
00192 
00193 void    btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
00194 {
00195     //approximation: take the inertia from the aabb for now
00196     btTransform ident;
00197     ident.setIdentity();
00198     btVector3 aabbMin,aabbMax;
00199     getAabb(ident,aabbMin,aabbMax);
00200 
00201     btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
00202 
00203     btScalar lx=btScalar(2.)*(halfExtents.x());
00204     btScalar ly=btScalar(2.)*(halfExtents.y());
00205     btScalar lz=btScalar(2.)*(halfExtents.z());
00206 
00207     inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
00208     inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
00209     inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
00210 
00211 }
00212 
00213 
00214 
00215 
00216 void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
00217 {
00218     int n = m_children.size();
00219 
00220     btScalar totalMass = 0;
00221     btVector3 center(0, 0, 0);
00222     int k;
00223 
00224     for (k = 0; k < n; k++)
00225     {
00226         btAssert(masses[k]>0);
00227         center += m_children[k].m_transform.getOrigin() * masses[k];
00228         totalMass += masses[k];
00229     }
00230 
00231     btAssert(totalMass>0);
00232 
00233     center /= totalMass;
00234     principal.setOrigin(center);
00235 
00236     btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
00237     for ( k = 0; k < n; k++)
00238     {
00239         btVector3 i;
00240         m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
00241 
00242         const btTransform& t = m_children[k].m_transform;
00243         btVector3 o = t.getOrigin() - center;
00244 
00245         //compute inertia tensor in coordinate system of compound shape
00246         btMatrix3x3 j = t.getBasis().transpose();
00247         j[0] *= i[0];
00248         j[1] *= i[1];
00249         j[2] *= i[2];
00250         j = t.getBasis() * j;
00251 
00252         //add inertia tensor
00253         tensor[0] += j[0];
00254         tensor[1] += j[1];
00255         tensor[2] += j[2];
00256 
00257         //compute inertia tensor of pointmass at o
00258         btScalar o2 = o.length2();
00259         j[0].setValue(o2, 0, 0);
00260         j[1].setValue(0, o2, 0);
00261         j[2].setValue(0, 0, o2);
00262         j[0] += o * -o.x(); 
00263         j[1] += o * -o.y(); 
00264         j[2] += o * -o.z();
00265 
00266         //add inertia tensor of pointmass
00267         tensor[0] += masses[k] * j[0];
00268         tensor[1] += masses[k] * j[1];
00269         tensor[2] += masses[k] * j[2];
00270     }
00271 
00272     tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
00273     inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
00274 }
00275 
00276 
00277 
00278 void btCompoundShape::setLocalScaling(const btVector3& scaling)
00279 {
00280 
00281     for(int i = 0; i < m_children.size(); i++)
00282     {
00283         btTransform childTrans = getChildTransform(i);
00284         btVector3 childScale = m_children[i].m_childShape->getLocalScaling();
00285 //      childScale = childScale * (childTrans.getBasis() * scaling);
00286         childScale = childScale * scaling / m_localScaling;
00287         m_children[i].m_childShape->setLocalScaling(childScale);
00288         childTrans.setOrigin((childTrans.getOrigin())*scaling);
00289         updateChildTransform(i, childTrans,false);
00290     }
00291     
00292     m_localScaling = scaling;
00293     recalculateLocalAabb();
00294 
00295 }
00296 
00297 
00298 void btCompoundShape::createAabbTreeFromChildren()
00299 {
00300     if ( !m_dynamicAabbTree )
00301     {
00302         void* mem = btAlignedAlloc(sizeof(btDbvt),16);
00303         m_dynamicAabbTree = new(mem) btDbvt();
00304         btAssert(mem==m_dynamicAabbTree);
00305 
00306         for ( int index = 0; index < m_children.size(); index++ )
00307         {
00308             btCompoundShapeChild &child = m_children[index];
00309 
00310             //extend the local aabbMin/aabbMax
00311             btVector3 localAabbMin,localAabbMax;
00312             child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax);
00313 
00314             const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
00315             child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
00316         }
00317     }
00318 }
00319 
00320 
00322 const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const
00323 {
00324 
00325     btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
00326     btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
00327 
00328     shapeData->m_collisionMargin = float(m_collisionMargin);
00329     shapeData->m_numChildShapes = m_children.size();
00330     shapeData->m_childShapePtr = 0;
00331     if (shapeData->m_numChildShapes)
00332     {
00333         btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes);
00334         btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr;
00335         shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr);
00336 
00337         for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++)
00338         {
00339             memPtr->m_childMargin = float(m_children[i].m_childMargin);
00340             memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape);
00341             //don't serialize shapes that already have been serialized
00342             if (!serializer->findPointer(m_children[i].m_childShape))
00343             {
00344                 btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1);
00345                 const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer);
00346                 serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape);
00347             } 
00348 
00349             memPtr->m_childShapeType = m_children[i].m_childShapeType;
00350             m_children[i].m_transform.serializeFloat(memPtr->m_transform);
00351         }
00352         serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr);
00353     }
00354     return "btCompoundShapeData";
00355 }
00356