diff --git a/src/user/user_cache.cc b/src/user/user_cache.cc index 4fb37022..38d41435 100644 --- a/src/user/user_cache.cc +++ b/src/user/user_cache.cc @@ -29,15 +29,14 @@ // makes a copy for user (strip unnecessary items) mjCAsset mjCAsset::Copy(const mjCAsset& other) { mjCAsset asset; - asset.id_ = other.Id(); + asset.id_ = other.Id(); asset.timestamp_ = other.Timestamp(); - asset.data_ = other.data_; - asset.size_ = other.size_; + asset.data_ = other.data_; + asset.size_ = other.size_; return asset; } - // sets the total maximum size of the cache in bytes // low-priority cached assets will be dropped to make the new memory // 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 const std::string* mjCCache::HasAsset(const std::string& id) { std::lock_guard lock(mutex_); - auto it = lookup_.find(id); - if (it == lookup_.end()) { - return nullptr; - } + auto it = lookup_.find(id); + if (it == lookup_.end()) { return nullptr; } return &(it->second.Timestamp()); } - // inserts an asset into the cache, if asset is already in the cache, its data // is updated only if the timestamps disagree -bool mjCCache::Insert(const std::string& modelname, const std::string& id, const mjResource *resource, - std::shared_ptr data, std::size_t size) { +bool mjCCache::Insert(const std::string& modelname, + const std::string& id, + const mjResource* resource, + std::shared_ptr data, + std::size_t size) { std::lock_guard lock(mutex_); // check if asset is too large to fit in the cache - if ((size_ + size > capacity_) && - lookup_.find(id) == lookup_.end()) { - return false; - } + if ((size_ + size > capacity_) && lookup_.find(id) == lookup_.end()) { return false; } mjCAsset asset(modelname, id, resource, data, size); auto [it, inserted] = lookup_.insert({id, asset}); mjCAsset* asset_ptr = &(it->second); if (!inserted) { - if (size_ - asset_ptr->BytesCount() + size > capacity_) { - return false; - } + if (size_ - asset_ptr->BytesCount() + size > capacity_) { return false; } models_[modelname].insert(asset_ptr); // add it for the model asset_ptr->AddReference(modelname); - if (it->second.Timestamp() == asset.Timestamp()) { - return true; - } + if (it->second.Timestamp() == asset.Timestamp()) { return true; } asset_ptr->SetTimestamp(asset.Timestamp()); size_ = size_ - asset_ptr->BytesCount() + size; 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 // copied bool mjCCache::PopulateData(const std::string& id, const mjResource* resource, mjCDataFunc fn) { std::lock_guard lock(mutex_); - auto it = lookup_.find(id); - if (it == lookup_.end()) { - return false; - } + auto it = lookup_.find(id); + if (it == lookup_.end()) { return false; } - if (mju_isModifiedResource(resource, it->second.Timestamp().c_str())) { - return false; - } + if (mju_isModifiedResource(resource, it->second.Timestamp().c_str())) { return false; } mjCAsset* asset = &(it->second); @@ -126,99 +112,78 @@ bool mjCCache::PopulateData(const std::string& id, const mjResource* resource, m } - // removes model from the cache along with assets referencing only this model void mjCCache::RemoveModel(const std::string& filename) { std::lock_guard lock(mutex_); for (mjCAsset* asset : models_[filename]) { asset->RemoveReference(filename); - if (!asset->HasReferences()) { - Delete(asset, filename); - } + if (!asset->HasReferences()) { Delete(asset, filename); } } models_.erase(filename); } - // Wipes out all internal data for the given model void mjCCache::Reset(const std::string& filename) { std::lock_guard lock(mutex_); - for (auto asset : models_[filename]) { - Delete(asset, filename); - } + for (auto asset : models_[filename]) { Delete(asset, filename); } models_.erase(filename); } - // Wipes out all internal data void mjCCache::Reset() { std::lock_guard lock(mutex_); entries_.clear(); lookup_.clear(); models_.clear(); - size_ = 0; + size_ = 0; insert_num_ = 0; } - std::size_t mjCCache::Capacity() const { std::lock_guard lock(mutex_); return capacity_; } - std::size_t mjCCache::Size() const { std::lock_guard lock(mutex_); return size_; } - // Deletes a single asset void mjCCache::DeleteAsset(const std::string& id) { std::lock_guard lock(mutex_); - auto it = lookup_.find(id); - if (it != lookup_.end()) { - Delete(&(it->second)); - } + auto it = lookup_.find(id); + if (it != lookup_.end()) { Delete(&(it->second)); } } - // Deletes a single asset (internal) void mjCCache::Delete(mjCAsset* asset) { size_ -= asset->BytesCount(); entries_.erase(asset); - for (auto& reference : asset->References()) { - models_[reference].erase(asset); - } + for (auto& reference : asset->References()) { models_[reference].erase(asset); } lookup_.erase(asset->Id()); } - // Deletes a single asset (internal) void mjCCache::Delete(mjCAsset* asset, const std::string& skip) { size_ -= asset->BytesCount(); entries_.erase(asset); for (auto& reference : asset->References()) { - if (reference != skip) { - models_[reference].erase(asset); - } + if (reference != skip) { models_[reference].erase(asset); } } lookup_.erase(asset->Id()); } - // trims out data to meet memory requirements void mjCCache::Trim() { - while (size_ > capacity_) { - Delete(*entries_.begin()); - } + while (size_ > capacity_) { Delete(*entries_.begin()); } } diff --git a/src/user/user_cache.h b/src/user/user_cache.h index b8b28240..c3f32878 100644 --- a/src/user/user_cache.h +++ b/src/user/user_cache.h @@ -38,40 +38,39 @@ typedef void (*mjCDeallocFunc)(const void*); // asset). class mjCAsset { friend class mjCCache; + public: - mjCAsset(std::string modelname, std::string id, const mjResource* resource, - std::shared_ptr data, std::size_t size) : - id_(id), timestamp_(resource->timestamp), - size_(size), data_(std::move(data)) { + mjCAsset(std::string modelname, + std::string id, + const mjResource* resource, + std::shared_ptr data, + std::size_t size) + : id_(id), timestamp_(resource->timestamp), size_(size), data_(std::move(data)) { AddReference(modelname); } // move and copy constructors - mjCAsset(mjCAsset&& other) = default; - mjCAsset& operator=(mjCAsset&& other) = default; - mjCAsset(const mjCAsset& other) = default; + mjCAsset(mjCAsset&& other) = default; + mjCAsset& operator=(mjCAsset&& other) = default; + mjCAsset(const mjCAsset& other) = default; mjCAsset& operator=(const mjCAsset& other) = default; const std::string& Timestamp() const { return timestamp_; } const std::string& Id() const { return id_; } - std::size_t InsertNum() const { return insert_num_; } - std::size_t AccessCount() const { return access_count_; } + std::size_t InsertNum() const { return insert_num_; } + std::size_t AccessCount() const { return access_count_; } // pass data in the cache to the given function, return true if data was copied - bool PopulateData(mjCDataFunc fn) const { - return fn(data_.get()); - } + bool PopulateData(mjCDataFunc fn) const { return fn(data_.get()); } private: mjCAsset() = default; // helpers for managing models referencing this asset void AddReference(std::string xml_file) { references_.insert(xml_file); } - void RemoveReference(const std::string& xml_file) { - references_.erase(xml_file); - } + void RemoveReference(const std::string& xml_file) { references_.erase(xml_file); } - void ReplaceData(const mjCAsset& other) { + void ReplaceData(const mjCAsset& other) { data_ = other.data_; size_ = other.size_; } @@ -88,18 +87,16 @@ class mjCAsset { void SetTimestamp(std::string timestamp) { timestamp_ = timestamp; } // accessors - std::size_t BytesCount() const { return size_; } - const void* Data() const { - return data_.get(); - } + std::size_t BytesCount() const { return size_; } + const void* Data() const { return data_.get(); } const std::set& References() const { return references_; } - std::string id_; // unique id associated with asset - std::string timestamp_; // opaque timestamp of asset - std::size_t insert_num_; // number when asset was inserted - std::size_t access_count_ = 0; // incremented when getting 0th block - std::size_t size_ = 0; // how many bytes taken up by the asset - std::shared_ptr data_; // actual data of the asset + std::string id_; // unique id associated with asset + std::string timestamp_; // opaque timestamp of asset + std::size_t insert_num_; // number when asset was inserted + std::size_t access_count_ = 0; // incremented when getting 0th block + std::size_t size_ = 0; // how many bytes taken up by the asset + std::shared_ptr data_; // actual data of the asset // list of models referencing this asset std::set references_; @@ -107,9 +104,7 @@ class mjCAsset { struct mjCAssetCompare { bool operator()(const mjCAsset* e1, const mjCAsset* e2) const { - if (e1->AccessCount() != e2->AccessCount()) { - return e1->AccessCount() < e2->AccessCount(); - } + if (e1->AccessCount() != e2->AccessCount()) { return e1->AccessCount() < e2->AccessCount(); } return e1->InsertNum() < e2->InsertNum(); } }; @@ -117,12 +112,12 @@ struct mjCAssetCompare { // the class container for a thread-safe asset cache class mjCCache { public: - explicit mjCCache(std::size_t size) : capacity_(size) {} + explicit mjCCache(std::size_t size) : capacity_(size) {} // move only - mjCCache(mjCCache&& other) = delete; - mjCCache& operator=(mjCCache&& other) = delete; - mjCCache(const mjCCache& other) = delete; + mjCCache(mjCCache&& other) = delete; + mjCCache& operator=(mjCCache&& other) = delete; + mjCCache(const mjCCache& other) = delete; mjCCache& operator=(const mjCCache& other) = delete; // sets the capacity of the cache in bytes @@ -136,8 +131,11 @@ class mjCCache { // inserts an asset into the cache, if asset is already in the cache, its data // is updated only if the timestamps disagree - bool Insert(const std::string& modelname, const std::string& id, const mjResource *resource, - std::shared_ptr data, std::size_t size); + bool Insert(const std::string& modelname, + const std::string& id, + const mjResource* resource, + std::shared_ptr data, + std::size_t size); // populate data from the cache into the given function bool PopulateData(const std::string& id, const mjResource* resource, mjCDataFunc fn); @@ -168,9 +166,9 @@ class mjCCache { // engine/engine_plugin.cc as some of these methods don't need to be fully // locked. mutable std::mutex mutex_; - std::size_t insert_num_ = 0; // a running counter of assets being inserted - std::size_t size_ = 0; // current size of the cache in bytes - std::size_t capacity_ = 0; // capacity of the cache in bytes + std::size_t insert_num_ = 0; // a running counter of assets being inserted + std::size_t size_ = 0; // current size of the cache in bytes + std::size_t capacity_ = 0; // capacity of the cache in bytes // internal constant look up table for assets std::unordered_map lookup_; diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 17378004..1dd4b4d0 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -70,18 +70,18 @@ class mjCComposite { void MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin); // common properties - std::string prefix; // name prefix - mjtCompType type; // composite type - int count[3]; // geom count in each dimension - double offset[3]; // position offset - double quat[4]; // quaternion offset + std::string prefix; // name prefix + mjtCompType type; // composite type + int count[3]; // geom count in each dimension + double offset[3]; // position offset + double quat[4]; // quaternion offset // currently used only for cable - std::string initial; // root boundary type - std::vector uservert; // user-specified vertex positions - double size[3]; // rope size (meaning depends on the shape) - mjtCompShape curve[3]; // geometric shape - mjsFrame* frame; // frame where the composite is defined + std::string initial; // root boundary type + std::vector uservert; // user-specified vertex positions + double size[3]; // rope size (meaning depends on the shape) + mjtCompShape curve[3]; // geometric shape + mjsFrame* frame; // frame where the composite is defined // body names used in the skin std::vector username; @@ -89,36 +89,37 @@ class mjCComposite { // plugin support std::string plugin_name; std::string plugin_instance_name; - mjsPlugin plugin; + mjsPlugin plugin; // skin - bool skin; // generate skin - bool skintexcoord; // generate texture coordinates - std::string skinmaterial; // skin material - float skinrgba[4]; // skin rgba - float skininflate; // inflate skin - int skinsubgrid; // number of skin subgrid points; 0: none (2D only) - int skingroup; // skin group of the composite object + bool skin; // generate skin + bool skintexcoord; // generate texture coordinates + std::string skinmaterial; // skin material + float skinrgba[4]; // skin rgba + float skininflate; // inflate skin + int skinsubgrid; // number of skin subgrid points; 0: none (2D only) + int skingroup; // skin group of the composite object // element options - bool add[mjNCOMPKINDS]; // add element - mjCDef def[mjNCOMPKINDS]; // default geom, site, tendon - std::unordered_map > defjoint; // default joints + bool add[mjNCOMPKINDS]; // add element + mjCDef def[mjNCOMPKINDS]; // default geom, site, tendon + std::unordered_map> defjoint; // default joints // computed internally - int dim; // dimensionality + int dim; // dimensionality 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 - void CopyIntoSkin(mjsSkin* skin); - std::vector face; - std::vector vert; - std::vector bindpos; - std::vector bindquat; - std::vector texcoord; - std::vector> vertid; + void CopyIntoSkin(mjsSkin* skin); + std::vector face; + std::vector vert; + std::vector bindpos; + std::vector bindquat; + std::vector texcoord; + std::vector> vertid; std::vector> vertweight; }; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index e5e8d494..218dae38 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -41,8 +41,8 @@ namespace { namespace mju = ::mujoco::util; -using std::vector; using std::stringstream; +using std::vector; } // namespace // strncpy with 0, return false @@ -65,9 +65,7 @@ static void ReadStrFromBuffer(char* dest, const char* src, int maxlen) { bool IsValidElementOrNodeHeader22(const std::string& line) { // making sure characters are numbers for (char c : line) { - if (!std::isdigit(c)) { - return false; - } + if (!std::isdigit(c)) { return false; } } return true; } @@ -75,36 +73,35 @@ bool IsValidElementOrNodeHeader22(const std::string& line) { // constructor: set defaults outside mjCDef mjCFlexcomp::mjCFlexcomp(void) { - type = mjFCOMPTYPE_GRID; + type = mjFCOMPTYPE_GRID; count[0] = count[1] = count[2] = 10; cellcount[0] = cellcount[1] = cellcount[2] = -1; mjuu_setvec(spacing, 0.02, 0.02, 0.02); mjuu_setvec(scale, 1, 1, 1); - mass = 1; + mass = 1; inertiabox = 0.005; - equality = 0; + equality = 0; mjuu_setvec(pos, 0, 0, 0); mjuu_setvec(quat, 1, 0, 0, 0); - rigid = false; + rigid = false; centered = false; - doftype = mjFCOMPDOF_FULL; + doftype = mjFCOMPDOF_FULL; mjs_defaultPlugin(&plugin); mjs_defaultOrientation(&alt); - plugin_name = ""; + plugin_name = ""; plugin_instance_name = ""; - plugin.plugin_name = (mjString*)&plugin_name; - plugin.name = (mjString*)&plugin_instance_name; + plugin.plugin_name = (mjString*)&plugin_name; + plugin.name = (mjString*)&plugin_instance_name; } // identify empty cells and pin nodes exclusively in empty cells -void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, - int npnt, const double minmax[6], - int nx, int ny, int nz) { - int cx = flex->spec.cellcount[0]; - int cy = flex->spec.cellcount[1]; - int cz = flex->spec.cellcount[2]; +void mjCFlexcomp::MarkEmptyCells( + mjCFlex* flex, const double* points, int npnt, const double minmax[6], int nx, int ny, int nz) { + int cx = flex->spec.cellcount[0]; + int cy = flex->spec.cellcount[1]; + int cz = flex->spec.cellcount[2]; int order = flex->spec.order; // delegate cell_empty computation to mjCFlex @@ -117,25 +114,23 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, for (int gk = 0; gk < nz; gk++) { // find all cells that reference this node bool all_empty = true; - int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order); - int ci_max = std::min(cx - 1, gi / order); - int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order); - int cj_max = std::min(cy - 1, gj / order); - int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order); - int ck_max = std::min(cz - 1, gk / order); + int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order); + int ci_max = std::min(cx - 1, gi / order); + int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order); + int cj_max = std::min(cy - 1, gj / order); + int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order); + int ck_max = std::min(cz - 1, gk / order); for (int ci = ci_min; ci <= ci_max && all_empty; ci++) { for (int cj = cj_min; cj <= cj_max && all_empty; cj++) { for (int ck = ck_min; ck <= ck_max && all_empty; ck++) { - if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) { - all_empty = false; - } + if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) { all_empty = false; } } } } if (all_empty) { - int idx = gi * ny * nz + gj * nz + gk; + int idx = gi * ny * nz + gj * nz + gk; pinned[idx] = true; } } @@ -146,12 +141,11 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, // make flexcomp object bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vfs) { - mjCModel* model = static_cast(body->element)->model; + mjCModel* model = static_cast(body->element)->model; mjsCompiler* compiler = static_cast(body->element)->compiler; - mjsFlex* dflex = def.spec.flex; - bool direct = (type == mjFCOMPTYPE_DIRECT || - type == mjFCOMPTYPE_MESH || - type == mjFCOMPTYPE_GMSH); + mjsFlex* dflex = def.spec.flex; + bool direct = + (type == mjFCOMPTYPE_DIRECT || type == mjFCOMPTYPE_MESH || type == mjFCOMPTYPE_GMSH); // check parent body name if (mjs_getName(body->element)->empty()) { @@ -164,18 +158,16 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } // check counts - for (int i=0; i < 3; i++) { + for (int i = 0; i < 3; i++) { if (count[i] < 1 || ((doftype == mjFCOMPDOF_RADIAL && count[i] < 2) && dflex->dim == 3)) { return comperr(error, "Count too small", error_sz); } } // check spacing - double minspace = 2*dflex->radius + dflex->margin; + double minspace = 2 * dflex->radius + dflex->margin; if (!direct) { - if (spacing[0] < minspace || - spacing[1] < minspace || - spacing[2] < minspace) { + if (spacing[0] < minspace || spacing[1] < minspace || spacing[2] < minspace) { return comperr(error, "Spacing must be larger than geometry size", error_sz); } } @@ -192,9 +184,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // compute orientation const char* alterr = mjs_resolveOrientation(quat, compiler->degree, compiler->eulerseq, &alt); - if (alterr) { - return comperr(error, alterr, error_sz); - } + if (alterr) { return comperr(error, alterr, error_sz); } // type-specific constructor: populate point and element, possibly set dim bool res; @@ -230,28 +220,29 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf default: return comperr(error, "Unknown flexcomp type", error_sz); } - if (!res) { - return false; - } + if (!res) { return false; } // force flatskin shading for box, cylinder and 3D grid - if (type == mjFCOMPTYPE_BOX || type == mjFCOMPTYPE_CYLINDER || + if (type == mjFCOMPTYPE_BOX || + type == mjFCOMPTYPE_CYLINDER || (type == mjFCOMPTYPE_GRID && dflex->dim == 3)) { dflex->flatskin = true; } // check pin sizes - if (pinrange.size()%2) { + if (pinrange.size() % 2) { return comperr(error, "Pin range number must be multiple of 2", error_sz); } - if (pingrid.size()%dflex->dim) { + if (pingrid.size() % dflex->dim) { return comperr(error, "Pin grid number must be multiple of dim", error_sz); } - if (pingridrange.size()%(2*dflex->dim)) { + if (pingridrange.size() % (2 * dflex->dim)) { return comperr(error, "Pin grid range number of must be multiple of 2*dim", error_sz); } - if (type != mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty()) && - doftype != mjFCOMPDOF_TRILINEAR && doftype != mjFCOMPDOF_QUADRATIC) { + if (type != mjFCOMPTYPE_GRID && + !(pingrid.empty() && pingridrange.empty()) && + doftype != mjFCOMPDOF_TRILINEAR && + doftype != mjFCOMPDOF_QUADRATIC) { return comperr(error, "Pin grid(range) can only be used with grid or interpolated", error_sz); } if (dflex->dim == 1 && !(pingrid.empty() && pingridrange.empty())) { @@ -264,52 +255,54 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } // check point size - if (point.size()%3) { - return comperr(error, "Point size must be a multiple of 3", error_sz); - } + if (point.size() % 3) { return comperr(error, "Point size must be a multiple of 3", error_sz); } // check element size - if (element.size()%(dflex->dim+1)) { + if (element.size() % (dflex->dim + 1)) { return comperr(error, "Element size must be a multiple of dim+1", error_sz); } // get number of points - int npnt = point.size()/3; + int npnt = point.size() / 3; // check elem vertex ids - for (int i=0; i < (int)element.size(); i++) { + for (int i = 0; i < (int)element.size(); i++) { if (element[i] < 0 || element[i] >= npnt) { char msg[100]; - snprintf(msg, sizeof(msg), "element %d has point id %d, number of points is %d", i, - element[i], npnt); + snprintf(msg, + sizeof(msg), + "element %d has point id %d, number of points is %d", + i, + element[i], + npnt); return comperr(error, msg, error_sz); } } // apply scaling for direct types if (direct && (scale[0] != 1 || scale[1] != 1 || scale[2] != 1)) { - for (int i=0; i < npnt; i++) { - point[3*i] *= scale[0]; - point[3*i+1] *= scale[1]; - point[3*i+2] *= scale[2]; + for (int i = 0; i < npnt; i++) { + point[3 * i] *= scale[0]; + point[3 * i + 1] *= scale[1]; + point[3 * i + 2] *= scale[2]; } } // apply pose transform to points - for (int i=0; i < npnt; i++) { - double newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]}; + for (int i = 0; i < npnt; i++) { + double newp[3], oldp[3] = {point[3 * i], point[3 * i + 1], point[3 * i + 2]}; mjuu_trnVecPose(newp, pos, quat, oldp); - point[3*i] = newp[0]; - point[3*i+1] = newp[1]; - point[3*i+2] = newp[2]; + point[3 * i] = newp[0]; + point[3 * i + 1] = newp[1]; + point[3 * i + 2] = newp[2]; } // compute bounding box of points double minmax[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; - for (int i=0; i < npnt; i++) { - for (int j=0; j < 3; j++) { - minmax[j+0] = std::min(minmax[j+0], point[3*i+j]); - minmax[j+3] = std::max(minmax[j+3], point[3*i+j]); + for (int i = 0; i < npnt; i++) { + for (int j = 0; j < 3; j++) { + minmax[j + 0] = std::min(minmax[j + 0], point[3 * i + j]); + minmax[j + 3] = std::max(minmax[j + 3], point[3 * i + j]); } } @@ -319,22 +312,21 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int order = doftype == mjFCOMPDOF_TRILINEAR ? 1 : 2; // multi-cell count for mesh/direct/gmsh, else single cell int cx = 1, cy = 1, cz = 1; - if (type == mjFCOMPTYPE_MESH || type == mjFCOMPTYPE_DIRECT || - type == mjFCOMPTYPE_GMSH) { + if (type == mjFCOMPTYPE_MESH || type == mjFCOMPTYPE_DIRECT || type == mjFCOMPTYPE_GMSH) { if (cellcount[0] >= 0) { cx = cellcount[0]; cy = cellcount[1]; cz = cellcount[2]; } } - nnode = (cx*order+1) * (cy*order+1) * (cz*order+1); + nnode = (cx * order + 1) * (cy * order + 1) * (cz * order + 1); } pinned = vector(std::max(npnt, nnode), rigid); // handle pins if user did not specify rigid if (!rigid) { // process pinid - for (int i=0; i < (int)pinid.size(); i++) { + for (int i = 0; i < (int)pinid.size(); i++) { // check range if (pinid[i] < 0 || pinid[i] >= npnt) { return comperr(error, "pinid out of range", error_sz); @@ -345,64 +337,62 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } // process pinrange - for (int i=0; i < (int)pinrange.size(); i+=2) { + for (int i = 0; i < (int)pinrange.size(); i += 2) { // check range - if (pinrange[i] < 0 || pinrange[i] >= npnt || - pinrange[i+1] < 0 || pinrange[i+1] >= npnt) { + if (pinrange[i] < 0 || + pinrange[i] >= npnt || + pinrange[i + 1] < 0 || + pinrange[i + 1] >= npnt) { return comperr(error, "pinrange out of range", error_sz); } // set - for (int k=pinrange[i]; k <= pinrange[i+1]; k++) { - pinned[k] = true; - } + for (int k = pinrange[i]; k <= pinrange[i + 1]; k++) { pinned[k] = true; } } // process pingrid - for (int i=0; i < (int)pingrid.size(); i+=dflex->dim) { + for (int i = 0; i < (int)pingrid.size(); i += dflex->dim) { // check range int count_check[3] = {count[0], count[1], count[2]}; - if (type != mjFCOMPTYPE_GRID && (doftype == mjFCOMPDOF_TRILINEAR || - doftype == mjFCOMPDOF_QUADRATIC)) { - int dim = (doftype == mjFCOMPDOF_TRILINEAR) ? 2 : 3; + if (type != mjFCOMPTYPE_GRID && + (doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC)) { + int dim = (doftype == mjFCOMPDOF_TRILINEAR) ? 2 : 3; count_check[0] = count_check[1] = count_check[2] = dim; } - for (int k=0; k < dflex->dim; k++) { - if (pingrid[i+k] < 0 || pingrid[i+k] >= count_check[k]) { + for (int k = 0; k < dflex->dim; k++) { + if (pingrid[i + k] < 0 || pingrid[i + k] >= count_check[k]) { return comperr(error, "pingrid out of range", error_sz); } } // set if (dflex->dim == 2) { - pinned[GridID(pingrid[i], pingrid[i+1])] = true; - } - else if (dflex->dim == 3) { - pinned[GridID(pingrid[i], pingrid[i+1], pingrid[i+2])] = true; + pinned[GridID(pingrid[i], pingrid[i + 1])] = true; + } else if (dflex->dim == 3) { + pinned[GridID(pingrid[i], pingrid[i + 1], pingrid[i + 2])] = true; } } // process pingridrange - for (int i=0; i < (int)pingridrange.size(); i+=2*dflex->dim) { + for (int i = 0; i < (int)pingridrange.size(); i += 2 * dflex->dim) { // check range - for (int k=0; k < 2*dflex->dim; k++) { - if (pingridrange[i+k] < 0 || pingridrange[i+k] >= count[k%dflex->dim]) { + for (int k = 0; k < 2 * dflex->dim; k++) { + if (pingridrange[i + k] < 0 || pingridrange[i + k] >= count[k % dflex->dim]) { return comperr(error, "pingridrange out of range", error_sz); } } // set if (dflex->dim == 2) { - for (int ix=pingridrange[i]; ix <= pingridrange[i+2]; ix++) { - for (int iy=pingridrange[i+1]; iy <= pingridrange[i+3]; iy++) { + for (int ix = pingridrange[i]; ix <= pingridrange[i + 2]; ix++) { + for (int iy = pingridrange[i + 1]; iy <= pingridrange[i + 3]; iy++) { pinned[GridID(ix, iy)] = true; } } - } - else if (dflex->dim == 3) { - for (int ix=pingridrange[i]; ix <= pingridrange[i+3]; ix++) { - for (int iy=pingridrange[i+1]; iy <= pingridrange[i+4]; iy++) { - for (int iz=pingridrange[i+2]; iz <= pingridrange[i+5]; iz++) { + } else if (dflex->dim == 3) { + for (int ix = pingridrange[i]; ix <= pingridrange[i + 3]; ix++) { + for (int iy = pingridrange[i + 1]; iy <= pingridrange[i + 4]; iy++) { + for (int iz = pingridrange[i + 2]; iz <= pingridrange[i + 5]; iz++) { pinned[GridID(ix, iy, iz)] = true; } } @@ -411,17 +401,14 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } // center of radial body is always pinned - if (doftype == mjFCOMPDOF_RADIAL) { - pinned[0] = true; - } + if (doftype == mjFCOMPDOF_RADIAL) { pinned[0] = true; } // check if all or none are pinned bool allpin = true, nopin = true; - for (int i=0; i < npnt; i++) { + for (int i = 0; i < npnt; i++) { if (pinned[i]) { nopin = false; - } - else { + } else { allpin = false; } } @@ -429,8 +416,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // adjust rigid and centered if (allpin) { rigid = true; - } - else if (nopin) { + } else if (nopin) { centered = true; } } @@ -438,40 +424,34 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // remove unreferenced for direct, mesh, gmsh if (direct) { // find used - used = std::vector (npnt, false); - for (int i=0; i < (int)element.size(); i++) { - used[element[i]] = true; - } + used = std::vector(npnt, false); + for (int i = 0; i < (int)element.size(); i++) { used[element[i]] = true; } // construct reindex - bool hasunused = false; - std::vector reindex (npnt, 0); - for (int i=0; i < npnt; i++) { + bool hasunused = false; + std::vector reindex(npnt, 0); + for (int i = 0; i < npnt; i++) { if (!used[i]) { hasunused = true; - for (int k=i+1; k < npnt; k++) { - reindex[k]--; - } + for (int k = i + 1; k < npnt; k++) { reindex[k]--; } } } // reindex elements if unused present if (hasunused) { - for (int i=0; i < (int)element.size(); i++) { - element[i] += reindex[element[i]]; - } + for (int i = 0; i < (int)element.size(); i++) { element[i] += reindex[element[i]]; } // compact point, texcoord, pinned arrays int new_npnt = 0; - for (int i=0; i < npnt; i++) { + for (int i = 0; i < npnt; i++) { if (used[i]) { - point[3*new_npnt+0] = point[3*i+0]; - point[3*new_npnt+1] = point[3*i+1]; - point[3*new_npnt+2] = point[3*i+2]; + point[3 * new_npnt + 0] = point[3 * i + 0]; + point[3 * new_npnt + 1] = point[3 * i + 1]; + point[3 * new_npnt + 2] = point[3 * i + 2]; if (!texcoord.empty()) { - texcoord[2*new_npnt+0] = texcoord[2*i+0]; - texcoord[2*new_npnt+1] = texcoord[2*i+1]; + texcoord[2 * new_npnt + 0] = texcoord[2 * i + 0]; + texcoord[2 * new_npnt + 1] = texcoord[2 * i + 1]; } pinned[new_npnt] = pinned[i]; @@ -480,10 +460,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } // resize arrays - point.resize(3*new_npnt); - if (!texcoord.empty()) { - texcoord.resize(2*new_npnt); - } + point.resize(3 * new_npnt); + if (!texcoord.empty()) { texcoord.resize(2 * new_npnt); } pinned.resize(std::max(new_npnt, nnode)); used.assign(new_npnt, true); @@ -494,26 +472,24 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // nothing to remove for auto-generated types else { - used = std::vector (npnt, true); + used = std::vector(npnt, true); } // create flex, copy parameters mjCFlex* flex = model->AddFlex(); - mjsFlex* pf = &flex->spec; - int id = flex->id; + mjsFlex* pf = &flex->spec; + int id = flex->id; *flex = def.Flex(); flex->PointToLocal(); flex->model = model; - flex->id = id; + flex->id = id; mjs_setName(pf->element, name.c_str()); mjs_setInt(pf->elem, element.data(), element.size()); mjs_setFloat(pf->texcoord, texcoord.data(), texcoord.size()); mjs_setInt(pf->elemtexcoord, elemtexcoord.data(), elemtexcoord.size()); - if (!centered) { - mjs_setDouble(pf->vert, point.data(), point.size()); - } + if (!centered) { mjs_setDouble(pf->vert, point.data(), point.size()); } // rigid: set parent name, nothing else to do if (rigid) { @@ -522,21 +498,19 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } // compute body mass and inertia matching specs - double bodymass = mass/npnt; - double bodyinertia = bodymass*(2.0*inertiabox*inertiabox)/3.0; + double bodymass = mass / npnt; + double bodyinertia = bodymass * (2.0 * inertiabox * inertiabox) / 3.0; // overwrite plugin name if (plugin.active && plugin_instance_name.empty()) { - plugin_instance_name = "flexcomp_" + name; + plugin_instance_name = "flexcomp_" + name; static_cast(plugin.element)->name = plugin_instance_name; } // create bodies, construct flex vert and vertbody - for (int i=0; i < npnt; i++) { + for (int i = 0; i < npnt; i++) { // not used: skip - if (!used[i]) { - continue; - } + if (!used[i]) { continue; } // pinned or trilinear or quadratic: parent body if (pinned[i] || doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) { @@ -545,8 +519,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // add plugin if (plugin.active) { mjsPlugin* pplugin = &body->plugin; - pplugin->active = true; - pplugin->element = static_cast(plugin.element); + pplugin->active = true; + pplugin->element = static_cast(plugin.element); mjs_setString(pplugin->plugin_name, mjs_getString(plugin.plugin_name)); mjs_setString(pplugin->name, plugin_instance_name.c_str()); } @@ -558,14 +532,14 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf mjsBody* pb = mjs_addBody(body, 0); // set frame and inertial - pb->pos[0] = point[3*i]; - pb->pos[1] = point[3*i+1]; - pb->pos[2] = point[3*i+2]; + pb->pos[0] = point[3 * i]; + pb->pos[1] = point[3 * i + 1]; + pb->pos[2] = point[3 * i + 2]; mjuu_zerovec(pb->ipos, 3); - pb->mass = bodymass; - pb->inertia[0] = bodyinertia; - pb->inertia[1] = bodyinertia; - pb->inertia[2] = bodyinertia; + pb->mass = bodymass; + pb->inertia[0] = bodyinertia; + pb->inertia[1] = bodyinertia; + pb->inertia[2] = bodyinertia; pb->explicitinertial = true; // add radial slider @@ -581,7 +555,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // add three orthogonal sliders else if (doftype == mjFCOMPDOF_FULL) { - for (int j=0; j < 3; j++) { + for (int j = 0; j < 3; j++) { // add joint to body mjsJoint* jnt = mjs_addJoint(pb, 0); @@ -595,9 +569,9 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // add two orthogonal sliders (x and y only) else if (doftype == mjFCOMPDOF_2D) { - for (int j=0; j < 2; j++) { + for (int j = 0; j < 2; j++) { mjsJoint* jnt = mjs_addJoint(pb, 0); - jnt->type = mjJNT_SLIDE; + jnt->type = mjJNT_SLIDE; mjuu_setvec(jnt->pos, 0, 0, 0); mjuu_setvec(jnt->axis, 0, 0, 0); jnt->axis[j] = 1; @@ -612,16 +586,16 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // clear flex vertex coordinates if allocated if (!centered) { - point[3*i] = 0; - point[3*i+1] = 0; - point[3*i+2] = 0; + point[3 * i] = 0; + point[3 * i + 1] = 0; + point[3 * i + 2] = 0; } // add plugin if (plugin.active) { mjsPlugin* pplugin = &pb->plugin; - pplugin->active = true; - pplugin->element = static_cast(plugin.element); + pplugin->active = true; + pplugin->element = static_cast(plugin.element); mjs_setString(pplugin->plugin_name, mjs_getString(plugin.plugin_name)); mjs_setString(pplugin->name, plugin_instance_name.c_str()); } @@ -639,27 +613,22 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } // total number of nodes with shared boundaries - int nx = flex->spec.cellcount[0] * flex->spec.order + 1; - int ny = flex->spec.cellcount[1] * flex->spec.order + 1; - int nz = flex->spec.cellcount[2] * flex->spec.order + 1; + int nx = flex->spec.cellcount[0] * flex->spec.order + 1; + int ny = flex->spec.cellcount[1] * flex->spec.order + 1; + int nz = flex->spec.cellcount[2] * flex->spec.order + 1; int nnode = nx * ny * nz; // mark empty cells and pin nodes exclusively in empty cells (volume mode only) - if (!dflex->elastic2d) { - MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz); - } + if (!dflex->elastic2d) { MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz); } // shell mode: pin all interior (non-boundary) nodes if (dflex->elastic2d) { for (int gi = 0; gi < nx; gi++) { for (int gj = 0; gj < ny; gj++) { for (int gk = 0; gk < nz; gk++) { - bool is_boundary = (gi == 0 || gi == nx-1 || - gj == 0 || gj == ny-1 || - gk == 0 || gk == nz-1); - if (!is_boundary) { - pinned[gi*ny*nz + gj*nz + gk] = true; - } + bool is_boundary = + (gi == 0 || gi == nx - 1 || gj == 0 || gj == ny - 1 || gk == 0 || gk == nz - 1); + if (!is_boundary) { pinned[gi * ny * nz + gj * nz + gk] = true; } } } } @@ -677,13 +646,12 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } std::vector node(3 * nnode, 0); - int idx = 0; + int idx = 0; // Simpson's rule weights for quadratic mass distribution double massP2[3] = {1. / 6., 2. / 3., 1. / 6.}; - // collect created bodies for mass normalization std::vector node_bodies; @@ -701,19 +669,18 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf double pz = minmax[2] + u * (minmax[5] - minmax[2]); if (pinned[idx]) { - node[3*idx+0] = px; - node[3*idx+1] = py; - node[3*idx+2] = pz; - mjs_appendString(pf->nodebody, - mjs_getName(body->element)->c_str()); + node[3 * idx + 0] = px; + node[3 * idx + 1] = py; + node[3 * idx + 2] = pz; + mjs_appendString(pf->nodebody, mjs_getName(body->element)->c_str()); idx++; continue; } mjsBody* pb = mjs_addBody(body, 0); - pb->pos[0] = px; - pb->pos[1] = py; - pb->pos[2] = pz; + pb->pos[0] = px; + pb->pos[1] = py; + pb->pos[2] = pz; mjuu_zerovec(pb->ipos, 3); // mass distribution @@ -725,26 +692,26 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int lj = gj % flex->spec.order; int lk = gk % flex->spec.order; // boundary nodes: average mass contribution - int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1; - int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1; - int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1; + int ncells_i = (gi > 0 && gi < nx - 1 && li == 0) ? 2 : 1; + int ncells_j = (gj > 0 && gj < ny - 1 && lj == 0) ? 2 : 1; + int ncells_k = (gk > 0 && gk < nz - 1 && lk == 0) ? 2 : 1; // use Simpson weights double wi = massP2[li == 0 ? 0 : li]; double wj = massP2[lj == 0 ? 0 : lj]; double wk = massP2[lk == 0 ? 0 : lk]; - pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k; + pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k; } node_bodies.push_back(pb); - pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; - pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; - pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; + pb->inertia[0] = pb->mass * (2.0 * inertiabox * inertiabox) / 3.0; + pb->inertia[1] = pb->mass * (2.0 * inertiabox * inertiabox) / 3.0; + pb->inertia[2] = pb->mass * (2.0 * inertiabox * inertiabox) / 3.0; pb->explicitinertial = true; - for (int d=0; d < 3; d++) { + for (int d = 0; d < 3; d++) { mjsJoint* jnt = mjs_addJoint(pb, 0); - jnt->type = mjJNT_SLIDE; + jnt->type = mjJNT_SLIDE; mjuu_setvec(jnt->pos, 0, 0, 0); mjuu_setvec(jnt->axis, 0, 0, 0); jnt->axis[d] = 1; @@ -763,22 +730,18 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // normalize masses so total equals prescribed mass double total_mass = 0; - for (mjsBody* pb : node_bodies) { - total_mass += pb->mass; - } + for (mjsBody* pb : node_bodies) { total_mass += pb->mass; } if (total_mass > 0) { double scale = mass / total_mass; for (mjsBody* pb : node_bodies) { - pb->mass *= scale; + pb->mass *= scale; pb->inertia[0] *= scale; pb->inertia[1] *= scale; pb->inertia[2] *= scale; } } - if (!centered) { - mjs_setDouble(pf->node, node.data(), node.size()); - } + if (!centered) { mjs_setDouble(pf->node, node.data(), node.size()); } } if (!centered || doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) { @@ -791,26 +754,25 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf if (equality == 1 || equality == 2) { mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); mjs_setDefault(pe->element, &model->Default()->spec); - pe->type = (equality == 1) ? mjEQ_FLEX : mjEQ_FLEXVERT; + pe->type = (equality == 1) ? mjEQ_FLEX : mjEQ_FLEXVERT; pe->active = true; mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { // create one strain constraint per finite element, storing element index flex->has_strain_eq = true; - int cell_cx = flex->spec.cellcount[0]; - int cell_cy = flex->spec.cellcount[1]; - int cell_cz = flex->spec.cellcount[2]; - bool shell = (doftype == mjFCOMPDOF_TRILINEAR || - doftype == mjFCOMPDOF_QUADRATIC) && - flex->spec.elastic2d; + int cell_cx = flex->spec.cellcount[0]; + int cell_cy = flex->spec.cellcount[1]; + int cell_cz = flex->spec.cellcount[2]; + bool shell = (doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) && + flex->spec.elastic2d; if (shell) { // shell mode: one constraint per boundary face element - int nelem_fe = 2*(cell_cy*cell_cz + cell_cx*cell_cz + cell_cx*cell_cy); + int nelem_fe = 2 * (cell_cy * cell_cz + cell_cx * cell_cz + cell_cx * cell_cy); for (int fe = 0; fe < nelem_fe; fe++) { mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); mjs_setDefault(pe->element, &model->Default()->spec); - pe->type = mjEQ_FLEXSTRAIN; + pe->type = mjEQ_FLEXSTRAIN; pe->active = true; mjs_setString(pe->name1, name.c_str()); pe->data[0] = fe; @@ -829,7 +791,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); mjs_setDefault(pe->element, &model->Default()->spec); - pe->type = mjEQ_FLEXSTRAIN; + pe->type = mjEQ_FLEXSTRAIN; pe->active = true; mjs_setString(pe->name1, name.c_str()); pe->data[0] = ci; @@ -846,50 +808,46 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } - // get point id from grid coordinates int mjCFlexcomp::GridID(int ix, int iy) { - return ix*count[1] + iy; + return ix * count[1] + iy; } int mjCFlexcomp::GridID(int ix, int iy, int iz) { - return ix*count[1]*count[2] + iy*count[2] + iz; + return ix * count[1] * count[2] + iy * count[2] + iz; } - // make grid bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { - int dim = def.Flex().spec.dim; + int dim = def.Flex().spec.dim; bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0]; // 1D if (dim == 1) { - for (int ix=0; ix < count[0]; ix++) { + for (int ix = 0; ix < count[0]; ix++) { if (type == mjFCOMPTYPE_CIRCLE) { - if (ix >= count[0]-1) { - continue; - } + if (ix >= count[0] - 1) { continue; } // add point - double theta = 2*mjPI/(count[0]-1); - double radius = spacing[0]/std::sin(theta/2)/2; - point.push_back(radius*std::cos(theta*ix)); - point.push_back(radius*std::sin(theta*ix)); + double theta = 2 * mjPI / (count[0] - 1); + double radius = spacing[0] / std::sin(theta / 2) / 2; + point.push_back(radius * std::cos(theta * ix)); + point.push_back(radius * std::sin(theta * ix)); point.push_back(0); // add element element.push_back(ix); - element.push_back(ix == count[0]-2 ? 0 : ix+1); + element.push_back(ix == count[0] - 2 ? 0 : ix + 1); } else { // add point - point.push_back(spacing[0]*(ix - 0.5*(count[0]-1))); + point.push_back(spacing[0] * (ix - 0.5 * (count[0] - 1))); point.push_back(0); point.push_back(0); // add element - if (ix < count[0]-1) { + if (ix < count[0] - 1) { element.push_back(ix); - element.push_back(ix+1); + element.push_back(ix + 1); } } } @@ -897,43 +855,43 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { // 2D else if (dim == 2) { - for (int ix=0; ix < count[0]; ix++) { - for (int iy=0; iy < count[1]; iy++) { - int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}}; + for (int ix = 0; ix < count[0]; ix++) { + for (int iy = 0; iy < count[1]; iy++) { + int quad2tri[2][3] = { + {0, 1, 2}, + {0, 2, 3} + }; // add point - double pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)), - spacing[1]*(iy - 0.5*(count[1]-1))}; + double pos[2] = {spacing[0] * (ix - 0.5 * (count[0] - 1)), + spacing[1] * (iy - 0.5 * (count[1] - 1))}; point.push_back(pos[0]); point.push_back(pos[1]); point.push_back(0); // add texture coordinates, if not specified explicitly if (needtex) { - texcoord.push_back(ix/(double)std::max(count[0]-1, 1)); - texcoord.push_back(iy/(double)std::max(count[1]-1, 1)); + texcoord.push_back(ix / (double)std::max(count[0] - 1, 1)); + texcoord.push_back(iy / (double)std::max(count[1] - 1, 1)); } // flip triangles if radial projection is requested - if (((pos[0] < -mjEPS && pos[1] > -mjEPS) || - (pos[0] > -mjEPS && pos[1] < -mjEPS)) && + if (((pos[0] < -mjEPS && pos[1] > -mjEPS) || (pos[0] > -mjEPS && pos[1] < -mjEPS)) && type == mjFCOMPTYPE_DISC) { quad2tri[0][2] = 3; quad2tri[1][0] = 1; } // add elements - if (ix < count[0]-1 && iy < count[1]-1) { + if (ix < count[0] - 1 && iy < count[1] - 1) { int vert[4] = { - count[2]*count[1]*(ix+0) + count[2]*(iy+0), - count[2]*count[1]*(ix+1) + count[2]*(iy+0), - count[2]*count[1]*(ix+1) + count[2]*(iy+1), - count[2]*count[1]*(ix+0) + count[2]*(iy+1), + count[2] * count[1] * (ix + 0) + count[2] * (iy + 0), + count[2] * count[1] * (ix + 1) + count[2] * (iy + 0), + count[2] * count[1] * (ix + 1) + count[2] * (iy + 1), + count[2] * count[1] * (ix + 0) + count[2] * (iy + 1), }; - for (int s =0; s < 2; s++) { - for (int v=0; v < 3; v++) { - element.push_back(vert[quad2tri[s][v]]); - } + for (int s = 0; s < 2; s++) { + for (int v = 0; v < 3; v++) { element.push_back(vert[quad2tri[s][v]]); } } } } @@ -942,39 +900,42 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { // 3D else { - int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7}, - {1, 3, 2, 7}, {1, 2, 6, 7}, - {1, 5, 4, 7}, {1, 6, 5, 7}}; - for (int ix=0; ix < count[0]; ix++) { - for (int iy=0; iy < count[1]; iy++) { - for (int iz=0; iz < count[2]; iz++) { + int cube2tets[6][4] = { + {0, 3, 1, 7}, + {0, 1, 4, 7}, + {1, 3, 2, 7}, + {1, 2, 6, 7}, + {1, 5, 4, 7}, + {1, 6, 5, 7} + }; + for (int ix = 0; ix < count[0]; ix++) { + for (int iy = 0; iy < count[1]; iy++) { + for (int iz = 0; iz < count[2]; iz++) { // add point - point.push_back(spacing[0]*(ix - 0.5*(count[0]-1))); - point.push_back(spacing[1]*(iy - 0.5*(count[1]-1))); - point.push_back(spacing[2]*(iz - 0.5*(count[2]-1))); + point.push_back(spacing[0] * (ix - 0.5 * (count[0] - 1))); + point.push_back(spacing[1] * (iy - 0.5 * (count[1] - 1))); + point.push_back(spacing[2] * (iz - 0.5 * (count[2] - 1))); // add texture coordinates, if not specified explicitly if (needtex) { - texcoord.push_back(ix/(float)std::max(count[0]-1, 1)); - texcoord.push_back(iy/(float)std::max(count[1]-1, 1)); + texcoord.push_back(ix / (float)std::max(count[0] - 1, 1)); + texcoord.push_back(iy / (float)std::max(count[1] - 1, 1)); } // add elements - if (ix < count[0]-1 && iy < count[1]-1 && iz < count[2]-1) { + if (ix < count[0] - 1 && iy < count[1] - 1 && iz < count[2] - 1) { int vert[8] = { - count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+0, - count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+0, - count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+0, - count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+0, - count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+1, - count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+1, - count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+1, - count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+1, + count[2] * count[1] * (ix + 0) + count[2] * (iy + 0) + iz + 0, + count[2] * count[1] * (ix + 1) + count[2] * (iy + 0) + iz + 0, + count[2] * count[1] * (ix + 1) + count[2] * (iy + 1) + iz + 0, + count[2] * count[1] * (ix + 0) + count[2] * (iy + 1) + iz + 0, + count[2] * count[1] * (ix + 0) + count[2] * (iy + 0) + iz + 1, + count[2] * count[1] * (ix + 1) + count[2] * (iy + 0) + iz + 1, + count[2] * count[1] * (ix + 1) + count[2] * (iy + 1) + iz + 1, + count[2] * count[1] * (ix + 0) + count[2] * (iy + 1) + iz + 1, }; - for (int s=0; s < 6; s++) { - for (int v=0; v < 4; v++) { - element.push_back(vert[cube2tets[s][v]]); - } + for (int s = 0; s < 6; s++) { + for (int v = 0; v < 4; v++) { element.push_back(vert[cube2tets[s][v]]); } } } } @@ -983,64 +944,68 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { } // check elements - if (element.empty()) { - return comperr(error, "No elements were created in grid", error_sz); - } + if (element.empty()) { return comperr(error, "No elements were created in grid", error_sz); } return true; } - // get point id from box coordinates and side int mjCFlexcomp::BoxID(int ix, int iy, int iz) { // side iz=0 if (iz == 0) { - return ix*count[1] + iy + 1; + return ix * count[1] + iy + 1; } // side iz=max - else if (iz == count[2]-1) { - return count[0]*count[1] + ix*count[1] + iy + 1; + else if (iz == count[2] - 1) { + return count[0] * count[1] + ix * count[1] + iy + 1; } // side iy=0 else if (iy == 0) { - return 2*count[0]*count[1] + ix*(count[2]-2) + iz - 1 + 1; + return 2 * count[0] * count[1] + ix * (count[2] - 2) + iz - 1 + 1; } // side iy=max - else if (iy == count[1]-1) { - return 2*count[0]*count[1] + count[0]*(count[2]-2) + ix*(count[2]-2) + iz - 1 + 1; + else if (iy == count[1] - 1) { + return 2 * count[0] * count[1] + count[0] * (count[2] - 2) + ix * (count[2] - 2) + iz - 1 + 1; } // side ix=0 else if (ix == 0) { - return 2*count[0]*count[1] + 2*count[0]*(count[2]-2) + (iy-1)*(count[2]-2) + iz - 1 + 1; + return 2 * count[0] * count[1] + + 2 * count[0] * (count[2] - 2) + + (iy - 1) * (count[2] - 2) + + iz - + 1 + + 1; } // side ix=max else { - return 2*count[0]*count[1] + 2*count[0]*(count[2]-2) + (count[1]-2)*(count[2]-2) + - (iy-1)*(count[2]-2) + iz - 1 + 1; + return 2 * count[0] * count[1] + + 2 * count[0] * (count[2] - 2) + + (count[1] - 2) * (count[2] - 2) + + (iy - 1) * (count[2] - 2) + + iz - + 1 + + 1; } } - // project from box to other shape void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) { // init point - pos[0] = 2.0*ix/(count[0]-1) - 1; - pos[1] = 2.0*iy/(count[1]-1) - 1; - pos[2] = 2.0*iz/(count[2]-1) - 1; + pos[0] = 2.0 * ix / (count[0] - 1) - 1; + pos[1] = 2.0 * iy / (count[1] - 1) - 1; + pos[2] = 2.0 * iz / (count[2] - 1) - 1; // determine sizes - double size[3] = { - 0.5*spacing[0]*(count[0]-1), - 0.5*spacing[1]*(count[1]-1), - 0.5*spacing[2]*(count[2]-1) - }; + double size[3] = {0.5 * spacing[0] * (count[0] - 1), + 0.5 * spacing[1] * (count[1] - 1), + 0.5 * spacing[2] * (count[2] - 1)}; // box if (type == mjFCOMPTYPE_BOX) { @@ -1053,8 +1018,8 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) { else if (type == mjFCOMPTYPE_CYLINDER) { double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); - pos[0] *= size[0]*L0; - pos[1] *= size[1]*L0; + pos[0] *= size[0] * L0; + pos[1] *= size[1] * L0; pos[2] *= size[2]; } @@ -1068,30 +1033,27 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) { } - // make 2d square or disc bool mjCFlexcomp::MakeSquare(char* error, int error_sz) { // set 2D def.spec.flex->dim = 2; // create square - if (!MakeGrid(error, error_sz)) { - return false; - } + if (!MakeGrid(error, error_sz)) { return false; } // do projection if (type == mjFCOMPTYPE_DISC) { double size[2] = { - 0.5*spacing[0]*(count[0]-1), - 0.5*spacing[1]*(count[1]-1), + 0.5 * spacing[0] * (count[0] - 1), + 0.5 * spacing[1] * (count[1] - 1), }; - for (int i=0; i < point.size()/3; i++) { - double* pos = point.data() + i*3; - double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); + for (int i = 0; i < point.size() / 3; i++) { + double* pos = point.data() + i * 3; + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); - pos[0] *= size[0]*L0; - pos[1] *= size[1]*L0; + pos[0] *= size[0] * L0; + pos[1] *= size[1] * L0; } } @@ -1099,17 +1061,15 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) { } - static int mat2lin(int ix, int iy, int iz, const int count[3]) { - return ix*count[1]*count[2] + iy*count[2] + iz; + return ix * count[1] * count[2] + iy * count[2] + iz; } - // make 3d box, ellipsoid or cylinder bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { double pos[3]; - bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0]; + bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0]; // set dimension def.spec.flex->dim = dim; @@ -1128,16 +1088,14 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { } // add points - int n = 0; - std::vector idx(count[0]*count[1]*count[2]); + int n = 0; + std::vector idx(count[0] * count[1] * count[2]); // iz=0/max - for (int iz=0; iz < count[2]; iz+=count[2]-1) { - for (int ix=0; ix < count[0]; ix++) { - for (int iy=0; iy < count[1]; iy++) { - if (open && dim == 2 && iz != 0) { - continue; - } + for (int iz = 0; iz < count[2]; iz += count[2] - 1) { + for (int ix = 0; ix < count[0]; ix++) { + for (int iy = 0; iy < count[1]; iy++) { + if (open && dim == 2 && iz != 0) { continue; } // add point BoxProject(pos, ix, iy, iz); @@ -1148,19 +1106,19 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { // add texture coordinates, if not specified explicitly if (needtex) { - texcoord.push_back(ix/(float)std::max(count[0]-1, 1)); - texcoord.push_back(iy/(float)std::max(count[1]-1, 1)); + texcoord.push_back(ix / (float)std::max(count[0] - 1, 1)); + texcoord.push_back(iy / (float)std::max(count[1] - 1, 1)); } } } } // iy=0/max - for (int iy=0; iy < count[1]; iy+=count[1]-1) { - for (int ix=0; ix < count[0]; ix++) { - for (int iz=0; iz < count[2]; iz++) { + for (int iy = 0; iy < count[1]; iy += count[1] - 1) { + for (int ix = 0; ix < count[0]; ix++) { + for (int iz = 0; iz < count[2]; iz++) { // add point - if (iz > 0 && ((open && dim == 2) || (iz < count[2]-1))) { + if (iz > 0 && ((open && dim == 2) || (iz < count[2] - 1))) { BoxProject(pos, ix, iy, iz); point.push_back(pos[0]); point.push_back(pos[1]); @@ -1169,8 +1127,8 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { // add texture coordinates if (needtex) { - texcoord.push_back(ix/(float)std::max(count[0]-1, 1)); - texcoord.push_back(iz/(float)std::max(count[2]-1, 1)); + texcoord.push_back(ix / (float)std::max(count[0] - 1, 1)); + texcoord.push_back(iz / (float)std::max(count[2] - 1, 1)); } } } @@ -1178,11 +1136,11 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { } // ix=0/max - for (int ix=0; ix < count[0]; ix+=count[0]-1) { - for (int iy=0; iy < count[1]; iy++) { - for (int iz=0; iz < count[2]; iz++) { + for (int ix = 0; ix < count[0]; ix += count[0] - 1) { + for (int iy = 0; iy < count[1]; iy++) { + for (int iz = 0; iz < count[2]; iz++) { // add point - if (iz > 0 && ((open && dim == 2) || (iz < count[2]-1)) && iy > 0 && iy < count[1]-1) { + if (iz > 0 && ((open && dim == 2) || (iz < count[2] - 1)) && iy > 0 && iy < count[1] - 1) { BoxProject(pos, ix, iy, iz); point.push_back(pos[0]); point.push_back(pos[1]); @@ -1191,8 +1149,8 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { // add texture coordinates if (needtex) { - texcoord.push_back(iy/(float)std::max(count[1]-1, 1)); - texcoord.push_back(iz/(float)std::max(count[2]-1, 1)); + texcoord.push_back(iy / (float)std::max(count[1] - 1, 1)); + texcoord.push_back(iz / (float)std::max(count[2] - 1, 1)); } } } @@ -1202,34 +1160,32 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { // add elements // iz=0/max - for (int iz=0; iz < count[2]; iz+=count[2]-1) { - for (int ix=0; ix < count[0]; ix++) { - for (int iy=0; iy < count[1]; iy++) { - if (open && dim == 2 && iz != 0) { - continue; - } + for (int iz = 0; iz < count[2]; iz += count[2] - 1) { + for (int ix = 0; ix < count[0]; ix++) { + for (int iy = 0; iy < count[1]; iy++) { + if (open && dim == 2 && iz != 0) { continue; } - if (ix < count[0]-1 && iy < count[1]-1) { - if (dim==3) { + if (ix < count[0] - 1 && iy < count[1] - 1) { + if (dim == 3) { element.push_back(0); element.push_back(BoxID(ix, iy, iz)); - element.push_back(BoxID(ix+1, iy, iz)); - element.push_back(BoxID(ix+1, iy+1, iz)); + element.push_back(BoxID(ix + 1, iy, iz)); + element.push_back(BoxID(ix + 1, iy + 1, iz)); element.push_back(0); element.push_back(BoxID(ix, iy, iz)); - element.push_back(BoxID(ix, iy+1, iz)); - element.push_back(BoxID(ix+1, iy+1, iz)); + element.push_back(BoxID(ix, iy + 1, iz)); + element.push_back(BoxID(ix + 1, iy + 1, iz)); } else { int step1 = iz == 0 ? 1 : 0; int step2 = iz == 0 ? 0 : 1; element.push_back(idx[mat2lin(ix, iy, iz, count)]); - element.push_back(idx[mat2lin(ix+1, iy+step1, iz, count)]); - element.push_back(idx[mat2lin(ix+1, iy+step2, iz, count)]); + element.push_back(idx[mat2lin(ix + 1, iy + step1, iz, count)]); + element.push_back(idx[mat2lin(ix + 1, iy + step2, iz, count)]); element.push_back(idx[mat2lin(ix, iy, iz, count)]); - element.push_back(idx[mat2lin(ix+step2, iy+1, iz, count)]); - element.push_back(idx[mat2lin(ix+step1, iy+1, iz, count)]); + element.push_back(idx[mat2lin(ix + step2, iy + 1, iz, count)]); + element.push_back(idx[mat2lin(ix + step1, iy + 1, iz, count)]); } } } @@ -1237,30 +1193,30 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { } // iy=0/max - for (int iy=0; iy < count[1]; iy+=count[1]-1) { - for (int ix=0; ix < count[0]; ix++) { - for (int iz=0; iz < count[2]; iz++) { - if (ix < count[0]-1 && iz < count[2]-1) { - if (dim==3) { + for (int iy = 0; iy < count[1]; iy += count[1] - 1) { + for (int ix = 0; ix < count[0]; ix++) { + for (int iz = 0; iz < count[2]; iz++) { + if (ix < count[0] - 1 && iz < count[2] - 1) { + if (dim == 3) { element.push_back(0); element.push_back(BoxID(ix, iy, iz)); - element.push_back(BoxID(ix+1, iy, iz)); - element.push_back(BoxID(ix+1, iy, iz+1)); + element.push_back(BoxID(ix + 1, iy, iz)); + element.push_back(BoxID(ix + 1, iy, iz + 1)); element.push_back(0); element.push_back(BoxID(ix, iy, iz)); - element.push_back(BoxID(ix, iy, iz+1)); - element.push_back(BoxID(ix+1, iy, iz+1)); + element.push_back(BoxID(ix, iy, iz + 1)); + element.push_back(BoxID(ix + 1, iy, iz + 1)); } else { - int ix0 = iy == 0 ? ix : ix+1; - int dx = iy == 0 ? 1 : -1; + int ix0 = iy == 0 ? ix : ix + 1; + int dx = iy == 0 ? 1 : -1; element.push_back(idx[mat2lin(ix0, iy, iz, count)]); - element.push_back(idx[mat2lin(ix0+dx, iy, iz, count)]); - element.push_back(idx[mat2lin(ix0+dx, iy, iz+1, count)]); + element.push_back(idx[mat2lin(ix0 + dx, iy, iz, count)]); + element.push_back(idx[mat2lin(ix0 + dx, iy, iz + 1, count)]); element.push_back(idx[mat2lin(ix0, iy, iz, count)]); - element.push_back(idx[mat2lin(ix0+dx, iy, iz+1, count)]); - element.push_back(idx[mat2lin(ix0, iy, iz+1, count)]); + element.push_back(idx[mat2lin(ix0 + dx, iy, iz + 1, count)]); + element.push_back(idx[mat2lin(ix0, iy, iz + 1, count)]); } } } @@ -1268,31 +1224,30 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { } // ix=0/max - for (int ix=0; ix < count[0]; ix+=count[0]-1) { - for (int iy=0; iy < count[1]; iy++) { - for (int iz=0; iz < count[2]; iz++) { - if (iy < count[1]-1 && iz < count[2]-1) { - if (dim==3) { + for (int ix = 0; ix < count[0]; ix += count[0] - 1) { + for (int iy = 0; iy < count[1]; iy++) { + for (int iz = 0; iz < count[2]; iz++) { + if (iy < count[1] - 1 && iz < count[2] - 1) { + if (dim == 3) { element.push_back(0); element.push_back(BoxID(ix, iy, iz)); - element.push_back(BoxID(ix, iy+1, iz)); - element.push_back(BoxID(ix, iy+1, iz+1)); + element.push_back(BoxID(ix, iy + 1, iz)); + element.push_back(BoxID(ix, iy + 1, iz + 1)); element.push_back(0); element.push_back(BoxID(ix, iy, iz)); - element.push_back(BoxID(ix, iy, iz+1)); - element.push_back(BoxID(ix, iy+1, iz+1)); + element.push_back(BoxID(ix, iy, iz + 1)); + element.push_back(BoxID(ix, iy + 1, iz + 1)); } else { - int iy0 = ix != 0 ? iy : iy+1; - int dy = ix != 0 ? 1 : -1; + int iy0 = ix != 0 ? iy : iy + 1; + int dy = ix != 0 ? 1 : -1; element.push_back(idx[mat2lin(ix, iy0, iz, count)]); - element.push_back(idx[mat2lin(ix, iy0+dy, iz, count)]); - element.push_back(idx[mat2lin(ix, iy0+dy, iz+1, count)]); + element.push_back(idx[mat2lin(ix, iy0 + dy, iz, count)]); + element.push_back(idx[mat2lin(ix, iy0 + dy, iz + 1, count)]); element.push_back(idx[mat2lin(ix, iy0, iz, count)]); - element.push_back(idx[mat2lin(ix, iy0+dy, iz+1, count)]); - element.push_back(idx[mat2lin(ix, iy0, iz+1, count)]); - + element.push_back(idx[mat2lin(ix, iy0 + dy, iz + 1, count)]); + element.push_back(idx[mat2lin(ix, iy0, iz + 1, count)]); } } } @@ -1303,35 +1258,29 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) { } - // copied from user_mesh.cc -template static T* VecToArray(std::vector& vector, bool clear = true){ +template +static T* VecToArray(std::vector& vector, bool clear = true) { if (vector.empty()) return nullptr; else { - int n = (int)vector.size(); - T* cvec = (T*) mju_malloc(n*sizeof(T)); - memcpy(cvec, vector.data(), n*sizeof(T)); - if (clear) { - vector.clear(); - } + int n = (int)vector.size(); + T* cvec = (T*)mju_malloc(n * sizeof(T)); + memcpy(cvec, vector.data(), n * sizeof(T)); + if (clear) { vector.clear(); } return cvec; } } - // make mesh -bool mjCFlexcomp::MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, const mjVFS* vfs) { +bool mjCFlexcomp::MakeMesh( + mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, const mjVFS* vfs) { // strip path - if (!file.empty() && model->spec.strippath) { - file = mjuu_strippath(file); - } + if (!file.empty() && model->spec.strippath) { file = mjuu_strippath(file); } // file is required - if (file.empty()) { - return comperr(error, "File is required", error_sz); - } + if (file.empty()) { return comperr(error, "File is required", error_sz); } // check dim if (def.spec.flex->dim < 1) { @@ -1348,11 +1297,8 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error, } try { - resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir), - filename, vfs); - } catch (mjCError err) { - return comperr(error, err.message, error_sz); - } + resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir), filename, vfs); + } catch (mjCError err) { return comperr(error, err.message, error_sz); } // load mesh @@ -1374,7 +1320,7 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error, point.assign(mesh.Vert().begin(), mesh.Vert().end()); if (mesh.HasTexcoord()) { - texcoord = mesh.Texcoord(); + texcoord = mesh.Texcoord(); elemtexcoord = mesh.FaceTexcoord(); } @@ -1388,28 +1334,24 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error, element.reserve(face.size() * 2 / 3); for (size_t i = 0; i < face.size(); i += 3) { element.push_back(face[i]); - element.push_back(face[i+1]); + element.push_back(face[i + 1]); } } else { point.insert(point.begin() + 0, origin[0]); point.insert(point.begin() + 1, origin[1]); point.insert(point.begin() + 2, origin[2]); - for (int i=0; i < mesh.Face().size(); i+=3) { + for (int i = 0; i < mesh.Face().size(); i += 3) { // only add tetrahedra with positive volume - int tet[3] = {mesh.Face()[i+0]+1, - mesh.Face()[i+1]+1, - mesh.Face()[i+2]+1}; + int tet[3] = {mesh.Face()[i + 0] + 1, mesh.Face()[i + 1] + 1, mesh.Face()[i + 2] + 1}; double edge1[3], edge2[3], edge3[3]; - for (int i=0; i < 3; i++) { - edge1[i] = point[3*tet[0]+i] - origin[i]; - edge2[i] = point[3*tet[1]+i] - origin[i]; - edge3[i] = point[3*tet[2]+i] - origin[i]; + for (int i = 0; i < 3; i++) { + edge1[i] = point[3 * tet[0] + i] - origin[i]; + edge2[i] = point[3 * tet[1] + i] - origin[i]; + edge3[i] = point[3 * tet[2] + i] - origin[i]; } double normal[3]; mjuu_crossvec(normal, edge1, edge2); - if (mjuu_dot3(normal, edge3) < mjMINVAL) { - continue; - } + if (mjuu_dot3(normal, edge3) < mjMINVAL) { continue; } element.push_back(0); element.push_back(tet[0]); element.push_back(tet[1]); @@ -1421,26 +1363,23 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error, } - // find string in buffer, return position or -1 if not found static int findstring(const char* buffer, int buffer_sz, const char* str) { int len = (int)strlen(str); // scan buffer - for (int i=0; i < buffer_sz-len; i++) { + for (int i = 0; i < buffer_sz - len; i++) { // check for string at position i bool found = true; - for (int k=0; k < len; k++) { - if (buffer[i+k] != str[k]) { + for (int k = 0; k < len; k++) { + if (buffer[i + k] != str[k]) { found = false; break; } } // string found - if (found) { - return i; - } + if (found) { return i; } } // not found @@ -1448,28 +1387,21 @@ static int findstring(const char* buffer, int buffer_sz, const char* str) { } - // load points and elements from GMSH file -bool mjCFlexcomp::MakeGMSH(mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, const mjVFS* vfs) { +bool mjCFlexcomp::MakeGMSH( + mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, const mjVFS* vfs) { // strip path - if (!file.empty() && model->spec.strippath) { - file = mjuu_strippath(file); - } + if (!file.empty() && model->spec.strippath) { file = mjuu_strippath(file); } // file is required - if (file.empty()) { - return comperr(error, "File is required", error_sz); - } + if (file.empty()) { return comperr(error, "File is required", error_sz); } // open resource mjResource* resource = nullptr; try { std::string filename = mjuu_combinePaths(mjs_getString(compiler->meshdir), file); - resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir), - filename, vfs); - } catch (mjCError err) { - return comperr(error, err.message, error_sz); - } + resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir), filename, vfs); + } catch (mjCError err) { return comperr(error, err.message, error_sz); } // try to load, close resource properly try { @@ -1487,15 +1419,14 @@ bool mjCFlexcomp::MakeGMSH(mjCModel* model, mjsCompiler* compiler, char* error, } - // load GMSH format 4.1 -void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, - int nodebegin, int elemend, int elembegin){ +void mjCFlexcomp::LoadGMSH41( + char* buffer, int binary, int nodeend, int nodebegin, int elemend, int elembegin) { // header size constexpr int kGmsh41HeaderSize = 52; // base for node tags, to be subtracted from element data size_t minNodeTag, numEntityBlocks, numNodes, maxNodeTag, numNodesInBlock, tag; - int entityDim, entityTag, parametric; + int entityDim, entityTag, parametric; // ascii nodes if (binary == 0) { @@ -1505,14 +1436,10 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, // read header ss >> numEntityBlocks >> numNodes >> minNodeTag >> maxNodeTag; ss >> entityDim >> entityTag >> parametric >> numNodesInBlock; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Nodes header"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading Nodes header"); } // check number of nodes is a positive number - if (numNodes < 0) { - throw mjCError(NULL, "Invalid number of nodes"); - } + if (numNodes < 0) { throw mjCError(NULL, "Invalid number of nodes"); } // require single block if (numEntityBlocks != 1 || numNodes != numNodesInBlock) { @@ -1520,48 +1447,38 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, } // require maximum number of nodes be equal to maximum number of nodes in a block - if (maxNodeTag != numNodesInBlock){ + if (maxNodeTag != numNodesInBlock) { throw mjCError(NULL, "Maximum number of nodes must be equal to number of nodes in a block"); } // check dimensionality and save - if (entityDim < 1 || entityDim > 3) { - throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); - } + if (entityDim < 1 || entityDim > 3) { throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); } def.spec.flex->dim = entityDim; // read and discard node tags; require range from minNodeTag to maxNodeTag - for (size_t i=0; i < numNodes; i++) { + for (size_t i = 0; i < numNodes; i++) { size_t tag; ss >> tag; - if (!ss.good()) { - throw mjCError(NULL, "Error reading node tags"); - } - if (tag != i+minNodeTag) { - throw mjCError(NULL, "Node tags must be sequential"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading node tags"); } + if (tag != i + minNodeTag) { throw mjCError(NULL, "Node tags must be sequential"); } } // read points if (numNodes < 0 || numNodes >= INT_MAX / 3) { throw mjCError(NULL, "Invalid number of nodes."); } - point.reserve(3*numNodes); - for (size_t i=0; i < 3*numNodes; i++) { + point.reserve(3 * numNodes); + for (size_t i = 0; i < 3 * numNodes; i++) { double x; ss >> x; - if (!ss.good()) { - throw mjCError(NULL, "Error reading node coordinates"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading node coordinates"); } point.push_back(x); } } // binary nodes else { // check header size - if (nodeend-nodebegin < kGmsh41HeaderSize) { - throw mjCError(NULL, "Invalid nodes header"); - } + if (nodeend - nodebegin < kGmsh41HeaderSize) { throw mjCError(NULL, "Invalid nodes header"); } // read header ReadFromBuffer(&numEntityBlocks, buffer + nodebegin); @@ -1579,50 +1496,44 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, } // check number of nodes is a positive number - if (numNodes < 0) { - throw mjCError(NULL, "Invalid number of nodes"); - } + if (numNodes < 0) { throw mjCError(NULL, "Invalid number of nodes"); } // check dimensionality and save - if (entityDim < 1 || entityDim > 3) { - throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); - } + if (entityDim < 1 || entityDim > 3) { throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); } def.spec.flex->dim = entityDim; // nodeData: node tag and 3 nodes constexpr int numNodeComponents = 4; - constexpr int componentSize = 8; - int nodeDataSize = numNodeComponents*componentSize; + constexpr int componentSize = 8; + int nodeDataSize = numNodeComponents * componentSize; // check section byte size - if (nodeend-nodebegin < kGmsh41HeaderSize + numNodes*nodeDataSize) { + if (nodeend - nodebegin < kGmsh41HeaderSize + numNodes * nodeDataSize) { throw mjCError(NULL, "Insufficient byte size of Nodes"); } // check node tags: must range from minNodeTag to maxNodeTag const char* tagbuffer = buffer + nodebegin + kGmsh41HeaderSize; - for (size_t i=0; i < numNodes; i++) { - ReadFromBuffer(&tag, tagbuffer + i*componentSize); - if (tag != i+minNodeTag) { - throw mjCError(NULL, "Node tags must be sequential"); - } + for (size_t i = 0; i < numNodes; i++) { + ReadFromBuffer(&tag, tagbuffer + i * componentSize); + if (tag != i + minNodeTag) { throw mjCError(NULL, "Node tags must be sequential"); } } // read points if (numNodes < 0 || numNodes >= INT_MAX / 3) { throw mjCError(NULL, "Invalid number of nodes."); } - point.reserve(3*numNodes); - const char* pointbuffer = buffer + nodebegin + kGmsh41HeaderSize + componentSize*numNodes; - for (size_t i=0; i < 3*numNodes; i++) { + point.reserve(3 * numNodes); + const char* pointbuffer = buffer + nodebegin + kGmsh41HeaderSize + componentSize * numNodes; + for (size_t i = 0; i < 3 * numNodes; i++) { double x; - ReadFromBuffer(&x, pointbuffer + i*componentSize); + ReadFromBuffer(&x, pointbuffer + i * componentSize); point.push_back(x); } } size_t numElements, minElementTag, maxElementTag, numElementsInBlock; - int elementType; + int elementType; // ascii elements if (binary == 0) { @@ -1633,9 +1544,7 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, // read header ss >> numEntityBlocks >> numElements >> minElementTag >> maxElementTag; ss >> entityDim >> entityTag >> elementType >> numElementsInBlock; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Elements header"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading Elements header"); } // require single block if (numEntityBlocks != 1 || numElements != numElementsInBlock) { @@ -1643,9 +1552,7 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, } // check number of elements is a positive number - if (numElements < 0) { - throw mjCError(NULL, "Invalid number of elements"); - } + if (numElements < 0) { throw mjCError(NULL, "Invalid number of elements"); } // dimensionality must be same as nodes if (entityDim != def.spec.flex->dim) { @@ -1664,16 +1571,14 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, } // read elements, discard tags - element.reserve((entityDim+1)*numElements); - for (size_t i=0; i < numElements; i++) { + element.reserve((entityDim + 1) * numElements); + for (size_t i = 0; i < numElements; i++) { size_t tag, nodeid; ss >> tag; - for (int k=0; k <= entityDim; k++) { + for (int k = 0; k <= entityDim; k++) { ss >> nodeid; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Elements"); - } - element.push_back((int)(nodeid-minNodeTag)); + if (!ss.good()) { throw mjCError(NULL, "Error reading Elements"); } + element.push_back((int)(nodeid - minNodeTag)); } } } @@ -1681,7 +1586,7 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, // binary elements else { // check header size - if (elemend-elembegin < kGmsh41HeaderSize) { + if (elemend - elembegin < kGmsh41HeaderSize) { throw mjCError(NULL, "Invalid elements header"); } @@ -1701,9 +1606,7 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, } // check number of elements is a positive number - if (numElements < 0) { - throw mjCError(NULL, "Invalid number of elements"); - } + if (numElements < 0) { throw mjCError(NULL, "Invalid number of elements"); } // dimensionality must be same as nodes if (entityDim != def.spec.flex->dim) { @@ -1722,25 +1625,25 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, } // elementData: element tag and n node tags - int numElementComponents = (entityDim+2); - constexpr int componentSize = 8; - int elementDataSize = numElementComponents*componentSize; + int numElementComponents = (entityDim + 2); + constexpr int componentSize = 8; + int elementDataSize = numElementComponents * componentSize; // check section byte size - if (elemend - elembegin < kGmsh41HeaderSize + numElements*elementDataSize) { + if (elemend - elembegin < kGmsh41HeaderSize + numElements * elementDataSize) { throw mjCError(NULL, "Insufficient byte size of Elements"); } // read elements, discard tags - element.reserve((entityDim+1)*numElements); + element.reserve((entityDim + 1) * numElements); const char* elembuffer = buffer + elembegin + kGmsh41HeaderSize; - for (size_t i=0; i < numElements; i++) { + for (size_t i = 0; i < numElements; i++) { // skip element tag elembuffer += componentSize; // read vertex ids size_t elemid; - for (int k=0; k <= entityDim; k++) { + for (int k = 0; k <= entityDim; k++) { ReadFromBuffer(&elemid, elembuffer); int elementid = elemid - minNodeTag; element.push_back(elementid); @@ -1751,10 +1654,9 @@ void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend, } - // load GMSH format 2.2 -void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, - int nodebegin, int elemend, int elembegin) { +void mjCFlexcomp::LoadGMSH22( + char* buffer, int binary, int nodeend, int nodebegin, int elemend, int elembegin) { // number of nodes size_t numNodes = 0; @@ -1762,21 +1664,17 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, if (binary == 0) { // convert node char buffer to stringstream stringstream ss(std::string(buffer + nodebegin, nodeend - nodebegin)); - std::string line; + std::string line; // checking header template std::getline(ss, line); - if (!IsValidElementOrNodeHeader22(line)) { - throw mjCError(NULL, "Invalid node header"); - } - ss.seekg(-(line.size()+1), std::ios::cur); + if (!IsValidElementOrNodeHeader22(line)) { throw mjCError(NULL, "Invalid node header"); } + ss.seekg(-(line.size() + 1), std::ios::cur); // read header size_t maxNodeTag = 0; ss >> maxNodeTag; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Nodes header"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading Nodes header"); } numNodes = maxNodeTag; if (numNodes < 0 || numNodes >= INT_MAX / 3) { @@ -1784,20 +1682,16 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, } // read points, discard tag - point.reserve(3*numNodes); - for (size_t i=0; i < numNodes; i++) { + point.reserve(3 * numNodes); + for (size_t i = 0; i < numNodes; i++) { size_t tag; double x; ss >> tag; - if (!ss.good()) { - throw mjCError(NULL, "Error reading node tags"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading node tags"); } // reading nodes - for (int k=0; k < 3; k++) { + for (int k = 0; k < 3; k++) { ss >> x; - if (!ss.good()) { - throw mjCError(NULL, "Error reading node coordinates"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading node coordinates"); } point.push_back(x); } } @@ -1810,9 +1704,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, // header size compatible with both gmshApp and Ftetwild constexpr int nodeHeaderSize = nodeHeaderSizeGmshApp - 1; // check header size - if (nodeend-nodebegin < nodeHeaderSize) { - throw mjCError(NULL, "Invalid nodes header"); - } + if (nodeend - nodebegin < nodeHeaderSize) { throw mjCError(NULL, "Invalid nodes header"); } // parse maxNodeTag and then cast it to int char maxNodeTagChar[11] = {0}; @@ -1821,23 +1713,19 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, size_t maxNodeTag; try { maxNodeTag = std::stoi(maxNodeTagChar); - } catch (const std::out_of_range& e) { - throw mjCError(NULL, "Invalid number of nodes"); - } + } catch (const std::out_of_range& e) { throw mjCError(NULL, "Invalid number of nodes"); } numNodes = maxNodeTag; // check number of nodes is a positive number - if (numNodes < 0) { - throw mjCError(NULL, "Invalid number of nodes"); - } + if (numNodes < 0) { throw mjCError(NULL, "Invalid number of nodes"); } // node data: node tag and 3 nodes - int nodeSize = sizeof(double); - int indexSize = sizeof(int); - int nodeDataSize = indexSize + 3*nodeSize; + int nodeSize = sizeof(double); + int indexSize = sizeof(int); + int nodeDataSize = indexSize + 3 * nodeSize; // check section byte size - if (nodeend - nodebegin < nodeHeaderSize + numNodes*nodeDataSize) { + if (nodeend - nodebegin < nodeHeaderSize + numNodes * nodeDataSize) { throw mjCError(NULL, "Insufficient byte size of Nodes"); } @@ -1845,16 +1733,16 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, if (numNodes < 0 || numNodes >= INT_MAX / 3) { throw mjCError(NULL, "Invalid number of nodes."); } - point.reserve(3*numNodes); + point.reserve(3 * numNodes); // beginning of buffer containing node info const char* tagBuffer = buffer + nodebegin + measuredHeaderSize; - for (int i=0; i < numNodes; i++) { + for (int i = 0; i < numNodes; i++) { int tag; - int offset = i*(sizeof(int) + sizeof(double)*3); + int offset = i * (sizeof(int) + sizeof(double) * 3); ReadFromBuffer(&tag, tagBuffer + offset); - for (int k=0; k < 3; k++) { - double x; - const char* nodeBuffer = tagBuffer + sizeof(int) + sizeof(double)*k; + for (int k = 0; k < 3; k++) { + double x; + const char* nodeBuffer = tagBuffer + sizeof(int) + sizeof(double) * k; ReadFromBuffer(&x, nodeBuffer + offset); point.push_back(x); } @@ -1867,20 +1755,16 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, // convert element char buffer to stringstream buffer[elemend] = 0; stringstream ss(std::string(buffer + elembegin, elemend - elembegin)); - std::string line; + std::string line; // checking header template std::getline(ss, line); - if (!IsValidElementOrNodeHeader22(line)) { - throw mjCError(NULL, "Invalid elements header"); - } - ss.seekg(-(line.size()+1), std::ios::cur); + if (!IsValidElementOrNodeHeader22(line)) { throw mjCError(NULL, "Invalid elements header"); } + ss.seekg(-(line.size() + 1), std::ios::cur); // read header size_t maxElementTag = 0; ss >> maxElementTag; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Elements header"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading Elements header"); } size_t numElements = maxElementTag; if (numElements < 0 || numElements >= INT_MAX / 4) { @@ -1889,62 +1773,48 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, // check number of elements is a positive number - if (numElements < 0) { - throw mjCError(NULL, "Invalid number of elements"); - } + if (numElements < 0) { throw mjCError(NULL, "Invalid number of elements"); } // reading first element's type int tag = 0, elementType = 0, numTags = 0; ss >> tag >> elementType >> numTags; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Elements"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading Elements"); } - size_t entityDim = 0; - int numNodeTags = 0; + size_t entityDim = 0; + int numNodeTags = 0; // surface if (elementType == 2) { - entityDim = 2; + entityDim = 2; numNodeTags = 3; } // tetrahedral else if (elementType == 4) { - entityDim = 3; + entityDim = 3; numNodeTags = 4; } - if (numNodeTags < 1 || numNodeTags > 4) { - throw mjCError(NULL, "Invalid number of node tags"); - } + if (numNodeTags < 1 || numNodeTags > 4) { throw mjCError(NULL, "Invalid number of node tags"); } // setting entityDim def.spec.flex->dim = entityDim; // read elements, discard all tags - element.reserve(numNodeTags*numElements); - for (size_t i=0; i < numElements; i++) { + element.reserve(numNodeTags * numElements); + for (size_t i = 0; i < numElements; i++) { int nodeTag = 0, physicalEntityTag = 0, elementModelEntityTag = 0; if (i != 0) { ss >> tag >> elementType >> numTags; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Elements"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading Elements"); } } if (numTags > 0) { ss >> physicalEntityTag >> elementModelEntityTag; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Elements"); - } + if (!ss.good()) { throw mjCError(NULL, "Error reading Elements"); } } - for (int k=0; k < numNodeTags; k++) { + for (int k = 0; k < numNodeTags; k++) { ss >> nodeTag; - if (!ss.good()) { - throw mjCError(NULL, "Error reading Elements"); - } - if (nodeTag > numNodes || nodeTag < 1) { - throw mjCError(NULL, "Invalid node tag"); - } - element.push_back((int)(nodeTag-1)); + if (!ss.good()) { throw mjCError(NULL, "Error reading Elements"); } + if (nodeTag > numNodes || nodeTag < 1) { throw mjCError(NULL, "Invalid node tag"); } + element.push_back((int)(nodeTag - 1)); } } } @@ -1966,64 +1836,57 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, int maxElementTag; try { maxElementTag = std::stoi(maxElementTagChar); - } catch (const std::out_of_range& e) { - throw mjCError(NULL, "Invalid number of elements"); - } + } catch (const std::out_of_range& e) { throw mjCError(NULL, "Invalid number of elements"); } int numElements = maxElementTag; int tag, numTags; int nodeTag; int elementType; // check number of elements is a positive number - if (numElements < 0) { - throw mjCError(NULL, "Invalid number of elements"); - } + if (numElements < 0) { throw mjCError(NULL, "Invalid number of elements"); } // size of single component in element data int componentSize = sizeof(int); // element buffer const char* elementsBuffer = buffer + elembegin + measuredHeaderSize; ReadFromBuffer(&elementType, elementsBuffer); - ReadFromBuffer(&numTags, elementsBuffer + componentSize*2); - ReadFromBuffer(&tag, elementsBuffer + componentSize*3); + ReadFromBuffer(&numTags, elementsBuffer + componentSize * 2); + ReadFromBuffer(&tag, elementsBuffer + componentSize * 3); // tetrahedral has 4 node tags and surface has 3 - int numNodeTags = 0; - size_t entityDim = 0; + int numNodeTags = 0; + size_t entityDim = 0; // surface if (elementType == 2) { - entityDim = 2; + entityDim = 2; numNodeTags = 3; } // tetrahedral else if (elementType == 4) { - entityDim = 3; + entityDim = 3; numNodeTags = 4; } - if (numNodeTags < 1 || numNodeTags > 4) { - throw mjCError(NULL, "Invalid number of node tags"); - } + if (numNodeTags < 1 || numNodeTags > 4) { throw mjCError(NULL, "Invalid number of node tags"); } def.spec.flex->dim = entityDim; // element data(Ftetwild): tag and 4 nodeTag constexpr int numComponentsFtetwild = 5; // element data(gmshApp): 4 Info components, 2 entity tag and entityDim+1 nodeTags - constexpr int numInfoComponents = 4; + constexpr int numInfoComponents = 4; constexpr int numEntityTagComponents = 2; int numComponentsGmshApp = numInfoComponents + numEntityTagComponents + numNodeTags; // single element data size - int elementDataSizeFtetwild = numComponentsFtetwild*componentSize; - int elementDataSizeGmshApp = numComponentsGmshApp*componentSize; + int elementDataSizeFtetwild = numComponentsFtetwild * componentSize; + int elementDataSizeGmshApp = numComponentsGmshApp * componentSize; // elements section buffer size - int elementsBufferSizeFtetwild = elementHeaderSizeFtetwild + - numElements*elementDataSizeFtetwild; - int elementsBufferSizeGmshApp = elementHeaderSizeGmshApp + - numElements*elementDataSizeGmshApp; + int elementsBufferSizeFtetwild = + elementHeaderSizeFtetwild + numElements * elementDataSizeFtetwild; + int elementsBufferSizeGmshApp = elementHeaderSizeGmshApp + numElements * elementDataSizeGmshApp; // check section byte size for ftetwild if (elemend - elembegin < elementsBufferSizeFtetwild) { @@ -2038,28 +1901,24 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, } // read first element - for (int k =0; k < numNodeTags; k++) { - ReadFromBuffer(&nodeTag, elementsBuffer + componentSize*(6+k)); - if (nodeTag > numNodes || nodeTag < 1) { - throw mjCError(NULL, "Invalid node tag"); - } - element.push_back(nodeTag-1); + for (int k = 0; k < numNodeTags; k++) { + ReadFromBuffer(&nodeTag, elementsBuffer + componentSize * (6 + k)); + if (nodeTag > numNodes || nodeTag < 1) { throw mjCError(NULL, "Invalid node tag"); } + element.push_back(nodeTag - 1); } // read every other element - for (int i=1; i < numElements; i++) { - const char* numTagsBuffer = elementsBuffer + componentSize*2; - const char* tagBuffer = elementsBuffer + componentSize*3; - int offset = i*elementDataSizeGmshApp; + for (int i = 1; i < numElements; i++) { + const char* numTagsBuffer = elementsBuffer + componentSize * 2; + const char* tagBuffer = elementsBuffer + componentSize * 3; + int offset = i * elementDataSizeGmshApp; ReadFromBuffer(&numTags, numTagsBuffer + offset); - ReadFromBuffer(&tag, tagBuffer+offset); - for (int k =0; k < numNodeTags; k++) { - const char* nodeTagBuffer = elementsBuffer + componentSize*(6+k); + ReadFromBuffer(&tag, tagBuffer + offset); + for (int k = 0; k < numNodeTags; k++) { + const char* nodeTagBuffer = elementsBuffer + componentSize * (6 + k); ReadFromBuffer(&nodeTag, nodeTagBuffer + offset); - if (nodeTag > numElements || nodeTag < 1) { - throw mjCError(NULL, "Invalid node tag"); - } - element.push_back(nodeTag-1); + if (nodeTag > numElements || nodeTag < 1) { throw mjCError(NULL, "Invalid node tag"); } + element.push_back(nodeTag - 1); } } } @@ -2068,26 +1927,22 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, else { // read first element for (int k = 0; k < numNodeTags; k++) { - const char* nodeTagBuffer = elementsBuffer + componentSize*(4+k); + const char* nodeTagBuffer = elementsBuffer + componentSize * (4 + k); ReadFromBuffer(&nodeTag, nodeTagBuffer); - if (nodeTag > numNodes || nodeTag < 1) { - throw mjCError(NULL, "Invalid node tag"); - } - element.push_back(nodeTag-1); + if (nodeTag > numNodes || nodeTag < 1) { throw mjCError(NULL, "Invalid node tag"); } + element.push_back(nodeTag - 1); } // read every other element - for (int i=0; i < numElements-1; i++) { - int offset = componentSize*(4+2) + i*elementDataSizeFtetwild; - const char* tagBuffer = elementsBuffer + componentSize*2; + for (int i = 0; i < numElements - 1; i++) { + int offset = componentSize * (4 + 2) + i * elementDataSizeFtetwild; + const char* tagBuffer = elementsBuffer + componentSize * 2; ReadFromBuffer(&tag, tagBuffer + offset); - for (int k=0; k < numNodeTags; k++) { - const char* nodeTagBuffer = elementsBuffer + componentSize*(3+k); + for (int k = 0; k < numNodeTags; k++) { + const char* nodeTagBuffer = elementsBuffer + componentSize * (3 + k); ReadFromBuffer(&nodeTag, nodeTagBuffer + offset); - if (nodeTag > numElements || nodeTag < 1) { - throw mjCError(NULL, "Invalid node tag"); - } - element.push_back(nodeTag-1); + if (nodeTag > numElements || nodeTag < 1) { throw mjCError(NULL, "Invalid node tag"); } + element.push_back(nodeTag - 1); } } } @@ -2095,12 +1950,11 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend, } - // load GMSH file from resource void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) { // get buffer from resource - char* buffer = 0; - int buffer_sz = mju_readResource(resource, (const void**) &buffer); + char* buffer = 0; + int buffer_sz = mju_readResource(resource, (const void**)&buffer); // check buffer if (buffer_sz < 0) { @@ -2113,19 +1967,19 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) { // check version, determine ascii or binary double version; - int binary; + int binary; if (sscanf(buffer + 11, "%lf %d", &version, &binary) != 2) { throw mjCError(NULL, "Could not read GMSH file header"); } - if (mju_round(100*version) != 220 && mju_round(100*version) != 410) { + if (mju_round(100 * version) != 220 && mju_round(100 * version) != 410) { throw mjCError(NULL, "Only GMSH file format versions 4.1 and 2.2 are supported"); } // find section begin/end int nodebegin = findstring(buffer, buffer_sz, "$Nodes"); - int nodeend = findstring(buffer, buffer_sz, "$EndNodes"); + int nodeend = findstring(buffer, buffer_sz, "$EndNodes"); int elembegin = findstring(buffer, buffer_sz, "$Elements"); - int elemend = findstring(buffer, buffer_sz, "$EndElements"); + int elemend = findstring(buffer, buffer_sz, "$EndElements"); // correct begin for string size, +1 for LF in binary (CRLF in Win ascii works) @@ -2133,26 +1987,20 @@ void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) { elembegin += (int)strlen("$Elements") + 1; // check sections - if (nodebegin < 0) { - throw mjCError(NULL, "GMSH file missing $Nodes"); - } - if (nodeend < nodebegin) { - throw mjCError(NULL, "GMSH file missing $EndNodes after $Nodes"); - } - if (elembegin < 0) { - throw mjCError(NULL, "GMSH file missing $Elements"); - } + if (nodebegin < 0) { throw mjCError(NULL, "GMSH file missing $Nodes"); } + if (nodeend < nodebegin) { throw mjCError(NULL, "GMSH file missing $EndNodes after $Nodes"); } + if (elembegin < 0) { throw mjCError(NULL, "GMSH file missing $Elements"); } if (elemend < elembegin) { throw mjCError(NULL, "GMSH file missing $EndElements after $Elements"); } // Support for 4.1 - if (mju_round(100*version) == 410) { + if (mju_round(100 * version) == 410) { LoadGMSH41(buffer, binary, nodeend, nodebegin, elemend, elembegin); } // Support for 2.2 - else if (mju_round(100*version) == 220) { + else if (mju_round(100 * version) == 220) { LoadGMSH22(buffer, binary, nodeend, nodebegin, elemend, elembegin); } else { throw mjCError(NULL, "Unsupported GMSH file format version"); diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 782a7915..70f6941d 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -59,35 +59,39 @@ class mjCFlexcomp { bool MakeGrid(char* error, int error_sz); bool MakeBox(char* error, int error_sz, int dim, bool open = true); 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); - 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); void LoadGMSH(mjCModel* model, mjResource* resource); - void LoadGMSH41(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); + void LoadGMSH41(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, int iz); - int BoxID(int ix, int iy, int iz); + int GridID(int ix, int iy); + int GridID(int ix, int iy, int iz); + int BoxID(int ix, int iy, int iz); void BoxProject(double* pos, int ix, int iy, int iz); // common properties set by user - std::string name; // flex name - mjtFcompType type; // flexcomp type - int count[3]; // grid count in each dimension - int cellcount[3]; // number of cells for interpolation - double spacing[3]; // spacing between grid elements - double scale[3]; // scaling for mesh and direct - double origin[3]; // origin for generating a 3D mesh from a convex 2D mesh - double mass; // total mass of auto-generated bodies - double inertiabox; // size of inertia box for each body - int equality; // create equality constraint, 0:none, 1:edge, 2:vert, 3:strain - std::string file; // mesh/gmsh file name - mjtDof doftype; // dof type, all vertices or trilinear interpolation + std::string name; // flex name + mjtFcompType type; // flexcomp type + int count[3]; // grid count in each dimension + int cellcount[3]; // number of cells for interpolation + double spacing[3]; // spacing between grid elements + double scale[3]; // scaling for mesh and direct + double origin[3]; // origin for generating a 3D mesh from a convex 2D mesh + double mass; // total mass of auto-generated bodies + double inertiabox; // size of inertia box for each body + int equality; // create equality constraint, 0:none, 1:edge, 2:vert, 3:strain + std::string file; // mesh/gmsh file name + mjtDof doftype; // dof type, all vertices or trilinear interpolation // pin specifications std::vector pinid; // ids of points to pin @@ -96,32 +100,37 @@ class mjCFlexcomp { std::vector pingridrange; // range of grid coordinates to pin // all other properties - mjCDef def; // local copy, parsed parameters stored here + mjCDef def; // local copy, parsed parameters stored here // pose transform relative to parent body - double pos[3]; // position - double quat[4]; // orientation - mjsOrientation alt; // alternative orientation + double pos[3]; // position + double quat[4]; // orientation + mjsOrientation alt; // alternative orientation // set by user or computed internally - bool rigid; // all vertices are in parent body (all pinned) - bool centered; // all vertex coordinates are (0,0,0) (nothing pinned) - std::vector point; // flex bodies/vertices - std::vector pinned; // is point pinned (true: no new body) - std::vector used; // is point used by any element (false: skip) - std::vector element; // flex elements - std::vector texcoord; // vertex texture coordinates - std::vector elemtexcoord; // face texture coordinates (OBJ only) + bool rigid; // all vertices are in parent body (all pinned) + bool centered; // all vertex coordinates are (0,0,0) (nothing pinned) + std::vector point; // flex bodies/vertices + std::vector pinned; // is point pinned (true: no new body) + std::vector used; // is point used by any element (false: skip) + std::vector element; // flex elements + std::vector texcoord; // vertex texture coordinates + std::vector elemtexcoord; // face texture coordinates (OBJ only) // plugin support std::string plugin_name; std::string plugin_instance_name; - mjsPlugin plugin; + mjsPlugin plugin; private: // identify empty cells and pin nodes exclusively in empty cells - void MarkEmptyCells(mjCFlex* flex, const double* points, int npnt, - const double minmax[6], int nx, int ny, int nz); + void MarkEmptyCells(mjCFlex* flex, + const double* points, + int npnt, + const double minmax[6], + int nx, + int ny, + int nz); }; #endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ diff --git a/src/user/user_threadpool.cc b/src/user/user_threadpool.cc index 1fcfa1aa..664a8e66 100644 --- a/src/user/user_threadpool.cc +++ b/src/user/user_threadpool.cc @@ -35,14 +35,10 @@ ThreadPool::ThreadPool(int num_threads) : ctr_(0) { ThreadPool::~ThreadPool() { { std::unique_lock lock(m_); - for (int i = 0; i < threads_.size(); i++) { - queue_.push(nullptr); - } + for (int i = 0; i < threads_.size(); i++) { queue_.push(nullptr); } cv_in_.notify_all(); } - for (auto& thread : threads_) { - thread.join(); - } + for (auto& thread : threads_) { thread.join(); } } // ThreadPool scheduler diff --git a/src/user/user_threadpool.h b/src/user/user_threadpool.h index 99354136..5761ff45 100644 --- a/src/user/user_threadpool.h +++ b/src/user/user_threadpool.h @@ -72,12 +72,12 @@ class ThreadPool { constinit static thread_local int worker_id_; // ----- members ----- // - std::vector threads_; - std::mutex m_; - std::condition_variable cv_in_; - std::condition_variable cv_ext_; + std::vector threads_; + std::mutex m_; + std::condition_variable cv_in_; + std::condition_variable cv_ext_; std::queue> queue_; - std::uint64_t ctr_; + std::uint64_t ctr_; }; } // namespace mujoco::user diff --git a/src/user/user_util.cc b/src/user/user_util.cc index 570ee96f..c94908d0 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -36,7 +36,7 @@ // workaround with locale bug on some MacOS machines -#if defined (__APPLE__) && defined (__MACH__) +#if defined(__APPLE__) && defined(__MACH__) #include #include @@ -52,17 +52,15 @@ bool mjuu_defined(double num) { // compute address of M[g1][g2] where M is triangular n-by-n int mjuu_matadr(int g1, int g2, int n) { - if (g1 < 0 || g2 < 0 || g1 >= n || g2 >= n) { - return -1; - } + if (g1 < 0 || g2 < 0 || g1 >= n || g2 >= n) { return -1; } if (g1 > g2) { int tmp = g1; - g1 = g2; - g2 = tmp; + g1 = g2; + g2 = tmp; } - return g1*n + g2; + return g1 * n + g2; } @@ -103,43 +101,37 @@ void mjuu_setvec(double* dest, double x, double y) { // add to double array void mjuu_addtovec(double* dest, const double* src, int n) { - for (int i=0; i < n; i++) { - dest[i] += src[i]; - } + for (int i = 0; i < n; i++) { dest[i] += src[i]; } } // zero double array void mjuu_zerovec(double* dest, int n) { - for (int i=0; i < n; i++) { - dest[i] = 0; - } + for (int i = 0; i < n; i++) { dest[i] = 0; } } // zero float array void mjuu_zerovec(float* dest, int n) { - for (int i=0; i < n; i++) { - dest[i] = 0; - } + for (int i = 0; i < n; i++) { dest[i] = 0; } } // dot-product in 3D double mjuu_dot3(const double* a, const double* b) { - return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; } // 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])); + 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 double mjuu_L1(const double* a, const double* b, int n) { double res = 0; - for (int i=0; i < n; i++) { - res += std::abs(a[i]-b[i]); - } + for (int i = 0; i < n; i++) { res += std::abs(a[i] - b[i]); } 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 nrm = 0; - for (int i=0; i < n; i++) { - nrm += vec[i]*vec[i]; - } - if (nrm < mjEPS) { - return 0; - } + for (int i = 0; i < n; i++) { nrm += vec[i] * vec[i]; } + if (nrm < mjEPS) { return 0; } nrm = sqrt(nrm); // don't normalize if nrm is within mjEPS of 1 if (std::abs(nrm - 1) > mjEPS) { - for (int i=0; i < n; i++) { - vec[i] /= nrm; - } + for (int i = 0; i < n; i++) { vec[i] /= nrm; } } return nrm; @@ -173,20 +159,14 @@ double mjuu_normvec(double* vec, const int n) { float mjuu_normvec(float* vec, const int n) { float nrm = 0; - for (int i=0; i < n; i++) { - nrm += vec[i]*vec[i]; - } - if (nrm < mjEPS) { - return 0; - } + for (int i = 0; i < n; i++) { nrm += vec[i] * vec[i]; } + if (nrm < mjEPS) { return 0; } nrm = sqrt(nrm); // don't normalize if nrm is within mjEPS of 1 if (std::abs(nrm - 1) > mjEPS) { - for (int i=0; i < n; i++) { - vec[i] /= nrm; - } + for (int i = 0; i < n; i++) { vec[i] /= nrm; } } return nrm; @@ -194,9 +174,7 @@ float mjuu_normvec(float* vec, const int n) { // scale vector by scalar void mjuu_scalevec(double* res, const double* vec, double s, int n) { - for (int i = 0; i < n; i++) { - res[i] = s * vec[i]; - } + for (int i = 0; i < n; i++) { res[i] = s * vec[i]; } } // convert quaternion to rotation matrix @@ -216,38 +194,37 @@ void mjuu_quat2mat(double* res, const double* quat) { } // regular processing - double q00 = quat[0]*quat[0]; - double q01 = quat[0]*quat[1]; - double q02 = quat[0]*quat[2]; - double q03 = quat[0]*quat[3]; - double q11 = quat[1]*quat[1]; - double q12 = quat[1]*quat[2]; - double q13 = quat[1]*quat[3]; - double q22 = quat[2]*quat[2]; - double q23 = quat[2]*quat[3]; - double q33 = quat[3]*quat[3]; + double q00 = quat[0] * quat[0]; + double q01 = quat[0] * quat[1]; + double q02 = quat[0] * quat[2]; + double q03 = quat[0] * quat[3]; + double q11 = quat[1] * quat[1]; + double q12 = quat[1] * quat[2]; + double q13 = quat[1] * quat[3]; + double q22 = quat[2] * quat[2]; + double q23 = quat[2] * quat[3]; + double q33 = quat[3] * quat[3]; res[0] = q00 + q11 - q22 - q33; res[4] = q00 - q11 + q22 - q33; res[8] = q00 - q11 - q22 + q33; - res[1] = 2*(q12 - q03); - res[2] = 2*(q13 + q02); - res[3] = 2*(q12 + q03); - res[5] = 2*(q23 - q01); - res[6] = 2*(q13 - q02); - res[7] = 2*(q23 + q01); - + res[1] = 2 * (q12 - q03); + res[2] = 2 * (q13 + q02); + res[3] = 2 * (q12 + q03); + res[5] = 2 * (q23 - q01); + res[6] = 2 * (q13 - q02); + res[7] = 2 * (q23 + q01); } // multiply two unit quaternions void mjuu_mulquat(double* res, const double* qa, const double* qb) { double tmp[4]; - tmp[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3]; - tmp[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2]; - tmp[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1]; - tmp[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0]; + tmp[0] = qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2] - qa[3] * qb[3]; + tmp[1] = qa[0] * qb[1] + qa[1] * qb[0] + qa[2] * qb[3] - qa[3] * qb[2]; + tmp[2] = qa[0] * qb[2] - qa[1] * qb[3] + qa[2] * qb[0] + qa[3] * qb[1]; + tmp[3] = qa[0] * qb[3] + qa[1] * qb[2] - qa[2] * qb[1] + qa[3] * qb[0]; mjuu_normvec(tmp, 4); mjuu_copyvec(res, tmp, 4); } @@ -255,29 +232,28 @@ void mjuu_mulquat(double* res, const double* qa, const double* qb) { // multiply matrix by vector, 3-by-3 void mjuu_mulvecmat(double* res, const double* vec, const double* mat) { - double tmp[3] = { - 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[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2] - }; - res[0] = tmp[0]; - res[1] = tmp[1]; - res[2] = tmp[2]; + double tmp[3] = {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[6] * vec[0] + mat[7] * vec[1] + mat[8] * vec[2]}; + + res[0] = tmp[0]; + res[1] = tmp[1]; + res[2] = tmp[2]; } // multiply transposed matrix by vector, 3-by-3 void mjuu_mulvecmatT(double* res, const double* vec, const double* mat) { - double tmp[3] = { - 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[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2] - }; - res[0] = tmp[0]; - res[1] = tmp[1]; - res[2] = tmp[2]; + double tmp[3] = {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[2] * vec[0] + mat[5] * vec[1] + mat[8] * vec[2]}; + + res[0] = tmp[0]; + res[1] = tmp[1]; + res[2] = tmp[2]; } +// clang-format off // compute res = R * M * R' void mjuu_mulRMRT(double* res, const double* R, const double* M) { 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]; mjuu_copyvec(res, tmp, 9); } - +// clang-format on // transpose 3-by-3 matrix void mjuu_transposemat(double* res, const double* mat) { - double tmp[9] = {mat[0], mat[3], mat[6], - mat[1], mat[4], mat[7], - mat[2], mat[5], mat[8]}; + double tmp[9] = {mat[0], mat[3], mat[6], mat[1], mat[4], mat[7], mat[2], mat[5], mat[8]}; mjuu_copyvec(res, tmp, 9); } @@ -344,7 +318,7 @@ void mjuu_localaxis(double* al, const double* ag, const double* quat) { // convert global to local position relative to given frame void mjuu_localpos(double* pl, const double* pg, const double* pos, const double* quat) { - double a[3] = {pg[0]-pos[0], pg[1]-pos[1], pg[2]-pos[2]}; + double a[3] = {pg[0] - pos[0], pg[1] - pos[1], pg[2] - pos[2]}; mjuu_localaxis(pl, a, quat); } @@ -356,22 +330,24 @@ void mjuu_localquat(double* local, const double* child, const double* parent) { } +// clang-format off // compute vector cross-product a = b x c void mjuu_crossvec(double* a, const double* b, const double* c) { a[0] = b[1]*c[2] - b[2]*c[1]; a[1] = b[2]*c[0] - b[0]*c[2]; a[2] = b[0]*c[1] - b[1]*c[0]; } +// clang-format on // compute normal vector to given triangle, return length -template 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 v2[3] = {b[0], b[1], b[2]}; - double v3[3] = {c[0], c[1], c[2]}; - double diffAB[3] = {v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]}; - double diffAC[3] = {v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2]}; +template +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 v2[3] = {b[0], b[1], b[2]}; + double v3[3] = {c[0], c[1], c[2]}; + double diffAB[3] = {v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]}; + double diffAC[3] = {v3[0] - v1[0], v3[1] - v1[1], v3[2] - v1[2]}; mjuu_crossvec(normal, diffAB, diffAC); double nrm = std::sqrt(mjuu_dot3(normal, normal)); @@ -387,25 +363,29 @@ template double mjuu_makenormal(double* normal, const T a[3], return nrm; } -template double mjuu_makenormal(double* normal, const double a[3], - const double b[3], const double c[3]); -template double mjuu_makenormal(double* normal, const float a[3], - const float b[3], const float c[3]); +template double mjuu_makenormal(double* normal, + const double a[3], + const double b[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 void mjuu_z2quat(double* quat, const double* vec) { double z[3] = {0, 0, 1}; - mjuu_crossvec(quat+1, z, vec); - double s = mjuu_normvec(quat+1, 3); + mjuu_crossvec(quat + 1, z, vec); + double s = mjuu_normvec(quat + 1, 3); if (s < 1E-10) { quat[1] = 1; quat[2] = quat[3] = 0; } - double ang = atan2(s, vec[2]); - quat[0] = cos(ang/2); - quat[1] *= sin(ang/2); - quat[2] *= sin(ang/2); - quat[3] *= sin(ang/2); + double ang = atan2(s, vec[2]); + quat[0] = cos(ang / 2); + quat[1] *= sin(ang / 2); + quat[2] *= sin(ang / 2); + quat[3] *= sin(ang / 2); } @@ -414,7 +394,7 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl const double* mat[3] = {x, y, z}; // mat[c][r] indexing // q0 largest - if (mat[0][0]+mat[1][1]+mat[2][2] > 0) { + if (mat[0][0] + mat[1][1] + mat[2][2] > 0) { quat[0] = 0.5 * sqrt(1 + mat[0][0] + mat[1][1] + mat[2][2]); quat[1] = 0.25 * (mat[1][2] - mat[2][1]) / quat[0]; quat[2] = 0.25 * (mat[2][0] - mat[0][2]) / quat[0]; @@ -450,8 +430,10 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl // invert frame transformation -void mjuu_frameinvert(double newpos[3], double newquat[4], - const double oldpos[3], const double oldquat[4]) { +void mjuu_frameinvert(double newpos[3], + double newquat[4], + const double oldpos[3], + const double oldquat[4]) { // position mjuu_localaxis(newpos, oldpos, oldquat); newpos[0] = -newpos[0]; @@ -467,8 +449,10 @@ void mjuu_frameinvert(double newpos[3], double newquat[4], // accumulate frame transformations (forward kinematics) -void mjuu_frameaccum(double pos[3], double quat[4], - const double childpos[3], const double childquat[4]) { +void mjuu_frameaccum(double pos[3], + double quat[4], + const double childpos[3], + const double childquat[4]) { double mat[9], vec[3], qtmp[4]; mjuu_quat2mat(mat, quat); mjuu_mulvecmat(vec, childpos, mat); @@ -481,8 +465,10 @@ void mjuu_frameaccum(double pos[3], double quat[4], // accumulate frame transformation in second frame -void mjuu_frameaccumChild(const double pos[3], const double quat[4], - double childpos[3], double childquat[4]) { +void mjuu_frameaccumChild(const double pos[3], + const double quat[4], + double childpos[3], + double childquat[4]) { double p[] = {pos[0], pos[1], pos[2]}; double q[] = {quat[0], quat[1], quat[2], quat[3]}; mjuu_frameaccum(p, q, childpos, childquat); @@ -492,8 +478,10 @@ void mjuu_frameaccumChild(const double pos[3], const double quat[4], // invert frame accumulation -void mjuu_frameaccuminv(double pos[3], double quat[4], - const double childpos[3], const double childquat[4]) { +void mjuu_frameaccuminv(double pos[3], + double quat[4], + const double childpos[3], + const double childquat[4]) { double mat[9], vec[3], qtmp[4]; double qneg[4] = {childquat[0], -childquat[1], -childquat[2], -childquat[3]}; 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] void mjuu_globalinertia(double* global, const double* local, const double* quat) { double mat[9]; @@ -524,17 +513,18 @@ 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[5] = mat[3]*tmp[2] + mat[4]*tmp[5] + mat[5]*tmp[8]; } +// clang-format on // compute off-center correction to inertia matrix // mass * [y^2+z^2, -x*y, -x*z; -x*y, x^2+z^2, -y*z; -x*z, -y*z, x^2+y^2] void mjuu_offcenter(double* res, const double mass, const double* vec) { - res[0] = mass*(vec[1]*vec[1] + vec[2]*vec[2]); - res[1] = mass*(vec[0]*vec[0] + vec[2]*vec[2]); - res[2] = mass*(vec[0]*vec[0] + vec[1]*vec[1]); - res[3] = -mass*vec[0]*vec[1]; - res[4] = -mass*vec[0]*vec[2]; - res[5] = -mass*vec[1]*vec[2]; + res[0] = mass * (vec[1] * vec[1] + vec[2] * vec[2]); + res[1] = mass * (vec[0] * vec[0] + vec[2] * vec[2]); + res[2] = mass * (vec[0] * vec[0] + vec[1] * vec[1]); + res[3] = -mass * vec[0] * vec[1]; + res[4] = -mass * vec[0] * vec[2]; + res[5] = -mass * vec[1] * vec[2]; } @@ -549,14 +539,17 @@ void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double // apply formula for box (or rather cross) viscosity // torque components - visccoef[0] = scl * 4.0 / 3.0 * ebox[0] * (ebox[1]*ebox[1]*ebox[1] + ebox[2]*ebox[2]*ebox[2]); - 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]); + visccoef[0] = + scl * 4.0 / 3.0 * ebox[0] * (ebox[1] * ebox[1] * ebox[1] + ebox[2] * ebox[2] * ebox[2]); + 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 - visccoef[3] = scl * 4*ebox[1]*ebox[2]; - visccoef[4] = scl * 4*ebox[0]*ebox[2]; - visccoef[5] = scl * 4*ebox[0]*ebox[1]; + visccoef[3] = scl * 4 * ebox[1] * ebox[2]; + visccoef[4] = scl * 4 * ebox[0] * ebox[2]; + visccoef[5] = scl * 4 * ebox[0] * ebox[1]; } @@ -572,11 +565,11 @@ static void mjuu_axisAngle2Quat(double res[4], const double axis[3], double angl // regular processing else { - double s = sin(angle*0.5); - res[0] = cos(angle*0.5); - res[1] = axis[0]*s; - res[2] = axis[1]*s; - res[3] = axis[2]*s; + double s = sin(angle * 0.5); + res[0] = cos(angle * 0.5); + res[1] = axis[0] * s; + res[2] = axis[1] * s; + res[3] = axis[2] * s; } } @@ -595,16 +588,14 @@ void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]) { // regular processing else { // tmp = q_w * v + cross(q_xyz, v) - double tmp[3] = { - 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[2] + quat[1]*vec[1] - quat[2]*vec[0] - }; + double tmp[3] = {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[2] + quat[1] * vec[1] - quat[2] * vec[0]}; // res = v + 2 * cross(q_xyz, t) - res[0] = vec[0] + 2 * (quat[2]*tmp[2] - quat[3]*tmp[1]); - res[1] = vec[1] + 2 * (quat[3]*tmp[0] - quat[1]*tmp[2]); - res[2] = vec[2] + 2 * (quat[1]*tmp[1] - quat[2]*tmp[0]); + res[0] = vec[0] + 2 * (quat[2] * tmp[2] - quat[3] * tmp[1]); + res[1] = vec[1] + 2 * (quat[3] * tmp[0] - quat[1] * tmp[2]); + res[2] = vec[2] + 2 * (quat[1] * tmp[1] - quat[2] * tmp[0]); } } @@ -618,8 +609,12 @@ void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]) { // outputs: // quat - frame orientation // normal - unit normal vector -double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3], - const double tprv[3], const double tnxt[3], int first) { +double mjuu_updateFrame(double quat[4], + double normal[3], + const double edge[3], + const double tprv[3], + const double tnxt[3], + int first) { double tangent[3], binormal[3]; // normalize tangent @@ -662,14 +657,14 @@ static const double kEigEPS = 1E-12; int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]) { double D[9], tmp[9], tmp2[9]; double tau, t, c; - int iter, rk, ck, rotk; + int iter, rk, ck, rotk; // initialize with unit quaternion quat[0] = 1; quat[1] = quat[2] = quat[3] = 0; // Jacobi iteration - for (iter=0; iter < 500; iter++) { + for (iter = 0; iter < 500; iter++) { // make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec mjuu_quat2mat(eigvec, quat); mjuu_transposemat(tmp2, eigvec); @@ -683,45 +678,39 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m // find max off-diagonal element, set indices if (std::abs(D[1]) > std::abs(D[2]) && std::abs(D[1]) > std::abs(D[5])) { - rk = 0; // row - ck = 1; // column - rotk = 2; // rotation axis + rk = 0; // row + ck = 1; // column + rotk = 2; // rotation axis } else if (std::abs(D[2]) > std::abs(D[5])) { - rk = 0; - ck = 2; + rk = 0; + ck = 2; rotk = 1; } else { - rk = 1; - ck = 2; + rk = 1; + ck = 2; rotk = 0; } // terminate if max off-diagonal element too small - if (std::abs(D[3*rk+ck]) < kEigEPS) { - break; - } + if (std::abs(D[3 * rk + ck]) < kEigEPS) { break; } // 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]); if (tau >= 0) { - t = 1.0/(tau + sqrt(1 + tau*tau)); + t = 1.0 / (tau + sqrt(1 + tau * tau)); } else { - t = -1.0/(-tau + sqrt(1 + tau*tau)); + t = -1.0 / (-tau + sqrt(1 + tau * tau)); } - c = 1.0/sqrt(1 + t*t); + c = 1.0 / sqrt(1 + t * t); // terminate if cosine too close to 1 - if (c > 1.0-kEigEPS) { - break; - } + if (c > 1.0 - kEigEPS) { break; } // express rotation as quaternion tmp[1] = tmp[2] = tmp[3] = 0; - tmp[rotk+1] = (tau >= 0 ? -sqrt(0.5-0.5*c) : sqrt(0.5-0.5*c)); - if (rotk == 1) { - tmp[rotk+1] = -tmp[rotk+1]; - } - tmp[0] = sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]); + tmp[rotk + 1] = (tau >= 0 ? -sqrt(0.5 - 0.5 * c) : sqrt(0.5 - 0.5 * c)); + if (rotk == 1) { tmp[rotk + 1] = -tmp[rotk + 1]; } + tmp[0] = sqrt(1.0 - tmp[rotk + 1] * tmp[rotk + 1]); mjuu_normvec(tmp, 4); // accumulate quaternion rotation @@ -730,20 +719,20 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m } // sort eigenvalues in decreasing order (bubblesort: 0, 1, 0) - for (int j=0; j < 3; j++) { - int j1 = j%2; // lead index + for (int j = 0; j < 3; j++) { + int j1 = j % 2; // lead index // only swap if the eigenvalues are different - if (eigval[j1]+kEigEPS < eigval[j1+1]) { + if (eigval[j1] + kEigEPS < eigval[j1 + 1]) { // swap eigenvalues - t = eigval[j1]; - eigval[j1] = eigval[j1+1]; - eigval[j1+1] = t; + t = eigval[j1]; + eigval[j1] = eigval[j1 + 1]; + eigval[j1 + 1] = t; // rotate quaternion - tmp[0] = 0.707106781186548; // cos(pi/4) = sin(pi/4) + tmp[0] = 0.707106781186548; // cos(pi/4) = sin(pi/4) tmp[1] = tmp[2] = tmp[3] = 0; - tmp[(j1+2)%3+1] = tmp[0]; + tmp[(j1 + 2) % 3 + 1] = tmp[0]; mjuu_mulquat(quat, quat, tmp); mjuu_normvec(quat, 4); } @@ -762,76 +751,71 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m // The input matrix mat is destroyed. int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) { // initialize eigvec to identity - std::fill(eigvec, eigvec + n*n, 0.0); - for (int i = 0; i < n; i++) { - eigvec[i*n + i] = 1.0; - } + std::fill(eigvec, eigvec + n * n, 0.0); + for (int i = 0; i < n; i++) { eigvec[i * n + i] = 1.0; } - const int max_sweeps = 200; - const double tol = 1e-12; + const int max_sweeps = 200; + const double tol = 1e-12; int sweep; for (sweep = 0; sweep < max_sweeps; sweep++) { // check convergence: sum of squared off-diagonal elements double off_diag = 0; for (int i = 0; i < n; i++) { - for (int j = i+1; j < n; j++) { - off_diag += mat[i*n + j] * mat[i*n + j]; - } + for (int j = i + 1; j < n; j++) { off_diag += mat[i * n + j] * mat[i * n + j]; } } if (off_diag < tol * tol) break; // sweep over all off-diagonal pairs for (int p = 0; p < n; p++) { - for (int q = p+1; q < n; q++) { - double apq = mat[p*n + q]; + for (int q = p + 1; q < n; q++) { + double apq = mat[p * n + q]; if (std::abs(apq) < tol * 1e-3) continue; // compute rotation angle - double app = mat[p*n + p]; - double aqq = mat[q*n + q]; + double app = mat[p * n + p]; + double aqq = mat[q * n + q]; double tau = (aqq - app) / (2.0 * apq); - double t = (tau >= 0 ? 1.0 : -1.0) / - (std::abs(tau) + std::sqrt(1.0 + tau*tau)); - double c = 1.0 / std::sqrt(1.0 + t*t); - double s = t * c; + double t = (tau >= 0 ? 1.0 : -1.0) / (std::abs(tau) + std::sqrt(1.0 + tau * tau)); + double c = 1.0 / std::sqrt(1.0 + t * t); + double s = t * c; // update matrix (Jacobi rotation) - mat[p*n + p] -= t * apq; - mat[q*n + q] += t * apq; - mat[p*n + q] = 0; - mat[q*n + p] = 0; + mat[p * n + p] -= t * apq; + mat[q * n + q] += t * apq; + mat[p * n + q] = 0; + mat[q * n + p] = 0; for (int r = 0; r < n; r++) { if (r == p || r == q) continue; - double mrp = mat[r*n + p]; - double mrq = mat[r*n + q]; - mat[r*n + p] = mat[p*n + r] = c*mrp - s*mrq; - mat[r*n + q] = mat[q*n + r] = s*mrp + c*mrq; + double mrp = mat[r * n + p]; + double mrq = mat[r * n + q]; + mat[r * n + p] = mat[p * n + r] = c * mrp - s * mrq; + mat[r * n + q] = mat[q * n + r] = s * mrp + c * mrq; } // accumulate eigenvectors for (int r = 0; r < n; r++) { - double vrp = eigvec[r*n + p]; - double vrq = eigvec[r*n + q]; - eigvec[r*n + p] = c*vrp - s*vrq; - eigvec[r*n + q] = s*vrp + c*vrq; + double vrp = eigvec[r * n + p]; + double vrq = eigvec[r * n + q]; + eigvec[r * n + p] = c * vrp - s * vrq; + eigvec[r * n + q] = s * vrp + c * vrq; } } } } // extract eigenvalues from diagonal - for (int i = 0; i < n; i++) { - eigval[i] = mat[i*n + i]; - } + for (int i = 0; i < n; i++) { eigval[i] = mat[i * n + i]; } return sweep; } // 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]) { // res = quat*vec + pos mjuu_rotVecQuat(res, vec, quat); @@ -852,39 +836,35 @@ std::string mjuu_strippath(std::string filename) { // return name without path else { - return filename.substr(start+1, filename.size()-start-1); + return filename.substr(start + 1, filename.size() - start - 1); } } // 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]) { - if (!mjuu_defined(fullinertia[0])) { - return nullptr; - } + if (!mjuu_defined(fullinertia[0])) { return nullptr; } double eigval[3], eigvec[9], quattmp[4]; - double full[9] = { - fullinertia[0], fullinertia[3], fullinertia[4], - fullinertia[3], fullinertia[1], fullinertia[5], - fullinertia[4], fullinertia[5], fullinertia[2] - }; + double full[9] = {fullinertia[0], + fullinertia[3], + fullinertia[4], + fullinertia[3], + fullinertia[1], + fullinertia[5], + fullinertia[4], + fullinertia[5], + fullinertia[2]}; mjuu_eig3(eigval, eigvec, quattmp, full); // check mimimal eigenvalue - if (eigval[2] < mjEPS) { - return "inertia must have positive eigenvalues"; - } + if (eigval[2] < mjEPS) { return "inertia must have positive eigenvalues"; } // copy - if (quat) { - mjuu_copyvec(quat, quattmp, 4); - } + if (quat) { mjuu_copyvec(quat, quattmp, 4); } - if (inertia) { - mjuu_copyvec(inertia, eigval, 3); - } + if (inertia) { mjuu_copyvec(inertia, eigval, 3); } return nullptr; } @@ -896,9 +876,7 @@ std::string mjuu_stripext(std::string filename) { size_t end = filename.find_last_of('.'); // no path found: return original - if (end == std::string::npos) { - return filename; - } + if (end == std::string::npos) { return filename; } // return name without extension return filename.substr(0, end); @@ -907,9 +885,7 @@ std::string mjuu_stripext(std::string filename) { std::string mjuu_getext(std::string_view filename) { size_t dot = filename.find_last_of('.'); - if (dot == std::string::npos) { - return ""; - } + if (dot == std::string::npos) { return ""; } 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 bool mjuu_isabspath(std::string path) { // empty: not absolute - if (path.empty()) { - return false; - } + if (path.empty()) { return false; } // path is scheme:filename which we consider an absolute path // e.g. file URI's are always absolute paths - if (mjp_getResourceProvider(path.c_str()) != nullptr) { - return true; - } + if (mjp_getResourceProvider(path.c_str()) != nullptr) { return true; } // check first char const char* str = path.c_str(); - if (str[0] == '\\' || str[0] == '/') { - return true; - } + if (str[0] == '\\' || str[0] == '/') { return true; } // find ":/" or ":\" - if (path.find(":/") != std::string::npos || - path.find(":\\") != std::string::npos) { + if (path.find(":/") != std::string::npos || path.find(":\\") != std::string::npos) { return true; } @@ -943,92 +912,69 @@ bool mjuu_isabspath(std::string path) { } - // assemble two file paths std::string mjuu_combinePaths(const std::string& path1, const std::string& path2) { // path2 has absolute path - if (mjuu_isabspath(path2)) { - return path2; - } + if (mjuu_isabspath(path2)) { return path2; } std::size_t n = path1.size(); - if (n > 0 && path1[n - 1] != '\\' && path1[n - 1] != '/') { - return path1 + "/" + path2; - } + if (n > 0 && path1[n - 1] != '\\' && path1[n - 1] != '/') { return path1 + "/" + path2; } return path1 + path2; } - // 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) { return mjuu_combinePaths(path1, mjuu_combinePaths(path2, path3)); } - // return true if the text is in a valid content type format: // {type}/{subtype}[;{parameter}={value}] static bool mjuu_isValidContentType(std::string_view text) { // find a forward slash that's not the last character size_t n = text.find('/'); - if (n == std::string::npos || n == text.size() - 1) { - return false; - } + if (n == std::string::npos || n == text.size() - 1) { return false; } size_t m = text.find(';'); - if (m == std::string::npos) { - return true; - } + if (m == std::string::npos) { return true; } - if (m + 1 <= n) { - return false; - } + if (m + 1 <= n) { return false; } // 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 // function size_t s = text.find('='); - if (s == std::string::npos || s + 1 <= m) { - return false; - } + if (s == std::string::npos || s + 1 <= m) { return false; } return true; } - // return type from content_type format {type}/{subtype}[;{parameter}={value}] // return empty string on invalid format std::optional mjuu_parseContentTypeAttrType(std::string_view text) { - if (!mjuu_isValidContentType(text)) { - return std::nullopt; - } + if (!mjuu_isValidContentType(text)) { 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 empty string on invalid format std::optional mjuu_parseContentTypeAttrSubtype(std::string_view text) { - if (!mjuu_isValidContentType(text)) { - return std::nullopt; - } + if (!mjuu_isValidContentType(text)) { return std::nullopt; } size_t n = text.find('/'); size_t m = text.find(';', n + 1); - if (m == std::string::npos) { - return { text.substr(n+1) }; - } + if (m == std::string::npos) { 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 std::string mjuu_extToContentType(std::string_view 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 int mjuu_dirnamelen(const char* path) { - if (!path) { - return 0; - } + if (!path) { return 0; } int pos = -1; for (int i = 0; path[i]; ++i) { - if (path[i] == '/' || path[i] == '\\') { - pos = i; - } + if (path[i] == '/' || path[i] == '\\') { pos = i; } } return pos + 1; @@ -1070,27 +1012,23 @@ namespace mujoco::user { std::string FilePath::Combine(const std::string& s1, const std::string& s2) { // str2 has absolute path - if (!AbsPrefix(s2).empty()) { - return s2; - } + if (!AbsPrefix(s2).empty()) { return s2; } std::size_t n = s1.size(); - if (n > 0 && s1[n - 1] != '\\' && s1[n - 1] != '/') { - return s1 + "/" + s2; - } + if (n > 0 && s1[n - 1] != '\\' && s1[n - 1] != '/') { return s1 + "/" + s2; } return s1 + s2; } std::string FilePath::PathReduce(const std::string& str) { std::vector dirs; - std::string abs_prefix = AbsPrefix(str); + std::string abs_prefix = AbsPrefix(str); int j = abs_prefix.size(); for (int i = j; i < str.size(); ++i) { if (IsSeparator(str[i])) { std::string temp = str.substr(j, i - j); - j = i + 1; + j = i + 1; if (temp == ".." && !dirs.empty() && dirs.back() != "..") { dirs.pop_back(); } else if (temp != ".") { @@ -1104,11 +1042,9 @@ std::string FilePath::PathReduce(const std::string& str) { // join the path std::stringstream path; - auto it = dirs.begin(); + auto it = dirs.begin(); path << abs_prefix << *it++; - for (; it != dirs.end(); ++it) { - path << "/" << *it; - } + for (; it != dirs.end(); ++it) { path << "/" << *it; } return path.str(); } @@ -1119,9 +1055,7 @@ FilePath FilePath::operator+(const FilePath& path) const { std::string FilePath::Ext() const { std::size_t n = path_.find_last_of('.'); - if (n == std::string::npos) { - return ""; - } + if (n == std::string::npos) { return ""; } return path_.substr(n, path_.size() - n); } @@ -1129,9 +1063,7 @@ FilePath FilePath::StripExt() const { size_t n = path_.find_last_of('.'); // no extension - if (n == std::string::npos) { - return FilePathFast(path_); - } + if (n == std::string::npos) { return FilePathFast(path_); } // return path without extension return FilePathFast(path_.substr(0, n)); @@ -1140,9 +1072,7 @@ FilePath FilePath::StripExt() const { // is directory absolute path std::string FilePath::AbsPrefix(const std::string& str) { // empty: not absolute - if (str.empty()) { - return ""; - } + if (str.empty()) { return ""; } // path is scheme:filename which we consider an absolute path // e.g. file URI's are always absolute paths @@ -1153,20 +1083,14 @@ std::string FilePath::AbsPrefix(const std::string& str) { } // check first char - if (str[0] == '\\' || str[0] == '/') { - return str.substr(0, 1); - } + if (str[0] == '\\' || str[0] == '/') { return str.substr(0, 1); } // find ":/" or ":\" std::size_t pos = str.find(":/"); - if (pos != std::string::npos) { - return str.substr(0, pos + 2); - } + if (pos != std::string::npos) { return str.substr(0, pos + 2); } pos = str.find(":\\"); - if (pos != std::string::npos) { - return str.substr(0, pos + 2); - } + if (pos != std::string::npos) { return str.substr(0, pos + 2); } return ""; } @@ -1176,28 +1100,23 @@ FilePath FilePath::StripPath() const { std::size_t n = path_.find_last_of("/\\"); // no path - if (n == std::string::npos) { - return FilePathFast(path_); - } + if (n == std::string::npos) { return FilePathFast(path_); } return FilePathFast(path_.substr(n + 1, path_.size() - (n + 1))); } std::string FilePath::StrLower() const { std::string str = path_; - std::transform(str.begin(), str.end(), str.begin(), - [](unsigned char c) { - return std::tolower(c); - }); + std::transform(str.begin(), str.end(), str.begin(), [](unsigned char c) { + return std::tolower(c); + }); return str; } // read file into memory buffer std::vector FileToMemory(const char* filename) { FILE* fp = fopen(filename, "rb"); - if (!fp) { - return {}; - } + if (!fp) { return {}; } // find size if (fseek(fp, 0, SEEK_END) != 0) { @@ -1253,12 +1172,11 @@ std::vector FileToMemory(const char* filename) { } // convert vector to string separating elements by whitespace -template std::string VectorToString(const std::vector& v) { +template +std::string VectorToString(const std::vector& v) { std::stringstream ss; - for (const T& t : v) { - ss << t << " "; - } + for (const T& t : v) { ss << t << " "; } std::string s = ss.str(); if (!s.empty()) s.pop_back(); // remove trailing space @@ -1272,27 +1190,32 @@ template MJAPI std::string VectorToString(const std::vector& v); namespace { -template T StrToNum(char* str, char** c); +template +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); if (n < INT_MIN || n > INT_MAX) errno = ERANGE; return n; } -template<> float StrToNum(char* str, char** c) { +template <> +float StrToNum(char* str, char** c) { float f = strtof(str, c); if (std::isnan(f)) errno = EDOM; return f; } -template<> double StrToNum(char* str, char** c) { +template <> +double StrToNum(char* str, char** c) { double d = strtod(str, c); if (std::isnan(d)) errno = EDOM; 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); if (n < 0 || n > UCHAR_MAX) errno = ERANGE; return n; @@ -1304,17 +1227,16 @@ inline bool IsNullOrSpace(char* c) { inline char* SkipSpace(char* c) { for (; *c != '\0'; c++) { - if (!IsNullOrSpace(c)) { - break; - } + if (!IsNullOrSpace(c)) { break; } } return c; } } // namespace -template std::vector StringToVector(char* cs) { +template +std::vector StringToVector(char* cs) { std::vector v; - char* ch = cs; + char* ch = cs; errno = 0; // reserve worst case @@ -1334,31 +1256,32 @@ template std::vector StringToVector(char* cs) { return v; } -template<> MJAPI std::vector StringToVector(char* cs) { +template <> +MJAPI std::vector StringToVector(char* cs) { return StringToVector(std::string(cs)); } -template<> MJAPI std::vector StringToVector(const std::string& s) { +template <> +MJAPI std::vector StringToVector(const std::string& s) { std::vector v; - std::stringstream ss(s); - std::string word; - while (ss >> word) { - v.push_back(word); - } + std::stringstream ss(s); + std::string word; + while (ss >> word) { v.push_back(word); } return v; } -template MJAPI std::vector StringToVector(char* cs); -template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); template MJAPI std::vector StringToVector(char* cs); template MJAPI std::vector StringToVector(char* cs); -template std::vector StringToVector(const std::string& s) { +template +std::vector StringToVector(const std::string& s) { return StringToVector(const_cast(s.c_str())); } -template MJAPI std::vector StringToVector(const std::string& s); -template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); template MJAPI std::vector StringToVector(const std::string& s); template MJAPI std::vector StringToVector(const std::string& s); diff --git a/src/user/user_util.h b/src/user/user_util.h index 65a21a7b..0d3ffa67 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -28,8 +28,8 @@ #include -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(double num); @@ -76,7 +76,7 @@ double mjuu_L1(const double* a, const double* b, int n); // normalize vector to unit length, return previous length // if norm(vec) double mjuu_makenormal(double* normal, const T a[3], - const T b[3], const T c[3]); +template +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 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); // invert frame transformation -void mjuu_frameinvert(double newpos[3], double newquat[4], - const double oldpos[3], const double oldquat[4]); +void mjuu_frameinvert(double newpos[3], + double newquat[4], + const double oldpos[3], + const double oldquat[4]); // accumulate frame transformation into parent frame -void mjuu_frameaccum(double pos[3], double quat[4], - const double childpos[3], const double childquat[4]); +void mjuu_frameaccum(double pos[3], + double quat[4], + const double childpos[3], + const double childquat[4]); // accumulate frame transformation into child frame -void mjuu_frameaccumChild(const double pos[3], const double quat[4], - double childpos[3], double childquat[4]); +void mjuu_frameaccumChild(const double pos[3], + const double quat[4], + double childpos[3], + double childquat[4]); // invert frame accumulation -void mjuu_frameaccuminv(double pos[3], double quat[4], - const double childpos[3], const double childquat[4]); +void mjuu_frameaccuminv(double pos[3], + double quat[4], + const double childpos[3], + const double childquat[4]); // convert local_inertia[3] to global_inertia[6] void mjuu_globalinertia(double* global, const double* local, const double* quat); @@ -147,14 +155,18 @@ void mjuu_globalinertia(double* global, const double* local, const double* quat) 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); +void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double scl = 1); // rotate vector by quaternion 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 -double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3], - const double tprv[3], const double tnxt[3], int first); +double mjuu_updateFrame(double quat[4], + double normal[3], + const double edge[3], + const double tprv[3], + const double tnxt[3], + int first); // eigenvalue decomposition of symmetric 3x3 matrix int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]); @@ -182,10 +194,10 @@ class MJAPI FilePath { FilePath(const std::string& str1, const std::string& str2) { path_ = PathReduce(Combine(str1, str2)); } - FilePath(FilePath&& other) = default; + FilePath(FilePath&& other) = default; FilePath& operator=(FilePath&& other) = default; - FilePath(const FilePath&) = default; - FilePath& operator=(const FilePath&) = default; + FilePath(const FilePath&) = default; + FilePath& operator=(const FilePath&) = default; // return true if the path is absolute bool IsAbs() const { return !AbsPrefix(path_).empty(); } @@ -219,15 +231,13 @@ class MJAPI FilePath { // C++ string methods std::size_t size() const { return path_.size(); } const char* c_str() const { return path_.c_str(); } - bool empty() const { return path_.empty(); } - char operator[](int i) const { return path_[i]; } + bool empty() const { return path_.empty(); } + char operator[](int i) const { return path_[i]; } private: static std::string AbsPrefix(const std::string& str); static std::string PathReduce(const std::string& str); - static bool IsSeparator(char c) { - return c == '/' || c == '\\'; - } + static bool IsSeparator(char c) { return c == '/' || c == '\\'; } static std::string Combine(const std::string& s1, const std::string& s2); // fast constructor that does not call PathReduce @@ -249,8 +259,11 @@ class MJAPI FilePath { // utility class for scoping resources to functions struct Cleanup { using Fn = std::function; - ~Cleanup() { for (auto& f : cleanup) f(); } + ~Cleanup() { + for (auto& f : cleanup) f(); + } void operator+=(Fn f) { cleanup.push_front(std::move(f)); } + std::deque cleanup; }; @@ -259,13 +272,18 @@ struct Cleanup { std::vector FileToMemory(const char* filename); // convert vector to string separating elements by whitespace -template MJAPI std::string VectorToString(const std::vector& v); +template +MJAPI std::string VectorToString(const std::vector& v); // convert string to vector -template MJAPI std::vector StringToVector(char *cs); -template MJAPI std::vector StringToVector(const std::string& s); -template<> MJAPI std::vector StringToVector(char* cs); -template<> MJAPI std::vector StringToVector(const std::string& s); +template +MJAPI std::vector StringToVector(char* cs); +template +MJAPI std::vector StringToVector(const std::string& s); +template <> +MJAPI std::vector StringToVector(char* cs); +template <> +MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user @@ -283,7 +301,8 @@ bool mjuu_isabspath(std::string path); // 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, const std::string& path3); // return type from content_type format {type}/{subtype}[;{parameter}={value}]