diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 47f4bb8d8bb6251a179076bacd25fb7769abe93f..1774c68f641c24091dc7cc1e4cd59a3c295f85c2 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -486,6 +486,12 @@ class HeightFieldShapeCollisionTraversalNode ConvexTriangle convex1, convex2; details::buildConvexTriangles(node, *this->model1, convex1, convex2); + // Compute aabb_local for BoundingVolumeGuess case in the GJK solver + if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { + convex1.computeLocalAABB(); + convex2.computeLocalAABB(); + } + FCL_REAL distance; // Vec3f contact_point, normal; Vec3f c1, c2, normal, normal_top;