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

Update Bullet physics to commit 126b676

1 files changed, 126 insertions, 160 deletions

diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
index 441a89c6bb..c85ce2498e 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
@@ -17,23 +17,17 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransformUtil.h"
 
-
-
-btHeightfieldTerrainShape::btHeightfieldTerrainShape
-(
-int heightStickWidth, int heightStickLength, const void* heightfieldData,
-btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis,
-PHY_ScalarType hdt, bool flipQuadEdges
-)
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const void* heightfieldData,
+	btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
+	PHY_ScalarType hdt, bool flipQuadEdges)
 {
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
-	           heightScale, minHeight, maxHeight, upAxis, hdt,
-	           flipQuadEdges);
+			   heightScale, minHeight, maxHeight, upAxis, hdt,
+			   flipQuadEdges);
 }
 
-
-
-btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges)
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
 {
 	// legacy constructor: support only float or unsigned char,
 	// 	and min height is zero
@@ -45,27 +39,23 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h
 	btScalar heightScale = maxHeight / 65535;
 
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
-	           heightScale, minHeight, maxHeight, upAxis, hdt,
-	           flipQuadEdges);
+			   heightScale, minHeight, maxHeight, upAxis, hdt,
+			   flipQuadEdges);
 }
 
-
-
-void btHeightfieldTerrainShape::initialize
-(
-int heightStickWidth, int heightStickLength, const void* heightfieldData,
-btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
-PHY_ScalarType hdt, bool flipQuadEdges
-)
+void btHeightfieldTerrainShape::initialize(
+	int heightStickWidth, int heightStickLength, const void* heightfieldData,
+	btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
+	PHY_ScalarType hdt, bool flipQuadEdges)
 {
 	// validation
-	btAssert(heightStickWidth > 1);// && "bad width");
-	btAssert(heightStickLength > 1);// && "bad length");
-	btAssert(heightfieldData);// && "null heightfield data");
+	btAssert(heightStickWidth > 1);   // && "bad width");
+	btAssert(heightStickLength > 1);  // && "bad length");
+	btAssert(heightfieldData);        // && "null heightfield data");
 	// btAssert(heightScale) -- do we care?  Trust caller here
-	btAssert(minHeight <= maxHeight);// && "bad min/max height");
-	btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]");
-	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum");
+	btAssert(minHeight <= maxHeight);                                    // && "bad min/max height");
+	btAssert(upAxis >= 0 && upAxis < 3);                                 // && "bad upAxis--should be in range [0,2]");
+	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);  // && "Bad height data type enum");
 
 	// initialize member variables
 	m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -73,8 +63,8 @@ PHY_ScalarType hdt, bool flipQuadEdges
 	m_heightStickLength = heightStickLength;
 	m_minHeight = minHeight;
 	m_maxHeight = maxHeight;
-	m_width = (btScalar) (heightStickWidth - 1);
-	m_length = (btScalar) (heightStickLength - 1);
+	m_width = (btScalar)(heightStickWidth - 1);
+	m_length = (btScalar)(heightStickLength - 1);
 	m_heightScale = heightScale;
 	m_heightfieldDataUnknown = heightfieldData;
 	m_heightDataType = hdt;
@@ -87,28 +77,28 @@ PHY_ScalarType hdt, bool flipQuadEdges
 	// determine min/max axis-aligned bounding box (aabb) values
 	switch (m_upAxis)
 	{
-	case 0:
+		case 0:
 		{
 			m_localAabbMin.setValue(m_minHeight, 0, 0);
 			m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
 			break;
 		}
-	case 1:
+		case 1:
 		{
 			m_localAabbMin.setValue(0, m_minHeight, 0);
 			m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
 			break;
 		};
-	case 2:
+		case 2:
 		{
 			m_localAabbMin.setValue(0, 0, m_minHeight);
 			m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
 			break;
 		}
-	default:
+		default:
 		{
 			//need to get valid m_upAxis
-			btAssert(0);// && "Bad m_upAxis");
+			btAssert(0);  // && "Bad m_upAxis");
 		}
 	}
 
@@ -116,62 +106,57 @@ PHY_ScalarType hdt, bool flipQuadEdges
 	m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
 }
 
-
-
 btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 {
 }
 
-
-
-void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
+void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 {
-	btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
+	btVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * btScalar(0.5);
 
 	btVector3 localOrigin(0, 0, 0);
 	localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
 	localOrigin *= m_localScaling;
 
-	btMatrix3x3 abs_b = t.getBasis().absolute();  
+	btMatrix3x3 abs_b = t.getBasis().absolute();
 	btVector3 center = t.getOrigin();
-    btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
-	extent += btVector3(getMargin(),getMargin(),getMargin());
+	btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
+	extent += btVector3(getMargin(), getMargin(), getMargin());
 
 	aabbMin = center - extent;
 	aabbMax = center + extent;
 }
 
-
 /// This returns the "raw" (user's initial) height, not the actual height.
 /// The actual height needs to be adjusted to be relative to the center
 ///   of the heightfield's AABB.
 btScalar
-btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
+btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const
 {
 	btScalar val = 0.f;
 	switch (m_heightDataType)
 	{
-	case PHY_FLOAT:
+		case PHY_FLOAT:
 		{
-			val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x];
+			val = m_heightfieldDataFloat[(y * m_heightStickWidth) + x];
 			break;
 		}
 
-	case PHY_UCHAR:
+		case PHY_UCHAR:
 		{
-			unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x];
+			unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x];
 			val = heightFieldValue * m_heightScale;
 			break;
 		}
 
-	case PHY_SHORT:
+		case PHY_SHORT:
 		{
 			short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x];
 			val = hfValue * m_heightScale;
 			break;
 		}
 
-	default:
+		default:
 		{
 			btAssert(!"Bad m_heightDataType");
 		}
@@ -180,74 +165,63 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
 	return val;
 }
 
-
-
-
 /// this returns the vertex in bullet-local coordinates
-void	btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
+void btHeightfieldTerrainShape::getVertex(int x, int y, btVector3& vertex) const
 {
-	btAssert(x>=0);
-	btAssert(y>=0);
-	btAssert(x<m_heightStickWidth);
-	btAssert(y<m_heightStickLength);
+	btAssert(x >= 0);
+	btAssert(y >= 0);
+	btAssert(x < m_heightStickWidth);
+	btAssert(y < m_heightStickLength);
 
-	btScalar	height = getRawHeightFieldValue(x,y);
+	btScalar height = getRawHeightFieldValue(x, y);
 
 	switch (m_upAxis)
 	{
-	case 0:
+		case 0:
 		{
-		vertex.setValue(
-			height - m_localOrigin.getX(),
-			(-m_width/btScalar(2.0)) + x,
-			(-m_length/btScalar(2.0) ) + y
-			);
+			vertex.setValue(
+				height - m_localOrigin.getX(),
+				(-m_width / btScalar(2.0)) + x,
+				(-m_length / btScalar(2.0)) + y);
 			break;
 		}
-	case 1:
+		case 1:
 		{
 			vertex.setValue(
-			(-m_width/btScalar(2.0)) + x,
-			height - m_localOrigin.getY(),
-			(-m_length/btScalar(2.0)) + y
-			);
+				(-m_width / btScalar(2.0)) + x,
+				height - m_localOrigin.getY(),
+				(-m_length / btScalar(2.0)) + y);
 			break;
 		};
-	case 2:
+		case 2:
 		{
 			vertex.setValue(
-			(-m_width/btScalar(2.0)) + x,
-			(-m_length/btScalar(2.0)) + y,
-			height - m_localOrigin.getZ()
-			);
+				(-m_width / btScalar(2.0)) + x,
+				(-m_length / btScalar(2.0)) + y,
+				height - m_localOrigin.getZ());
 			break;
 		}
-	default:
+		default:
 		{
 			//need to get valid m_upAxis
 			btAssert(0);
 		}
 	}
 
-	vertex*=m_localScaling;
+	vertex *= m_localScaling;
 }
 
-
-
 static inline int
-getQuantized
-(
-btScalar x
-)
+getQuantized(
+	btScalar x)
 {
-	if (x < 0.0) {
-		return (int) (x - 0.5);
+	if (x < 0.0)
+	{
+		return (int)(x - 0.5);
 	}
-	return (int) (x + 0.5);
+	return (int)(x + 0.5);
 }
 
-
-
 /// given input vector, return quantized version
 /**
   This routine is basically determining the gridpoint indices for a given
@@ -257,7 +231,7 @@ btScalar x
   "with clamp" means that we restrict the point to be in the heightfield's
   axis-aligned bounding box.
  */
-void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
+void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point, int /*isMax*/) const
 {
 	btVector3 clampedPoint(point);
 	clampedPoint.setMax(m_localAabbMin);
@@ -266,11 +240,8 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi
 	out[0] = getQuantized(clampedPoint.getX());
 	out[1] = getQuantized(clampedPoint.getY());
 	out[2] = getQuantized(clampedPoint.getZ());
-		
 }
 
-
-
 /// process all triangles within the provided axis-aligned bounding box
 /**
   basic algorithm:
@@ -278,128 +249,123 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi
     - convert input aabb to a range of heightfield grid points (quantize)
     - iterate over all triangles in that subset of the grid
  */
-void	btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 {
 	// scale down the input aabb's so they are in local (non-scaled) coordinates
-	btVector3	localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
-	btVector3	localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+	btVector3 localAabbMin = aabbMin * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]);
+	btVector3 localAabbMax = aabbMax * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]);
 
 	// account for local origin
 	localAabbMin += m_localOrigin;
 	localAabbMax += m_localOrigin;
 
 	//quantize the aabbMin and aabbMax, and adjust the start/end ranges
-	int	quantizedAabbMin[3];
-	int	quantizedAabbMax[3];
-	quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
-	quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
-	
+	int quantizedAabbMin[3];
+	int quantizedAabbMax[3];
+	quantizeWithClamp(quantizedAabbMin, localAabbMin, 0);
+	quantizeWithClamp(quantizedAabbMax, localAabbMax, 1);
+
 	// expand the min/max quantized values
 	// this is to catch the case where the input aabb falls between grid points!
-	for (int i = 0; i < 3; ++i) {
+	for (int i = 0; i < 3; ++i)
+	{
 		quantizedAabbMin[i]--;
 		quantizedAabbMax[i]++;
-	}	
+	}
 
-	int startX=0;
-	int endX=m_heightStickWidth-1;
-	int startJ=0;
-	int endJ=m_heightStickLength-1;
+	int startX = 0;
+	int endX = m_heightStickWidth - 1;
+	int startJ = 0;
+	int endJ = m_heightStickLength - 1;
 
 	switch (m_upAxis)
 	{
-	case 0:
+		case 0:
 		{
-			if (quantizedAabbMin[1]>startX)
+			if (quantizedAabbMin[1] > startX)
 				startX = quantizedAabbMin[1];
-			if (quantizedAabbMax[1]<endX)
+			if (quantizedAabbMax[1] < endX)
 				endX = quantizedAabbMax[1];
-			if (quantizedAabbMin[2]>startJ)
+			if (quantizedAabbMin[2] > startJ)
 				startJ = quantizedAabbMin[2];
-			if (quantizedAabbMax[2]<endJ)
+			if (quantizedAabbMax[2] < endJ)
 				endJ = quantizedAabbMax[2];
 			break;
 		}
-	case 1:
+		case 1:
 		{
-			if (quantizedAabbMin[0]>startX)
+			if (quantizedAabbMin[0] > startX)
 				startX = quantizedAabbMin[0];
-			if (quantizedAabbMax[0]<endX)
+			if (quantizedAabbMax[0] < endX)
 				endX = quantizedAabbMax[0];
-			if (quantizedAabbMin[2]>startJ)
+			if (quantizedAabbMin[2] > startJ)
 				startJ = quantizedAabbMin[2];
-			if (quantizedAabbMax[2]<endJ)
+			if (quantizedAabbMax[2] < endJ)
 				endJ = quantizedAabbMax[2];
 			break;
 		};
-	case 2:
+		case 2:
 		{
-			if (quantizedAabbMin[0]>startX)
+			if (quantizedAabbMin[0] > startX)
 				startX = quantizedAabbMin[0];
-			if (quantizedAabbMax[0]<endX)
+			if (quantizedAabbMax[0] < endX)
 				endX = quantizedAabbMax[0];
-			if (quantizedAabbMin[1]>startJ)
+			if (quantizedAabbMin[1] > startJ)
 				startJ = quantizedAabbMin[1];
-			if (quantizedAabbMax[1]<endJ)
+			if (quantizedAabbMax[1] < endJ)
 				endJ = quantizedAabbMax[1];
 			break;
 		}
-	default:
+		default:
 		{
 			//need to get valid m_upAxis
 			btAssert(0);
 		}
 	}
 
-	
-  
-
-	for(int j=startJ; j<endJ; j++)
+	for (int j = startJ; j < endJ; j++)
 	{
-		for(int x=startX; x<endX; x++)
+		for (int x = startX; x < endX; x++)
 		{
 			btVector3 vertices[3];
-			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision  && !(j & 1)))
+			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1)))
 			{
-        //first triangle
-        getVertex(x,j,vertices[0]);
-		getVertex(x, j + 1, vertices[1]);
-		getVertex(x + 1, j + 1, vertices[2]);
-        callback->processTriangle(vertices,x,j);
-        //second triangle
-      //  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
-        getVertex(x+1,j+1,vertices[1]);
-		getVertex(x + 1, j, vertices[2]);
-		callback->processTriangle(vertices, x, j);
-
-			} else
+				//first triangle
+				getVertex(x, j, vertices[0]);
+				getVertex(x, j + 1, vertices[1]);
+				getVertex(x + 1, j + 1, vertices[2]);
+				callback->processTriangle(vertices, x, j);
+				//second triangle
+				//  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
+				getVertex(x + 1, j + 1, vertices[1]);
+				getVertex(x + 1, j, vertices[2]);
+				callback->processTriangle(vertices, x, j);
+			}
+			else
 			{
-        //first triangle
-        getVertex(x,j,vertices[0]);
-        getVertex(x,j+1,vertices[1]);
-        getVertex(x+1,j,vertices[2]);
-        callback->processTriangle(vertices,x,j);
-        //second triangle
-        getVertex(x+1,j,vertices[0]);
-        //getVertex(x,j+1,vertices[1]);
-        getVertex(x+1,j+1,vertices[2]);
-        callback->processTriangle(vertices,x,j);
+				//first triangle
+				getVertex(x, j, vertices[0]);
+				getVertex(x, j + 1, vertices[1]);
+				getVertex(x + 1, j, vertices[2]);
+				callback->processTriangle(vertices, x, j);
+				//second triangle
+				getVertex(x + 1, j, vertices[0]);
+				//getVertex(x,j+1,vertices[1]);
+				getVertex(x + 1, j + 1, vertices[2]);
+				callback->processTriangle(vertices, x, j);
 			}
 		}
 	}
-
-	
-
 }
 
-void	btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
+void btHeightfieldTerrainShape::calculateLocalInertia(btScalar, btVector3& inertia) const
 {
 	//moving concave objects not supported
-	
-	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+
+	inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 }
 
-void	btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
+void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
 {
 	m_localScaling = scaling;
 }