Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

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";
 }
-