diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp | 192 |
1 files changed, 84 insertions, 108 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp index 85572da307..fd7828b104 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -19,26 +19,25 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity) -: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)), -m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)), -m_dynamicAabbTree(0), -m_updateRevision(1), -m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) + : m_localAabbMin(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)), + m_localAabbMax(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)), + m_dynamicAabbTree(0), + m_updateRevision(1), + m_collisionMargin(btScalar(0.)), + m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)) { m_shapeType = COMPOUND_SHAPE_PROXYTYPE; if (enableDynamicAabbTree) { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); } m_children.reserve(initialChildCapacity); } - btCompoundShape::~btCompoundShape() { if (m_dynamicAabbTree) @@ -48,7 +47,7 @@ btCompoundShape::~btCompoundShape() } } -void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) +void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape) { m_updateRevision++; //m_childTransforms.push_back(localTransform); @@ -60,11 +59,10 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio child.m_childShapeType = shape->getShapeType(); child.m_childMargin = shape->getMargin(); - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - shape->getAabb(localTransform,localAabbMin,localAabbMax); - for (int i=0;i<3;i++) + btVector3 localAabbMin, localAabbMax; + shape->getAabb(localTransform, localAabbMin, localAabbMax); + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) { @@ -74,31 +72,30 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio { m_localAabbMax[i] = localAabbMax[i]; } - } if (m_dynamicAabbTree) { - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index = m_children.size(); - child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) ); + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index)); } m_children.push_back(child); - } -void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb) +void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb) { m_children[childIndex].m_transform = newChildTransform; if (m_dynamicAabbTree) { ///update the dynamic aabb tree - btVector3 localAabbMin,localAabbMax; - m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax); - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + btVector3 localAabbMin, localAabbMax; + m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); //int index = m_children.size()-1; - m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); + m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds); } if (shouldRecalculateLocalAabb) @@ -110,35 +107,30 @@ void btCompoundShape::updateChildTransform(int childIndex, const btTransform& ne void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) { m_updateRevision++; - btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); + btAssert(childShapeIndex >= 0 && childShapeIndex < m_children.size()); if (m_dynamicAabbTree) { m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node); } - m_children.swap(childShapeIndex,m_children.size()-1); - if (m_dynamicAabbTree) + m_children.swap(childShapeIndex, m_children.size() - 1); + if (m_dynamicAabbTree) m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex; m_children.pop_back(); - } - - void btCompoundShape::removeChildShape(btCollisionShape* shape) { m_updateRevision++; // Find the children containing the shape specified, and remove those children. //note: there might be multiple children using the same shape! - for(int i = m_children.size()-1; i >= 0 ; i--) + for (int i = m_children.size() - 1; i >= 0; i--) { - if(m_children[i].m_childShape == shape) + if (m_children[i].m_childShape == shape) { removeChildShapeByIndex(i); } } - - recalculateLocalAabb(); } @@ -147,15 +139,15 @@ void btCompoundShape::recalculateLocalAabb() // Recalculate the local aabb // Brute force, it iterates over all the shapes left. - m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); //extend the local aabbMin/aabbMax for (int j = 0; j < m_children.size(); j++) { - btVector3 localAabbMin,localAabbMax; + btVector3 localAabbMin, localAabbMax; m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) m_localAabbMin[i] = localAabbMin[i]; @@ -166,53 +158,47 @@ void btCompoundShape::recalculateLocalAabb() } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - + btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin); + btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin); + //avoid an illegal AABB when there are no children if (!m_children.size()) { - localHalfExtents.setValue(0,0,0); - localCenter.setValue(0,0,0); + localHalfExtents.setValue(0, 0, 0); + localCenter.setValue(0, 0, 0); } - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); - + localHalfExtents += btVector3(getMargin(), getMargin(), getMargin()); - btMatrix3x3 abs_b = trans.getBasis().absolute(); + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center-extent; - aabbMax = center+extent; - + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMin = center - extent; + aabbMax = center + extent; } -void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //approximation: take the inertia from the aabb for now btTransform ident; ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); + btVector3 aabbMin, aabbMax; + getAabb(ident, aabbMin, aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); - inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); - inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz); + inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz); + inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly); } - - - void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const { int n = m_children.size(); @@ -223,18 +209,18 @@ void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, bt for (k = 0; k < n; k++) { - btAssert(masses[k]>0); + btAssert(masses[k] > 0); center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; } - btAssert(totalMass>0); + btAssert(totalMass > 0); center /= totalMass; principal.setOrigin(center); btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); - for ( k = 0; k < n; k++) + for (k = 0; k < n; k++) { btVector3 i; m_children[k].m_childShape->calculateLocalInertia(masses[k], i); @@ -259,8 +245,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, bt j[0].setValue(o2, 0, 0); j[1].setValue(0, o2, 0); j[2].setValue(0, 0, o2); - j[0] += o * -o.x(); - j[1] += o * -o.y(); + j[0] += o * -o.x(); + j[1] += o * -o.y(); j[2] += o * -o.z(); //add inertia tensor of pointmass @@ -273,59 +259,50 @@ void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, bt inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); } - - - - void btCompoundShape::setLocalScaling(const btVector3& scaling) { - - for(int i = 0; i < m_children.size(); i++) + for (int i = 0; i < m_children.size(); i++) { btTransform childTrans = getChildTransform(i); btVector3 childScale = m_children[i].m_childShape->getLocalScaling(); -// childScale = childScale * (childTrans.getBasis() * scaling); + // childScale = childScale * (childTrans.getBasis() * scaling); childScale = childScale * scaling / m_localScaling; m_children[i].m_childShape->setLocalScaling(childScale); childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling); - updateChildTransform(i, childTrans,false); + updateChildTransform(i, childTrans, false); } - + m_localScaling = scaling; recalculateLocalAabb(); - } - void btCompoundShape::createAabbTreeFromChildren() { - if ( !m_dynamicAabbTree ) - { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + if (!m_dynamicAabbTree) + { + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); - for ( int index = 0; index < m_children.size(); index++ ) - { - btCompoundShapeChild &child = m_children[index]; + for (int index = 0; index < m_children.size(); index++) + { + btCompoundShapeChild& child = m_children[index]; - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax); + //extend the local aabbMin/aabbMax + btVector3 localAabbMin, localAabbMax; + child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax); - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index2 = index; - child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) ); - } - } + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2)); + } + } } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const { - - btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer; + btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer; btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); shapeData->m_collisionMargin = float(m_collisionMargin); @@ -333,27 +310,26 @@ const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serialize shapeData->m_childShapePtr = 0; if (shapeData->m_numChildShapes) { - btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes); + btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes); btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr; shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr); - for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++) + for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++) { memPtr->m_childMargin = float(m_children[i].m_childMargin); memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape); //don't serialize shapes that already have been serialized if (!serializer->findPointer(m_children[i].m_childShape)) { - btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1); - const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape); - } + btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1); + const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape); + } memPtr->m_childShapeType = m_children[i].m_childShapeType; m_children[i].m_transform.serializeFloat(memPtr->m_transform); } - serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr); } return "btCompoundShapeData"; } - |