3 files changed, 149 insertions, 90 deletions

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