From de918a8b2d3114f0416e763928c69823d56c0987 Mon Sep 17 00:00:00 2001
From: Nimrod Gileadi <nimrod@google.com>
Date: Mon, 18 Mar 2024 06:18:28 -0700
Subject: [PATCH] Small cosmetic fixes to MakeBVH.

Remove the index member of BVElement, since it's redundant for the purpose of sorting. BVElement is now 32 bytes instead of 40, which should make things a bit better, as this data gets moved around during MakeBVH. Unfortunately, this has no measurable effect on the benchmark.

PiperOrigin-RevId: 616806520
Change-Id: I3b8a28c42a4a21dc25985e63ac57764fdb669afa
---
 src/user/user_objects.cc | 5 ++---
 src/user/user_objects.h  | 5 ++---
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
index dd21fab0..997bcde4 100644
--- a/src/user/user_objects.cc
+++ b/src/user/user_objects.cc
@@ -301,7 +301,6 @@ void mjCBoundingVolumeHierarchy::CreateBVH() {
     if (bvleaf_[i].conaffinity || bvleaf_[i].contype) {
       BVElement element;
       element.e = &bvleaf_[i];
-      element.index = i;
       mjtNum vert[3] = {element.e->pos[0] - ipos_[0],
                         element.e->pos[1] - ipos_[1],
                         element.e->pos[2] - ipos_[2]};
@@ -398,10 +397,10 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
   axis = edges[axis] > edges[2] ? axis : 2;
 
   // find median along the axis
-  auto m = nelements/2;
-  // Note: nth element performs a partial sort of elements
+  // note: nth_element performs a partial sort of elements
   BVElementCompare compare;
   compare.axis = axis;
+  int m = nelements / 2;
   std::nth_element(elements_begin, elements_begin + m, elements_end, compare);
 
   // recursive calls
diff --git a/src/user/user_objects.h b/src/user/user_objects.h
index a62436e1..01333478 100644
--- a/src/user/user_objects.h
+++ b/src/user/user_objects.h
@@ -146,8 +146,6 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
   // 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];
   };
@@ -159,7 +157,8 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
       if (e1.lpos[axis] != e2.lpos[axis]) {
         return e1.lpos[axis] < e2.lpos[axis];
       }
-      return e1.index < e2.index;
+      // comparing pointers gives a stable sort, because they both come from the same array
+      return e1.e < e2.e;
     }
   };