Fix hfield transform

include/coal/internal/traversal_node_hfield_shape.h

@@ -321,11 +321,16 @@ inline Scalar distanceContactPointToFace(const size_t face_id,
 template <typename Polygone, typename Shape>
 bool binCorrection(const ConvexTpl<Polygone>& convex,
                    const int convex_active_faces, const Shape& shape,
+                   const Transform3s& hfield_pose,
                    const Transform3s& shape_pose, Scalar& distance,
                    Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal,
                    Vec3s& face_normal, const bool is_collision) {
   const Scalar prec = Scalar(1e-12);
   const std::vector<Vec3s>& points = *(convex.points);
+  // Bin vertices are stored in the heightfield frame, whereas the narrowphase
+  // returns contact_1 in the world frame. Face selection must compare points
+  // expressed in the same frame.
+  const Vec3s local_contact_1 = hfield_pose.inverseTransform(contact_1);
 
   bool hfield_witness_is_on_bin_side = true;
 
@@ -347,7 +352,7 @@ bool binCorrection(const ConvexTpl<Polygone>& convex,
   for (const size_t active_face : active_faces) {
     size_t closest_face_id;
     const Scalar distance_to_face = distanceContactPointToFace(
-        active_face, contact_1, convex, closest_face_id);
+        active_face, local_contact_1, convex, closest_face_id);
 
     const bool contact_point_is_on_face = distance_to_face <= prec;
     if (contact_point_is_on_face) {
@@ -366,32 +371,44 @@ bool binCorrection(const ConvexTpl<Polygone>& convex,
     if (!face_triangle.isValid())
       COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error);
 
-    const Vec3s face_pointA = points[face_triangle[0]];
-    face_normal = computeTriangleNormal(face_triangle, points);
+    const Vec3s local_face_point = points[face_triangle[0]];
+    const Vec3s local_face_normal =
+        computeTriangleNormal(face_triangle, points);
+    // Express the shape relative to the heightfield so support mapping and all
+    // subsequent projections use the same frame as the bin face geometry.
+    const Transform3s local_shape_pose = hfield_pose.inverseTimes(shape_pose);
 
     int hint = 0;
     // Since we compute the support manually, we need to take into account the
     // sphere swept radius of the shape.
     // TODO: take into account the swept-sphere radius of the bin.
     const Vec3s _support = getSupport<details::SupportOptions::WithSweptSphere>(
-        &shape, -shape_pose.rotation().transpose() * face_normal, hint);
-    const Vec3s support =
-        shape_pose.rotation() * _support + shape_pose.translation();
+        &shape, -local_shape_pose.rotation().transpose() * local_face_normal,
+        hint);
+    const Vec3s local_support = local_shape_pose.transform(_support);
 
     // Project support into the inclined bin having triangle
-    const Scalar offset_plane = face_normal.dot(face_pointA);
-    const Plane projection_plane(face_normal, offset_plane);
+    const Scalar offset_plane = local_face_normal.dot(local_face_point);
+    const Plane projection_plane(local_face_normal, offset_plane);
     const Scalar distance_support_projection_plane =
-        projection_plane.signedDistance(support);
+        projection_plane.signedDistance(local_support);
 
-    const Vec3s projected_support =
-        support - distance_support_projection_plane * face_normal;
+    const Vec3s local_projected_support =
+        local_support - distance_support_projection_plane * local_face_normal;
 
     // We need now to project the projected in the triangle shape
-    contact_1 =
-        projectPointOnTriangle(projected_support, face_triangle, points);
-    contact_2 = contact_1 + distance_support_projection_plane * face_normal;
-    normal = face_normal;
+    const Vec3s local_corrected_contact_1 =
+        projectPointOnTriangle(local_projected_support, face_triangle, points);
+    const Vec3s local_corrected_contact_2 =
+        local_corrected_contact_1 +
+        distance_support_projection_plane * local_face_normal;
+
+    // CollisionResult contacts and normals are world-space quantities. A
+    // rigid transform preserves the signed distance computed above.
+    contact_1 = hfield_pose.transform(local_corrected_contact_1);
+    contact_2 = hfield_pose.transform(local_corrected_contact_2);
+    normal = hfield_pose.rotation() * local_face_normal;
+    face_normal = normal;
     distance = -std::fabs(distance_support_projection_plane);
   }
 
@@ -433,7 +450,7 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
                      request.collision_distance_threshold);
 
   bool hfield_witness_is_on_bin_side1 =
-      binCorrection(convex1, convex1_active_faces, shape, tf2, distance1,
+      binCorrection(convex1, convex1_active_faces, shape, tf1, tf2, distance1,
                     contact1_1, contact1_2, normal1, normal1_top, collision1);
 
   if (RTIsIdentity) {
@@ -449,7 +466,7 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
                      request.collision_distance_threshold);
 
   bool hfield_witness_is_on_bin_side2 =
-      binCorrection(convex2, convex2_active_faces, shape, tf2, distance2,
+      binCorrection(convex2, convex2_active_faces, shape, tf1, tf2, distance2,
                     contact2_1, contact2_2, normal2, normal2_top, collision2);
 
   if (collision1 && collision2) {

test/hfields.cpp

@@ -720,6 +720,102 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) {
   }
 }
 
+BOOST_AUTO_TEST_CASE(hfield_contacts_follow_world_transform) {
+  const Scalar field_x = 4.;
+  const Scalar field_y = 4.;
+  const Scalar field_bottom = -1.;
+  const Scalar field_top = 0.;
+  const Scalar sphere_radius = 0.2;
+  const Scalar penetration = 0.001;
+  const Scalar tolerance = 1e-12;
+
+  // A constant heightfield has the same top contact surface as this box. The
+  // box provides an independent analytical reference for the specialized
+  // heightfield-shape traversal.
+  const MatrixXs heights = MatrixXs::Zero(21, 21);
+  const HeightField<OBBRSS> hfield(field_x, field_y, heights, field_bottom);
+  const Box equivalent_box(field_x, field_y, field_top - field_bottom);
+  const Sphere sphere(sphere_radius);
+
+  const Matrix3s tilted_rotation =
+      (Eigen::AngleAxis<Scalar>(Scalar(0.55), Vec3s(1., 2., 0.5).normalized()) *
+       Eigen::AngleAxis<Scalar>(Scalar(-0.31), Vec3s::UnitZ()))
+          .toRotationMatrix();
+
+  const std::vector<std::pair<const char*, Transform3s>> field_poses{
+      {"identity", Transform3s::Identity()},
+      {"translated", Transform3s(Matrix3s::Identity(), Vec3s(2., -2., 0.3))},
+      {"translated and rotated",
+       Transform3s(tilted_rotation, Vec3s(1.2, -0.8, 0.7))},
+  };
+
+  for (const auto& [name, hfield_pose] : field_poses) {
+    BOOST_TEST_CONTEXT(name) {
+      // Place the sphere over the interior of one cell and penetrate the top
+      // surface by a known amount in the heightfield's local frame.
+      const Vec3s local_witness1(0.05, -0.07, field_top);
+      const Vec3s local_sphere_center(local_witness1[0], local_witness1[1],
+                                      field_top + sphere_radius - penetration);
+
+      const Matrix3s& rotation = hfield_pose.rotation();
+      const Vec3s expected_normal = rotation * Vec3s::UnitZ();
+      const Vec3s expected_witness1 = hfield_pose.transform(local_witness1);
+      const Vec3s expected_witness2 =
+          expected_witness1 - penetration * expected_normal;
+
+      const Transform3s sphere_pose(rotation,
+                                    hfield_pose.transform(local_sphere_center));
+      const Transform3s box_pose(
+          rotation, hfield_pose.transform(
+                        Vec3s(0., 0., (field_top + field_bottom) / Scalar(2))));
+
+      CollisionRequest request;
+
+      CollisionResult hfield_result;
+      collide(&hfield, hfield_pose, &sphere, sphere_pose, request,
+              hfield_result);
+      BOOST_REQUIRE(hfield_result.isCollision());
+      BOOST_REQUIRE_EQUAL(hfield_result.numContacts(), 1);
+
+      CollisionResult box_result;
+      collide(&equivalent_box, box_pose, &sphere, sphere_pose, request,
+              box_result);
+      BOOST_REQUIRE(box_result.isCollision());
+      BOOST_REQUIRE_EQUAL(box_result.numContacts(), 1);
+
+      const Contact& hfield_contact = hfield_result.getContact(0);
+      const Contact& box_contact = box_result.getContact(0);
+
+      // First verify that the analytical box behaves as expected, then demand
+      // the same world-space result from the native heightfield path.
+      BOOST_CHECK_SMALL(box_contact.penetration_depth + penetration, tolerance);
+      BOOST_CHECK(
+          box_contact.nearest_points[0].isApprox(expected_witness1, tolerance));
+      BOOST_CHECK(
+          box_contact.nearest_points[1].isApprox(expected_witness2, tolerance));
+      BOOST_CHECK(box_contact.normal.isApprox(expected_normal, tolerance));
+
+      BOOST_CHECK_SMALL(hfield_contact.penetration_depth + penetration,
+                        tolerance);
+      BOOST_CHECK(hfield_contact.nearest_points[0].isApprox(expected_witness1,
+                                                            tolerance));
+      BOOST_CHECK(hfield_contact.nearest_points[1].isApprox(expected_witness2,
+                                                            tolerance));
+      BOOST_CHECK(hfield_contact.normal.isApprox(expected_normal, tolerance));
+
+      BOOST_CHECK_SMALL(
+          hfield_contact.penetration_depth - box_contact.penetration_depth,
+          tolerance);
+      BOOST_CHECK(hfield_contact.nearest_points[0].isApprox(
+          box_contact.nearest_points[0], tolerance));
+      BOOST_CHECK(hfield_contact.nearest_points[1].isApprox(
+          box_contact.nearest_points[1], tolerance));
+      BOOST_CHECK(
+          hfield_contact.normal.isApprox(box_contact.normal, tolerance));
+    }
+  }
+}
+
 BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
   const Scalar sphere_radius = 1.;
   Sphere sphere(sphere_radius);