Move mju_combineSparseCount into engine_util_sparse.

PiperOrigin-RevId: 561025741
Change-Id: I0bdc48554928fc4545a38a8cea3bdeb593bd7ecd

test/engine/engine_util_sparse_test.cc

@@ -14,6 +14,8 @@
 
 // Tests for engine/engine_util_sparse.c
 
+#include <array>
+
 #include "src/engine/engine_util_sparse.h"
 
 #include <gmock/gmock.h>
@@ -43,6 +45,78 @@ TEST_F(EngineUtilSparseTest, MjuDot) {
   EXPECT_EQ(mju_dotSparse(a, b, 7, i), 7 + 2*6 + 3*5 + 4*4 + 5*3 + 6*2 + 7);
 }
 
+TEST_F(EngineUtilSparseTest, CombineSparseCount) {
+  {
+    std::array a_ind{0, 1};
+    std::array b_ind{2};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 3);
+  }
+
+  {
+    std::array a_ind{2};
+    std::array b_ind{0, 1};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 3);
+  }
+
+  {
+    std::array a_ind{0, 1};
+    std::array b_ind{2, 3, 4};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 5);
+  }
+
+  {
+    std::array a_ind{5, 6};
+    std::array b_ind{1, 3, 8};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 5);
+  }
+
+  {
+    std::array a_ind{1, 2, 3};
+    std::array b_ind{0, 4};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 5);
+  }
+
+  {
+    std::array a_ind{1, 4};
+    std::array b_ind{2, 3};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 4);
+  }
+
+  {
+    std::array a_ind{0, 1, 3};
+    std::array b_ind{0, 3, 4};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 4);
+  }
+
+  {
+    std::array a_ind{1, 3, 5, 6};
+    std::array b_ind{1, 3, 5, 6};
+    EXPECT_EQ(mju_combineSparseCount(
+        a_ind.size(), b_ind.size(), a_ind.data(), b_ind.data()), 4);
+  }
+
+  EXPECT_EQ(mju_combineSparseCount(0, 0, nullptr, nullptr), 0);
+
+  {
+    std::array b_ind{1, 2};
+    EXPECT_EQ(
+        mju_combineSparseCount(0, b_ind.size(), nullptr, b_ind.data()), 2);
+  }
+
+  {
+    std::array a_ind{0};
+    EXPECT_EQ(
+        mju_combineSparseCount(a_ind.size(), 0, a_ind.data(), nullptr), 1);
+  }
+}
+
 TEST_F(EngineUtilSparseTest, MjuTranspose3by3) {
   // 1 2 0      1 0 0
   // 0 1 0 -->  2 1 3