Merge pull request #29418 from AndreaCatania/upbul

Updated Bullet version to the actual bullet master commit

16 files changed, 227 insertions, 155 deletions

diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp
index 37156fd589..166cb04c0b 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp
@@ -37,7 +37,7 @@ static DBVT_INLINE int indexof(const btDbvtNode* node)
 static DBVT_INLINE btDbvtVolume merge(const btDbvtVolume& a,
 									  const btDbvtVolume& b)
 {
-#if (DBVT_MERGE_IMPL == DBVT_IMPL_SSE)
+#ifdef BT_USE_SSE
 	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
 	btDbvtVolume* ptr = (btDbvtVolume*)locals;
 	btDbvtVolume& res = *ptr;
@@ -80,6 +80,7 @@ static DBVT_INLINE void deletenode(btDbvt* pdbvt,
 static void recursedeletenode(btDbvt* pdbvt,
 							  btDbvtNode* node)
 {
+	if (node == 0) return;
 	if (!node->isleaf())
 	{
 		recursedeletenode(pdbvt, node->childs[0]);
@@ -298,7 +299,7 @@ static int split(btDbvtNode** leaves,
 static btDbvtVolume bounds(btDbvtNode** leaves,
 						   int count)
 {
-#if DBVT_MERGE_IMPL == DBVT_IMPL_SSE
+#ifdef BT_USE_SSE
 	ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]);
 	btDbvtVolume* ptr = (btDbvtVolume*)locals;
 	btDbvtVolume& volume = *ptr;
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
index 166cf771fe..b7fe0a1f34 100644
--- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -123,11 +123,11 @@ protected:
 
 void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher)
 {
+	m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
+
 	btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
 	freeHandle(proxy0);
 
-	m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
-
 	//validate();
 }
 
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index 98a02d0c45..b48d9301d7 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -43,6 +43,7 @@ btCollisionObject::btCollisionObject()
 	  m_userObjectPointer(0),
 	  m_userIndex2(-1),
 	  m_userIndex(-1),
+	  m_userIndex3(-1),
 	  m_hitFraction(btScalar(1.)),
 	  m_ccdSweptSphereRadius(btScalar(0.)),
 	  m_ccdMotionThreshold(btScalar(0.)),
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
index 56b3d89e56..85dc488c8c 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -101,6 +101,8 @@ protected:
 
 	int m_userIndex;
 
+	int m_userIndex3;
+
 	///time of impact calculation
 	btScalar m_hitFraction;
 
@@ -526,6 +528,11 @@ public:
 		return m_userIndex2;
 	}
 
+	int getUserIndex3() const
+	{
+		return m_userIndex3;
+	}
+
 	///users can point to their objects, userPointer is not used by Bullet
 	void setUserPointer(void* userPointer)
 	{
@@ -543,6 +550,11 @@ public:
 		m_userIndex2 = index;
 	}
 
+	void setUserIndex3(int index)
+	{
+		m_userIndex3 = index;
+	}
+
 	int getUpdateRevisionInternal() const
 	{
 		return m_updateRevision;
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index b30ce03164..71184f36ac 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -19,10 +19,10 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting
-#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting
-#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting
-#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" //for raycasting
+#include "BulletCollision/CollisionShapes/btSphereShape.h"                 //for raycasting
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"        //for raycasting
+#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"  //for raycasting
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"     //for raycasting
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
@@ -414,7 +414,9 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 				triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled);
 			}
-			else if (collisionShape->getShapeType()==TERRAIN_SHAPE_PROXYTYPE)
+			else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) 
+				&& collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE 
+				)
 			{
 				///optimized version for btHeightfieldTerrainShape
 				btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape;
@@ -422,7 +424,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co
 				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
 				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
 
-				BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),heightField,colObjWorldTransform);
+				BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform);
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 				heightField->performRaycast(&rcb, rayFromLocal, rayToLocal);
 			}
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
index 65b669e1c0..9694f4ddb3 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
@@ -27,7 +27,7 @@ btConvexPolyhedron::~btConvexPolyhedron()
 {
 }
 
-inline bool IsAlmostZero(const btVector3& v)
+inline bool IsAlmostZero1(const btVector3& v)
 {
 	if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
 	return true;
@@ -122,8 +122,8 @@ void btConvexPolyhedron::initialize()
 
 			for (int p = 0; p < m_uniqueEdges.size(); p++)
 			{
-				if (IsAlmostZero(m_uniqueEdges[p] - edge) ||
-					IsAlmostZero(m_uniqueEdges[p] + edge))
+				if (IsAlmostZero1(m_uniqueEdges[p] - edge) ||
+					IsAlmostZero1(m_uniqueEdges[p] + edge))
 				{
 					found = true;
 					break;
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
index 4adf27e6bb..34ec2d8c45 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
@@ -71,9 +71,10 @@ void btHeightfieldTerrainShape::initialize(
 	m_flipQuadEdges = flipQuadEdges;
 	m_useDiamondSubdivision = false;
 	m_useZigzagSubdivision = false;
+	m_flipTriangleWinding = false;
 	m_upAxis = upAxis;
 	m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
-	m_vboundsGrid = NULL;
+	
 	m_vboundsChunkSize = 0;
 	m_vboundsGridWidth = 0;
 	m_vboundsGridLength = 0;
@@ -335,30 +336,37 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 		for (int x = startX; x < endX; x++)
 		{
 			btVector3 vertices[3];
+			int indices[3] = { 0, 1, 2 };
+			if (m_flipTriangleWinding)
+			{
+				indices[0] = 2;
+				indices[2] = 0;
+			}
+
 			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]);
+				getVertex(x, j, vertices[indices[0]]);
+				getVertex(x, j + 1, vertices[indices[1]]);
+				getVertex(x + 1, j + 1, vertices[indices[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]);
+				getVertex(x + 1, j + 1, vertices[indices[1]]);
+				getVertex(x + 1, j, vertices[indices[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]);
+				getVertex(x, j, vertices[indices[0]]);
+				getVertex(x, j + 1, vertices[indices[1]]);
+				getVertex(x + 1, j, vertices[indices[2]]);
 				callback->processTriangle(vertices, x, j);
 				//second triangle
-				getVertex(x + 1, j, vertices[0]);
+				getVertex(x + 1, j, vertices[indices[0]]);
 				//getVertex(x,j+1,vertices[1]);
-				getVertex(x + 1, j + 1, vertices[2]);
+				getVertex(x + 1, j + 1, vertices[indices[2]]);
 				callback->processTriangle(vertices, x, j);
 			}
 		}
@@ -381,39 +389,42 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
 	return m_localScaling;
 }
 
-
-
-struct GridRaycastState
+namespace
 {
-	int x; // Next quad coords
-	int z;
-	int prev_x; // Previous quad coords
-	int prev_z;
-	btScalar param; // Exit param for previous quad
-	btScalar prevParam; // Enter param for previous quad
-	btScalar maxDistanceFlat;
-	btScalar maxDistance3d;
-};
-
+	struct GridRaycastState
+	{
+		int x;  // Next quad coords
+		int z;
+		int prev_x;  // Previous quad coords
+		int prev_z;
+		btScalar param;      // Exit param for previous quad
+		btScalar prevParam;  // Enter param for previous quad
+		btScalar maxDistanceFlat;
+		btScalar maxDistance3d;
+	};
+}
 
 // TODO Does it really need to take 3D vectors?
 /// Iterates through a virtual 2D grid of unit-sized square cells,
 /// and executes an action on each cell intersecting the given segment, ordered from begin to end.
 /// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf
 template <typename Action_T>
-void gridRaycast(Action_T &quadAction, const btVector3 &beginPos, const btVector3 &endPos)
+void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos, int indices[3])
 {
 	GridRaycastState rs;
 	rs.maxDistance3d = beginPos.distance(endPos);
 	if (rs.maxDistance3d < 0.0001)
+	{
 		// Consider the ray is too small to hit anything
 		return;
+	}
+	
 
-	btScalar rayDirectionFlatX = endPos[0] - beginPos[0];
-	btScalar rayDirectionFlatZ = endPos[2] - beginPos[2];
+	btScalar rayDirectionFlatX = endPos[indices[0]] - beginPos[indices[0]];
+	btScalar rayDirectionFlatZ = endPos[indices[2]] - beginPos[indices[2]];
 	rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ);
 
-	if(rs.maxDistanceFlat < 0.0001)
+	if (rs.maxDistanceFlat < 0.0001)
 	{
 		// Consider the ray vertical
 		rayDirectionFlatX = 0;
@@ -433,34 +444,46 @@ void gridRaycast(Action_T &quadAction, const btVector3 &beginPos, const btVector
 	const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite;
 
 	// pos = param * dir
-	btScalar paramCrossX; // At which value of `param` we will cross a x-axis lane?
-	btScalar paramCrossZ; // At which value of `param` we will cross a z-axis lane?
+	btScalar paramCrossX;  // At which value of `param` we will cross a x-axis lane?
+	btScalar paramCrossZ;  // At which value of `param` we will cross a z-axis lane?
 
 	// paramCrossX and paramCrossZ are initialized as being the first cross
 	// X initialization
 	if (xiStep != 0)
 	{
 		if (xiStep == 1)
-			paramCrossX = (ceil(beginPos[0]) - beginPos[0]) * paramDeltaX;
+		{
+			paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX;
+		}
 		else
-			paramCrossX = (beginPos[0] - floor(beginPos[0])) * paramDeltaX;
+		{
+			paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX;
+		}
 	}
 	else
-		paramCrossX = infinite; // Will never cross on X
+	{
+		paramCrossX = infinite;  // Will never cross on X
+	}
 
 	// Z initialization
 	if (ziStep != 0)
 	{
 		if (ziStep == 1)
-			paramCrossZ = (ceil(beginPos[2]) - beginPos[2]) * paramDeltaZ;
+		{
+			paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ;
+		}
 		else
-			paramCrossZ = (beginPos[2] - floor(beginPos[2])) * paramDeltaZ;
+		{
+			paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ;
+		}
 	}
 	else
-		paramCrossZ = infinite; // Will never cross on Z
+	{
+		paramCrossZ = infinite;  // Will never cross on Z
+	}
 
-	rs.x = static_cast<int>(floor(beginPos[0]));
-	rs.z = static_cast<int>(floor(beginPos[2]));
+	rs.x = static_cast<int>(floor(beginPos[indices[0]]));
+	rs.z = static_cast<int>(floor(beginPos[indices[2]]));
 
 	// Workaround cases where the ray starts at an integer position
 	if (paramCrossX == 0.0)
@@ -469,7 +492,9 @@ void gridRaycast(Action_T &quadAction, const btVector3 &beginPos, const btVector
 		// If going backwards, we should ignore the position we would get by the above flooring,
 		// because the ray is not heading in that direction
 		if (xiStep == -1)
+		{
 			rs.x -= 1;
+		}
 	}
 
 	if (paramCrossZ == 0.0)
@@ -513,14 +538,15 @@ void gridRaycast(Action_T &quadAction, const btVector3 &beginPos, const btVector
 			break;
 		}
 		else
+		{
 			quadAction(rs);
+		}
 	}
 }
 
-
 struct ProcessTrianglesAction
 {
-	const btHeightfieldTerrainShape *shape;
+	const btHeightfieldTerrainShape* shape;
 	bool flipQuadEdges;
 	bool useDiamondSubdivision;
 	int width;
@@ -529,11 +555,15 @@ struct ProcessTrianglesAction
 
 	void exec(int x, int z) const
 	{
-		if(x < 0 || z < 0 || x >= width || z >= length)
+		if (x < 0 || z < 0 || x >= width || z >= length)
+		{
 			return;
+		}
 
 		btVector3 vertices[3];
 
+		// TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit
+
 		// Check quad
 		if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0)))
 		{
@@ -565,16 +595,15 @@ struct ProcessTrianglesAction
 		}
 	}
 
-	void operator ()(const GridRaycastState &bs) const
+	void operator()(const GridRaycastState& bs) const
 	{
 		exec(bs.prev_x, bs.prev_z);
 	}
 };
 
-
 struct ProcessVBoundsAction
 {
-	const btHeightfieldTerrainShape::Range *vbounds;
+	const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& vbounds;
 	int width;
 	int length;
 	int chunkSize;
@@ -583,15 +612,23 @@ struct ProcessVBoundsAction
 	btVector3 rayEnd;
 	btVector3 rayDir;
 
+	int* m_indices;
 	ProcessTrianglesAction processTriangles;
 
-	void operator ()(const GridRaycastState &rs) const
+	ProcessVBoundsAction(const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& bnd, int* indices)
+		: vbounds(bnd),
+		m_indices(indices)
+	{
+	}
+	void operator()(const GridRaycastState& rs) const
 	{
 		int x = rs.prev_x;
 		int z = rs.prev_z;
 
-		if(x < 0 || z < 0 || x >= width || z >= length)
+		if (x < 0 || z < 0 || x >= width || z >= length)
+		{
 			return;
+		}
 
 		const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width];
 
@@ -608,10 +645,14 @@ struct ProcessVBoundsAction
 
 			// We did enter the flat projection of the AABB,
 			// but we have to check if we intersect it on the vertical axis
-			if (enterPos[1] > chunk.max && exitPos[1] > chunk.max)
+			if (enterPos[1] > chunk.max && exitPos[m_indices[1]] > chunk.max)
+			{
 				return;
-			if (enterPos[1] < chunk.min && exitPos[1] < chunk.min)
+			}
+			if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min)
+			{
 				return;
+			}
 		}
 		else
 		{
@@ -621,13 +662,12 @@ struct ProcessVBoundsAction
 			exitPos = rayEnd;
 		}
 
-		gridRaycast(processTriangles, enterPos, exitPos);
+		gridRaycast(processTriangles, enterPos, exitPos, m_indices);
 		// Note: it could be possible to have more than one grid at different levels,
 		// to do this there would be a branch using a pointer to another ProcessVBoundsAction
 	}
 };
 
-
 // TODO How do I interrupt the ray when there is a hit? `callback` does not return any result
 /// Performs a raycast using a hierarchical Bresenham algorithm.
 /// Does not allocate any memory by itself.
@@ -648,10 +688,16 @@ void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, con
 	processTriangles.length = m_heightStickLength - 1;
 
 	// TODO Transform vectors to account for m_upAxis
-	int iBeginX = static_cast<int>(floor(beginPos[0]));
-	int iBeginZ = static_cast<int>(floor(beginPos[2]));
-	int iEndX = static_cast<int>(floor(endPos[0]));
-	int iEndZ = static_cast<int>(floor(endPos[2]));
+	int indices[3] = { 0, 1, 2 };
+	if (m_upAxis == 2)
+	{
+		indices[1] = 2;
+		indices[2] = 1;
+	}
+	int iBeginX = static_cast<int>(floor(beginPos[indices[0]]));
+	int iBeginZ = static_cast<int>(floor(beginPos[indices[2]]));
+	int iEndX = static_cast<int>(floor(endPos[indices[0]]));
+	int iEndZ = static_cast<int>(floor(endPos[indices[2]]));
 
 	if (iBeginX == iEndX && iBeginZ == iEndZ)
 	{
@@ -662,36 +708,36 @@ void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, con
 		return;
 	}
 
-	if (m_vboundsGrid == NULL)
+	
+
+	if (m_vboundsGrid.size()==0)
 	{
 		// Process all quads intersecting the flat projection of the ray
-		gridRaycast(processTriangles, beginPos, endPos);
+		gridRaycast(processTriangles, beginPos, endPos, &indices[0]);
 	}
 	else
 	{
 		btVector3 rayDiff = endPos - beginPos;
-		btScalar flatDistance2 = rayDiff[0] * rayDiff[0] + rayDiff[2] * rayDiff[2];
+		btScalar flatDistance2 = rayDiff[indices[0]] * rayDiff[indices[0]] + rayDiff[indices[2]] * rayDiff[indices[2]];
 		if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize)
 		{
 			// Don't use chunks, the ray is too short in the plane
-			gridRaycast(processTriangles, beginPos, endPos);
+			gridRaycast(processTriangles, beginPos, endPos, &indices[0]);
 		}
 
-		ProcessVBoundsAction processVBounds;
+		ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]);
 		processVBounds.width = m_vboundsGridWidth;
 		processVBounds.length = m_vboundsGridLength;
-		processVBounds.vbounds = m_vboundsGrid;
 		processVBounds.rayBegin = beginPos;
 		processVBounds.rayEnd = endPos;
 		processVBounds.rayDir = rayDiff.normalized();
 		processVBounds.processTriangles = processTriangles;
 		processVBounds.chunkSize = m_vboundsChunkSize;
 		// The ray is long, run raycast on a higher-level grid
-		gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize);
+		gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize, indices);
 	}
 }
 
-
 /// Builds a grid data structure storing the min and max heights of the terrain in chunks.
 /// if chunkSize is zero, that accelerator is removed.
 /// If you modify the heights, you need to rebuild this accelerator.
@@ -708,11 +754,15 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
 	int nChunksZ = m_heightStickLength / chunkSize;
 
 	if (m_heightStickWidth % chunkSize > 0)
-		++nChunksX; // In case terrain size isn't dividable by chunk size
+	{
+		++nChunksX;  // In case terrain size isn't dividable by chunk size
+	}
 	if (m_heightStickLength % chunkSize > 0)
+	{
 		++nChunksZ;
+	}
 
-	if(m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ)
+	if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ)
 	{
 		clearAccelerator();
 		m_vboundsGridWidth = nChunksX;
@@ -720,13 +770,13 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
 	}
 
 	if (nChunksX == 0 || nChunksZ == 0)
+	{
 		return;
+	}
 
-	// TODO What is the recommended way to allocate this?
 	// This data structure is only reallocated if the required size changed
-	if (m_vboundsGrid == NULL)
-		m_vboundsGrid = new Range[nChunksX * nChunksZ];
-
+	m_vboundsGrid.resize(nChunksX * nChunksZ);
+	
 	// Compute min and max height for all chunks
 	for (int cz = 0; cz < nChunksZ; ++cz)
 	{
@@ -760,19 +810,27 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
 			for (int z = z0; z < z0 + chunkSize + 1; ++z)
 			{
 				if (z >= m_heightStickLength)
+				{
 					continue;
+				}
 
 				for (int x = x0; x < x0 + chunkSize + 1; ++x)
 				{
 					if (x >= m_heightStickWidth)
+					{
 						continue;
+					}
 
 					btScalar height = getRawHeightFieldValue(x, z);
 
 					if (height < r.min)
+					{
 						r.min = height;
+					}
 					else if (height > r.max)
+					{
 						r.max = height;
+					}
 				}
 			}
 
@@ -781,15 +839,7 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
 	}
 }
 
-
 void btHeightfieldTerrainShape::clearAccelerator()
 {
-	if (m_vboundsGrid)
-	{
-		// TODO What is the recommended way to deallocate this?
-		delete[] m_vboundsGrid;
-		m_vboundsGrid = 0;
-	}
-}
-
-
+	m_vboundsGrid.clear();
+}
\ No newline at end of file
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
index e23b548cb2..43e1d25e3d 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
@@ -17,7 +17,7 @@ subject to the following restrictions:
 #define BT_HEIGHTFIELD_TERRAIN_SHAPE_H
 
 #include "btConcaveShape.h"
-
+#include "LinearMath/btAlignedObjectArray.h"
 
 ///btHeightfieldTerrainShape simulates a 2D heightfield terrain
 /**
@@ -73,7 +73,8 @@ ATTRIBUTE_ALIGNED16(class)
 btHeightfieldTerrainShape : public btConcaveShape
 {
 public:
-	struct Range {
+	struct Range
+	{
 		btScalar min;
 		btScalar max;
 	};
@@ -102,13 +103,13 @@ protected:
 	bool m_flipQuadEdges;
 	bool m_useDiamondSubdivision;
 	bool m_useZigzagSubdivision;
-
+	bool m_flipTriangleWinding;
 	int m_upAxis;
 
 	btVector3 m_localScaling;
 
 	// Accelerator
-	Range *m_vboundsGrid;
+	btAlignedObjectArray<Range> m_vboundsGrid;
 	int m_vboundsGridWidth;
 	int m_vboundsGridLength;
 	int m_vboundsChunkSize;
@@ -157,6 +158,10 @@ public:
 	///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625
 	void setUseZigzagSubdivision(bool useZigzagSubdivision = true) { m_useZigzagSubdivision = useZigzagSubdivision; }
 
+	void setFlipTriangleWinding(bool flipTriangleWinding)
+	{
+		m_flipTriangleWinding = flipTriangleWinding;
+	}
 	virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const;
 
 	virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;
@@ -166,16 +171,20 @@ public:
 	virtual void setLocalScaling(const btVector3& scaling);
 
 	virtual const btVector3& getLocalScaling() const;
-	
-	void		getVertex(int x,int y,btVector3& vertex) const;
 
-	void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const;
+	void getVertex(int x, int y, btVector3& vertex) const;
+
+	void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget) const;
 
-	void buildAccelerator(int chunkSize=16);
+	void buildAccelerator(int chunkSize = 16);
 	void clearAccelerator();
 
+	int getUpAxis() const
+	{
+		return m_upAxis;
+	}
 	//debugging
 	virtual const char* getName() const { return "HEIGHTFIELD"; }
 };
 
-#endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
+#endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
\ No newline at end of file
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h
index 54888c6757..8f78c234b4 100644
--- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h
+++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h
@@ -28,28 +28,7 @@ subject to the following restrictions:
 
 #include "btBoxCollision.h"
 #include "btTriangleShapeEx.h"
-
-//! Overlapping pair
-struct GIM_PAIR
-{
-	int m_index1;
-	int m_index2;
-	GIM_PAIR()
-	{
-	}
-
-	GIM_PAIR(const GIM_PAIR& p)
-	{
-		m_index1 = p.m_index1;
-		m_index2 = p.m_index2;
-	}
-
-	GIM_PAIR(int index1, int index2)
-	{
-		m_index1 = index1;
-		m_index2 = index2;
-	}
-};
+#include "gim_pair.h" //for GIM_PAIR
 
 ///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box
 struct GIM_BVH_DATA
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
index 3d8ab9f520..73e3db1010 100644
--- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
@@ -18,7 +18,7 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 /*
-Author: Francisco Len Nßjera
+Author: Francisco Leon Najera
 Concave-Concave Collision
 
 */
@@ -590,14 +590,16 @@ void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrappe
 		}
 
 		btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0);
-		const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap();
+		const btCollisionObjectWrapper* prevObj;
 
 		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject())
 		{
+			prevObj = m_resultOut->getBody0Wrap();
 			m_resultOut->setBody0Wrap(&ob0);
 		}
 		else
 		{
+			prevObj = m_resultOut->getBody1Wrap();
 			m_resultOut->setBody1Wrap(&ob0);
 		}
 
@@ -610,7 +612,15 @@ void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrappe
 		{
 			shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1);
 		}
-		m_resultOut->setBody0Wrap(prevObj0);
+
+		if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject())
+		{
+			m_resultOut->setBody0Wrap(prevObj);
+		}
+		else
+		{
+			m_resultOut->setBody1Wrap(prevObj);
+		}
 	}
 
 	shape0->unlockChildShapes();
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h
index 5b85e87041..eb33ce05e2 100644
--- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h
+++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h
@@ -1,5 +1,5 @@
 /*! \file btGImpactShape.h
-\author Francisco Len Nßjera
+\author Francisco Leon Najera
 */
 /*
 This source file is part of GIMPACT Library.
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h
index 0522007e4f..afc591dac0 100644
--- a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h
+++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h
@@ -37,28 +37,7 @@ email: projectileman@yahoo.com
 #include "gim_radixsort.h"
 #include "gim_box_collision.h"
 #include "gim_tri_collision.h"
-
-//! Overlapping pair
-struct GIM_PAIR
-{
-	GUINT m_index1;
-	GUINT m_index2;
-	GIM_PAIR()
-	{
-	}
-
-	GIM_PAIR(const GIM_PAIR& p)
-	{
-		m_index1 = p.m_index1;
-		m_index2 = p.m_index2;
-	}
-
-	GIM_PAIR(GUINT index1, GUINT index2)
-	{
-		m_index1 = index1;
-		m_index2 = index2;
-	}
-};
+#include "gim_pair.h"
 
 //! A pairset array
 class gim_pair_set : public gim_array<GIM_PAIR>
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_pair.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_pair.h
new file mode 100644
index 0000000000..56c185a5dc
--- /dev/null
+++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_pair.h
@@ -0,0 +1,28 @@
+#ifndef GIM_PAIR_H
+#define GIM_PAIR_H
+
+
+//! Overlapping pair
+struct GIM_PAIR
+{
+        int m_index1;
+        int m_index2;
+        GIM_PAIR()
+        {
+        }
+
+        GIM_PAIR(const GIM_PAIR& p)
+        {
+                m_index1 = p.m_index1;
+                m_index2 = p.m_index2;
+        }
+
+        GIM_PAIR(int index1, int index2)
+        {
+                m_index1 = index1;
+                m_index2 = index2;
+        }
+};
+
+#endif //GIM_PAIR_H
+
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h
index 76f54699c5..77b19be599 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h
@@ -23,11 +23,11 @@ class btMinkowskiSumShape;
 #include "LinearMath/btIDebugDraw.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define MAX_ITERATIONS 64
-#define MAX_EPSILON (SIMD_EPSILON * 10)
+#define MAX_CONVEX_CAST_ITERATIONS 64
+#define MAX_CONVEX_CAST_EPSILON (SIMD_EPSILON * 10)
 #else
-#define MAX_ITERATIONS 32
-#define MAX_EPSILON btScalar(0.0001)
+#define MAX_CONVEX_CAST_ITERATIONS 32
+#define MAX_CONVEX_CAST_EPSILON btScalar(0.0001)
 #endif
 ///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
 ///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
@@ -58,8 +58,8 @@ public:
 			: m_fraction(btScalar(BT_LARGE_FLOAT)),
 			  m_debugDrawer(0),
 			  m_allowedPenetration(btScalar(0)),
-			  m_subSimplexCastMaxIterations(MAX_ITERATIONS),
-			  m_subSimplexCastEpsilon(MAX_EPSILON)
+			  m_subSimplexCastMaxIterations(MAX_CONVEX_CAST_ITERATIONS),
+			  m_subSimplexCastEpsilon(MAX_CONVEX_CAST_EPSILON)
 		{
 		}
 
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index 803f6e0671..4339b2ea75 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -1,4 +1,4 @@
-/*
+/*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
index 2b2dfabec2..2d0df718a2 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
@@ -37,6 +37,7 @@ public:
 													 ///SubSimplexConvexCastRaytest is the default, even if kF_None is set.
 		kF_UseSubSimplexConvexCastRaytest = 1 << 2,  // Uses an approximate but faster ray versus convex intersection algorithm
 		kF_UseGjkConvexCastRaytest = 1 << 3,
+		kF_DisableHeightfieldAccelerator  = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062
 		kF_Terminator = 0xFFFFFFFF
 	};
 	unsigned int m_flags;