mjQUICKSORT uses std::sort with C++.

fixes #1638

PiperOrigin-RevId: 660887790
Change-Id: Ia3b493feda98b246d963aa749bc81b96c42fddaa

doc/changelog.rst

@@ -21,23 +21,24 @@ General
 3. Added sub-elements to the MJCF :ref:`material<asset-material>` element, to allow specification of multiple textures
    for rendering (e.g., :ref:`occlusion-roughness-metallic<material-orm>`). Note that the MuJoCo renderer doesn't
    support these new features, and they are made available for use with external renderers.
+4. Sorting (``mjQUICKSORT``) now calls ``std::sort`` when building with C++ (:github:issue:`1638`).
 
 MJX
 ^^^
-4. Added more fields to ``mjx.Model`` and ``mjx.Data`` for further compatibility with the corresponding MuJoCo structs.
-5. Added support for :ref:`fixed tendons <tendon-fixed>`.
-6. Added support for tendon length limits (``mjCNSTR_LIMIT_TENDON`` in :ref:`mjtConstraint`).
-7. Added support for tendon equality constraints (``mjEQ_TENDON`` in :ref:`mjtEq`).
-8. Added support for tendon actuator transmission (``mjTRN_TENDON`` in :ref:`mjtTrn`).
+5. Added more fields to ``mjx.Model`` and ``mjx.Data`` for further compatibility with the corresponding MuJoCo structs.
+6. Added support for :ref:`fixed tendons <tendon-fixed>`.
+7. Added support for tendon length limits (``mjCNSTR_LIMIT_TENDON`` in :ref:`mjtConstraint`).
+8. Added support for tendon equality constraints (``mjEQ_TENDON`` in :ref:`mjtEq`).
+9. Added support for tendon actuator transmission (``mjTRN_TENDON`` in :ref:`mjtTrn`).
 
 Python bindings
 ^^^^^^^^^^^^^^^
-9. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and
+10. Added support for asset dictionary argument in ``mujoco.spec.from_file``, ``mujoco.spec.from_string`` and
    ``mujoco.spec.compile``.
 
 Bug fixes
 ^^^^^^^^^
-10. Fixed a bug where implicit integrators did not take into account disabled actuators (:github:issue:`1838`).
+11. Fixed a bug where implicit integrators did not take into account disabled actuators (:github:issue:`1838`).
 
 Version 3.2.0 (Jul 15, 2024)
 ----------------------------

src/engine/CMakeLists.txt

@@ -63,6 +63,7 @@ set(MUJOCO_ENGINE_SRCS
     engine_setconst.h
     engine_solver.c
     engine_solver.h
+    engine_sort.h
     engine_support.c
     engine_support.h
     engine_util_blas.c

src/engine/engine_collision_driver.c

@@ -26,9 +26,9 @@
 #include "engine/engine_collision_primitive.h"
 #include "engine/engine_collision_sdf.h"
 #include "engine/engine_core_constraint.h"
-#include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
 #include "engine/engine_macro.h"
+#include "engine/engine_sort.h"
 #include "engine/engine_support.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"

src/engine/engine_collision_sdf.c

@@ -21,9 +21,9 @@
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjtnum.h>
 #include "engine/engine_collision_primitive.h"
-#include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
 #include "engine/engine_plugin.h"
+#include "engine/engine_sort.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
 #include "engine/engine_util_misc.h"

src/engine/engine_crossplatform.h

@@ -25,29 +25,12 @@
 #endif
 // IWYU pragma: end_keep
 
-// Sorting and case-insensitive comparison functions.
+// Case-insensitive comparison functions.
 #ifdef _WIN32
   #define strcasecmp _stricmp
   #define strncasecmp _strnicmp
-
-  #define mjQUICKSORT(buf, elnum, elsz, func, context) \
-      qsort_s(buf, elnum, elsz, func, context)
-  #define quicksortfunc(name, context, el1, el2) \
-      static int name(void* context, const void* el1, const void* el2)
 #else  // assumes POSIX
   #include <strings.h>
-
-  #ifdef __APPLE__
-    #define mjQUICKSORT(buf, elnum, elsz, func, context) \
-        qsort_r(buf, elnum, elsz, context, func)
-    #define quicksortfunc(name, context, el1, el2) \
-        static int name(void* context, const void* el1, const void* el2)
-  #else  // non-Apple
-    #define mjQUICKSORT(buf, elnum, elsz, func, context) \
-        qsort_r(buf, elnum, elsz, func, context)
-    #define quicksortfunc(name, context, el1, el2) \
-        static int name(const void* el1, const void* el2, void* context)
-  #endif
 #endif
 
 // Switch-case fallthrough annotation.

src/engine/engine_sort.h

@@ -0,0 +1,60 @@
+// Copyright 2024 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MUJOCO_SRC_ENGINE_ENGINE_SORT_H_
+#define MUJOCO_SRC_ENGINE_ENGINE_SORT_H_
+
+#if !defined(__cplusplus)
+#include <stddef.h>
+#include <stdlib.h>
+
+// sorting functions using q_sort_s/r
+#ifdef _WIN32
+#define mjQUICKSORT(buf, elnum, elsz, func, context) \
+  qsort_s(buf, elnum, elsz, func, context)
+#define quicksortfunc(name, context, el1, el2) \
+  static int name(void* context, const void* el1, const void* el2)
+#else  // assumes POSIX
+#ifdef __APPLE__
+#define mjQUICKSORT(buf, elnum, elsz, func, context) \
+  qsort_r(buf, elnum, elsz, context, func)
+#define quicksortfunc(name, context, el1, el2) \
+  static int name(void* context, const void* el1, const void* el2)
+#else  // non-Apple
+#define mjQUICKSORT(buf, elnum, elsz, func, context) \
+  qsort_r(buf, elnum, elsz, func, context)
+#define quicksortfunc(name, context, el1, el2) \
+  static int name(const void* el1, const void* el2, void* context)
+#endif
+#endif
+#else
+#include <algorithm>
+#include <cstddef>
+#include <cstdlib>
+
+// sorting function using std::sort
+template <typename T>
+void mjQUICKSORT(T* buf, size_t elnum, size_t elsz,
+                 int (*compare)(const void* a, const void* b, void* c),
+                 void* context) {
+  std::sort(buf, buf + elnum, [compare, context](const T& a, const T& b) {
+    return compare(&a, &b, context) < 0;
+  });
+}
+
+#define quicksortfunc(name, context, el1, el2) \
+  static int name(const void* el1, const void* el2, void* context)
+#endif
+
+#endif  // MUJOCO_SRC_ENGINE_ENGINE_SORT_H_

src/render/render_gl3.c

@@ -22,7 +22,7 @@
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjvisualize.h>
 #include <mujoco/mujoco.h>
-#include "engine/engine_crossplatform.h"
+#include "engine/engine_sort.h"
 #include "engine/engine_vis_init.h"
 #include "render/render_context.h"
 #include "render/render_gl2.h"

src/user/user_mesh.cc

@@ -56,6 +56,7 @@
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
 #include "engine/engine_plugin.h"
+#include "engine/engine_sort.h"
 #include "engine/engine_util_errmem.h"
 #include "user/user_cache.h"
 #include "user/user_model.h"

test/engine/CMakeLists.txt

@@ -78,6 +78,9 @@ target_link_libraries(engine_sensor_test fixture gmock)
 mujoco_test(engine_solver_test)
 target_link_libraries(engine_solver_test fixture gmock)
 
+mujoco_test(engine_sort_test)
+target_link_libraries(engine_sort_test fixture gmock)
+
 mujoco_test(engine_support_test)
 target_link_libraries(engine_support_test fixture gmock)
 

test/engine/engine_sort_test.cc

@@ -0,0 +1,74 @@
+// Copyright 2024 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "src/engine/engine_sort.h"
+
+#include <cstddef>
+#include <vector>
+
+#include <gtest/gtest.h>
+#include "src/engine/engine_sort.h"
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+
+using EngineSortTest = MujocoTest;
+
+struct IntStruct {
+  int value;
+};
+
+quicksortfunc(int_compare, context, x, y) {
+  int a = *(int*)x;
+  int b = *(int*)y;
+  if (a < b) {
+    return -1;
+  } else if (a == b) {
+    return 0;
+  } else {
+    return 1;
+  }
+}
+
+quicksortfunc(intstruct_compare, context, x, y) {
+  IntStruct* a = (IntStruct*)x;
+  IntStruct* b = (IntStruct*)y;
+  if (a->value < b->value) {
+    return -1;
+  } else if (a->value == b->value) {
+    return 0;
+  } else {
+    return 1;
+  }
+}
+
+TEST_F(EngineSortTest, Sort) {
+  // test int
+  std::vector<int> x = {1, 3, 2};
+  mjQUICKSORT(x.data(), x.size(), sizeof(int), int_compare, x.data());
+  EXPECT_EQ(x[0], 1);
+  EXPECT_EQ(x[1], 2);
+  EXPECT_EQ(x[2], 3);
+
+  // test custom struct with mjQUICKSORT
+  std::vector<IntStruct> y = {{.value = 1}, {.value = 3}, {.value = 2}};
+  mjQUICKSORT(y.data(), y.size(), sizeof(IntStruct), intstruct_compare, NULL);
+  EXPECT_EQ(y[0].value, 1);
+  EXPECT_EQ(y[1].value, 2);
+  EXPECT_EQ(y[2].value, 3);
+}
+
+}  // namespace
+}  // namespace mujoco