3rdparty: update bullet3

3rdparty/bullet3/src/Bullet3Common/b3Vector3.h

@@ -36,7 +36,7 @@ subject to the following restrictions:
 #pragma warning(disable : 4556)  // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
 #endif
 
-#define B3_SHUFFLE(x, y, z, w) ((w) << 6 | (z) << 4 | (y) << 2 | (x))
+#define B3_SHUFFLE(x, y, z, w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
 //#define b3_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
 #define b3_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask))
 #define b3_splat3_ps(_a, _i) b3_pshufd_ps((_a), B3_SHUFFLE(_i, _i, _i, 3))

3rdparty/bullet3/src/BulletCollision/BroadphaseCollision/btDbvt.cpp

@@ -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]);

3rdparty/bullet3/src/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();
 }
 

3rdparty/bullet3/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -22,6 +22,7 @@ subject to the following restrictions:
 #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"
@@ -413,6 +414,20 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 				triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled);
 			}
+			else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) 
+				&& collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE 
+				)
+			{
+				///optimized version for btHeightfieldTerrainShape
+				btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape;
+				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+				BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+				heightField->performRaycast(&rcb, rayFromLocal, rayToLocal);
+			}
 			else
 			{
 				//generic (slower) case

3rdparty/bullet3/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -71,8 +71,13 @@ 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_vboundsChunkSize = 0;
+	m_vboundsGridWidth = 0;
+	m_vboundsGridLength = 0;
 
 	// determine min/max axis-aligned bounding box (aabb) values
 	switch (m_upAxis)
@@ -108,6 +113,7 @@ void btHeightfieldTerrainShape::initialize(
 
 btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 {
+	clearAccelerator();
 }
 
 void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
@@ -323,35 +329,44 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 		}
 	}
 
+	// TODO If m_vboundsGrid is available, use it to determine if we really need to process this area
+
 	for (int j = startJ; j < endJ; j++)
 	{
 		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);
 			}
 		}
@@ -373,3 +388,458 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
 {
 	return m_localScaling;
 }
+
+namespace
+{
+	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, 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[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)
+	{
+		// Consider the ray vertical
+		rayDirectionFlatX = 0;
+		rayDirectionFlatZ = 0;
+	}
+	else
+	{
+		rayDirectionFlatX /= rs.maxDistanceFlat;
+		rayDirectionFlatZ /= rs.maxDistanceFlat;
+	}
+
+	const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0;
+	const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0;
+
+	const float infinite = 9999999;
+	const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite;
+	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?
+
+	// paramCrossX and paramCrossZ are initialized as being the first cross
+	// X initialization
+	if (xiStep != 0)
+	{
+		if (xiStep == 1)
+		{
+			paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX;
+		}
+		else
+		{
+			paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX;
+		}
+	}
+	else
+	{
+		paramCrossX = infinite;  // Will never cross on X
+	}
+
+	// Z initialization
+	if (ziStep != 0)
+	{
+		if (ziStep == 1)
+		{
+			paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ;
+		}
+		else
+		{
+			paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ;
+		}
+	}
+	else
+	{
+		paramCrossZ = infinite;  // Will never cross on Z
+	}
+
+	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)
+	{
+		paramCrossX += paramDeltaX;
+		// 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)
+	{
+		paramCrossZ += paramDeltaZ;
+		if (ziStep == -1)
+			rs.z -= 1;
+	}
+
+	rs.prev_x = rs.x;
+	rs.prev_z = rs.z;
+	rs.param = 0;
+
+	while (true)
+	{
+		rs.prev_x = rs.x;
+		rs.prev_z = rs.z;
+		rs.prevParam = rs.param;
+
+		if (paramCrossX < paramCrossZ)
+		{
+			// X lane
+			rs.x += xiStep;
+			// Assign before advancing the param,
+			// to be in sync with the initialization step
+			rs.param = paramCrossX;
+			paramCrossX += paramDeltaX;
+		}
+		else
+		{
+			// Z lane
+			rs.z += ziStep;
+			rs.param = paramCrossZ;
+			paramCrossZ += paramDeltaZ;
+		}
+
+		if (rs.param > rs.maxDistanceFlat)
+		{
+			rs.param = rs.maxDistanceFlat;
+			quadAction(rs);
+			break;
+		}
+		else
+		{
+			quadAction(rs);
+		}
+	}
+}
+
+struct ProcessTrianglesAction
+{
+	const btHeightfieldTerrainShape* shape;
+	bool flipQuadEdges;
+	bool useDiamondSubdivision;
+	int width;
+	int length;
+	btTriangleCallback* callback;
+
+	void exec(int x, int z) const
+	{
+		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)))
+		{
+			// First triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x + 1, z, vertices[1]);
+			shape->getVertex(x + 1, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+
+			// Second triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x + 1, z + 1, vertices[1]);
+			shape->getVertex(x, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+		}
+		else
+		{
+			// First triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x, z + 1, vertices[1]);
+			shape->getVertex(x + 1, z, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+
+			// Second triangle
+			shape->getVertex(x + 1, z, vertices[0]);
+			shape->getVertex(x, z + 1, vertices[1]);
+			shape->getVertex(x + 1, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+		}
+	}
+
+	void operator()(const GridRaycastState& bs) const
+	{
+		exec(bs.prev_x, bs.prev_z);
+	}
+};
+
+struct ProcessVBoundsAction
+{
+	const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& vbounds;
+	int width;
+	int length;
+	int chunkSize;
+
+	btVector3 rayBegin;
+	btVector3 rayEnd;
+	btVector3 rayDir;
+
+	int* m_indices;
+	ProcessTrianglesAction processTriangles;
+
+	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)
+		{
+			return;
+		}
+
+		const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width];
+
+		btVector3 enterPos;
+		btVector3 exitPos;
+
+		if (rs.maxDistanceFlat > 0.0001)
+		{
+			btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat;
+			btScalar enterParam3d = rs.prevParam * flatTo3d;
+			btScalar exitParam3d = rs.param * flatTo3d;
+			enterPos = rayBegin + rayDir * enterParam3d;
+			exitPos = rayBegin + rayDir * exitParam3d;
+
+			// 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[m_indices[1]] > chunk.max)
+			{
+				return;
+			}
+			if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min)
+			{
+				return;
+			}
+		}
+		else
+		{
+			// Consider the ray vertical
+			// (though we shouldn't reach this often because there is an early check up-front)
+			enterPos = rayBegin;
+			exitPos = rayEnd;
+		}
+
+		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.
+void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const
+{
+	// Transform to cell-local
+	btVector3 beginPos = raySource / m_localScaling;
+	btVector3 endPos = rayTarget / m_localScaling;
+	beginPos += m_localOrigin;
+	endPos += m_localOrigin;
+
+	ProcessTrianglesAction processTriangles;
+	processTriangles.shape = this;
+	processTriangles.flipQuadEdges = m_flipQuadEdges;
+	processTriangles.useDiamondSubdivision = m_useDiamondSubdivision;
+	processTriangles.callback = callback;
+	processTriangles.width = m_heightStickWidth - 1;
+	processTriangles.length = m_heightStickLength - 1;
+
+	// TODO Transform vectors to account for m_upAxis
+	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)
+	{
+		// The ray will never cross quads within the plane,
+		// so directly process triangles within one quad
+		// (typically, vertical rays should end up here)
+		processTriangles.exec(iBeginX, iEndZ);
+		return;
+	}
+
+	
+
+	if (m_vboundsGrid.size()==0)
+	{
+		// Process all quads intersecting the flat projection of the ray
+		gridRaycast(processTriangles, beginPos, endPos, &indices[0]);
+	}
+	else
+	{
+		btVector3 rayDiff = endPos - beginPos;
+		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, &indices[0]);
+		}
+
+		ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]);
+		processVBounds.width = m_vboundsGridWidth;
+		processVBounds.length = m_vboundsGridLength;
+		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, 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.
+void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
+{
+	if (chunkSize <= 0)
+	{
+		clearAccelerator();
+		return;
+	}
+
+	m_vboundsChunkSize = chunkSize;
+	int nChunksX = m_heightStickWidth / chunkSize;
+	int nChunksZ = m_heightStickLength / chunkSize;
+
+	if (m_heightStickWidth % chunkSize > 0)
+	{
+		++nChunksX;  // In case terrain size isn't dividable by chunk size
+	}
+	if (m_heightStickLength % chunkSize > 0)
+	{
+		++nChunksZ;
+	}
+
+	if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ)
+	{
+		clearAccelerator();
+		m_vboundsGridWidth = nChunksX;
+		m_vboundsGridLength = nChunksZ;
+	}
+
+	if (nChunksX == 0 || nChunksZ == 0)
+	{
+		return;
+	}
+
+	// This data structure is only reallocated if the required size changed
+	m_vboundsGrid.resize(nChunksX * nChunksZ);
+	
+	// Compute min and max height for all chunks
+	for (int cz = 0; cz < nChunksZ; ++cz)
+	{
+		int z0 = cz * chunkSize;
+
+		for (int cx = 0; cx < nChunksX; ++cx)
+		{
+			int x0 = cx * chunkSize;
+
+			Range r;
+
+			r.min = getRawHeightFieldValue(x0, z0);
+			r.max = r.min;
+
+			// Compute min and max height for this chunk.
+			// We have to include one extra cell to account for neighbors.
+			// Here is why:
+			// Say we have a flat terrain, and a plateau that fits a chunk perfectly.
+			//
+			//   Left        Right
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			//           x
+			//
+			// If the AABB for the Left chunk did not share vertices with the Right,
+			// then we would fail collision tests at x due to a gap.
+			//
+			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;
+					}
+				}
+			}
+
+			m_vboundsGrid[cx + cz * nChunksX] = r;
+		}
+	}
+}
+
+void btHeightfieldTerrainShape::clearAccelerator()
+{
+	m_vboundsGrid.clear();
+}

3rdparty/bullet3/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

@@ -17,6 +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
 /**
@@ -71,6 +72,13 @@ subject to the following restrictions:
 ATTRIBUTE_ALIGNED16(class)
 btHeightfieldTerrainShape : public btConcaveShape
 {
+public:
+	struct Range
+	{
+		btScalar min;
+		btScalar max;
+	};
+
 protected:
 	btVector3 m_localAabbMin;
 	btVector3 m_localAabbMax;
@@ -95,14 +103,19 @@ protected:
 	bool m_flipQuadEdges;
 	bool m_useDiamondSubdivision;
 	bool m_useZigzagSubdivision;
-
+	bool m_flipTriangleWinding;
 	int m_upAxis;
 
 	btVector3 m_localScaling;
 
+	// Accelerator
+	btAlignedObjectArray<Range> m_vboundsGrid;
+	int m_vboundsGridWidth;
+	int m_vboundsGridLength;
+	int m_vboundsChunkSize;
+
 	virtual btScalar getRawHeightFieldValue(int x, int y) const;
 	void quantizeWithClamp(int* out, const btVector3& point, int isMax) const;
-	void getVertex(int x, int y, btVector3& vertex) const;
 
 	/// protected initialization
 	/**
@@ -145,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;
@@ -155,8 +172,19 @@ public:
 
 	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 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

3rdparty/bullet3/src/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/
 

3rdparty/bullet3/src/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;

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp

@@ -22,6 +22,8 @@ subject to the following restrictions:
 
 #include <string.h>  //for memset
 
+#include <cmath>
+
 const int kNoMerge = -1;
 
 bool btBatchedConstraints::s_debugDrawBatches = false;
@@ -520,7 +522,7 @@ static void writeGrainSizes(btBatchedConstraints* bc)
 	{
 		const Range& phase = bc->m_phases[iPhase];
 		int numBatches = phase.end - phase.begin;
-		float grainSize = floor((0.25f * numBatches / float(numThreads)) + 0.0f);
+		float grainSize = std::floor((0.25f * numBatches / float(numThreads)) + 0.0f);
 		bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize));
 	}
 }

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -19,6 +19,7 @@ Written by: Marcus Hennix
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btMinMax.h"
+#include <cmath>
 #include <new>
 
 //#define CONETWIST_USE_OBSOLETE_SOLVER true
@@ -842,7 +843,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
 			btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
 			norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
 			btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
-			swingLimit = sqrt(swingLimit2);
+			swingLimit = std::sqrt(swingLimit2);
 		}
 
 		// test!
@@ -887,7 +888,7 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
 		btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
 		norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
 		btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
-		swingLimit = sqrt(swingLimit2);
+		swingLimit = std::sqrt(swingLimit2);
 	}
 
 	// convert into point in constraint space:

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -64,6 +64,7 @@ struct btContactSolverInfoData
 	btScalar m_restitutionVelocityThreshold;
 	bool m_jointFeedbackInWorldSpace;
 	bool m_jointFeedbackInJointFrame;
+	int m_reportSolverAnalytics;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
@@ -98,6 +99,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_restitutionVelocityThreshold = 0.2f;  //if the relative velocity is below this threshold, there is zero restitution
 		m_jointFeedbackInWorldSpace = false;
 		m_jointFeedbackInJointFrame = false;
+		m_reportSolverAnalytics = 0;
 	}
 };
 

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -40,6 +40,7 @@ http://gimpact.sf.net
 #include "btGeneric6DofSpring2Constraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include <cmath>
 #include <new>
 
 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
@@ -845,7 +846,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		if (m_rbA.getInvMass() == 0) m = mB; else
 		if (m_rbB.getInvMass() == 0) m = mA; else
 			m = mA*mB / (mA + mB);
-		btScalar angularfreq = sqrt(ks / m);
+		btScalar angularfreq = btSqrt(ks / m);
 
 		//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
 		if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
@@ -1085,7 +1086,7 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr
 		btScalar target = targetOrg + SIMD_PI;
 		if (1)
 		{
-			btScalar m = target - SIMD_2_PI * floor(target / SIMD_2_PI);
+			btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
 			// handle boundary cases resulted from floating-point cut off:
 			{
 				if (m >= SIMD_2_PI)

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -2239,6 +2239,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 #ifdef VERBOSE_RESIDUAL_PRINTF
 				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
 #endif
+				m_analyticsData.m_numSolverCalls++;
+				m_analyticsData.m_numIterationsUsed = iteration+1;
+				m_analyticsData.m_islandId = -2;
+				if (numBodies>0)
+					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+				m_analyticsData.m_numBodies = numBodies;
+				m_analyticsData.m_numContactManifolds = numManifolds;
+				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
 				break;
 			}
 		}

3rdparty/bullet3/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -91,10 +91,29 @@ struct btSISolverSingleIterationData
 	}
 };
 
+struct btSolverAnalyticsData
+{
+	btSolverAnalyticsData()
+	{
+		m_numSolverCalls = 0;
+		m_numIterationsUsed = -1;
+		m_remainingLeastSquaresResidual = -1;
+		m_islandId = -2;
+	}
+	int m_islandId;
+	int m_numBodies;
+	int m_numContactManifolds;
+	int m_numSolverCalls;
+	int m_numIterationsUsed;
+	double m_remainingLeastSquaresResidual;
+};
+
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class)
 btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+	
+
 protected:
 	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
 	btConstraintArray m_tmpSolverContactConstraintPool;
@@ -283,6 +302,8 @@ public:
 		m_resolveSingleConstraintRowLowerLimit = rowSolver;
 	}
 
+
+
 	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
 	static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
 	static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
@@ -296,6 +317,7 @@ public:
 	static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
 	static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
 
+	btSolverAnalyticsData m_analyticsData;
 };
 
 #endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -344,6 +344,7 @@ void btMultiBody::finalizeMultiDof()
 	m_deltaV.resize(6 + m_dofCount);
 	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
 	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
+	m_matrixBuf.resize(m_links.size() + 1);
 	for (int i = 0; i < m_vectorBuf.size(); i++)
 	{
 		m_vectorBuf[i].setValue(0, 0, 0);

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -207,6 +207,7 @@ public:
 	}
 };
 
+
 struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 {
 	btContactSolverInfo* m_solverInfo;
@@ -224,6 +225,8 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
 
+	btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
+
 	MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
 										 btDispatcher* dispatcher)
 		: m_solverInfo(NULL),
@@ -235,7 +238,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 	{
 	}
 
-	MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
+	MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
 	{
 		btAssert(0);
 		(void)other;
@@ -244,6 +247,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 
 	SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
 	{
+		m_islandAnalyticsData.clear();
 		btAssert(solverInfo);
 		m_solverInfo = solverInfo;
 
@@ -270,6 +274,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		{
 			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
 			m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+			if (m_solverInfo->m_reportSolverAnalytics&1)
+			{
+				m_solver->m_analyticsData.m_islandId = islandId;
+				m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+			}
 		}
 		else
 		{
@@ -335,7 +344,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 
 				if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
 				{
-					processConstraints();
+					processConstraints(islandId);
 				}
 				else
 				{
@@ -344,7 +353,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 			}
 		}
 	}
-	void processConstraints()
+	void processConstraints(int islandId=-1)
 	{
 		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
 		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
@@ -354,6 +363,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
 
 		m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+		if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+		{
+			m_solver->m_analyticsData.m_islandId = islandId;
+			m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+		}
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
 		m_constraints.resize(0);
@@ -361,6 +375,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 	}
 };
 
+void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
+{
+	islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
+}
+
 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
 	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
 	  m_multiBodyConstraintSolver(constraintSolver)
@@ -717,13 +736,17 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 				m_scratch_v.resize(bod->getNumLinks() + 1);
 				m_scratch_m.resize(bod->getNumLinks() + 1);
 
+				if (bod->internalNeedsJointFeedback())
 				{
 					if (!bod->isUsingRK4Integration())
 					{
-						bool isConstraintPass = true;
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
-						getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
+						if (bod->internalNeedsJointFeedback())
+						{
+							bool isConstraintPass = true;
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+								getSolverInfo().m_jointFeedbackInWorldSpace,
+								getSolverInfo().m_jointFeedbackInJointFrame);
+						}
 					}
 				}
 			}

3rdparty/bullet3/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -109,5 +109,7 @@ public:
 	virtual void serialize(btSerializer* serializer);
 	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
 	virtual void setConstraintSolver(btConstraintSolver* solver);
+	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H

3rdparty/bullet3/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp

@@ -45,14 +45,14 @@ subject to the following restrictions:
 
 int btGetNumHardwareThreads()
 {
-	return btMin<int>(BT_MAX_THREAD_COUNT, std::thread::hardware_concurrency());
+	return btMax(1u, btMin(BT_MAX_THREAD_COUNT, std::thread::hardware_concurrency()));
 }
 
 #else
 
 int btGetNumHardwareThreads()
 {
-	return btMin<int>(BT_MAX_THREAD_COUNT, sysconf(_SC_NPROCESSORS_ONLN));
+	return btMax(1, btMin<int>(BT_MAX_THREAD_COUNT, sysconf(_SC_NPROCESSORS_ONLN)));
 }
 
 #endif

3rdparty/bullet3/src/LinearMath/btVector3.h

@@ -36,7 +36,7 @@ subject to the following restrictions:
 #pragma warning(disable : 4556)  // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
 #endif
 
-#define BT_SHUFFLE(x, y, z, w) ((w) << 6 | (z) << 4 | (y) << 2 | (x))
+#define BT_SHUFFLE(x, y, z, w) (((w) << 6 | (z) << 4 | (y) << 2 | (x)) & 0xff)
 //#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
 #define bt_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask))
 #define bt_splat3_ps(_a, _i) bt_pshufd_ps((_a), BT_SHUFFLE(_i, _i, _i, 3))