chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Clean up user_util.

Browse Source 
PiperOrigin-RevId: 649166858
Change-Id: I79423dda628fb4bf3ceae438dd8dc5b96d69d2a9
This commit is contained in:
Kyle Bayes
2024-07-03 12:37:48 -07:00
committed by Copybara-Service
parent 2a4410e4e5
commit 5cce67439b
2 changed files with 41 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_util.cc
+25 -31
View File
@@ -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));
}
src/user/user_util.h
+16 -16
View File
@@ -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);