Merge pull request #28928 from godotengine/revert-28328-revert-25543-optimize_bullet_heightfield_raycast2

Revert "Revert "Implemented terrain raycast acceleration""

4 files changed, 463 insertions, 5 deletions

diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp
index b590d63167..f15bcec914 100644
--- a/modules/bullet/shape_bullet.cpp
+++ b/modules/bullet/shape_bullet.cpp
@@ -148,7 +148,13 @@ btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVector<rea
 	const bool flipQuadEdges = false;
 	const void *heightsPtr = p_heights.read().ptr();
 
-	return bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, p_min_height, p_max_height, YAxis, PHY_FLOAT, flipQuadEdges));
+	btHeightfieldTerrainShape *heightfield = bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, p_min_height, p_max_height, YAxis, PHY_FLOAT, flipQuadEdges));
+
+	// The shape can be created without params when you do PhysicsServer.shape_create(PhysicsServer.SHAPE_HEIGHTMAP)
+	if (heightsPtr)
+		heightfield->buildAccelerator(16);
+
+	return heightfield;
 }
 
 btRayShape *ShapeBullet::create_shape_ray(real_t p_length, bool p_slips_on_slope) {
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index 782e9efaf1..b30ce03164 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -19,9 +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/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,18 @@ 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)
+			{
+				///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
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
index c85ce2498e..4adf27e6bb 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
@@ -73,6 +73,10 @@ void btHeightfieldTerrainShape::initialize(
 	m_useZigzagSubdivision = 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;
 
 	// determine min/max axis-aligned bounding box (aabb) values
 	switch (m_upAxis)
@@ -108,6 +112,7 @@ void btHeightfieldTerrainShape::initialize(
 
 btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 {
+	clearAccelerator();
 }
 
 void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
@@ -323,6 +328,8 @@ 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++)
@@ -373,3 +380,416 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
 {
 	return m_localScaling;
 }
+
+
+
+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)
+{
+	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];
+	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[0]) - beginPos[0]) * paramDeltaX;
+		else
+			paramCrossX = (beginPos[0] - floor(beginPos[0])) * paramDeltaX;
+	}
+	else
+		paramCrossX = infinite; // Will never cross on X
+
+	// Z initialization
+	if (ziStep != 0)
+	{
+		if (ziStep == 1)
+			paramCrossZ = (ceil(beginPos[2]) - beginPos[2]) * paramDeltaZ;
+		else
+			paramCrossZ = (beginPos[2] - floor(beginPos[2])) * paramDeltaZ;
+	}
+	else
+		paramCrossZ = infinite; // Will never cross on Z
+
+	rs.x = static_cast<int>(floor(beginPos[0]));
+	rs.z = static_cast<int>(floor(beginPos[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];
+
+		// 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 btHeightfieldTerrainShape::Range *vbounds;
+	int width;
+	int length;
+	int chunkSize;
+
+	btVector3 rayBegin;
+	btVector3 rayEnd;
+	btVector3 rayDir;
+
+	ProcessTrianglesAction processTriangles;
+
+	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[1] > chunk.max)
+				return;
+			if (enterPos[1] < chunk.min && exitPos[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);
+		// 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 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]));
+
+	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 == NULL)
+	{
+		// Process all quads intersecting the flat projection of the ray
+		gridRaycast(processTriangles, beginPos, endPos);
+	}
+	else
+	{
+		btVector3 rayDiff = endPos - beginPos;
+		btScalar flatDistance2 = rayDiff[0] * rayDiff[0] + rayDiff[2] * rayDiff[2];
+		if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize)
+		{
+			// Don't use chunks, the ray is too short in the plane
+			gridRaycast(processTriangles, beginPos, endPos);
+		}
+
+		ProcessVBoundsAction processVBounds;
+		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);
+	}
+}
+
+
+/// 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;
+
+	// 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];
+
+	// 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()
+{
+	if (m_vboundsGrid)
+	{
+		// TODO What is the recommended way to deallocate this?
+		delete[] m_vboundsGrid;
+		m_vboundsGrid = 0;
+	}
+}
+
+
diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
index 8a50a57e31..e23b548cb2 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "btConcaveShape.h"
 
+
 ///btHeightfieldTerrainShape simulates a 2D heightfield terrain
 /**
   The caller is responsible for maintaining the heightfield array; this
@@ -71,6 +72,12 @@ 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;
@@ -100,9 +107,14 @@ protected:
 
 	btVector3 m_localScaling;
 
+	// Accelerator
+	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
 	/**
@@ -154,6 +166,13 @@ 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 buildAccelerator(int chunkSize=16);
+	void clearAccelerator();
 
 	//debugging
 	virtual const char* getName() const { return "HEIGHTFIELD"; }