Speed up MakeBVH.

1. Avoid creating unnecessary temporary vectors for the left and right sides of the recursive call, since std::nth_element already splits the array in the right way.
2. Filter contype and conaffinity before calling MakeBVH. This only affects the top level of the MakeBVH recursion.
3. Avoid repeatedly computing mju_rotVecQuat for each element at each nesting level.

This is roughly a 2x speedup.

PiperOrigin-RevId: 616152673
Change-Id: I6ce7d99cd4a923c43156beb3f2493c649602260e

src/user/user_objects.h

@@ -15,10 +15,8 @@
 #ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_
 #define MUJOCO_SRC_USER_USER_OBJECTS_H_
 
-#include <array>
 #include <functional>
 #include <map>
-#include <optional>
 #include <string>
 #include <string_view>
 #include <utility>
@@ -144,7 +142,28 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
   mjCBoundingVolume* GetBoundingVolume(int id);
 
  private:
-  int MakeBVH(std::vector<const mjCBoundingVolume*>& elements, int lev = 0);
+  // internal class used during BVH construction, for partial sorting of bounding volumes
+  struct BVElement {
+    const mjCBoundingVolume* e;
+    // index of the element in the original input to BVH, used to ensure a stable sort
+    int index;
+    // position of the element in the BVH axes
+    mjtNum lpos[3];
+  };
+
+  struct BVElementCompare {
+    int axis = 0;
+
+    bool operator()(const BVElement& e1, const BVElement& e2) const {
+      if (e1.lpos[axis] != e2.lpos[axis]) {
+        return e1.lpos[axis] < e2.lpos[axis];
+      }
+      return e1.index < e2.index;
+    }
+  };
+
+  int MakeBVH(std::vector<BVElement>::iterator elements_begin,
+              std::vector<BVElement>::iterator elements_end, int lev = 0);
 };