| /* | |
| Bullet Continuous Collision Detection and Physics Library | |
| Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 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. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #include "BulletCollision/CollisionShapes/btCompoundShape.h" | |
| #include "BulletCollision/CollisionShapes/btCollisionShape.h" | |
| #include "BulletCollision/BroadphaseCollision/btDbvt.h" | |
| btCompoundShape::btCompoundShape(bool enableDynamicAabbTree) | |
| : 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); | |
| } | |
| } | |
| btCompoundShape::~btCompoundShape() | |
| { | |
| if (m_dynamicAabbTree) | |
| { | |
| m_dynamicAabbTree->~btDbvt(); | |
| btAlignedFree(m_dynamicAabbTree); | |
| } | |
| } | |
| void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) | |
| { | |
| m_updateRevision++; | |
| //m_childTransforms.push_back(localTransform); | |
| //m_childShapes.push_back(shape); | |
| btCompoundShapeChild child; | |
| child.m_transform = localTransform; | |
| child.m_childShape = shape; | |
| 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++) | |
| { | |
| if (m_localAabbMin[i] > localAabbMin[i]) | |
| { | |
| m_localAabbMin[i] = localAabbMin[i]; | |
| } | |
| if (m_localAabbMax[i] < localAabbMax[i]) | |
| { | |
| m_localAabbMax[i] = localAabbMax[i]; | |
| } | |
| } | |
| if (m_dynamicAabbTree) | |
| { | |
| const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); | |
| int index = m_children.size(); | |
| child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); | |
| } | |
| m_children.push_back(child); | |
| } | |
| void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform) | |
| { | |
| 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); | |
| //int index = m_children.size()-1; | |
| m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); | |
| } | |
| recalculateLocalAabb(); | |
| } | |
| void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) | |
| { | |
| m_updateRevision++; | |
| 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); | |
| 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--) | |
| { | |
| if(m_children[i].m_childShape == shape) | |
| { | |
| removeChildShapeByIndex(i); | |
| } | |
| } | |
| recalculateLocalAabb(); | |
| } | |
| 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)); | |
| //extend the local aabbMin/aabbMax | |
| for (int j = 0; j < m_children.size(); j++) | |
| { | |
| btVector3 localAabbMin,localAabbMax; | |
| m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); | |
| for (int i=0;i<3;i++) | |
| { | |
| if (m_localAabbMin[i] > localAabbMin[i]) | |
| m_localAabbMin[i] = localAabbMin[i]; | |
| if (m_localAabbMax[i] < localAabbMax[i]) | |
| m_localAabbMax[i] = localAabbMax[i]; | |
| } | |
| } | |
| } | |
| ///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 | |
| { | |
| 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 += btVector3(getMargin(),getMargin(),getMargin()); | |
| btMatrix3x3 abs_b = trans.getBasis().absolute(); | |
| btVector3 center = trans(localCenter); | |
| btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), | |
| abs_b[1].dot(localHalfExtents), | |
| abs_b[2].dot(localHalfExtents)); | |
| aabbMin = center-extent; | |
| aabbMax = center+extent; | |
| } | |
| 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 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); | |
| } | |
| void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const | |
| { | |
| int n = m_children.size(); | |
| btScalar totalMass = 0; | |
| btVector3 center(0, 0, 0); | |
| int k; | |
| for (k = 0; k < n; k++) | |
| { | |
| center += m_children[k].m_transform.getOrigin() * masses[k]; | |
| totalMass += masses[k]; | |
| } | |
| center /= totalMass; | |
| principal.setOrigin(center); | |
| btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); | |
| for ( k = 0; k < n; k++) | |
| { | |
| btVector3 i; | |
| m_children[k].m_childShape->calculateLocalInertia(masses[k], i); | |
| const btTransform& t = m_children[k].m_transform; | |
| btVector3 o = t.getOrigin() - center; | |
| //compute inertia tensor in coordinate system of compound shape | |
| btMatrix3x3 j = t.getBasis().transpose(); | |
| j[0] *= i[0]; | |
| j[1] *= i[1]; | |
| j[2] *= i[2]; | |
| j = t.getBasis() * j; | |
| //add inertia tensor | |
| tensor[0] += j[0]; | |
| tensor[1] += j[1]; | |
| tensor[2] += j[2]; | |
| //compute inertia tensor of pointmass at o | |
| btScalar o2 = o.length2(); | |
| 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[2] += o * -o.z(); | |
| //add inertia tensor of pointmass | |
| tensor[0] += masses[k] * j[0]; | |
| tensor[1] += masses[k] * j[1]; | |
| tensor[2] += masses[k] * j[2]; | |
| } | |
| tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20); | |
| inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); | |
| } | |