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

Format some more files in user/

Browse Source 
PiperOrigin-RevId: 958383817
Change-Id: I17fcdbf32967ec5a5c4d2552823ef5100faaea3b
This commit is contained in:
Yuval Tassa
2026-08-03 08:16:21 -07:00
committed by Copybara-Service
parent 7927e908a3
commit 8a643f4e2b
9 changed files with 961 additions and 1202 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_cache.cc
+17 -52
View File
@@ -37,7 +37,6 @@ mjCAsset mjCAsset::Copy(const mjCAsset& other) {
} }
// sets the total maximum size of the cache in bytes // sets the total maximum size of the cache in bytes
// low-priority cached assets will be dropped to make the new memory // low-priority cached assets will be dropped to make the new memory
// requirement // requirement
@@ -48,44 +47,36 @@ void mjCCache::SetCapacity(std::size_t size) {
} }
// returns the corresponding timestamp, if the given asset is stored in the cache // returns the corresponding timestamp, if the given asset is stored in the cache
const std::string* mjCCache::HasAsset(const std::string& id) { const std::string* mjCCache::HasAsset(const std::string& id) {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
auto it = lookup_.find(id); auto it = lookup_.find(id);
if (it == lookup_.end()) { if (it == lookup_.end()) { return nullptr; }
return nullptr;
}
return &(it->second.Timestamp()); return &(it->second.Timestamp());
} }
// inserts an asset into the cache, if asset is already in the cache, its data // inserts an asset into the cache, if asset is already in the cache, its data
// is updated only if the timestamps disagree // is updated only if the timestamps disagree
bool mjCCache::Insert(const std::string& modelname, const std::string& id, const mjResource *resource, bool mjCCache::Insert(const std::string& modelname,
std::shared_ptr<const void> data, std::size_t size) { const std::string& id,
const mjResource* resource,
std::shared_ptr<const void> data,
std::size_t size) {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
// check if asset is too large to fit in the cache // check if asset is too large to fit in the cache
if ((size_ + size > capacity_) && if ((size_ + size > capacity_) && lookup_.find(id) == lookup_.end()) { return false; }
lookup_.find(id) == lookup_.end()) {
return false;
}
mjCAsset asset(modelname, id, resource, data, size); mjCAsset asset(modelname, id, resource, data, size);
auto [it, inserted] = lookup_.insert({id, asset}); auto [it, inserted] = lookup_.insert({id, asset});
mjCAsset* asset_ptr = &(it->second); mjCAsset* asset_ptr = &(it->second);
if (!inserted) { if (!inserted) {
if (size_ - asset_ptr->BytesCount() + size > capacity_) { if (size_ - asset_ptr->BytesCount() + size > capacity_) { return false; }
return false;
}
models_[modelname].insert(asset_ptr); // add it for the model models_[modelname].insert(asset_ptr); // add it for the model
asset_ptr->AddReference(modelname); asset_ptr->AddReference(modelname);
if (it->second.Timestamp() == asset.Timestamp()) { if (it->second.Timestamp() == asset.Timestamp()) { return true; }
return true;
}
asset_ptr->SetTimestamp(asset.Timestamp()); asset_ptr->SetTimestamp(asset.Timestamp());
size_ = size_ - asset_ptr->BytesCount() + size; size_ = size_ - asset_ptr->BytesCount() + size;
asset_ptr->ReplaceData(asset); asset_ptr->ReplaceData(asset);
@@ -101,19 +92,14 @@ bool mjCCache::Insert(const std::string& modelname, const std::string& id, const
} }
// populate data from the cache into the given function, return true if data was // populate data from the cache into the given function, return true if data was
// copied // copied
bool mjCCache::PopulateData(const std::string& id, const mjResource* resource, mjCDataFunc fn) { bool mjCCache::PopulateData(const std::string& id, const mjResource* resource, mjCDataFunc fn) {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
auto it = lookup_.find(id); auto it = lookup_.find(id);
if (it == lookup_.end()) { if (it == lookup_.end()) { return false; }
return false;
}
if (mju_isModifiedResource(resource, it->second.Timestamp().c_str())) { if (mju_isModifiedResource(resource, it->second.Timestamp().c_str())) { return false; }
return false;
}
mjCAsset* asset = &(it->second); mjCAsset* asset = &(it->second);
@@ -126,32 +112,25 @@ bool mjCCache::PopulateData(const std::string& id, const mjResource* resource, m
} }
// removes model from the cache along with assets referencing only this model // removes model from the cache along with assets referencing only this model
void mjCCache::RemoveModel(const std::string& filename) { void mjCCache::RemoveModel(const std::string& filename) {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
for (mjCAsset* asset : models_[filename]) { for (mjCAsset* asset : models_[filename]) {
asset->RemoveReference(filename); asset->RemoveReference(filename);
if (!asset->HasReferences()) { if (!asset->HasReferences()) { Delete(asset, filename); }
Delete(asset, filename);
}
} }
models_.erase(filename); models_.erase(filename);
} }
// Wipes out all internal data for the given model // Wipes out all internal data for the given model
void mjCCache::Reset(const std::string& filename) { void mjCCache::Reset(const std::string& filename) {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
for (auto asset : models_[filename]) { for (auto asset : models_[filename]) { Delete(asset, filename); }
Delete(asset, filename);
}
models_.erase(filename); models_.erase(filename);
} }
// Wipes out all internal data // Wipes out all internal data
void mjCCache::Reset() { void mjCCache::Reset() {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
@@ -163,62 +142,48 @@ void mjCCache::Reset() {
} }
std::size_t mjCCache::Capacity() const { std::size_t mjCCache::Capacity() const {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
return capacity_; return capacity_;
} }
std::size_t mjCCache::Size() const { std::size_t mjCCache::Size() const {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
return size_; return size_;
} }
// Deletes a single asset // Deletes a single asset
void mjCCache::DeleteAsset(const std::string& id) { void mjCCache::DeleteAsset(const std::string& id) {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
auto it = lookup_.find(id); auto it = lookup_.find(id);
if (it != lookup_.end()) { if (it != lookup_.end()) { Delete(&(it->second)); }
Delete(&(it->second));
} }
}
// Deletes a single asset (internal) // Deletes a single asset (internal)
void mjCCache::Delete(mjCAsset* asset) { void mjCCache::Delete(mjCAsset* asset) {
size_ -= asset->BytesCount(); size_ -= asset->BytesCount();
entries_.erase(asset); entries_.erase(asset);
for (auto& reference : asset->References()) { for (auto& reference : asset->References()) { models_[reference].erase(asset); }
models_[reference].erase(asset);
}
lookup_.erase(asset->Id()); lookup_.erase(asset->Id());
} }
// Deletes a single asset (internal) // Deletes a single asset (internal)
void mjCCache::Delete(mjCAsset* asset, const std::string& skip) { void mjCCache::Delete(mjCAsset* asset, const std::string& skip) {
size_ -= asset->BytesCount(); size_ -= asset->BytesCount();
entries_.erase(asset); entries_.erase(asset);
for (auto& reference : asset->References()) { for (auto& reference : asset->References()) {
if (reference != skip) { if (reference != skip) { models_[reference].erase(asset); }
models_[reference].erase(asset);
}
} }
lookup_.erase(asset->Id()); lookup_.erase(asset->Id());
} }
// trims out data to meet memory requirements // trims out data to meet memory requirements
void mjCCache::Trim() { void mjCCache::Trim() {
while (size_ > capacity_) { while (size_ > capacity_) { Delete(*entries_.begin()); }
Delete(*entries_.begin());
}
} }
src/user/user_cache.h
+16 -18
View File
@@ -38,11 +38,14 @@ typedef void (*mjCDeallocFunc)(const void*);
// asset). // asset).
class mjCAsset { class mjCAsset {
friend class mjCCache; friend class mjCCache;
public: public:
mjCAsset(std::string modelname, std::string id, const mjResource* resource, mjCAsset(std::string modelname,
std::shared_ptr<const void> data, std::size_t size) : std::string id,
id_(id), timestamp_(resource->timestamp), const mjResource* resource,
size_(size), data_(std::move(data)) { std::shared_ptr<const void> data,
std::size_t size)
: id_(id), timestamp_(resource->timestamp), size_(size), data_(std::move(data)) {
AddReference(modelname); AddReference(modelname);
} }
@@ -58,18 +61,14 @@ class mjCAsset {
std::size_t AccessCount() const { return access_count_; } std::size_t AccessCount() const { return access_count_; }
// pass data in the cache to the given function, return true if data was copied // pass data in the cache to the given function, return true if data was copied
bool PopulateData(mjCDataFunc fn) const { bool PopulateData(mjCDataFunc fn) const { return fn(data_.get()); }
return fn(data_.get());
}
private: private:
mjCAsset() = default; mjCAsset() = default;
// helpers for managing models referencing this asset // helpers for managing models referencing this asset
void AddReference(std::string xml_file) { references_.insert(xml_file); } void AddReference(std::string xml_file) { references_.insert(xml_file); }
void RemoveReference(const std::string& xml_file) { void RemoveReference(const std::string& xml_file) { references_.erase(xml_file); }
references_.erase(xml_file);
}
void ReplaceData(const mjCAsset& other) { void ReplaceData(const mjCAsset& other) {
data_ = other.data_; data_ = other.data_;
@@ -89,9 +88,7 @@ class mjCAsset {
// accessors // accessors
std::size_t BytesCount() const { return size_; } std::size_t BytesCount() const { return size_; }
const void* Data() const { const void* Data() const { return data_.get(); }
return data_.get();
}
const std::set<std::string>& References() const { return references_; } const std::set<std::string>& References() const { return references_; }
std::string id_; // unique id associated with asset std::string id_; // unique id associated with asset
@@ -107,9 +104,7 @@ class mjCAsset {
struct mjCAssetCompare { struct mjCAssetCompare {
bool operator()(const mjCAsset* e1, const mjCAsset* e2) const { bool operator()(const mjCAsset* e1, const mjCAsset* e2) const {
if (e1->AccessCount() != e2->AccessCount()) { if (e1->AccessCount() != e2->AccessCount()) { return e1->AccessCount() < e2->AccessCount(); }
return e1->AccessCount() < e2->AccessCount();
}
return e1->InsertNum() < e2->InsertNum(); return e1->InsertNum() < e2->InsertNum();
} }
}; };
@@ -136,8 +131,11 @@ class mjCCache {
// inserts an asset into the cache, if asset is already in the cache, its data // inserts an asset into the cache, if asset is already in the cache, its data
// is updated only if the timestamps disagree // is updated only if the timestamps disagree
bool Insert(const std::string& modelname, const std::string& id, const mjResource *resource, bool Insert(const std::string& modelname,
std::shared_ptr<const void> data, std::size_t size); const std::string& id,
const mjResource* resource,
std::shared_ptr<const void> data,
std::size_t size);
// populate data from the cache into the given function // populate data from the cache into the given function
bool PopulateData(const std::string& id, const mjResource* resource, mjCDataFunc fn); bool PopulateData(const std::string& id, const mjResource* resource, mjCDataFunc fn);
src/user/user_composite.h
+2 -1
View File
@@ -109,7 +109,8 @@ class mjCComposite {
int dim; // dimensionality int dim; // dimensionality
private: private:
mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]); mjsBody* AddCableBody(
mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]);
// temporary skin vectors // temporary skin vectors
void CopyIntoSkin(mjsSkin* skin); void CopyIntoSkin(mjsSkin* skin);
src/user/user_flexcomp.cc
+150 -302
View File
File diff suppressed because it is too large Load Diff
src/user/user_flexcomp.h
+17 -8
View File
@@ -59,15 +59,19 @@ class mjCFlexcomp {
bool MakeGrid(char* error, int error_sz); bool MakeGrid(char* error, int error_sz);
bool MakeBox(char* error, int error_sz, int dim, bool open = true); bool MakeBox(char* error, int error_sz, int dim, bool open = true);
bool MakeSquare(char* error, int error_sz); bool MakeSquare(char* error, int error_sz);
bool MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, bool MakeMesh(mjCModel* model,
mjsCompiler* compiler,
char* error,
int error_sz,
const mjVFS* vfs = nullptr); const mjVFS* vfs = nullptr);
bool MakeGMSH(mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, bool MakeGMSH(mjCModel* model,
mjsCompiler* compiler,
char* error,
int error_sz,
const mjVFS* vfs = nullptr); const mjVFS* vfs = nullptr);
void LoadGMSH(mjCModel* model, mjResource* resource); void LoadGMSH(mjCModel* model, mjResource* resource);
void LoadGMSH41(char* buffer, int binary, int nodeend, int nodebegin, void LoadGMSH41(char* buffer, int binary, int nodeend, int nodebegin, int elemend, int elembegin);
int elemend, int elembegin); void LoadGMSH22(char* buffer, int binary, int nodeend, int nodebegin, int elemend, int elembegin);
void LoadGMSH22(char* buffer, int binary, int nodeend, int nodebegin,
int elemend, int elembegin);
int GridID(int ix, int iy); int GridID(int ix, int iy);
@@ -120,8 +124,13 @@ class mjCFlexcomp {
private: private:
// identify empty cells and pin nodes exclusively in empty cells // identify empty cells and pin nodes exclusively in empty cells
void MarkEmptyCells(mjCFlex* flex, const double* points, int npnt, void MarkEmptyCells(mjCFlex* flex,
const double minmax[6], int nx, int ny, int nz); const double* points,
int npnt,
const double minmax[6],
int nx,
int ny,
int nz);
}; };
#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ #endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_
src/user/user_threadpool.cc
+2 -6
View File
@@ -35,14 +35,10 @@ ThreadPool::ThreadPool(int num_threads) : ctr_(0) {
ThreadPool::~ThreadPool() { ThreadPool::~ThreadPool() {
{ {
std::unique_lock<std::mutex> lock(m_); std::unique_lock<std::mutex> lock(m_);
for (int i = 0; i < threads_.size(); i++) { for (int i = 0; i < threads_.size(); i++) { queue_.push(nullptr); }
queue_.push(nullptr);
}
cv_in_.notify_all(); cv_in_.notify_all();
} }
for (auto& thread : threads_) { for (auto& thread : threads_) { thread.join(); }
thread.join();
}
} }
// ThreadPool scheduler // ThreadPool scheduler
src/user/user_util.cc
+145 -222
View File
@@ -52,9 +52,7 @@ bool mjuu_defined(double 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, 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;
}
if (g1 > g2) { if (g1 > g2) {
int tmp = g1; int tmp = g1;
@@ -103,23 +101,17 @@ void mjuu_setvec(double* dest, double x, double y) {
// add to double array // add to double array
void mjuu_addtovec(double* dest, const double* src, 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, 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, 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;
}
} }
// dot-product in 3D // dot-product in 3D
@@ -130,16 +122,16 @@ double mjuu_dot3(const double* a, const double* b) {
// distance between 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]));
} }
// L1 norm between vectors // L1 norm between vectors
double mjuu_L1(const double* a, const double* b, int n) { double mjuu_L1(const double* a, const double* b, int n) {
double res = 0; double res = 0;
for (int i=0; i < n; i++) { for (int i = 0; i < n; i++) { res += std::abs(a[i] - b[i]); }
res += std::abs(a[i]-b[i]);
}
return res; return res;
} }
@@ -149,20 +141,14 @@ double mjuu_L1(const double* a, const double* b, int n) {
double mjuu_normvec(double* vec, const int n) { double mjuu_normvec(double* vec, const int n) {
double nrm = 0; double nrm = 0;
for (int i=0; i < n; i++) { for (int i = 0; i < n; i++) { nrm += vec[i] * vec[i]; }
nrm += vec[i]*vec[i]; if (nrm < mjEPS) { return 0; }
}
if (nrm < mjEPS) {
return 0;
}
nrm = sqrt(nrm); nrm = sqrt(nrm);
// don't normalize if nrm is within mjEPS of 1 // don't normalize if nrm is within mjEPS of 1
if (std::abs(nrm - 1) > mjEPS) { if (std::abs(nrm - 1) > mjEPS) {
for (int i=0; i < n; i++) { for (int i = 0; i < n; i++) { vec[i] /= nrm; }
vec[i] /= nrm;
}
} }
return nrm; return nrm;
@@ -173,20 +159,14 @@ double mjuu_normvec(double* vec, const int n) {
float mjuu_normvec(float* vec, const int n) { float mjuu_normvec(float* vec, const int n) {
float nrm = 0; float nrm = 0;
for (int i=0; i < n; i++) { for (int i = 0; i < n; i++) { nrm += vec[i] * vec[i]; }
nrm += vec[i]*vec[i]; if (nrm < mjEPS) { return 0; }
}
if (nrm < mjEPS) {
return 0;
}
nrm = sqrt(nrm); nrm = sqrt(nrm);
// don't normalize if nrm is within mjEPS of 1 // don't normalize if nrm is within mjEPS of 1
if (std::abs(nrm - 1) > mjEPS) { if (std::abs(nrm - 1) > mjEPS) {
for (int i=0; i < n; i++) { for (int i = 0; i < n; i++) { vec[i] /= nrm; }
vec[i] /= nrm;
}
} }
return nrm; return nrm;
@@ -194,9 +174,7 @@ float mjuu_normvec(float* vec, const int n) {
// scale vector by scalar // scale vector by scalar
void mjuu_scalevec(double* res, const double* vec, double s, int n) { void mjuu_scalevec(double* res, const double* vec, double s, int n) {
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) { res[i] = s * vec[i]; }
res[i] = s * vec[i];
}
} }
// convert quaternion to rotation matrix // convert quaternion to rotation matrix
@@ -237,7 +215,6 @@ void mjuu_quat2mat(double* res, const double* quat) {
res[5] = 2 * (q23 - q01); res[5] = 2 * (q23 - q01);
res[6] = 2 * (q13 - q02); res[6] = 2 * (q13 - q02);
res[7] = 2 * (q23 + q01); res[7] = 2 * (q23 + q01);
} }
@@ -255,11 +232,10 @@ void mjuu_mulquat(double* res, const double* qa, const double* qb) {
// multiply matrix by vector, 3-by-3 // multiply matrix by vector, 3-by-3
void mjuu_mulvecmat(double* res, const double* vec, const double* mat) { void mjuu_mulvecmat(double* res, const double* vec, const double* mat) {
double tmp[3] = { double tmp[3] = {mat[0] * vec[0] + mat[1] * vec[1] + mat[2] * vec[2],
mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2],
mat[3] * vec[0] + mat[4] * vec[1] + mat[5] * vec[2], mat[3] * vec[0] + mat[4] * vec[1] + mat[5] * vec[2],
mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2] mat[6] * vec[0] + mat[7] * vec[1] + mat[8] * vec[2]};
};
res[0] = tmp[0]; res[0] = tmp[0];
res[1] = tmp[1]; res[1] = tmp[1];
res[2] = tmp[2]; res[2] = tmp[2];
@@ -268,16 +244,16 @@ void mjuu_mulvecmat(double* res, const double* vec, const double* mat) {
// multiply transposed matrix by vector, 3-by-3 // multiply transposed matrix by vector, 3-by-3
void mjuu_mulvecmatT(double* res, const double* vec, const double* mat) { void mjuu_mulvecmatT(double* res, const double* vec, const double* mat) {
double tmp[3] = { double tmp[3] = {mat[0] * vec[0] + mat[3] * vec[1] + mat[6] * vec[2],
mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2],
mat[1] * vec[0] + mat[4] * vec[1] + mat[7] * vec[2], mat[1] * vec[0] + mat[4] * vec[1] + mat[7] * vec[2],
mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2] mat[2] * vec[0] + mat[5] * vec[1] + mat[8] * vec[2]};
};
res[0] = tmp[0]; res[0] = tmp[0];
res[1] = tmp[1]; res[1] = tmp[1];
res[2] = tmp[2]; res[2] = tmp[2];
} }
// clang-format off
// compute res = R * M * R' // compute res = R * M * R'
void mjuu_mulRMRT(double* res, const double* R, const double* M) { void mjuu_mulRMRT(double* res, const double* R, const double* M) {
double tmp[9]; double tmp[9];
@@ -322,13 +298,11 @@ void mjuu_mulmat(double* res, const double* A, const double* B) {
tmp[8] = A[6]*B[2] + A[7]*B[5] + A[8]*B[8]; tmp[8] = A[6]*B[2] + A[7]*B[5] + A[8]*B[8];
mjuu_copyvec(res, tmp, 9); mjuu_copyvec(res, tmp, 9);
} }
// clang-format on
// transpose 3-by-3 matrix // transpose 3-by-3 matrix
void mjuu_transposemat(double* res, const double* mat) { void mjuu_transposemat(double* res, const double* mat) {
double tmp[9] = {mat[0], mat[3], mat[6], double tmp[9] = {mat[0], mat[3], mat[6], mat[1], mat[4], mat[7], mat[2], mat[5], mat[8]};
mat[1], mat[4], mat[7],
mat[2], mat[5], mat[8]};
mjuu_copyvec(res, tmp, 9); mjuu_copyvec(res, tmp, 9);
} }
@@ -356,17 +330,19 @@ void mjuu_localquat(double* local, const double* child, const double* parent) {
} }
// clang-format off
// compute vector cross-product a = b x c // compute vector cross-product a = b x c
void mjuu_crossvec(double* a, const double* b, const double* c) { void mjuu_crossvec(double* a, const double* b, const double* c) {
a[0] = b[1]*c[2] - b[2]*c[1]; a[0] = b[1]*c[2] - b[2]*c[1];
a[1] = b[2]*c[0] - b[0]*c[2]; a[1] = b[2]*c[0] - b[0]*c[2];
a[2] = b[0]*c[1] - b[1]*c[0]; a[2] = b[0]*c[1] - b[1]*c[0];
} }
// clang-format on
// compute normal vector to given triangle, return length // compute normal vector to given triangle, return length
template<typename T> double mjuu_makenormal(double* normal, const T a[3], template <typename T>
const T b[3], const T c[3]) { double mjuu_makenormal(double* normal, const T a[3], const T b[3], const T c[3]) {
double v1[3] = {a[0], a[1], a[2]}; double v1[3] = {a[0], a[1], a[2]};
double v2[3] = {b[0], b[1], b[2]}; double v2[3] = {b[0], b[1], b[2]};
double v3[3] = {c[0], c[1], c[2]}; double v3[3] = {c[0], c[1], c[2]};
@@ -387,10 +363,14 @@ template<typename T> double mjuu_makenormal(double* normal, const T a[3],
return nrm; return nrm;
} }
template double mjuu_makenormal(double* normal, const double a[3], template double mjuu_makenormal(double* normal,
const double b[3], const double c[3]); const double a[3],
template double mjuu_makenormal(double* normal, const float a[3], const double b[3],
const float b[3], const float c[3]); const double c[3]);
template double mjuu_makenormal(double* normal,
const float a[3],
const float b[3],
const float c[3]);
// compute quaternion as minimal rotation from [0;0;1] to vec // compute quaternion as minimal rotation from [0;0;1] to vec
void mjuu_z2quat(double* quat, const double* vec) { void mjuu_z2quat(double* quat, const double* vec) {
@@ -450,8 +430,10 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl
// invert frame transformation // invert frame transformation
void mjuu_frameinvert(double newpos[3], double newquat[4], void mjuu_frameinvert(double newpos[3],
const double oldpos[3], const double oldquat[4]) { double newquat[4],
const double oldpos[3],
const double oldquat[4]) {
// position // position
mjuu_localaxis(newpos, oldpos, oldquat); mjuu_localaxis(newpos, oldpos, oldquat);
newpos[0] = -newpos[0]; newpos[0] = -newpos[0];
@@ -467,8 +449,10 @@ void mjuu_frameinvert(double newpos[3], double newquat[4],
// accumulate frame transformations (forward kinematics) // accumulate frame transformations (forward kinematics)
void mjuu_frameaccum(double pos[3], double quat[4], void mjuu_frameaccum(double pos[3],
const double childpos[3], const double childquat[4]) { double quat[4],
const double childpos[3],
const double childquat[4]) {
double mat[9], vec[3], qtmp[4]; double mat[9], vec[3], qtmp[4];
mjuu_quat2mat(mat, quat); mjuu_quat2mat(mat, quat);
mjuu_mulvecmat(vec, childpos, mat); mjuu_mulvecmat(vec, childpos, mat);
@@ -481,8 +465,10 @@ void mjuu_frameaccum(double pos[3], double quat[4],
// accumulate frame transformation in second frame // accumulate frame transformation in second frame
void mjuu_frameaccumChild(const double pos[3], const double quat[4], void mjuu_frameaccumChild(const double pos[3],
double childpos[3], double childquat[4]) { const double quat[4],
double childpos[3],
double childquat[4]) {
double p[] = {pos[0], pos[1], pos[2]}; double p[] = {pos[0], pos[1], pos[2]};
double q[] = {quat[0], quat[1], quat[2], quat[3]}; double q[] = {quat[0], quat[1], quat[2], quat[3]};
mjuu_frameaccum(p, q, childpos, childquat); mjuu_frameaccum(p, q, childpos, childquat);
@@ -492,8 +478,10 @@ void mjuu_frameaccumChild(const double pos[3], const double quat[4],
// invert frame accumulation // invert frame accumulation
void mjuu_frameaccuminv(double pos[3], double quat[4], void mjuu_frameaccuminv(double pos[3],
const double childpos[3], const double childquat[4]) { double quat[4],
const double childpos[3],
const double childquat[4]) {
double mat[9], vec[3], qtmp[4]; double mat[9], vec[3], qtmp[4];
double qneg[4] = {childquat[0], -childquat[1], -childquat[2], -childquat[3]}; double qneg[4] = {childquat[0], -childquat[1], -childquat[2], -childquat[3]};
mjuu_mulquat(qtmp, quat, qneg); mjuu_mulquat(qtmp, quat, qneg);
@@ -506,6 +494,7 @@ void mjuu_frameaccuminv(double pos[3], double quat[4],
} }
// clang-format off
// convert local_inertia[3] to global_inertia[6] // convert local_inertia[3] to global_inertia[6]
void mjuu_globalinertia(double* global, const double* local, const double* quat) { void mjuu_globalinertia(double* global, const double* local, const double* quat) {
double mat[9]; double mat[9];
@@ -524,6 +513,7 @@ void mjuu_globalinertia(double* global, const double* local, const double* quat)
global[4] = mat[0]*tmp[2] + mat[1]*tmp[5] + mat[2]*tmp[8]; global[4] = mat[0]*tmp[2] + mat[1]*tmp[5] + mat[2]*tmp[8];
global[5] = mat[3]*tmp[2] + mat[4]*tmp[5] + mat[5]*tmp[8]; global[5] = mat[3]*tmp[2] + mat[4]*tmp[5] + mat[5]*tmp[8];
} }
// clang-format on
// compute off-center correction to inertia matrix // compute off-center correction to inertia matrix
@@ -549,9 +539,12 @@ void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double
// apply formula for box (or rather cross) viscosity // apply formula for box (or rather cross) viscosity
// torque components // torque components
visccoef[0] = scl * 4.0 / 3.0 * ebox[0] * (ebox[1]*ebox[1]*ebox[1] + ebox[2]*ebox[2]*ebox[2]); visccoef[0] =
visccoef[1] = scl * 4.0 / 3.0 * ebox[1] * (ebox[0]*ebox[0]*ebox[0] + ebox[2]*ebox[2]*ebox[2]); scl * 4.0 / 3.0 * ebox[0] * (ebox[1] * ebox[1] * ebox[1] + ebox[2] * ebox[2] * ebox[2]);
visccoef[2] = scl * 4.0 / 3.0 * ebox[2] * (ebox[0]*ebox[0]*ebox[0] + ebox[1]*ebox[1]*ebox[1]); visccoef[1] =
scl * 4.0 / 3.0 * ebox[1] * (ebox[0] * ebox[0] * ebox[0] + ebox[2] * ebox[2] * ebox[2]);
visccoef[2] =
scl * 4.0 / 3.0 * ebox[2] * (ebox[0] * ebox[0] * ebox[0] + ebox[1] * ebox[1] * ebox[1]);
// force components // force components
visccoef[3] = scl * 4 * ebox[1] * ebox[2]; visccoef[3] = scl * 4 * ebox[1] * ebox[2];
@@ -595,11 +588,9 @@ void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]) {
// regular processing // regular processing
else { else {
// tmp = q_w * v + cross(q_xyz, v) // tmp = q_w * v + cross(q_xyz, v)
double tmp[3] = { double tmp[3] = {quat[0] * vec[0] + quat[2] * vec[2] - quat[3] * vec[1],
quat[0]*vec[0] + quat[2]*vec[2] - quat[3]*vec[1],
quat[0] * vec[1] + quat[3] * vec[0] - quat[1] * vec[2], quat[0] * vec[1] + quat[3] * vec[0] - quat[1] * vec[2],
quat[0]*vec[2] + quat[1]*vec[1] - quat[2]*vec[0] quat[0] * vec[2] + quat[1] * vec[1] - quat[2] * vec[0]};
};
// res = v + 2 * cross(q_xyz, t) // res = v + 2 * cross(q_xyz, t)
res[0] = vec[0] + 2 * (quat[2] * tmp[2] - quat[3] * tmp[1]); res[0] = vec[0] + 2 * (quat[2] * tmp[2] - quat[3] * tmp[1]);
@@ -618,8 +609,12 @@ void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]) {
// outputs: // outputs:
// quat - frame orientation // quat - frame orientation
// normal - unit normal vector // normal - unit normal vector
double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3], double mjuu_updateFrame(double quat[4],
const double tprv[3], const double tnxt[3], int first) { double normal[3],
const double edge[3],
const double tprv[3],
const double tnxt[3],
int first) {
double tangent[3], binormal[3]; double tangent[3], binormal[3];
// normalize tangent // normalize tangent
@@ -697,9 +692,7 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m
} }
// terminate if max off-diagonal element too small // terminate if max off-diagonal element too small
if (std::abs(D[3*rk+ck]) < kEigEPS) { if (std::abs(D[3 * rk + ck]) < kEigEPS) { break; }
break;
}
// 2x2 symmetric Schur decomposition // 2x2 symmetric Schur decomposition
tau = (D[4 * ck] - D[4 * rk]) / (2 * D[3 * rk + ck]); tau = (D[4 * ck] - D[4 * rk]) / (2 * D[3 * rk + ck]);
@@ -711,16 +704,12 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m
c = 1.0 / sqrt(1 + t * t); c = 1.0 / sqrt(1 + t * t);
// terminate if cosine too close to 1 // terminate if cosine too close to 1
if (c > 1.0-kEigEPS) { if (c > 1.0 - kEigEPS) { break; }
break;
}
// express rotation as quaternion // express rotation as quaternion
tmp[1] = tmp[2] = tmp[3] = 0; tmp[1] = tmp[2] = tmp[3] = 0;
tmp[rotk + 1] = (tau >= 0 ? -sqrt(0.5 - 0.5 * c) : sqrt(0.5 - 0.5 * c)); tmp[rotk + 1] = (tau >= 0 ? -sqrt(0.5 - 0.5 * c) : sqrt(0.5 - 0.5 * c));
if (rotk == 1) { if (rotk == 1) { tmp[rotk + 1] = -tmp[rotk + 1]; }
tmp[rotk+1] = -tmp[rotk+1];
}
tmp[0] = sqrt(1.0 - tmp[rotk + 1] * tmp[rotk + 1]); tmp[0] = sqrt(1.0 - tmp[rotk + 1] * tmp[rotk + 1]);
mjuu_normvec(tmp, 4); mjuu_normvec(tmp, 4);
@@ -763,9 +752,7 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m
int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) { int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) {
// initialize eigvec to identity // initialize eigvec to identity
std::fill(eigvec, eigvec + n * n, 0.0); std::fill(eigvec, eigvec + n * n, 0.0);
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) { eigvec[i * n + i] = 1.0; }
eigvec[i*n + i] = 1.0;
}
const int max_sweeps = 200; const int max_sweeps = 200;
const double tol = 1e-12; const double tol = 1e-12;
@@ -775,9 +762,7 @@ int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) {
// check convergence: sum of squared off-diagonal elements // check convergence: sum of squared off-diagonal elements
double off_diag = 0; double off_diag = 0;
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) {
for (int j = i+1; j < n; j++) { for (int j = i + 1; j < n; j++) { off_diag += mat[i * n + j] * mat[i * n + j]; }
off_diag += mat[i*n + j] * mat[i*n + j];
}
} }
if (off_diag < tol * tol) break; if (off_diag < tol * tol) break;
@@ -791,8 +776,7 @@ int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) {
double app = mat[p * n + p]; double app = mat[p * n + p];
double aqq = mat[q * n + q]; double aqq = mat[q * n + q];
double tau = (aqq - app) / (2.0 * apq); double tau = (aqq - app) / (2.0 * apq);
double t = (tau >= 0 ? 1.0 : -1.0) / double t = (tau >= 0 ? 1.0 : -1.0) / (std::abs(tau) + std::sqrt(1.0 + tau * tau));
(std::abs(tau) + std::sqrt(1.0 + tau*tau));
double c = 1.0 / std::sqrt(1.0 + t * t); double c = 1.0 / std::sqrt(1.0 + t * t);
double s = t * c; double s = t * c;
@@ -822,16 +806,16 @@ int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) {
} }
// extract eigenvalues from diagonal // extract eigenvalues from diagonal
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) { eigval[i] = mat[i * n + i]; }
eigval[i] = mat[i*n + i];
}
return sweep; return sweep;
} }
// transform vector by pose // transform vector by pose
void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], void mjuu_trnVecPose(double res[3],
const double pos[3],
const double quat[4],
const double vec[3]) { const double vec[3]) {
// res = quat*vec + pos // res = quat*vec + pos
mjuu_rotVecQuat(res, vec, quat); mjuu_rotVecQuat(res, vec, quat);
@@ -859,32 +843,28 @@ std::string mjuu_strippath(std::string filename) {
// compute frame quat and diagonal inertia from full inertia matrix, return error if any // compute frame quat and diagonal inertia from full inertia matrix, return error if any
const char* mjuu_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { const char* mjuu_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) {
if (!mjuu_defined(fullinertia[0])) { if (!mjuu_defined(fullinertia[0])) { return nullptr; }
return nullptr;
}
double eigval[3], eigvec[9], quattmp[4]; double eigval[3], eigvec[9], quattmp[4];
double full[9] = { double full[9] = {fullinertia[0],
fullinertia[0], fullinertia[3], fullinertia[4], fullinertia[3],
fullinertia[3], fullinertia[1], fullinertia[5], fullinertia[4],
fullinertia[4], fullinertia[5], fullinertia[2] fullinertia[3],
}; fullinertia[1],
fullinertia[5],
fullinertia[4],
fullinertia[5],
fullinertia[2]};
mjuu_eig3(eigval, eigvec, quattmp, full); mjuu_eig3(eigval, eigvec, quattmp, full);
// check mimimal eigenvalue // check mimimal eigenvalue
if (eigval[2] < mjEPS) { if (eigval[2] < mjEPS) { return "inertia must have positive eigenvalues"; }
return "inertia must have positive eigenvalues";
}
// copy // copy
if (quat) { if (quat) { mjuu_copyvec(quat, quattmp, 4); }
mjuu_copyvec(quat, quattmp, 4);
}
if (inertia) { if (inertia) { mjuu_copyvec(inertia, eigval, 3); }
mjuu_copyvec(inertia, eigval, 3);
}
return nullptr; return nullptr;
} }
@@ -896,9 +876,7 @@ std::string mjuu_stripext(std::string filename) {
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 == std::string::npos) { if (end == std::string::npos) { return filename; }
return filename;
}
// return name without extension // return name without extension
return filename.substr(0, end); return filename.substr(0, end);
@@ -907,9 +885,7 @@ std::string mjuu_stripext(std::string filename) {
std::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 == std::string::npos) { if (dot == std::string::npos) { return ""; }
return "";
}
return std::string(filename.substr(dot, filename.size() - dot)); return std::string(filename.substr(dot, filename.size() - dot));
} }
@@ -917,25 +893,18 @@ std::string mjuu_getext(std::string_view filename) {
// is directory path absolute // is directory path absolute
bool mjuu_isabspath(std::string path) { bool mjuu_isabspath(std::string path) {
// empty: not absolute // empty: not absolute
if (path.empty()) { if (path.empty()) { return false; }
return false;
}
// path is scheme:filename which we consider an absolute path // path is scheme:filename which we consider an absolute path
// e.g. file URI's are always absolute paths // e.g. file URI's are always absolute paths
if (mjp_getResourceProvider(path.c_str()) != nullptr) { if (mjp_getResourceProvider(path.c_str()) != nullptr) { return true; }
return true;
}
// check first char // check first char
const char* str = path.c_str(); const char* str = path.c_str();
if (str[0] == '\\' || str[0] == '/') { if (str[0] == '\\' || str[0] == '/') { return true; }
return true;
}
// find ":/" or ":\" // find ":/" or ":\"
if (path.find(":/") != std::string::npos || if (path.find(":/") != std::string::npos || path.find(":\\") != std::string::npos) {
path.find(":\\") != std::string::npos) {
return true; return true;
} }
@@ -943,92 +912,69 @@ bool mjuu_isabspath(std::string path) {
} }
// assemble two file paths // assemble two file paths
std::string mjuu_combinePaths(const std::string& path1, const std::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;
}
std::size_t n = path1.size(); std::size_t n = path1.size();
if (n > 0 && path1[n - 1] != '\\' && path1[n - 1] != '/') { if (n > 0 && path1[n - 1] != '\\' && path1[n - 1] != '/') { return path1 + "/" + path2; }
return path1 + "/" + path2;
}
return path1 + path2; return path1 + path2;
} }
// assemble three file paths // assemble three file paths
std::string mjuu_combinePaths(const std::string& path1, const std::string& path2, std::string mjuu_combinePaths(const std::string& path1,
const std::string& path2,
const std::string& path3) { const std::string& path3) {
return mjuu_combinePaths(path1, mjuu_combinePaths(path2, path3)); return mjuu_combinePaths(path1, mjuu_combinePaths(path2, path3));
} }
// return true if the text is in a valid content type format: // return true if the text is in a valid content type format:
// {type}/{subtype}[;{parameter}={value}] // {type}/{subtype}[;{parameter}={value}]
static bool mjuu_isValidContentType(std::string_view text) { static bool mjuu_isValidContentType(std::string_view text) {
// find a forward slash that's not the last character // find a forward slash that's not the last character
size_t n = text.find('/'); size_t n = text.find('/');
if (n == std::string::npos || n == text.size() - 1) { if (n == std::string::npos || n == text.size() - 1) { return false; }
return false;
}
size_t m = text.find(';'); size_t m = text.find(';');
if (m == std::string::npos) { if (m == std::string::npos) { return true; }
return true;
}
if (m + 1 <= n) { if (m + 1 <= n) { return false; }
return false;
}
// just check if there's an equal sign; this isn't robust enough for general // just check if there's an equal sign; this isn't robust enough for general
// validation, but works for our scope, hence this is a private helper // validation, but works for our scope, hence this is a private helper
// function // function
size_t s = text.find('='); size_t s = text.find('=');
if (s == std::string::npos || s + 1 <= m) { if (s == std::string::npos || s + 1 <= m) { return false; }
return false;
}
return true; return true;
} }
// return type from content_type format {type}/{subtype}[;{parameter}={value}] // return type from content_type format {type}/{subtype}[;{parameter}={value}]
// return empty string on invalid format // return empty string on invalid format
std::optional<std::string_view> mjuu_parseContentTypeAttrType(std::string_view text) { std::optional<std::string_view> mjuu_parseContentTypeAttrType(std::string_view text) {
if (!mjuu_isValidContentType(text)) { if (!mjuu_isValidContentType(text)) { return std::nullopt; }
return std::nullopt;
}
return {text.substr(0, text.find('/'))}; return {text.substr(0, text.find('/'))};
} }
// return subtype from content_type format {type}/{subtype}[;{parameter}={value}] // return subtype from content_type format {type}/{subtype}[;{parameter}={value}]
// return empty string on invalid format // return empty string on invalid format
std::optional<std::string_view> mjuu_parseContentTypeAttrSubtype(std::string_view text) { std::optional<std::string_view> mjuu_parseContentTypeAttrSubtype(std::string_view text) {
if (!mjuu_isValidContentType(text)) { if (!mjuu_isValidContentType(text)) { return std::nullopt; }
return std::nullopt;
}
size_t n = text.find('/'); size_t n = text.find('/');
size_t m = text.find(';', n + 1); size_t m = text.find(';', n + 1);
if (m == std::string::npos) { if (m == std::string::npos) { return {text.substr(n + 1)}; }
return { text.substr(n+1) };
}
return {text.substr(n + 1, m - n - 1)}; return {text.substr(n + 1, m - n - 1)};
} }
// convert filename extension to content type; return empty string if not found // convert filename extension to content type; return empty string if not found
std::string mjuu_extToContentType(std::string_view filename) { std::string mjuu_extToContentType(std::string_view filename) {
std::string ext = mjuu_getext(filename); std::string ext = mjuu_getext(filename);
@@ -1052,15 +998,11 @@ std::string mjuu_extToContentType(std::string_view filename) {
// get the length of the dirname portion of a given path // get the length of the dirname portion of a given path
int mjuu_dirnamelen(const char* path) { int mjuu_dirnamelen(const char* path) {
if (!path) { if (!path) { return 0; }
return 0;
}
int pos = -1; int pos = -1;
for (int i = 0; path[i]; ++i) { for (int i = 0; path[i]; ++i) {
if (path[i] == '/' || path[i] == '\\') { if (path[i] == '/' || path[i] == '\\') { pos = i; }
pos = i;
}
} }
return pos + 1; return pos + 1;
@@ -1070,14 +1012,10 @@ namespace mujoco::user {
std::string FilePath::Combine(const std::string& s1, const std::string& s2) { std::string FilePath::Combine(const std::string& s1, const std::string& s2) {
// str2 has absolute path // str2 has absolute path
if (!AbsPrefix(s2).empty()) { if (!AbsPrefix(s2).empty()) { return s2; }
return s2;
}
std::size_t n = s1.size(); std::size_t n = s1.size();
if (n > 0 && s1[n - 1] != '\\' && s1[n - 1] != '/') { if (n > 0 && s1[n - 1] != '\\' && s1[n - 1] != '/') { return s1 + "/" + s2; }
return s1 + "/" + s2;
}
return s1 + s2; return s1 + s2;
} }
@@ -1106,9 +1044,7 @@ std::string FilePath::PathReduce(const std::string& str) {
std::stringstream path; std::stringstream path;
auto it = dirs.begin(); auto it = dirs.begin();
path << abs_prefix << *it++; path << abs_prefix << *it++;
for (; it != dirs.end(); ++it) { for (; it != dirs.end(); ++it) { path << "/" << *it; }
path << "/" << *it;
}
return path.str(); return path.str();
} }
@@ -1119,9 +1055,7 @@ FilePath FilePath::operator+(const FilePath& path) const {
std::string FilePath::Ext() const { std::string FilePath::Ext() const {
std::size_t n = path_.find_last_of('.'); std::size_t n = path_.find_last_of('.');
if (n == std::string::npos) { if (n == std::string::npos) { return ""; }
return "";
}
return path_.substr(n, path_.size() - n); return path_.substr(n, path_.size() - n);
} }
@@ -1129,9 +1063,7 @@ FilePath FilePath::StripExt() const {
size_t n = path_.find_last_of('.'); size_t n = path_.find_last_of('.');
// no extension // no extension
if (n == std::string::npos) { if (n == std::string::npos) { return FilePathFast(path_); }
return FilePathFast(path_);
}
// return path without extension // return path without extension
return FilePathFast(path_.substr(0, n)); return FilePathFast(path_.substr(0, n));
@@ -1140,9 +1072,7 @@ FilePath FilePath::StripExt() const {
// is directory absolute path // is directory absolute path
std::string FilePath::AbsPrefix(const std::string& str) { std::string FilePath::AbsPrefix(const std::string& str) {
// empty: not absolute // empty: not absolute
if (str.empty()) { if (str.empty()) { return ""; }
return "";
}
// path is scheme:filename which we consider an absolute path // path is scheme:filename which we consider an absolute path
// e.g. file URI's are always absolute paths // e.g. file URI's are always absolute paths
@@ -1153,20 +1083,14 @@ std::string FilePath::AbsPrefix(const std::string& str) {
} }
// check first char // check first char
if (str[0] == '\\' || str[0] == '/') { if (str[0] == '\\' || str[0] == '/') { return str.substr(0, 1); }
return str.substr(0, 1);
}
// find ":/" or ":\" // find ":/" or ":\"
std::size_t pos = str.find(":/"); std::size_t pos = str.find(":/");
if (pos != std::string::npos) { if (pos != std::string::npos) { return str.substr(0, pos + 2); }
return str.substr(0, pos + 2);
}
pos = str.find(":\\"); pos = str.find(":\\");
if (pos != std::string::npos) { if (pos != std::string::npos) { return str.substr(0, pos + 2); }
return str.substr(0, pos + 2);
}
return ""; return "";
} }
@@ -1176,17 +1100,14 @@ FilePath FilePath::StripPath() const {
std::size_t n = path_.find_last_of("/\\"); std::size_t n = path_.find_last_of("/\\");
// no path // no path
if (n == std::string::npos) { if (n == std::string::npos) { return FilePathFast(path_); }
return FilePathFast(path_);
}
return FilePathFast(path_.substr(n + 1, path_.size() - (n + 1))); return FilePathFast(path_.substr(n + 1, path_.size() - (n + 1)));
} }
std::string FilePath::StrLower() const { std::string FilePath::StrLower() const {
std::string str = path_; std::string str = path_;
std::transform(str.begin(), str.end(), str.begin(), std::transform(str.begin(), str.end(), str.begin(), [](unsigned char c) {
[](unsigned char c) {
return std::tolower(c); return std::tolower(c);
}); });
return str; return str;
@@ -1195,9 +1116,7 @@ std::string FilePath::StrLower() const {
// read file into memory buffer // read file into memory buffer
std::vector<uint8_t> FileToMemory(const char* filename) { std::vector<uint8_t> FileToMemory(const char* filename) {
FILE* fp = fopen(filename, "rb"); FILE* fp = fopen(filename, "rb");
if (!fp) { if (!fp) { return {}; }
return {};
}
// find size // find size
if (fseek(fp, 0, SEEK_END) != 0) { if (fseek(fp, 0, SEEK_END) != 0) {
@@ -1253,12 +1172,11 @@ std::vector<uint8_t> FileToMemory(const char* filename) {
} }
// convert vector to string separating elements by whitespace // convert vector to string separating elements by whitespace
template<typename T> std::string VectorToString(const std::vector<T>& v) { template <typename T>
std::string VectorToString(const std::vector<T>& v) {
std::stringstream ss; std::stringstream ss;
for (const T& t : v) { for (const T& t : v) { ss << t << " "; }
ss << t << " ";
}
std::string s = ss.str(); std::string s = ss.str();
if (!s.empty()) s.pop_back(); // remove trailing space if (!s.empty()) s.pop_back(); // remove trailing space
@@ -1272,27 +1190,32 @@ template MJAPI std::string VectorToString(const std::vector<std::string>& v);
namespace { namespace {
template<typename T> T StrToNum(char* str, char** c); template <typename T>
T StrToNum(char* str, char** c);
template<> int StrToNum(char* str, char** c) { template <>
int StrToNum(char* str, char** c) {
long n = std::strtol(str, c, 10); long n = std::strtol(str, c, 10);
if (n < INT_MIN || n > INT_MAX) errno = ERANGE; if (n < INT_MIN || n > INT_MAX) errno = ERANGE;
return n; return n;
} }
template<> float StrToNum(char* str, char** c) { template <>
float StrToNum(char* str, char** c) {
float f = strtof(str, c); float f = strtof(str, c);
if (std::isnan(f)) errno = EDOM; if (std::isnan(f)) errno = EDOM;
return f; return f;
} }
template<> double StrToNum(char* str, char** c) { template <>
double StrToNum(char* str, char** c) {
double d = strtod(str, c); double d = strtod(str, c);
if (std::isnan(d)) errno = EDOM; if (std::isnan(d)) errno = EDOM;
return d; return d;
} }
template<> unsigned char StrToNum(char* str, char** c) { template <>
unsigned char StrToNum(char* str, char** c) {
long n = std::strtol(str, c, 10); long n = std::strtol(str, c, 10);
if (n < 0 || n > UCHAR_MAX) errno = ERANGE; if (n < 0 || n > UCHAR_MAX) errno = ERANGE;
return n; return n;
@@ -1304,15 +1227,14 @@ inline bool IsNullOrSpace(char* c) {
inline char* SkipSpace(char* c) { inline char* SkipSpace(char* c) {
for (; *c != '\0'; c++) { for (; *c != '\0'; c++) {
if (!IsNullOrSpace(c)) { if (!IsNullOrSpace(c)) { break; }
break;
}
} }
return c; return c;
} }
} // namespace } // namespace
template <typename T> std::vector<T> StringToVector(char* cs) { template <typename T>
std::vector<T> StringToVector(char* cs) {
std::vector<T> v; std::vector<T> v;
char* ch = cs; char* ch = cs;
@@ -1334,17 +1256,17 @@ template <typename T> std::vector<T> StringToVector(char* cs) {
return v; return v;
} }
template<> MJAPI std::vector<std::string> StringToVector(char* cs) { template <>
MJAPI std::vector<std::string> StringToVector(char* cs) {
return StringToVector<std::string>(std::string(cs)); return StringToVector<std::string>(std::string(cs));
} }
template<> MJAPI std::vector<std::string> StringToVector(const std::string& s) { template <>
MJAPI std::vector<std::string> StringToVector(const std::string& s) {
std::vector<std::string> v; std::vector<std::string> v;
std::stringstream ss(s); std::stringstream ss(s);
std::string word; std::string word;
while (ss >> word) { while (ss >> word) { v.push_back(word); }
v.push_back(word);
}
return v; return v;
} }
@@ -1354,7 +1276,8 @@ template MJAPI std::vector<double> StringToVector(char* cs);
template MJAPI std::vector<unsigned char> StringToVector(char* cs); template MJAPI std::vector<unsigned char> StringToVector(char* cs);
template <typename T> std::vector<T> StringToVector(const std::string& s) { template <typename T>
std::vector<T> StringToVector(const std::string& s) {
return StringToVector<T>(const_cast<char*>(s.c_str())); return StringToVector<T>(const_cast<char*>(s.c_str()));
} }
template MJAPI std::vector<int> StringToVector(const std::string& s); template MJAPI std::vector<int> StringToVector(const std::string& s);
src/user/user_util.h
+41 -22
View File
@@ -115,8 +115,8 @@ void mjuu_localquat(double* local, const double* child, const double* parent);
void mjuu_crossvec(double* a, const double* b, const double* c); void mjuu_crossvec(double* a, const double* b, const double* c);
// compute normal vector to given triangle // compute normal vector to given triangle
template<typename T> double mjuu_makenormal(double* normal, const T a[3], template <typename T>
const T b[3], const T c[3]); double mjuu_makenormal(double* normal, const T a[3], const T b[3], const T c[3]);
// compute quaternion corresponding to minimal rotation from [0;0;1] to vec // compute quaternion corresponding to minimal rotation from [0;0;1] to vec
void mjuu_z2quat(double* quat, const double* vec); void mjuu_z2quat(double* quat, const double* vec);
@@ -125,20 +125,28 @@ void mjuu_z2quat(double* quat, const double* vec);
void mjuu_frame2quat(double* quat, const double* x, const double* y, const double* z); void mjuu_frame2quat(double* quat, const double* x, const double* y, const double* z);
// invert frame transformation // invert frame transformation
void mjuu_frameinvert(double newpos[3], double newquat[4], void mjuu_frameinvert(double newpos[3],
const double oldpos[3], const double oldquat[4]); double newquat[4],
const double oldpos[3],
const double oldquat[4]);
// accumulate frame transformation into parent frame // accumulate frame transformation into parent frame
void mjuu_frameaccum(double pos[3], double quat[4], void mjuu_frameaccum(double pos[3],
const double childpos[3], const double childquat[4]); double quat[4],
const double childpos[3],
const double childquat[4]);
// accumulate frame transformation into child frame // accumulate frame transformation into child frame
void mjuu_frameaccumChild(const double pos[3], const double quat[4], void mjuu_frameaccumChild(const double pos[3],
double childpos[3], double childquat[4]); const double quat[4],
double childpos[3],
double childquat[4]);
// invert frame accumulation // invert frame accumulation
void mjuu_frameaccuminv(double pos[3], double quat[4], void mjuu_frameaccuminv(double pos[3],
const double childpos[3], const double childquat[4]); double quat[4],
const double childpos[3],
const double childquat[4]);
// convert local_inertia[3] to global_inertia[6] // convert local_inertia[3] to global_inertia[6]
void mjuu_globalinertia(double* global, const double* local, const double* quat); void mjuu_globalinertia(double* global, const double* local, const double* quat);
@@ -153,8 +161,12 @@ void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double
void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]); void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]);
// update moving frame along a discrete curve or initialize it, returns edge length // update moving frame along a discrete curve or initialize it, returns edge length
double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3], double mjuu_updateFrame(double quat[4],
const double tprv[3], const double tnxt[3], int first); double normal[3],
const double edge[3],
const double tprv[3],
const double tnxt[3],
int first);
// eigenvalue decomposition of symmetric 3x3 matrix // eigenvalue decomposition of symmetric 3x3 matrix
int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]); int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]);
@@ -225,9 +237,7 @@ class MJAPI FilePath {
private: private:
static std::string AbsPrefix(const std::string& str); static std::string AbsPrefix(const std::string& str);
static std::string PathReduce(const std::string& str); static std::string PathReduce(const std::string& str);
static bool IsSeparator(char c) { static bool IsSeparator(char c) { return c == '/' || c == '\\'; }
return c == '/' || c == '\\';
}
static std::string Combine(const std::string& s1, const std::string& s2); static std::string Combine(const std::string& s1, const std::string& s2);
// fast constructor that does not call PathReduce // fast constructor that does not call PathReduce
@@ -249,8 +259,11 @@ class MJAPI FilePath {
// utility class for scoping resources to functions // utility class for scoping resources to functions
struct Cleanup { struct Cleanup {
using Fn = std::function<void()>; using Fn = std::function<void()>;
~Cleanup() { for (auto& f : cleanup) f(); } ~Cleanup() {
for (auto& f : cleanup) f();
}
void operator+=(Fn f) { cleanup.push_front(std::move(f)); } void operator+=(Fn f) { cleanup.push_front(std::move(f)); }
std::deque<Fn> cleanup; std::deque<Fn> cleanup;
}; };
@@ -259,13 +272,18 @@ struct Cleanup {
std::vector<uint8_t> FileToMemory(const char* filename); std::vector<uint8_t> FileToMemory(const char* filename);
// convert vector to string separating elements by whitespace // convert vector to string separating elements by whitespace
template<typename T> MJAPI std::string VectorToString(const std::vector<T>& v); template <typename T>
MJAPI std::string VectorToString(const std::vector<T>& v);
// convert string to vector // convert string to vector
template<typename T> MJAPI std::vector<T> StringToVector(char *cs); template <typename T>
template<typename T> MJAPI std::vector<T> StringToVector(const std::string& s); MJAPI std::vector<T> StringToVector(char* cs);
template<> MJAPI std::vector<std::string> StringToVector(char* cs); template <typename T>
template<> MJAPI std::vector<std::string> StringToVector(const std::string& s); MJAPI std::vector<T> StringToVector(const std::string& s);
template <>
MJAPI std::vector<std::string> StringToVector(char* cs);
template <>
MJAPI std::vector<std::string> StringToVector(const std::string& s);
} // namespace mujoco::user } // namespace mujoco::user
@@ -283,7 +301,8 @@ bool mjuu_isabspath(std::string path);
// assemble file paths // assemble file paths
std::string mjuu_combinePaths(const std::string& path1, const std::string& path2); std::string mjuu_combinePaths(const std::string& path1, const std::string& path2);
std::string mjuu_combinePaths(const std::string& path1, const std::string& path2, std::string mjuu_combinePaths(const std::string& path1,
const std::string& path2,
const std::string& path3); const std::string& path3);
// return type from content_type format {type}/{subtype}[;{parameter}={value}] // return type from content_type format {type}/{subtype}[;{parameter}={value}]