geometry/proximity opts into clang-format (RobotLocomotion#20030)

* geometry/proximity opts into clang format

geometry/proximity/BUILD.bazel

@@ -1421,4 +1421,4 @@ drake_cc_googletest(
     ],
 )
 
-add_lint_tests(enable_clang_format_lint = False)
+add_lint_tests()

geometry/proximity/aabb.h

@@ -13,8 +13,10 @@ namespace geometry {
 namespace internal {
 
 // Forward declarations.
-template <typename> class AabbMaker;
-template <typename> class BvhUpdater;
+template <typename>
+class AabbMaker;
+template <typename>
+class BvhUpdater;
 class Obb;
 
 /* Axis-aligned bounding box. The box is defined in a canonical frame B such
@@ -126,13 +128,13 @@ class Aabb {
    the same frame. */
   bool Equal(const Aabb& other) const {
     if (this == &other) return true;
-    return center_ == other.center_ &&
-           half_width_ == other.half_width_;
+    return center_ == other.center_ && half_width_ == other.half_width_;
   }
 
  private:
   // Allow the BvhUpdater access to set_bounds().
-  template <typename> friend class BvhUpdater;
+  template <typename>
+  friend class BvhUpdater;
 
   // Provides access to the BvhUpdater to refit the Aabb. Sets the extents of
   // the bounding box based on a box spanned by the given `lower` and `upper`

geometry/proximity/bvh.h

@@ -23,7 +23,8 @@ namespace geometry {
 namespace internal {
 
 /* Forward declaration to enable Bvh for deformable meshes. */
-template <typename> class BvhUpdater;
+template <typename>
+class BvhUpdater;
 
 template <class MeshType>
 struct MeshTraits;
@@ -100,9 +101,7 @@ class BvNode {
   }
 
   /* Returns whether this is a leaf node as opposed to a branch node.  */
-  bool is_leaf() const {
-    return std::holds_alternative<LeafData>(child_);
-  }
+  bool is_leaf() const { return std::holds_alternative<LeafData>(child_); }
 
   /* Compares this node with the given node in a strictly *topological* manner.
    For them to be considered "equal leaves", both nodes must be leaves and must
@@ -131,7 +130,8 @@ class BvNode {
   }
 
  private:
-  template <typename> friend class BvhUpdater;
+  template <typename>
+  friend class BvhUpdater;
 
   /* Provide disciplined access to BvhUpdater to a mutable child node. */
   BvNode<BvType, MeshType>& left() {
@@ -172,10 +172,7 @@ class BvNode {
 /* Resulting instruction from performing the bounding volume tree traversal
  (BVTT) callback on two potentially colliding pairs. Note that this is not the
  mathematical result but information on how the traversal should proceed.  */
-enum BvttCallbackResult {
-  Continue,
-  Terminate
-};
+enum BvttCallbackResult { Continue, Terminate };
 
 /* Bounding volume tree traversal (BVTT) callback. Returns a BvttCallbackResult
  for further action, e.g. deciding whether to exit early. The parameters are
@@ -221,9 +218,8 @@ class Bvh {
    @param callback        The callback to invoke on each unculled pair.
    @tparam OtherBvhType   The type of Bvh to collide against this.  */
   template <class OtherBvhType>
-  void Collide(
-      const OtherBvhType& bvh_B, const math::RigidTransformd& X_AB,
-      BvttCallback callback) const {
+  void Collide(const OtherBvhType& bvh_B, const math::RigidTransformd& X_AB,
+               BvttCallback callback) const {
     using NodePair =
         std::pair<const NodeType&, const typename OtherBvhType::NodeType&>;
     std::stack<NodePair, std::vector<NodePair>> node_pairs;
@@ -343,7 +339,8 @@ class Bvh {
   // Convenience class for testing.
   friend class BvhTester;
 
-  template <typename> friend class BvhUpdater;
+  template <typename>
+  friend class BvhUpdater;
 
   NodeType& mutable_root_node() { return *root_node_; }
 

geometry/proximity/collision_filter.cc

@@ -182,8 +182,7 @@ void CollisionFilter::AddFilteredPair(GeometryId id_A, GeometryId id_B,
                                       bool is_invariant,
                                       FilterState* state_out) {
   FilterState& filter_state = *state_out;
-  DRAKE_DEMAND(filter_state.count(id_A) == 1 &&
-               filter_state.count(id_B) == 1);
+  DRAKE_DEMAND(filter_state.count(id_A) == 1 && filter_state.count(id_B) == 1);
 
   if (id_A == id_B) return;
   PairRelationship& pair_relation =
@@ -195,8 +194,7 @@ void CollisionFilter::AddFilteredPair(GeometryId id_A, GeometryId id_B,
 void CollisionFilter::RemoveFilteredPair(GeometryId id_A, GeometryId id_B,
                                          FilterState* state_out) {
   FilterState& filter_state = *state_out;
-  DRAKE_DEMAND(filter_state.count(id_A) == 1 &&
-               filter_state.count(id_B) == 1);
+  DRAKE_DEMAND(filter_state.count(id_A) == 1 && filter_state.count(id_B) == 1);
   if (id_A == id_B) return;
   PairRelationship& pair_relation =
       id_A < id_B ? filter_state[id_A][id_B] : filter_state[id_B][id_A];

geometry/proximity/collision_filter.h

@@ -203,8 +203,8 @@ class CollisionFilter {
   static void AddFiltersBetween(const GeometrySet& set_A,
                                 const GeometrySet& set_B,
                                 const ExtractIds& extract_ids,
-                                CollisionFilterScope scope,
-                                bool is_invariant, FilterState* state_out);
+                                CollisionFilterScope scope, bool is_invariant,
+                                FilterState* state_out);
 
   /* Declares pairs (`id_A`, `id_B`) `∀ id_A ∈ set_A*, id_B ∈ set_B*` to be
    unfiltered (if the filter isn't invariant) where `set_A*` and `set_B*` are

geometry/proximity/collisions_exist_callback.cc

@@ -22,15 +22,14 @@ CallbackData::CallbackData(const CollisionFilter* collision_filter_in)
 }
 
 bool Callback(fcl::CollisionObjectd* object_A_ptr,
-              fcl::CollisionObjectd* object_B_ptr,
-              void* callback_data) {
+              fcl::CollisionObjectd* object_B_ptr, void* callback_data) {
   auto& data = *static_cast<CallbackData*>(callback_data);
 
   const EncodedData encoding_a(*object_A_ptr);
   const EncodedData encoding_b(*object_B_ptr);
 
-  const bool can_collide = data.collision_filter.CanCollideWith(
-      encoding_a.id(), encoding_b.id());
+  const bool can_collide =
+      data.collision_filter.CanCollideWith(encoding_a.id(), encoding_b.id());
   if (!can_collide) return false;
 
   // Unpack the callback data.

geometry/proximity/collisions_exist_callback.h

@@ -46,10 +46,11 @@ struct CallbackData {
           process early.
   */
 bool Callback(fcl::CollisionObjectd* object_A_ptr,
-              fcl::CollisionObjectd* object_B_ptr,
-              void* callback_data);
+              fcl::CollisionObjectd* object_B_ptr, void* callback_data);
 
+// clang-format off
 }  // namespace has_collisions
+// clang-format on
 }  // namespace internal
 }  // namespace geometry
 }  // namespace drake

geometry/proximity/contact_surface_utility.cc

@@ -63,9 +63,9 @@ void ThrowIfInvalidForCentroid(const char* prefix,
 
   // First test for sufficient length.
   if (n_F.norm() < 1e-10) {
-    throw std::runtime_error(fmt::format(
-        "{}: given normal is too small; normal [{}] with length {}",
-        prefix, fmt_eigen(n_F.transpose()), n_F.norm()));
+    throw std::runtime_error(
+        fmt::format("{}: given normal is too small; normal [{}] with length {}",
+                    prefix, fmt_eigen(n_F.transpose()), n_F.norm()));
   }
 
   // Now test for orthogonality.
@@ -97,9 +97,7 @@ void ThrowIfInvalidForCentroid(const char* prefix,
   for (int i = 0; i < v_count; ++i) {
     const Vector3<T>& v = vertices_F[polygon[i]];
     A.block(i, 0, 1, 4) << ExtractDoubleOrThrow(v(0)),
-                           ExtractDoubleOrThrow(v(1)),
-                           ExtractDoubleOrThrow(v(2)),
-                           1.0;
+        ExtractDoubleOrThrow(v(1)), ExtractDoubleOrThrow(v(2)), 1.0;
   }
 
   Eigen::FullPivLU<MatrixX<double>> lu(A);
@@ -131,21 +129,19 @@ void ThrowIfInvalidForCentroid(const char* prefix,
   // side of the polygon plane, simply throw out the sign of the dot product.
   using std::abs;
   if (abs(plane_norm.dot(n_F.normalized())) < 0.7071) {
-    throw std::runtime_error(
-        fmt::format("{}: the given normal is not perpendicular to the "
-                    "polygon's plane; given normal: [{}], plane normal: [{}]",
-                    prefix, fmt_eigen(n_F.transpose()),
-                    fmt_eigen(plane_norm.transpose())));
+    throw std::runtime_error(fmt::format(
+        "{}: the given normal is not perpendicular to the "
+        "polygon's plane; given normal: [{}], plane normal: [{}]",
+        prefix, fmt_eigen(n_F.transpose()), fmt_eigen(plane_norm.transpose())));
   }
 }
 
 }  // namespace
 
 template <typename T>
-int TriMeshBuilder<T>::AddPolygon(
-    const std::vector<int>& polygon_vertices,
-    const Vector3<T>& nhat_B,
-    const Vector3<T>& grad_e_MN_B) {
+int TriMeshBuilder<T>::AddPolygon(const std::vector<int>& polygon_vertices,
+                                  const Vector3<T>& nhat_B,
+                                  const Vector3<T>& grad_e_MN_B) {
   // Vertices and pressure values at vertex positions must already have been
   // explicitly added to use this method.
   DRAKE_ASSERT_VOID(
@@ -193,10 +189,9 @@ PolyMeshBuilder<T>::PolyMeshBuilder() {
 }
 
 template <typename T>
-int PolyMeshBuilder<T>::AddPolygon(
-    const std::vector<int>& polygon_vertices,
-    const Vector3<T>& /* nhat_B */,
-    const Vector3<T>& grad_e_MN_B) {
+int PolyMeshBuilder<T>::AddPolygon(const std::vector<int>& polygon_vertices,
+                                   const Vector3<T>& /* nhat_B */,
+                                   const Vector3<T>& grad_e_MN_B) {
   // Vertices and pressure values at vertex positions must already have been
   // explicitly added to use this method.
   DRAKE_ASSERT_VOID(
@@ -364,17 +359,15 @@ bool IsFaceNormalInNormalDirection(const Vector3<T>& normal_F,
 }
 
 // Instantiation to facilitate unit testing of this support function.
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
-    &AddPolygonToTriangleMeshData<T>,
-    &IsFaceNormalInNormalDirection<T>,
-    &CalcPolygonCentroid<T>
-))
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    (&AddPolygonToTriangleMeshData<T>, &IsFaceNormalInNormalDirection<T>,
+     &CalcPolygonCentroid<T>))
 
 }  // namespace internal
 }  // namespace geometry
 }  // namespace drake
 
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
-  class ::drake::geometry::internal::TriMeshBuilder)
+    class ::drake::geometry::internal::TriMeshBuilder)
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
-  class ::drake::geometry::internal::PolyMeshBuilder)
+    class ::drake::geometry::internal::PolyMeshBuilder)

geometry/proximity/contact_surface_utility.h

@@ -172,9 +172,7 @@ class PolyMeshBuilder {
 
   /* Expose the accumulated vertices measured and expressed in the
    builder's frame B. */
-  const std::vector<Vector3<T>>& vertices() const {
-    return vertices_B_;
-  }
+  const std::vector<Vector3<T>>& vertices() const { return vertices_B_; }
 
  private:
   /* The number of polygons that have been added. It can't simply be inferred
@@ -259,7 +257,7 @@ void AddPolygonToTriangleMeshData(const std::vector<int>& polygon,
 
 /* Adds a polygon (defined by indices into a set of vertices) into the polygon
  face data (as defined by PolygonSurfaceMesh).
- 
+
  This is similar to AddPolygonToTriangleMeshData() in that the specified polygon
  is added to some representation of mesh faces. It's different in the following
  ways:

geometry/proximity/deformable_contact_internal.cc

@@ -60,8 +60,8 @@ DeformableContact<double> Geometries::ComputeDeformableContact(
        deformable_geometries_) {
     const VolumeMesh<double>& deformable_mesh =
         deformable_geometry.deformable_mesh().mesh();
-    result.RegisterDeformableGeometry(
-        deformable_id, deformable_mesh.num_vertices());
+    result.RegisterDeformableGeometry(deformable_id,
+                                      deformable_mesh.num_vertices());
     DRAKE_ASSERT(collision_filter.HasGeometry(deformable_id));
     for (const auto& [rigid_id, rigid_geometry] : rigid_geometries_) {
       DRAKE_ASSERT(collision_filter.HasGeometry(rigid_id));

geometry/proximity/deformable_volume_mesh.h

@@ -95,7 +95,7 @@ class DeformableVolumeMesh {
   // I have not been able to create a constructor that supports both optional
   // and maintains semantics. So, the mesh-only constructor defines its own
   // initialization.
-  DeformableVolumeMesh(VolumeMesh<T> mesh_M, Bvh<Aabb, VolumeMesh<T>> bvh_M )
+  DeformableVolumeMesh(VolumeMesh<T> mesh_M, Bvh<Aabb, VolumeMesh<T>> bvh_M)
       : mesh_(std::move(mesh_M)),
         deformer_(&mesh_),
         bvh_(std::move(bvh_M)),

geometry/proximity/distance_to_point_callback.cc

@@ -154,10 +154,12 @@ void ComputeDistanceToPrimitive(const fcl::Capsuled& capsule,
   }
 }
 
+// clang-format off
 #define INSTANTIATE_DISTANCE_TO_PRIMITIVE(Shape, S)                         \
 template void ComputeDistanceToPrimitive<S>(                                \
     const fcl::Shape&, const math::RigidTransform<S>&, const Vector3<S>&,   \
     Vector3<S>*, S*, Vector3<S>*)
+// clang-format on
 
 // INSTANTIATE_DISTANCE_TO_PRIMITIVE(Sphered, double);
 // INSTANTIATE_DISTANCE_TO_PRIMITIVE(Sphered, AutoDiffXd);
@@ -302,7 +304,8 @@ SignedDistanceToPoint<T> DistanceToPoint<T>::operator()(
   if constexpr (std::is_same_v<T, double>) {
     // TODO(SeanCurtis-TRI): Replace this short-term hack with something that
     //  provides higher precision. Can an iterative method provide derivatives?
-    //  See: https://www.geometrictools.com/Documentation/DistancePointEllipseEllipsoid.pdf
+    //  See:
+    //  https://www.geometrictools.com/Documentation/DistancePointEllipseEllipsoid.pdf
 
     // For now, we'll simply use FCL's sphere-ellipsoid algorithm to compute
     // the signed distance for a zero-radius sphere. (Note: this uses the
@@ -319,9 +322,9 @@ SignedDistanceToPoint<T> DistanceToPoint<T>::operator()(
     // world frame.
     fcl::DistanceResultd result_W;
 
-    fcl::distance(&ellipsoid, X_WG_.GetAsIsometry3(),
-                  &sphere_Q, math::RigidTransformd(p_WQ_).GetAsIsometry3(),
-                  request, result_W);
+    fcl::distance(&ellipsoid, X_WG_.GetAsIsometry3(), &sphere_Q,
+                  math::RigidTransformd(p_WQ_).GetAsIsometry3(), request,
+                  result_W);
 
     const Vector3d& p_WN = result_W.nearest_points[0];
     const Vector3d p_GN = X_WG_.inverse() * p_WN;
@@ -377,7 +380,6 @@ SignedDistanceToPoint<T> DistanceToPoint<T>::operator()(
   return SignedDistanceToPoint<T>{geometry_id_, p_GN_G, distance, grad_W};
 }
 
-
 template <typename T>
 template <int dim, typename U>
 int DistanceToPoint<T>::ExtremalAxis(const Vector<U, dim>& p,
@@ -576,9 +578,7 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
   return false;  // Returning false tells fcl to continue to other objects.
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
-    &Callback<T>
-))
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((&Callback<T>))
 
 }  // namespace point_distance
 }  // namespace internal

geometry/proximity/distance_to_point_callback.h

@@ -46,8 +46,7 @@ struct CallbackData {
    @param distances_in[out]   The output results. Aliased.
    */
   CallbackData(
-      fcl::CollisionObjectd* query_in,
-      const double threshold_in,
+      fcl::CollisionObjectd* query_in, const double threshold_in,
       const Vector3<T>& p_WQ_W_in,
       const std::unordered_map<GeometryId, math::RigidTransform<T>>* X_WGs_in,
       std::vector<SignedDistanceToPoint<T>>* distances_in)
@@ -143,10 +142,9 @@ class DistanceToPoint {
    @param id    The id of the geometry G,
    @param X_WG  The pose of the G in world frame,
    @param p_WQ  The position of the query point Q in world frame.  */
-  DistanceToPoint(const GeometryId id,
-                  const math::RigidTransform<T>& X_WG,
-                  const Vector3<T>& p_WQ) :
-      geometry_id_(id), X_WG_(X_WG), p_WQ_(p_WQ) {}
+  DistanceToPoint(const GeometryId id, const math::RigidTransform<T>& X_WG,
+                  const Vector3<T>& p_WQ)
+      : geometry_id_(id), X_WG_(X_WG), p_WQ_(p_WQ) {}
 
   // TODO(DamrongGuoy): Revisit computation over operator() overloads as per
   //  issue: https://github.com/RobotLocomotion/drake/issues/11227
@@ -173,7 +171,9 @@ class DistanceToPoint {
    @tparam U  Templated to allow DistanceToPoint<AutoDiffXd> to still compute
               Sign<double> or Sign<AutoDiffXd> as needed.  */
   template <typename U = T>
-  static U Sign(const U& x) { return (x < U(0.0)) ? U(-1.0) : U(1.0); }
+  static U Sign(const U& x) {
+    return (x < U(0.0)) ? U(-1.0) : U(1.0);
+  }
 
   /* Picks the axis i whose coordinate p(i) is closest to the boundary value
    ±bounds(i). If there are ties, we prioritize according to an arbitrary
@@ -271,7 +271,9 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
               // NOLINTNEXTLINE
               void* callback_data, double& threshold);
 
+// clang-format off
 }  // namespace point_distance
+// clang-format on
 }  // namespace internal
 }  // namespace geometry
 }  // namespace drake