[wpimath] Clean up StateSpaceUtil (#5891)

wpiutil/src/main/native/include/wpi/Algorithm.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <algorithm>
+#include <cstddef>
+#include <utility>
 #include <vector>
 
 namespace wpi {
@@ -15,4 +17,19 @@ typename std::vector<T>::iterator insert_sorted(std::vector<T>& vec,
                                                 T const& item) {
   return vec.insert(std::upper_bound(vec.begin(), vec.end(), item), item);
 }
+
+/**
+ * Calls f(i, elem) for each element of elems where i is the index of the
+ * element in elems and elem is the element.
+ *
+ * @param f The callback.
+ * @param elems The elements.
+ */
+template <typename F, typename... Ts>
+constexpr void for_each(F&& f, Ts&&... elems) {
+  [&]<size_t... Is>(std::index_sequence<Is...>) {
+    (f(Is, elems), ...);
+  }(std::index_sequence_for<Ts...>{});
+}
+
 }  // namespace wpi