From 5cce67439b488cc62c97ee5ec39251ad0aeff6d2 Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Wed, 3 Jul 2024 12:37:48 -0700
Subject: [PATCH] Clean up user_util.

PiperOrigin-RevId: 649166858
Change-Id: I79423dda628fb4bf3ceae438dd8dc5b96d69d2a9
---
 src/user/user_util.cc | 56 +++++++++++++++++++------------------------
 src/user/user_util.h  | 32 ++++++++++++-------------
 2 files changed, 41 insertions(+), 47 deletions(-)

diff --git a/src/user/user_util.cc b/src/user/user_util.cc
index 4d640091..ed3a7158 100644
--- a/src/user/user_util.cc
+++ b/src/user/user_util.cc
@@ -17,28 +17,21 @@
 #include <cmath>
 #include <cstddef>
 #include <cstdio>
-#include <limits>
 #include <optional>
 #include <string>
 #include <string_view>
 
-#include <mujoco/mjtnum.h>
 #include <mujoco/mujoco.h>
 #include "engine/engine_crossplatform.h"
 
-using std::isnan;
-using std::string;
-using std::numeric_limits;
-
-
 // check if numeric variable is defined
-bool mjuu_defined(const double num) {
-  return !isnan(num);
+bool mjuu_defined(double num) {
+  return !std::isnan(num);
 }
 
 
 // compute address of M[g1][g2] where M is triangular n-by-n
-int mjuu_matadr(int g1, int g2, const int n) {
+int mjuu_matadr(int g1, int g2, int n) {
   if (g1<0 || g2<0 || g1>=n || g2>=n) {
     return -1;
   }
@@ -54,13 +47,13 @@ int mjuu_matadr(int g1, int g2, const int n) {
 
 
 // set 4D vector
-void mjuu_setvec(double* dest, const double x, const double y, const double z, const double w) {
+void mjuu_setvec(double* dest, double x, double y, double z, double w) {
   dest[0] = x;
   dest[1] = y;
   dest[2] = z;
   dest[3] = w;
 }
-void mjuu_setvec(float* dest, const double x, const double y, const double z, const double w) {
+void mjuu_setvec(float* dest, double x, double y, double z, double w) {
   dest[0] = (float)x;
   dest[1] = (float)y;
   dest[2] = (float)z;
@@ -69,12 +62,12 @@ void mjuu_setvec(float* dest, const double x, const double y, const double z, co
 
 
 // set 3D vector
-void mjuu_setvec(double* dest, const double x, const double y, const double z) {
+void mjuu_setvec(double* dest, double x, double y, double z) {
   dest[0] = x;
   dest[1] = y;
   dest[2] = z;
 }
-void mjuu_setvec(float* dest, const double x, const double y, const double z) {
+void mjuu_setvec(float* dest, double x, double y, double z) {
   dest[0] = (float)x;
   dest[1] = (float)y;
   dest[2] = (float)z;
@@ -82,28 +75,28 @@ void mjuu_setvec(float* dest, const double x, const double y, const double z) {
 
 
 // set 2D vector
-void mjuu_setvec(double* dest, const double x, const double y) {
+void mjuu_setvec(double* dest, double x, double y) {
   dest[0] = x;
   dest[1] = y;
 }
 
 
 // add to double array
-void mjuu_addtovec(double* dest, const double* src, const int n) {
+void mjuu_addtovec(double* dest, const double* src, int n) {
   for (int i=0; i<n; i++) {
     dest[i] += src[i];
   }
 }
 
 // zero double array
-void mjuu_zerovec(double* dest, const int n) {
+void mjuu_zerovec(double* dest, int n) {
   for (int i=0; i<n; i++) {
     dest[i] = 0;
   }
 }
 
 // zero float array
-void mjuu_zerovec(float* dest, const int n) {
+void mjuu_zerovec(float* dest, int n) {
   for (int i=0; i<n; i++) {
     dest[i] = 0;
   }
@@ -115,7 +108,7 @@ double mjuu_dot3(const double* a, const double* b) {
 }
 
 
-// distance beween 3D points
+// distance between 3D points
 double mjuu_dist3(const double* a, const double* b) {
   return sqrt((a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]));
 }
@@ -735,12 +728,12 @@ void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4],
 }
 
 // strip directory from filename
-string mjuu_strippath(string filename) {
+std::string mjuu_strippath(std::string filename) {
   // find last pathsymbol
   size_t start = filename.find_last_of("/\\");
 
   // no path found: return original
-  if (start==string::npos) {
+  if (start==std::string::npos) {
     return filename;
   }
 
@@ -785,12 +778,12 @@ const char* mjuu_fullInertia(double quat[4], double inertia[3], const double ful
 
 
 // strip extension
-string mjuu_stripext(string filename) {
+std::string mjuu_stripext(std::string filename) {
   // find last dot
   size_t end = filename.find_last_of('.');
 
   // no path found: return original
-  if (end==string::npos) {
+  if (end == std::string::npos) {
     return filename;
   }
 
@@ -798,18 +791,18 @@ string mjuu_stripext(string filename) {
   return filename.substr(0, end);
 }
 
-string mjuu_getext(std::string_view filename) {
+std::string mjuu_getext(std::string_view filename) {
   size_t dot = filename.find_last_of('.');
 
-  if (dot==string::npos) {
+  if (dot == std::string::npos) {
     return "";
   }
-  return string(filename.substr(dot, filename.size() - dot));
+  return std::string(filename.substr(dot, filename.size() - dot));
 }
 
 
 // is directory path absolute
-bool mjuu_isabspath(string path) {
+bool mjuu_isabspath(std::string path) {
   // empty: not absolute
   if (path.empty()) {
     return false;
@@ -828,7 +821,8 @@ bool mjuu_isabspath(string path) {
   }
 
   // find ":/" or ":\"
-  if (path.find(":/")!=string::npos || path.find(":\\")!=string::npos) {
+  if (path.find(":/") != std::string::npos ||
+      path.find(":\\") != std::string::npos) {
     return true;
   }
 
@@ -838,7 +832,7 @@ bool mjuu_isabspath(string path) {
 
 
 // assemble two file paths
-std::string mjuu_combinePaths(const string& path1, const string& path2) {
+std::string mjuu_combinePaths(const std::string& path1, const std::string& path2) {
   // path2 has absolute path
   if (mjuu_isabspath(path2)) {
     return path2;
@@ -854,8 +848,8 @@ std::string mjuu_combinePaths(const string& path1, const string& path2) {
 
 
 // assemble three file paths
-std::string mjuu_combinePaths(const string& path1, const string& path2,
-                              const string& path3) {
+std::string mjuu_combinePaths(const std::string& path1, const std::string& path2,
+                              const std::string& path3) {
   return mjuu_combinePaths(path1, mjuu_combinePaths(path2, path3));
 }
 
diff --git a/src/user/user_util.h b/src/user/user_util.h
index c67a0a6a..38932964 100644
--- a/src/user/user_util.h
+++ b/src/user/user_util.h
@@ -20,26 +20,26 @@
 #include <string_view>
 
 
-const double mjEPS = 1E-14;                     // minimum value in various calculations
-const double mjMINMASS = 1E-6;                  // minimum mass allowed
+const double mjEPS = 1E-14;     // minimum value in various calculations
+const double mjMINMASS = 1E-6;  // minimum mass allowed
 
 // check if numeric variable is defined:  !_isnan(num)
-bool mjuu_defined(const double num);
+bool mjuu_defined(double num);
 
 // compute linear address of M[g1][g2] where M is triangular n-by-n
 // return -1 if inputs are invalid
-int mjuu_matadr(int g1, int g2, const int n);
+int mjuu_matadr(int g1, int g2, int n);
 
 // set 4D vector
-void mjuu_setvec(double* dest, const double x, const double y, const double z, const double w);
-void mjuu_setvec(float* dest, const double x, const double y, const double z, const double w);
+void mjuu_setvec(double* dest, double x, double y, double z, double w);
+void mjuu_setvec(float* dest, double x, double y, double z, double w);
 
 // set 3D vector
-void mjuu_setvec(double* dest, const double x, const double y, const double z);
-void mjuu_setvec(float* dest, const double x, const double y, const double z);
+void mjuu_setvec(double* dest, double x, double y, double z);
+void mjuu_setvec(float* dest, double x, double y, double z);
 
 // set 2D vector
-void mjuu_setvec(double* dest, const double x, const double y);
+void mjuu_setvec(double* dest, double x, double y);
 
 // copy real-valued vector
 template <class T1, class T2>
@@ -50,18 +50,18 @@ void mjuu_copyvec(T1* dest, const T2* src, int n) {
 }
 
 // add to double array
-void mjuu_addtovec(double* dest, const double* src, const int n);
+void mjuu_addtovec(double* dest, const double* src, int n);
 
 // zero array
-void mjuu_zerovec(double* dest, const int n);
+void mjuu_zerovec(double* dest, int n);
 
 // zero float array
-void mjuu_zerovec(float* dest, const int n);
+void mjuu_zerovec(float* dest, int n);
 
 // dot-product in 3D
 double mjuu_dot3(const double* a, const double* b);
 
-// distance beween 3D points
+// distance between 3D points
 double mjuu_dist3(const double* a, const double* b);
 
 // L1 norm between vectors
@@ -69,8 +69,8 @@ double mjuu_L1(const double* a, const double* b, int n);
 
 // normalize vector to unit length, return previous length
 //  if norm(vec)<mjEPS, return 0 and do not change vector
-double mjuu_normvec(double* vec, const int n);
-float mjuu_normvec(float* vec, const int n);
+double mjuu_normvec(double* vec, int n);
+float mjuu_normvec(float* vec, int n);
 
 // convert quaternion to rotation matrix
 void mjuu_quat2mat(double* res, const double* quat);
@@ -134,7 +134,7 @@ void mjuu_frameaccuminv(double pos[3], double quat[4],
 void mjuu_globalinertia(double* global, const double* local, const double* quat);
 
 // compute off-center correction to inertia matrix
-void mjuu_offcenter(double* res, const double mass, const double* vec);
+void mjuu_offcenter(double* res, double mass, const double* vec);
 
 // compute viscosity coefficients from mass and inertia
 void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double scl=1);