00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
00053
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
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
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
00129
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
00146
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
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
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
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
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
00251 tensor[0] += j[0];
00252 tensor[1] += j[1];
00253 tensor[2] += j[2];
00254
00255
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
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
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
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
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