diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index cab6980b65..4ab9795d81 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/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(); -} \ No newline at end of file +} diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 2cf3c00721..987ea324fe 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/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 \ No newline at end of file +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H diff --git a/test/collision/CMakeLists.txt b/test/collision/CMakeLists.txt index 33f257d6f4..71e0960572 100644 --- a/test/collision/CMakeLists.txt +++ b/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) diff --git a/test/collision/main.cpp b/test/collision/main.cpp index 8f85873ffe..5be7a91f71 100644 --- a/test/collision/main.cpp +++ b/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 + #include #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 *triangles; + + explicit TriangleCollector(std::vector* 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 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