diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h index 109f8aee9..fdc39ccb8 100644 --- a/include/coal/internal/traversal_node_hfield_shape.h +++ b/include/coal/internal/traversal_node_hfield_shape.h @@ -321,11 +321,16 @@ inline Scalar distanceContactPointToFace(const size_t face_id, template bool binCorrection(const ConvexTpl& 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& 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& 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& 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( - &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) { diff --git a/test/hfields.cpp b/test/hfields.cpp index 033698fbe..a448cd39d 100644 --- a/test/hfields.cpp +++ b/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 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(0.55), Vec3s(1., 2., 0.5).normalized()) * + Eigen::AngleAxis(Scalar(-0.31), Vec3s::UnitZ())) + .toRotationMatrix(); + + const std::vector> 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);