1 files changed, 359 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/thirdparty/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
new file mode 100644
index 0000000000..e8c8c336cd
--- /dev/null
+++ b/thirdparty/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
@@ -0,0 +1,359 @@
+/*
+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 "btCompoundShape.h"
+#include "btCollisionShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#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_shapeType = COMPOUND_SHAPE_PROXYTYPE;
+
+	if (enableDynamicAabbTree)
+	{
+		void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+		m_dynamicAabbTree = new(mem) btDbvt();
+		btAssert(mem==m_dynamicAabbTree);
+	}
+
+	m_children.reserve(initialChildCapacity);
+}
+
+
+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_node = 0;
+	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);
+		size_t index = m_children.size();
+		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)
+{
+	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);
+	}
+
+	if (shouldRecalculateLocalAabb)
+	{
+		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);
+    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--)
+	{
+		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 = 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
+{
+	//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++)
+	{
+		btAssert(masses[k]>0);
+		center += m_children[k].m_transform.getOrigin() * masses[k];
+		totalMass += masses[k];
+	}
+
+	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++)
+	{
+		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]);
+}
+
+
+
+
+
+void btCompoundShape::setLocalScaling(const btVector3& scaling)
+{
+
+	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 * scaling / m_localScaling;
+		m_children[i].m_childShape->setLocalScaling(childScale);
+		childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
+		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);
+
+        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);
+
+            const btDbvtVolume  bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+			size_t index2 = index;
+            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
+{
+
+	btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer;
+	btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer);
+
+	shapeData->m_collisionMargin = float(m_collisionMargin);
+	shapeData->m_numChildShapes = m_children.size();
+	shapeData->m_childShapePtr = 0;
+	if (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++)
+		{
+			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);
+			} 
+
+			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);
+	}
+	return "btCompoundShapeData";
+}
+