Merge pull request bulletphysics#3287 from glebm/heightfield-accel

heightfield: improve `processAllTriangles` performance

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -232,6 +232,30 @@ getQuantized(
 	return (int)(x + 0.5);
 }
 
+// Equivalent to std::minmax({a, b, c}).
+// Performs at most 3 comparisons.
+static btHeightfieldTerrainShape::Range minmaxRange(btScalar a, btScalar b, btScalar c)
+{
+	if (a > b)
+	{
+		if (b > c)
+			return btHeightfieldTerrainShape::Range(c, a);
+		else if (a > c)
+			return btHeightfieldTerrainShape::Range(b, a);
+		else
+			return btHeightfieldTerrainShape::Range(b, c);
+	}
+	else
+	{
+		if (a > c)
+			return btHeightfieldTerrainShape::Range(c, b);
+		else if (b > c)
+			return btHeightfieldTerrainShape::Range(a, b);
+		else
+			return btHeightfieldTerrainShape::Range(a, c);
+	}
+}
+
 /// given input vector, return quantized version
 /**
   This routine is basically determining the gridpoint indices for a given
@@ -334,7 +358,8 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 	}
 
 	// TODO If m_vboundsGrid is available, use it to determine if we really need to process this area
-
+
+	const Range aabbUpRange(aabbMin[m_upAxis], aabbMax[m_upAxis]);
 	for (int j = startJ; j < endJ; j++)
 	{
 		for (int x = startX; x < endX; x++)
@@ -349,29 +374,51 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 
 			if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1)))
 			{
-				//first triangle
 				getVertex(x, j, vertices[indices[0]]);
 				getVertex(x, j + 1, vertices[indices[1]]);
 				getVertex(x + 1, j + 1, vertices[indices[2]]);
-				callback->processTriangle(vertices, 2 * x, j);
-				//second triangle
-				//  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
-				getVertex(x + 1, j + 1, vertices[indices[1]]);
+
+				// Skip triangle processing if the triangle is out-of-AABB.
+				Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
+					callback->processTriangle(vertices, 2 * x, j);
+
+				// already set: getVertex(x, j, vertices[indices[0]])
+
+				// equivalent to: getVertex(x + 1, j + 1, vertices[indices[1]]);
+				vertices[indices[1]] = vertices[indices[2]];
+
 				getVertex(x + 1, j, vertices[indices[2]]);
-				callback->processTriangle(vertices, 2 * x+1, j);
+				upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]);
+				upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
+					callback->processTriangle(vertices, 2 * x + 1, j);
 			}
 			else
 			{
-				//first triangle
 				getVertex(x, j, vertices[indices[0]]);
 				getVertex(x, j + 1, vertices[indices[1]]);
 				getVertex(x + 1, j, vertices[indices[2]]);
-				callback->processTriangle(vertices, 2 * x, j);
-				//second triangle
-				getVertex(x + 1, j, vertices[indices[0]]);
-				//getVertex(x,j+1,vertices[1]);
+
+				// Skip triangle processing if the triangle is out-of-AABB.
+				Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
+					callback->processTriangle(vertices, 2 * x, j);
+
+				// already set: getVertex(x, j + 1, vertices[indices[1]]);
+
+				// equivalent to: getVertex(x + 1, j, vertices[indices[0]]);
+				vertices[indices[0]] = vertices[indices[2]];
+
 				getVertex(x + 1, j + 1, vertices[indices[2]]);
-				callback->processTriangle(vertices, 2 * x+1, j);
+				upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]);
+				upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]);
+
+				if (upRange.overlaps(aabbUpRange))
+					callback->processTriangle(vertices, 2 * x + 1, j);
 			}
 		}
 	}
@@ -846,4 +893,4 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
 void btHeightfieldTerrainShape::clearAccelerator()
 {
 	m_vboundsGrid.clear();
-}
+}

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

@@ -75,6 +75,14 @@ btHeightfieldTerrainShape : public btConcaveShape
 public:
 	struct Range
 	{
+		Range() {}
+		Range(btScalar min, btScalar max) : min(min), max(max) {}
+
+		bool overlaps(const Range& other) const
+		{
+			return !(min > other.max || max < other.min);
+		}
+
 		btScalar min;
 		btScalar max;
 	};
@@ -218,4 +226,4 @@ btHeightfieldTerrainShape : public btConcaveShape
 	}
 };
 
-#endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
+#endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H

test/collision/CMakeLists.txt

@@ -25,10 +25,13 @@ ENDIF()
 		../../src/BulletCollision/CollisionShapes/btSphereShape.cpp
 		../../src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
 		../../src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp
+		../../src/BulletCollision/CollisionShapes/btConcaveShape.cpp
 		../../src/BulletCollision/CollisionShapes/btConvexShape.cpp
 		../../src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp
 		../../src/BulletCollision/CollisionShapes/btCollisionShape.cpp
 		../../src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp
+		../../src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+		../../src/BulletCollision/CollisionShapes/btTriangleCallback.cpp
 	)
 
 ADD_TEST(Test_Collision_PASS Test_Collision)

test/collision/main.cpp

@@ -20,16 +20,21 @@ subject to the following restrictions:
 ///Todo: the test needs proper coverage and using a convex hull point cloud
 ///Also the GJK, EPA and MPR should be improved, both quality and performance
 
+#include <vector>
+
 #include <gtest/gtest.h>
 
 #include "SphereSphereCollision.h"
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
 
 #include "BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa3.h"
 #include "BulletCollision/NarrowPhaseCollision/btMprPenetration.h"
 
+namespace {
+
 btVector3 MyBulletShapeSupportFunc(const void* shapeAptr, const btVector3& dir, bool includeMargin)
 {
 	btConvexShape* shape = (btConvexShape*)shapeAptr;
@@ -249,6 +254,48 @@ TEST(BulletCollisionTest, AnalyticSphereSphereDistance)
 	testSphereSphereDistance(SSTM_ANALYTIC, 0.00001);
 }
 
+class TriangleCollector : public btTriangleCallback
+{
+public:
+	std::vector<btVector3> *triangles;
+
+	explicit TriangleCollector(std::vector<btVector3>* triangles) : triangles(triangles) {}
+	virtual ~TriangleCollector() {}
+
+	virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+	{
+		triangles->push_back(*triangle);
+	}
+};
+
+TEST(BulletCollisionTest, Heightfield_ProcessAllTriangles_FiltersByUpAxis)
+{
+	// A flat 2x2 heightfield.
+	const btScalar heightFieldData[] = {
+		10.0, 10.0,
+		10.0, 10.0,
+	};
+	btHeightfieldTerrainShape shape(
+		/*heightStickWidth=*/2, /*heightStickLength=*/2,
+		&heightFieldData[0], /*heightScale=*/1,
+		/*minHeight=*/-10.0, /*maxHeight=*/10.0,
+		/*upAxis=*/2, PHY_FLOAT, /*flipQuadEdges=*/false);
+
+	std::vector<btVector3> triangles;
+	TriangleCollector collector(&triangles);
+
+	// AABB overlaps with the heightfield on upAxis.
+	shape.processAllTriangles(&collector, btVector3(0, 0, 0), btVector3(20, 20, 20));
+	EXPECT_EQ(triangles.size(), 2);
+
+	// AABB does not overlap with the heightfield on upAxis.
+	triangles.clear();
+	shape.processAllTriangles(&collector, btVector3(0, 0, 0), btVector3(20, 20, 5));
+	EXPECT_EQ(triangles.size(), 0);
+}
+
+}  // namespace
+
 int main(int argc, char** argv)
 {
 #if _MSC_VER