From 84108589c4b4d87323ee356779cf453daa36f741 Mon Sep 17 00:00:00 2001
From: Nimrod Gileadi <nimrod@google.com>
Date: Fri, 12 Jul 2024 05:44:33 -0700
Subject: [PATCH] Move the map lookup outside of the mesh processing loop.
 Change to unordered_map.

PiperOrigin-RevId: 651745837
Change-Id: Ibfcbe4b8cefa53a3dbaa99c4ca68442e2e10bc5d
---
 src/user/user_mesh.cc | 6 ++++--
 src/user/user_model.h | 3 ++-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc
index 350babc7..2a500909 100644
--- a/src/user/user_mesh.cc
+++ b/src/user/user_mesh.cc
@@ -1365,6 +1365,8 @@ void mjCMesh::Process() {
   // find centroid of faces
   ComputeFaceCentroid(facecen);
 
+  double density = model->def_map[classname]->Geom().density;
+
   // compute inertial properties for both inertia types
   for ( const auto type : { mjtGeomInertia::mjINERTIA_VOLUME, mjtGeomInertia::mjINERTIA_SHELL } ) {
     double CoM[3] = {0, 0, 0};
@@ -1423,7 +1425,7 @@ void mjCMesh::Process() {
       // apply formula, accumulate
       GetVolumeRef(type) += vol;
       for (int j=0; j<6; j++) {
-        P[j] += model->def_map[classname]->Geom().density*vol /
+        P[j] += density*vol /
                   (type==mjINERTIA_SHELL ? 12 : 20) * (
                   2*(D[k[j][0]] * D[k[j][1]] +
                     E[k[j][0]] * E[k[j][1]] +
@@ -1464,7 +1466,7 @@ void mjCMesh::Process() {
     }
 
     // compute sizes of equivalent inertia box
-    double mass = GetVolumeRef(type) * model->def_map[classname]->Geom().density;
+    double mass = GetVolumeRef(type) * density;
     double* boxsz = GetInertiaBoxPtr(type);
     boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
     boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
diff --git a/src/user/user_model.h b/src/user/user_model.h
index 7b86aaef..a4bd11ed 100644
--- a/src/user/user_model.h
+++ b/src/user/user_model.h
@@ -20,6 +20,7 @@
 #include <map>
 #include <string>
 #include <string_view>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -276,7 +277,7 @@ class mjCModel : public mjCModel_, private mjSpec {
   void RestoreState(const mjModel* m, mjData** dest);
 
   // map from default class name to default class pointer
-  std::map<std::string, mjCDef*> def_map;
+  std::unordered_map<std::string, mjCDef*> def_map;
 
  private:
   // settings for each defaults class