Add poisson disk sampling (#22243)

geometry/BUILD.bazel

@@ -44,6 +44,7 @@ drake_cc_package_library(
         ":meshcat_point_cloud_visualizer",
         ":meshcat_visualizer",
         ":meshcat_visualizer_params",
+        ":poisson_disk",
         ":proximity_engine",
         ":proximity_properties",
         ":read_gltf_to_memory",
@@ -287,6 +288,22 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "poisson_disk",
+    srcs = ["poisson_disk.cc"],
+    hdrs = ["poisson_disk.h"],
+    deps = [
+        ":scene_graph",
+        ":shape_specification",
+        "//common:essential",
+        "//geometry/proximity:bv",
+        "//math:geometric_transform",
+    ],
+    implementation_deps = [
+        "@poisson_disk_sampling_internal//:poisson_disk_sampling",
+    ],
+)
+
 drake_cc_library(
     name = "read_gltf_to_memory",
     srcs = ["read_gltf_to_memory.cc"],
@@ -1040,6 +1057,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "poisson_disk_test",
+    deps = [
+        ":poisson_disk",
+        "//common/test_utilities:expect_throws_message",
+    ],
+)
+
 drake_cc_googletest(
     name = "proximity_properties_test",
     deps = [

geometry/poisson_disk.cc

@@ -0,0 +1,144 @@
+#include "drake/geometry/poisson_disk.h"
+
+#include <array>
+#include <memory>
+#include <utility>
+
+#include <tph_poisson.h>
+
+#include "drake/common/overloaded.h"
+#include "drake/geometry/proximity/aabb.h"
+#include "drake/geometry/query_object.h"
+#include "drake/geometry/scene_graph.h"
+#include "drake/math/rigid_transform.h"
+
+using Eigen::Vector3d;
+
+namespace drake {
+namespace geometry {
+namespace internal {
+namespace {
+
+/* Generates a set of samples within a axis-aligned bounding box (AABB)
+ (including boundary) such that no two samples are closer than a user-specified
+ distance.
+ @pre radius > 0. */
+std::vector<Vector3d> PoissonDiskSampling(double radius, const Aabb& box) {
+  /* The AABB is around a shape in its geometry frame, so it's never too far
+   away from the origin. Hence, we don't worry about the floating point
+   precision here, even when the TPH library uses single precision by default.
+  */
+  Vector3<float> bounds_min = box.lower().cast<float>();
+  Vector3<float> bounds_max = box.upper().cast<float>();
+  tph_poisson_args args = {};
+  args.radius = radius;
+  args.ndims = 3;
+  args.seed = 0;  // Ensure deterministic results.
+  args.bounds_min = bounds_min.data();
+  args.bounds_max = bounds_max.data();
+  args.max_sample_attempts = 100;
+
+  auto deleter = [](tph_poisson_sampling* s) {
+    tph_poisson_destroy(s);
+    delete s;
+  };
+  std::unique_ptr<tph_poisson_sampling, decltype(deleter)> sampling(
+      new tph_poisson_sampling{}, deleter);
+
+  /* Populate sampling with points. */
+  if (const int ret = tph_poisson_create(
+          &args, /* Use default allocator */ nullptr, sampling.get());
+      ret != TPH_POISSON_SUCCESS) {
+    throw std::runtime_error("PoissonDiskSampling: Sample creation failed!");
+  }
+
+  /* Retrieve sampling points. */
+  const float* data = tph_poisson_get_samples(sampling.get());
+  if (data == nullptr) {
+    throw std::runtime_error("PoissonDiskSampling: Sample retrieval failed!");
+  }
+
+  const int num_points = sampling->nsamples;
+  std::vector<Vector3d> results;
+  for (int i = 0; i < num_points; ++i) {
+    const Vector3d p{data[i * 3], data[i * 3 + 1], data[i * 3 + 2]};
+    results.push_back(p);
+  }
+  return results;
+}
+
+/* Given a list of candidate point positions in the geometry's frame, returns a
+ list of points that are inside (or on the boudary of) the given `shape`. */
+std::vector<Vector3d> FilterPoints(const std::vector<Vector3d>& q_GPs,
+                                   const geometry::Shape& shape) {
+  std::vector<Vector3d> results;
+  /* Ensures the geometries we add don't get hydroelastic representations
+   because it's unnecessary and may be expensive. */
+  const SceneGraphConfig config{
+      .default_proximity_properties = {.compliance_type = "undefined"}};
+  SceneGraph<double> scene_graph(config);
+  const SourceId source_id = scene_graph.RegisterSource();
+  const FrameId frame_id = scene_graph.world_frame_id();
+  auto geometry_instance = std::make_unique<GeometryInstance>(
+      math::RigidTransform<double>::Identity(), shape, "shape");
+  geometry_instance->set_proximity_properties(ProximityProperties());
+  scene_graph.RegisterGeometry(source_id, frame_id,
+                               std::move(geometry_instance));
+  auto context = scene_graph.CreateDefaultContext();
+  const auto& query_object =
+      scene_graph.get_query_output_port().Eval<QueryObject<double>>(*context);
+  for (const Vector3d& q_GP : q_GPs) {
+    const std::vector<SignedDistanceToPoint<double>> signed_distances =
+        query_object.ComputeSignedDistanceToPoint(q_GP, 0.0 /* threshold */);
+    DRAKE_DEMAND(signed_distances.size() <= 1);
+    if (!signed_distances.empty()) {
+      DRAKE_DEMAND(signed_distances[0].distance <= 0);
+      results.push_back(q_GP);
+    }
+  }
+  return results;
+}
+
+/* Computes a tight Aabb for the given shape.
+ @throws std::exception if the derived type hasn't overloaded this
+  implementation (yet). */
+Aabb CalcAabb(const Shape& shape) {
+  // TODO(xuchenhan-tri): Implement the rest of the shapes.
+  return shape.Visit(overloaded{
+      [](const Box& box) {
+        return Aabb(Vector3d::Zero(), box.size() / 2.0);
+      },
+      [](const HalfSpace&) {
+        throw std::runtime_error(
+            "Computing Aabb for half space is not supported.");
+        DRAKE_UNREACHABLE();
+        return Aabb(Vector3d::Zero(), Vector3d::Zero());
+      },
+      [](const MeshcatCone&) {
+        throw std::runtime_error(
+            "Computing Aabb for Meshcat Cone shape is not supported.");
+        DRAKE_UNREACHABLE();
+        return Aabb(Vector3d::Zero(), Vector3d::Zero());
+      },
+      [](const auto& unsupported) {
+        throw std::logic_error(
+            fmt::format("Computing Aabb for {} is not supported yet.",
+                        unsupported.type_name()));
+        DRAKE_UNREACHABLE();
+        return Aabb(Vector3d::Zero(), Vector3d::Zero());
+      }});
+}
+
+}  // namespace
+
+std::vector<Vector3d> PoissonDiskSampling(double radius, const Shape& shape) {
+  DRAKE_THROW_UNLESS(radius > 0);
+  const Aabb aabb = CalcAabb(shape);
+  const std::vector<Vector3d> q_GPs = PoissonDiskSampling(radius, aabb);
+  /* Reject points that fall outside of the shape. */
+  return FilterPoints(q_GPs, shape);
+}
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/poisson_disk.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <vector>
+
+#include "drake/common/eigen_types.h"
+#include "drake/geometry/shape_specification.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+/* Generates a set of samples within the given shape. A point is deemed as
+ "inside" the shape if its signed distance to the shape is non-positive. The
+ generated set of points are such that no two samples are closer than a
+ user-specified distance. See [Bridson, 2007] for details.
+
+ [Bridson, 2007] Bridson, Robert. "Fast Poisson disk sampling in arbitrary
+ dimensions." SIGGRAPH sketches 10.1 (2007): 1.
+
+ @note The algorithm guarantees no point can be sampled in the AABB that's more
+ than `radius` away from all existing points.
+ @throws std::exception unless radius > 0.
+ @throws std::exception if there is an error from poisson_disk_sampling library.
+ @throws std::exception if the derived type of Shape hasn't provided an
+ implementation (yet). */
+std::vector<Vector3<double>> PoissonDiskSampling(double radius,
+                                                 const Shape& shape);
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/test/poisson_disk_test.cc

@@ -0,0 +1,56 @@
+#include "drake/geometry/poisson_disk.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/common/test_utilities/expect_throws_message.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+namespace {
+
+using Eigen::Vector3d;
+
+GTEST_TEST(PoissonDiskTest, TestOnePoint) {
+  const double r = 2;
+  const Box box = Box::MakeCube(0.2);
+  const std::vector<Vector3<double>> result = PoissonDiskSampling(r, box);
+  /* Radius is larger than diagonal of bounding box, should return only one
+   point. */
+  EXPECT_EQ(ssize(result), 1);
+  /* The sampled point is in the box. */
+  EXPECT_LT(result[0].cwiseAbs().maxCoeff(), 0.1);
+}
+
+GTEST_TEST(PoissonDiskTest, TestDistance) {
+  const double r = 0.05;
+  const Box box = Box::MakeCube(0.2);
+  const std::vector<Vector3<double>> result = PoissonDiskSampling(r, box);
+
+  for (int i = 0; i < ssize(result); ++i) {
+    /* Every sampled point is inside the bounding box. */
+    EXPECT_LT(result[i].cwiseAbs().maxCoeff(), 0.1);
+  }
+
+  /* There's at least two points 0.05 away in a cube with edge length 0.2. */
+  EXPECT_GT(ssize(result), 1);
+
+  for (int i = 0; i < ssize(result); ++i) {
+    for (int j = i + 1; j < ssize(result); ++j) {
+      const double distance_sq = (result[i] - result[j]).squaredNorm();
+      EXPECT_GE(distance_sq, r * r);
+    }
+  }
+}
+
+/* Halfspace is not supported for sampling points. */
+GTEST_TEST(PoissonDiskTest, UnsupportedShape) {
+  const HalfSpace hs;
+  const double r = 2;
+  DRAKE_EXPECT_THROWS_MESSAGE(PoissonDiskSampling(r, hs), ".*not supported.*");
+}
+
+}  // namespace
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

tools/workspace/BUILD.bazel

@@ -104,6 +104,7 @@ _DRAKE_EXTERNAL_PACKAGE_INSTALLS = ["@%s//:install" % p for p in [
     "nlopt_internal",
     "org_apache_xmlgraphics_commons",
     "picosha2_internal",
+    "poisson_disk_sampling_internal",
     "pybind11",
     "qhull_internal",
     "sdformat_internal",

tools/workspace/default.bzl

@@ -70,6 +70,7 @@ load("//tools/workspace/org_apache_xmlgraphics_commons:repository.bzl", "org_apa
 load("//tools/workspace/osqp_internal:repository.bzl", "osqp_internal_repository")  # noqa
 load("//tools/workspace/picosha2_internal:repository.bzl", "picosha2_internal_repository")  # noqa
 load("//tools/workspace/platforms:repository.bzl", "platforms_repository")
+load("//tools/workspace/poisson_disk_sampling_internal:repository.bzl", "poisson_disk_sampling_internal_repository")  # noqa
 load("//tools/workspace/pybind11:repository.bzl", "pybind11_repository")
 load("//tools/workspace/pycodestyle:repository.bzl", "pycodestyle_repository")
 load("//tools/workspace/python:repository.bzl", "python_repository")
@@ -261,6 +262,8 @@ def add_default_repositories(excludes = [], mirrors = DEFAULT_MIRRORS):
         picosha2_internal_repository(name = "picosha2_internal", mirrors = mirrors)  # noqa
     if "platforms" not in excludes:
         platforms_repository(name = "platforms", mirrors = mirrors)
+    if "poisson_disk_sampling_internal" not in excludes:
+        poisson_disk_sampling_internal_repository(name = "poisson_disk_sampling_internal", mirrors = mirrors)  # noqa
     if "pybind11" not in excludes:
         pybind11_repository(name = "pybind11", mirrors = mirrors)
     if "pycodestyle" not in excludes:

tools/workspace/poisson_disk_sampling_internal/BUILD.bazel

@@ -0,0 +1,3 @@
+load("//tools/lint:lint.bzl", "add_lint_tests")
+
+add_lint_tests()

tools/workspace/poisson_disk_sampling_internal/package.BUILD.bazel

@@ -0,0 +1,38 @@
+# -*- bazel -*-
+
+load("@bazel_skylib//rules:copy_file.bzl", "copy_file")
+load("@drake//tools/install:install.bzl", "install")
+load("@drake//tools/skylark:cc.bzl", "cc_library")
+
+licenses(["notice"])  # MIT
+
+package(default_visibility = ["//visibility:public"])
+
+# The poisson-disk-sampling library is header-only, so we copy the header file
+# to an implementation file and compile it with the implementation macro
+# (TPH_POISSON_IMPLEMENTATION) defined. Together with the visibility setting,
+# this ensures that the symbols are not exposed.
+copy_file(
+    name = "implementation",
+    src = "include/thinks/tph_poisson.h",
+    out = "implementation.cc",
+    allow_symlink = True,
+    visibility = ["//visibility:private"],
+)
+
+cc_library(
+    name = "poisson_disk_sampling",
+    hdrs = ["include/thinks/tph_poisson.h"],
+    srcs = [":implementation"],
+    strip_include_prefix = "include/thinks",
+    linkstatic = 1,
+    copts = [
+        "-fvisibility=hidden",
+        "-DTPH_POISSON_IMPLEMENTATION",
+    ],
+)
+
+install(
+    name = "install",
+    docs = ["LICENSE"],
+)

tools/workspace/poisson_disk_sampling_internal/patches/upstream/positive_capacity.patch

@@ -0,0 +1,20 @@
+Ensure capacity is positive when reserving for point allocations.
+
+--- include/thinks/tph_poisson.h
++++ include/thinks/tph_poisson.h
+@@ -1037,10 +1037,11 @@ int tph_poisson_create(const tph_poisson_args *args,
+    * growing the buffer. Estimate that 25% of the grid cells will end up
+    * containing a sample, which is a fairly conservative guess. Prefering not
+    * to over-allocate up front here, at the cost of having to reallocate later. */
+-  ret = tph_poisson_vec_reserve(&internal->samples,
+-    &internal->alloc,
+-    (ctx.grid_linear_size / 4) * ((ptrdiff_t)sizeof(tph_poisson_real) * ctx.ndims),
+-    (ptrdiff_t)alignof(tph_poisson_real));
++  ptrdiff_t capacity = (ctx.grid_linear_size / 4) * (ptrdiff_t)sizeof(tph_poisson_real) * ctx.ndims;
++  /* Ensure capacity is at least 1 to guard against overflow and zero capacity. */
++  if (capacity < (ptrdiff_t)1) { capacity = (ptrdiff_t)1; }
++  ret = tph_poisson_vec_reserve(&internal->samples,
++    &internal->alloc, capacity, (ptrdiff_t)alignof(tph_poisson_real));
+   if (ret != TPH_POISSON_SUCCESS) {
+     tph_poisson_context_destroy(&ctx, &internal->alloc);
+     tph_poisson_destroy(sampling);

tools/workspace/poisson_disk_sampling_internal/repository.bzl

@@ -0,0 +1,16 @@
+load("//tools/workspace:github.bzl", "github_archive")
+
+def poisson_disk_sampling_internal_repository(
+        name,
+        mirrors = None):
+    github_archive(
+        name = name,
+        repository = "thinks/poisson-disk-sampling",
+        commit = "v0.4.0",
+        sha256 = "a1bae19286d2037f7dd23e554e37f3f44095bc2a1be961261a665d37fd1473ba",  # noqa
+        build_file = ":package.BUILD.bazel",
+        patches = [
+            ":patches/upstream/positive_capacity.patch",
+        ],
+        mirrors = mirrors,
+    )