geometry: create ellipsoid meshes. (RobotLocomotion#12323)

geometry/proximity/BUILD.bazel

@@ -24,6 +24,7 @@ drake_cc_package_library(
         ":make_box_field",
         ":make_box_mesh",
         ":make_cylinder_mesh",
+        ":make_ellipsoid_mesh",
         ":make_sphere_mesh",
         ":mesh_field",
         ":mesh_half_space_intersection",
@@ -238,6 +239,18 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "make_ellipsoid_mesh",
+    hdrs = ["make_ellipsoid_mesh.h"],
+    deps = [
+        ":make_sphere_mesh",
+        ":surface_mesh",
+        ":volume_mesh",
+        ":volume_to_surface_mesh",
+        "//geometry:shape_specification",
+    ],
+)
+
 drake_cc_library(
     name = "make_box_field",
     hdrs = ["make_box_field.h"],
@@ -472,6 +485,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "make_ellipsoid_mesh_test",
+    deps = [
+        ":make_ellipsoid_mesh",
+    ],
+)
+
 drake_cc_googletest(
     name = "make_box_field_test",
     deps = [

geometry/proximity/make_ellipsoid_mesh.h

@@ -0,0 +1,95 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <utility>
+#include <vector>
+
+#include "drake/geometry/proximity/make_sphere_mesh.h"
+#include "drake/geometry/proximity/surface_mesh.h"
+#include "drake/geometry/proximity/volume_mesh.h"
+#include "drake/geometry/proximity/volume_to_surface_mesh.h"
+#include "drake/geometry/shape_specification.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+/** Creates a volume mesh for the given `ellipsoid`; the level of
+ tessellation is guided by the `resolution_hint` parameter.
+
+ `resolution_hint` influences the resolution of the mesh. Smaller values
+ create higher-resolution meshes with smaller tetrahedra. The resolution hint
+ is interpreted as an upper bound on the edge lengths along the great
+ ellipses on each of the three coordinate planes of the frame of `ellipsoid`.
+
+ The resolution of the final mesh will change discontinuously. Small changes
+ to `resolution_hint` will likely produce the same mesh. However, in the
+ current implementation, cutting `resolution_hint` in half _will_ increase
+ the number of tetrahedra.
+
+ Ultimately, successively smaller values of `resolution_hint` will no longer
+ change the output mesh. This algorithm will not produce a tetrahedral mesh with
+ more than approximately 100 million tetrahedra. Similarly, for arbitrarily
+ large values of `resolution_hint`, the coarsest possible mesh is a tessellated
+ octohedron.
+
+ @param ellipsoid           The ellipsoid for which a mesh is created.
+ @param resolution_hint     The positive characteristic edge length for the
+                            ellipsoid. The coarsest possible mesh (an
+                            octahedron) is guaranteed for any value of
+                            `resolution_hint` greater than or equal to the
+                            `ellipsoid`'s major axis.
+ @return The volume mesh for the given ellipsoid.
+ @tparam T The Eigen-compatible scalar for representing the mesh vertex
+           positions.
+*/
+template <typename T>
+VolumeMesh<T> MakeEllipsoidVolumeMesh(const Ellipsoid& ellipsoid,
+                                      double resolution_hint) {
+  DRAKE_DEMAND(resolution_hint > 0.0);
+  const double a = ellipsoid.get_a();
+  const double b = ellipsoid.get_b();
+  const double c = ellipsoid.get_c();
+  const double r = std::max({a, b, c});
+
+  const double unit_sphere_resolution = resolution_hint / r;
+  auto unit_sphere_mesh =
+      MakeSphereVolumeMesh<T>(Sphere(1.0), unit_sphere_resolution);
+
+  const Vector3<T> scale{a, b, c};
+  std::vector<VolumeVertex<T>> vertices;
+  vertices.reserve(unit_sphere_mesh.num_vertices());
+  for (const auto& sphere_vertex : unit_sphere_mesh.vertices()) {
+    vertices.emplace_back(scale.cwiseProduct(sphere_vertex.r_MV()));
+  }
+  std::vector<VolumeElement> tetrahedra = unit_sphere_mesh.tetrahedra();
+
+  return VolumeMesh<T>(std::move(tetrahedra), std::move(vertices));
+}
+
+/** Creates a surface mesh for the given `ellipsoid`; the level of
+ tessellation is guided by the `resolution_hint` parameter in the same way as
+ MakeEllipsoidVolumeMesh.
+
+ @param ellipsoid           The ellipsoid for which a surface mesh is created.
+ @param resolution_hint     The positive characteristic edge length for the
+                            ellipsoid. The coarsest possible mesh (an
+                            octahedron) is guaranteed for any value of
+                            `resolution_hint` greater than or equal to the
+                            `ellipsoid`'s major axis.
+ @return The triangulated surface mesh for the given ellipsoid.
+ @tparam T The Eigen-compatible scalar for representing the mesh vertex
+           positions.
+*/
+template <typename T>
+SurfaceMesh<T> MakeEllipsoidSurfaceMesh(const Ellipsoid& ellipsoid,
+                                        double resolution_hint) {
+  DRAKE_DEMAND(resolution_hint > 0.0);
+  return ConvertVolumeToSurfaceMesh<T>(
+      MakeEllipsoidVolumeMesh<T>(ellipsoid, resolution_hint));
+}
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/proximity/test/make_ellipsoid_mesh_test.cc

@@ -0,0 +1,62 @@
+#include "drake/geometry/proximity/make_ellipsoid_mesh.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/geometry/shape_specification.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+namespace {
+
+// Correctness of MakeEllipsoidVolumeMesh() depends on MakeSphereVolumeMesh().
+// This is a smoke test only.
+GTEST_TEST(MakeEllipsoidMeshTest, MakeEllipsoidVolumeMesh) {
+  // For an ellipsoid with bounding box 10cm x 16cm x 6cm, its semi-axes are
+  // 5cm, 8cm, and 3cm long.
+  const Ellipsoid ellipsoid(0.05, 0.08, 0.03);
+
+  // Coarsest mesh for resolution_hint equals the length of the major
+  // axis 16cm. The coarsest mesh is an octahedron with 7 vertices and 8
+  // tetrahedra.
+  {
+    const auto coarse_mesh = MakeEllipsoidVolumeMesh<double>(ellipsoid, 0.16);
+    EXPECT_EQ(7, coarse_mesh.num_vertices());
+    EXPECT_EQ(8, coarse_mesh.num_elements());
+  }
+  // Cutting the resolution_hint in half from 16cm down to 8cm increases
+  // the number of tetrahedra by 8X.
+  {
+    const auto medium_mesh = MakeEllipsoidVolumeMesh<double>(ellipsoid, 0.08);
+    EXPECT_EQ(64, medium_mesh.num_elements());
+  }
+}
+
+// Correctness of MakeEllipsoidSurfaceMesh() depends on
+// MakeEllipsoidVolumeMesh(). This is a smoke test only.
+GTEST_TEST(MakeEllipsoidMeshTest, MakeEllipsoidSurfaceMesh) {
+  // For an ellipsoid with bounding box 10cm x 16cm x 6cm, its semi-axes are
+  // 5cm, 8cm, and 3cm long.
+  const Ellipsoid ellipsoid(0.05, 0.08, 0.03);
+
+  // Coarsest mesh for resolution_hint equals the length of the major
+  // axis 16cm. The coarsest mesh is an octahedron with 6 vertices and 8
+  // triangles.
+  {
+    const auto coarse_mesh = MakeEllipsoidSurfaceMesh<double>(ellipsoid, 0.16);
+    EXPECT_EQ(6, coarse_mesh.num_vertices());
+    EXPECT_EQ(8, coarse_mesh.num_faces());
+  }
+
+  // Cutting the resolution_hint in half from 16cm down to 8cm increases
+  // the number of triangles by 4X.
+  {
+    const auto medium_mesh = MakeEllipsoidSurfaceMesh<double>(ellipsoid, 0.08);
+    EXPECT_EQ(32, medium_mesh.num_faces());
+  }
+}
+
+}  // namespace
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake