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