1 files changed, 128 insertions, 78 deletions

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