Merge remote-tracking branch 'bp/master' into master

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

@@ -17,6 +17,47 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransformUtil.h"
 
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength,
+	const float* heightfieldData, btScalar minHeight, btScalar maxHeight,
+	int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_FLOAT,
+			   flipQuadEdges);
+}
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const double* heightfieldData,
+	btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_DOUBLE,
+			   flipQuadEdges);
+}
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const short* heightfieldData, btScalar heightScale,
+	btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   heightScale, minHeight, maxHeight, upAxis, PHY_SHORT,
+			   flipQuadEdges);
+}
+
+btHeightfieldTerrainShape::btHeightfieldTerrainShape(
+	int heightStickWidth, int heightStickLength, const unsigned char* heightfieldData, btScalar heightScale,
+	btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
+	: m_userValue3(0), m_triangleInfoMap(0)
+{
+	initialize(heightStickWidth, heightStickLength, heightfieldData,
+			   heightScale, minHeight, maxHeight, upAxis, PHY_UCHAR,
+			   flipQuadEdges);
+}
+
 btHeightfieldTerrainShape::btHeightfieldTerrainShape(
 	int heightStickWidth, int heightStickLength, const void* heightfieldData,
 	btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
@@ -24,6 +65,10 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
 	:m_userValue3(0),
 	m_triangleInfoMap(0)
 {
+	// legacy constructor: Assumes PHY_FLOAT means btScalar.
+#ifdef BT_USE_DOUBLE_PRECISION
+	if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE;
+#endif
 	initialize(heightStickWidth, heightStickLength, heightfieldData,
 			   heightScale, minHeight, maxHeight, upAxis, hdt,
 			   flipQuadEdges);
@@ -33,9 +78,12 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h
 	:	m_userValue3(0),
 	m_triangleInfoMap(0)
 {
-	// legacy constructor: support only float or unsigned char,
-	// 	and min height is zero
+	// legacy constructor: support only btScalar or unsigned char data,
+	// and min height is zero.
 	PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR;
+#ifdef BT_USE_DOUBLE_PRECISION
+	if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE;
+#endif
 	btScalar minHeight = 0.0f;
 
 	// previously, height = uchar * maxHeight / 65535.
@@ -59,7 +107,7 @@ void btHeightfieldTerrainShape::initialize(
 	// btAssert(heightScale) -- do we care?  Trust caller here
 	btAssert(minHeight <= maxHeight);                                    // && "bad min/max height");
 	btAssert(upAxis >= 0 && upAxis < 3);                                 // && "bad upAxis--should be in range [0,2]");
-	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);  // && "Bad height data type enum");
+	btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_DOUBLE || hdt != PHY_SHORT);  // && "Bad height data type enum");
 
 	// initialize member variables
 	m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
@@ -152,6 +200,12 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const
 			break;
 		}
 
+		case PHY_DOUBLE:
+		{
+			val = m_heightfieldDataDouble[(y * m_heightStickWidth) + x];
+			break;
+		}
+
 		case PHY_UCHAR:
 		{
 			unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x];
@@ -232,6 +286,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 +412,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 +428,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 +947,4 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
 void btHeightfieldTerrainShape::clearAccelerator()
 {
 	m_vboundsGrid.clear();
-}
+}

src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h

@@ -50,17 +50,15 @@ subject to the following restrictions:
   The heightfield heights are determined from the data type used for the
   heightfieldData array.  
 
-   - PHY_UCHAR: height at a point is the uchar value at the
+   - unsigned char: height at a point is the uchar value at the
        grid point, multipled by heightScale.  uchar isn't recommended
        because of its inability to deal with negative values, and
        low resolution (8-bit).
 
-   - PHY_SHORT: height at a point is the short int value at that grid
+   - short: height at a point is the short int value at that grid
        point, multipled by heightScale.
 
-   - PHY_FLOAT: height at a point is the float value at that grid
-       point.  heightScale is ignored when using the float heightfield
-       data type.
+   - float or dobule: height at a point is the value at that grid point.
 
   Whatever the caller specifies as minHeight and maxHeight will be honored.
   The class will not inspect the heightfield to discover the actual minimum
@@ -75,6 +73,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;
 	};
@@ -95,7 +101,8 @@ protected:
 	union {
 		const unsigned char* m_heightfieldDataUnsignedChar;
 		const short* m_heightfieldDataShort;
-		const btScalar* m_heightfieldDataFloat;
+		const float* m_heightfieldDataFloat;
+		const double* m_heightfieldDataDouble;
 		const void* m_heightfieldDataUnknown;
 	};
 
@@ -135,11 +142,33 @@ protected:
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	/// preferred constructor
+	/// preferred constructors
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const float* heightfieldData, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const double* heightfieldData, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const short* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+	btHeightfieldTerrainShape(
+		int heightStickWidth, int heightStickLength,
+		const unsigned char* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight,
+		int upAxis, bool flipQuadEdges);
+
+	/// legacy constructor
 	/**
 	  This constructor supports a range of heightfield
 	  data types, and allows for a non-zero minimum height value.
 	  heightScale is needed for any integer-based heightfield data types.
+
+	  This legacy constructor considers `PHY_FLOAT` to mean `btScalar`.
+	  With `BT_USE_DOUBLE_PRECISION`, it will expect `heightfieldData`
+	  to be double-precision.
 	 */
 	btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,
 							  const void* heightfieldData, btScalar heightScale,
@@ -150,7 +179,7 @@ public:
 	/// legacy constructor
 	/**
 	  The legacy constructor assumes the heightfield has a minimum height
-	  of zero.  Only unsigned char or floats are supported.  For legacy
+	  of zero.  Only unsigned char or btScalar data are supported.  For legacy
 	  compatibility reasons, heightScale is calculated as maxHeight / 65535 
 	  (and is only used when useFloatData = false).
  	 */
@@ -218,4 +247,4 @@ public:
 	}
 };
 
-#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,9 +20,12 @@ 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"
 
@@ -30,6 +33,8 @@ subject to the following restrictions:
 #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