[mpm] Add ParticleSorter

multibody/mpm/BUILD.bazel

@@ -17,6 +17,7 @@ drake_cc_package_library(
         ":bspline_weights",
         ":grid_data",
         ":particle_data",
+        ":particle_sorter",
     ],
 )
 
@@ -60,6 +61,21 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "particle_sorter",
+    srcs = [
+        "particle_sorter.cc",
+    ],
+    hdrs = [
+        "particle_sorter.h",
+    ],
+    deps = [
+        ":bspline_weights",
+        "//common:essential",
+        "//math:gradient",
+    ],
+)
+
 # TODO(xuchenhan-tri): when we enable SPGrid in our releases, we also need to
 # install its license file in drake/tools/workspace/BUILD.bazel.
 
@@ -99,6 +115,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest_linux_only(
+    name = "particle_sorter_test",
+    deps = [
+        ":bspline_weights",
+        ":grid_data",
+        ":particle_sorter",
+        ":spgrid",
+    ],
+)
+
 drake_cc_googletest_linux_only(
     name = "spgrid_test",
     deps = [

multibody/mpm/grid_data.h

@@ -108,33 +108,20 @@ struct GridData {
   static_assert(std::is_same_v<T, float> || std::is_same_v<T, double>,
                 "T must be float or double.");
 
-  /* Resets `this` GridData to its default state where all floating point values
-   are set to NAN and the index is inactive. */
+  /* Resets all floating point data to zero and the index to be inactive.*/
   void reset() { *this = {}; }
 
   /* Returns true iff `this` GridData is bit-wise equal to `other`. */
   bool operator==(const GridData<T>& other) const {
     return std::memcmp(this, &other, sizeof(GridData<T>)) == 0;
   }
 
-  Vector3<T> v{Vector3<T>::Constant(nan_with_all_bits_set())};
-  T m{nan_with_all_bits_set()};
-  Vector3<T> scratch{Vector3<T>::Constant(nan_with_all_bits_set())};
+  Vector3<T> v{Vector3<T>::Zero()};
+  T m{0};
+  Vector3<T> scratch{Vector3<T>::Zero()};
   std::conditional_t<std::is_same_v<T, float>, IndexOrFlag<int32_t>,
                      IndexOrFlag<int64_t>>
       index_or_flag{};
-
- private:
-  /* Returns a floating point NaN value with all bits set to one. This choice
-   makes the reset() function more efficient. In particlar, it allows the
-   generated machine code to memset all bits to 1 instead of handling each field
-   individually. */
-  static T nan_with_all_bits_set() {
-    using IntType =
-        std::conditional_t<std::is_same_v<T, float>, int32_t, int64_t>;
-    constexpr IntType kAllBitsOn = -1;
-    return drake::internal::bit_cast<T>(kAllBitsOn);
-  }
 };
 
 /* With T = float, GridData is expected to be 32 bytes. With T = double,

multibody/mpm/particle_sorter.cc

@@ -0,0 +1,27 @@
+#include "drake/multibody/mpm/particle_sorter.h"
+
+namespace drake {
+namespace multibody {
+namespace mpm {
+namespace internal {
+
+void ConvertToRangeVector(const std::vector<int>& data, RangeVector* ranges) {
+  ranges->clear();
+  ranges->reserve(data.size() - 1);
+  for (int i = 1; i < ssize(data); ++i) {
+    ranges->push_back({data[i - 1], data[i]});
+  }
+}
+
+std::vector<uint64_t> ParticleSorter::GetActiveBlockOffsets() const {
+  std::vector<uint64_t> offsets(ssize(sentinel_particles_) - 1);
+  for (int i = 0; i < ssize(sentinel_particles_) - 1; ++i) {
+    offsets[i] = base_node_offsets_[sentinel_particles_[i]];
+  }
+  return offsets;
+}
+
+}  // namespace internal
+}  // namespace mpm
+}  // namespace multibody
+}  // namespace drake