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

Cosmetic improvements to src/user/ source files.

Browse Source 
PiperOrigin-RevId: 737930914
Change-Id: I1cc254d420fcdf0c42ec3d2f601e5a02d925d958
This commit is contained in:
Yuval Tassa
2025-03-18 02:58:23 -07:00
committed by Copybara-Service
parent bae5175cf2
commit 205599ac1a
10 changed files with 1580 additions and 1553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+4 -4
View File
@@ -108,7 +108,7 @@ mjModel* mj_compile(mjSpec* s, const mjVFS* vfs) {
if (d) {
modelC->MakeData(m, &d);
modelC->RestoreState(state_name, m->qpos0, m->body_pos, m->body_quat, d->qpos, d->qvel,
d->act, d->ctrl, d->mocap_pos, d->mocap_quat);
d->act, d->ctrl, d->mocap_pos, d->mocap_quat);
d->time = time;
}
} catch (mjCError& e) {
@@ -166,7 +166,7 @@ mjsFrame* mjs_attachFrame(mjsBody* parent, const mjsFrame* child,
// attach child body to a parent site
mjsBody* mjs_attachToSite(mjsSite* parent, const mjsBody* child,
const char* prefix, const char* suffix) {
const char* prefix, const char* suffix) {
if (!parent) {
mju_error("parent site is null");
return nullptr;
@@ -1178,8 +1178,8 @@ const double* mjs_getDouble(const mjDoubleVec* source, int* size) {
// set plugin attributes
void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes) {
mjCPlugin* pluginC = static_cast<mjCPlugin*>(plugin->element);
std::map<std::string, std::string, std::less<>>* config_attribs =
reinterpret_cast<std::map<std::string, std::string, std::less<>>*>(attributes);
std::map<std::string, std::string, std::less<> >* config_attribs =
reinterpret_cast<std::map<std::string, std::string, std::less<> >*>(attributes);
pluginC->config_attribs = std::move(*config_attribs);
}
src/user/user_cache.cc
+4 -4
View File
@@ -172,8 +172,8 @@ std::size_t mjCCache::MaxSize() const {
std::size_t mjCCache::Size() const {
std::lock_guard<std::mutex> lock(mutex_);
return size_;
std::lock_guard<std::mutex> lock(mutex_);
return size_;
}
@@ -183,7 +183,7 @@ void mjCCache::DeleteAsset(const std::string& id) {
std::lock_guard<std::mutex> lock(mutex_);
auto it = lookup_.find(id);
if (it != lookup_.end()) {
Delete(&(it->second));
Delete(&(it->second));
}
}
@@ -209,7 +209,7 @@ void mjCCache::Delete(mjCAsset* asset, const std::string& skip) {
for (auto& reference : asset->References()) {
if (reference != skip) {
models_[reference].erase(asset);
}
}
}
lookup_.erase(asset->Id());
}
src/user/user_composite.cc
+136 -135
View File
@@ -28,6 +28,7 @@
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include "cc/array_safety.h"
#include "engine/engine_io.h"
#include "engine/engine_util_blas.h"
@@ -82,7 +83,7 @@ mjCComposite::mjCComposite(void) {
skingroup = 0;
// clear add flags
for (int i=0; i<mjNCOMPKINDS; i++) {
for (int i=0; i < mjNCOMPKINDS; i++) {
add[i] = false;
}
@@ -94,8 +95,8 @@ mjCComposite::mjCComposite(void) {
// create the array of default joint options, append new elements only for particles type
bool mjCComposite::AddDefaultJoint(char* error, int error_sz) {
for (int i=0; i<mjNCOMPKINDS; i++) {
if (!defjoint[(mjtCompKind)i].empty() && type!=mjCOMPTYPE_PARTICLE) {
for (int i=0; i < mjNCOMPKINDS; i++) {
if (!defjoint[(mjtCompKind)i].empty() && type != mjCOMPTYPE_PARTICLE) {
comperr(error, "Only particles are allowed to have multiple joints", error_sz);
return false;
} else {
@@ -112,7 +113,7 @@ bool mjCComposite::AddDefaultJoint(char* error, int error_sz) {
// set defaults, after reading top-level info and skin
void mjCComposite::SetDefault(void) {
// set all default groups to 3
for (int i=0; i<mjNCOMPKINDS; i++) {
for (int i=0; i < mjNCOMPKINDS; i++) {
def[i].spec.geom->group = 3;
def[i].spec.site->group = 3;
def[i].spec.tendon->group = 3;
@@ -122,8 +123,8 @@ void mjCComposite::SetDefault(void) {
AddDefaultJoint();
// set default geom and tendon group to 0 if needed to be visible
if (!skin || type==mjCOMPTYPE_CABLE) {
for (int i=0; i<mjNCOMPKINDS; i++) {
if (!skin || type == mjCOMPTYPE_CABLE) {
for (int i=0; i < mjNCOMPKINDS; i++) {
def[i].spec.geom->group = 0;
def[i].spec.tendon->group = 0;
}
@@ -137,20 +138,20 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz)
mjCModel* model = (mjCModel*)spec->element;
// check counts
for (int i=0; i<3; i++) {
if (count[i]<1) {
for (int i=0; i < 3; i++) {
if (count[i] < 1) {
return comperr(error, "Positive counts expected in composite", error_sz);
}
}
// check cable sizes are nonzero if vertices are not prescribed
if (mjuu_dot3(size, size)<mjMINVAL && uservert.empty()) {
if (mjuu_dot3(size, size) < mjMINVAL && uservert.empty()) {
return comperr(error, "Positive spacing or length expected in composite", error_sz);
}
// check either uservert or count but not both
if (!uservert.empty()) {
if (count[0]>1) {
if (count[0] > 1) {
return comperr(error, "Either vertex or count can be specified, not both", error_sz);
}
count[0] = uservert.size()/3;
@@ -159,8 +160,8 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz)
// determine dimensionality, check singleton order
bool first = false;
for (int i=0; i<3; i++) {
if (count[i]==1) {
for (int i=0; i < 3; i++) {
if (count[i] == 1) {
first = true;
} else {
dim++;
@@ -180,8 +181,8 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz)
vertweight.clear();
// require 3x3 for subgrid
if (skin && skinsubgrid>0 && type!=mjCOMPTYPE_CABLE) {
if (count[0]<3 || count[1]<3) {
if (skin && skinsubgrid > 0 && type != mjCOMPTYPE_CABLE) {
if (count[0] < 3 || count[1] < 3) {
return comperr(error, "At least 3x3 required for skin subgrid", error_sz);
}
}
@@ -205,41 +206,41 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz)
// dispatch
switch (type) {
case mjCOMPTYPE_PARTICLE:
return comperr(error,
"The \"particle\" composite type is deprecated. Please use "
"\"replicate\" instead.",
error_sz);
case mjCOMPTYPE_PARTICLE:
return comperr(error,
"The \"particle\" composite type is deprecated. Please use "
"\"replicate\" instead.",
error_sz);
case mjCOMPTYPE_GRID:
return comperr(error,
"The \"grid\" composite type is deprecated. Please use "
"\"flex\" instead.",
error_sz);
case mjCOMPTYPE_GRID:
return comperr(error,
"The \"grid\" composite type is deprecated. Please use "
"\"flex\" instead.",
error_sz);
case mjCOMPTYPE_ROPE:
return comperr(error,
"The \"rope\" composite type is deprecated. Please use "
"\"cable\" instead.",
error_sz);
case mjCOMPTYPE_ROPE:
return comperr(error,
"The \"rope\" composite type is deprecated. Please use "
"\"cable\" instead.",
error_sz);
case mjCOMPTYPE_LOOP:
return comperr(error,
"The \"loop\" composite type is deprecated. Please use "
"\"flexcomp\" instead.",
error_sz);
case mjCOMPTYPE_LOOP:
return comperr(error,
"The \"loop\" composite type is deprecated. Please use "
"\"flexcomp\" instead.",
error_sz);
case mjCOMPTYPE_CABLE:
return MakeCable(model, body, error, error_sz);
case mjCOMPTYPE_CABLE:
return MakeCable(model, body, error, error_sz);
case mjCOMPTYPE_CLOTH:
return comperr(error,
"The \"cloth\" composite type is deprecated. Please use "
"\"shell\" instead.",
error_sz);
case mjCOMPTYPE_CLOTH:
return comperr(error,
"The \"cloth\" composite type is deprecated. Please use "
"\"shell\" instead.",
error_sz);
default:
return comperr(error, "Unknown shape in composite", error_sz);
default:
return comperr(error, "Unknown shape in composite", error_sz);
}
}
@@ -247,14 +248,14 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz)
bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz) {
// check dim
if (dim!=1) {
if (dim != 1) {
return comperr(error, "Cable must be one-dimensional", error_sz);
}
// check geom type
if (def[0].spec.geom->type!=mjGEOM_CYLINDER &&
def[0].spec.geom->type!=mjGEOM_CAPSULE &&
def[0].spec.geom->type!=mjGEOM_BOX) {
if (def[0].spec.geom->type != mjGEOM_CYLINDER &&
def[0].spec.geom->type != mjGEOM_CAPSULE &&
def[0].spec.geom->type != mjGEOM_BOX) {
return comperr(error, "Cable geom type must be sphere, capsule or box", error_sz);
}
@@ -265,25 +266,25 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er
// populate uservert if not specified
if (uservert.empty()) {
for (int ix=0; ix<count[0]; ix++) {
for (int k=0; k<3; k++) {
for (int ix=0; ix < count[0]; ix++) {
for (int k=0; k < 3; k++) {
switch (curve[k]) {
case mjCOMPSHAPE_LINE:
uservert.push_back(ix*size[0]/(count[0]-1));
break;
case mjCOMPSHAPE_COS:
uservert.push_back(size[1]*cos(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_SIN:
uservert.push_back(size[1]*sin(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_ZERO:
uservert.push_back(0);
break;
default:
// SHOULD NOT OCCUR
mju_error("Invalid composite shape: %d", curve[k]);
break;
case mjCOMPSHAPE_LINE:
uservert.push_back(ix*size[0]/(count[0]-1));
break;
case mjCOMPSHAPE_COS:
uservert.push_back(size[1]*cos(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_SIN:
uservert.push_back(size[1]*sin(mjPI*ix*size[2]/(count[0]-1)));
break;
case mjCOMPSHAPE_ZERO:
uservert.push_back(0);
break;
default:
// SHOULD NOT OCCUR
mju_error("Invalid composite shape: %d", curve[k]);
break;
}
}
}
@@ -295,13 +296,13 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er
mjuu_setvec(prev_quat, 1, 0, 0, 0);
// add one body after the other
for (int ix=0; ix<count[0]-1; ix++) {
for (int ix=0; ix < count[0]-1; ix++) {
body = AddCableBody(model, body, ix, normal, prev_quat);
}
// add skin
if (def[0].spec.geom->type==mjGEOM_BOX) {
if (skinsubgrid>0) {
if (def[0].spec.geom->type == mjGEOM_BOX) {
if (skinsubgrid > 0) {
count[1]+=2;
MakeSkin2Subgrid(model, 2*def[0].spec.geom->size[2]);
count[1]-=2;
@@ -324,9 +325,9 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
// set flags
int lastidx = count[0]-2;
bool first = ix==0;
bool last = ix==lastidx;
bool secondlast = ix==lastidx-1;
bool first = ix == 0;
bool last = ix == lastidx;
bool secondlast = ix == lastidx-1;
// compute edge and tangent vectors
double edge[3], tprev[3], tnext[3], length_prev = 0;
@@ -391,11 +392,11 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
mjs_setDefault(geom->element, mjs_getDefault(body->element));
mjs_setString(geom->name, txt_geom);
if (def[0].spec.geom->type==mjGEOM_CYLINDER ||
def[0].spec.geom->type==mjGEOM_CAPSULE) {
if (def[0].spec.geom->type == mjGEOM_CYLINDER ||
def[0].spec.geom->type == mjGEOM_CAPSULE) {
mjuu_zerovec(geom->fromto, 6);
geom->fromto[3] = length;
} else if (def[0].spec.geom->type==mjGEOM_BOX) {
} else if (def[0].spec.geom->type == mjGEOM_BOX) {
mjuu_zerovec(geom->pos, 3);
geom->pos[0] = length/2;
geom->size[0] = length/2;
@@ -417,10 +418,10 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
if (!first || strcmp(initial.c_str(), "none")) {
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
jnt->type = (first && strcmp(initial.c_str(), "free")==0) ? mjJNT_FREE : mjJNT_BALL;
jnt->damping = jnt->type==mjJNT_FREE ? 0 : jnt->damping;
jnt->armature = jnt->type==mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type==mjJNT_FREE ? 0 : jnt->frictionloss;
jnt->type = (first && strcmp(initial.c_str(), "free") == 0) ? mjJNT_FREE : mjJNT_BALL;
jnt->damping = jnt->type == mjJNT_FREE ? 0 : jnt->damping;
jnt->armature = jnt->type == mjJNT_FREE ? 0 : jnt->armature;
jnt->frictionloss = jnt->type == mjJNT_FREE ? 0 : jnt->frictionloss;
mjs_setString(jnt->name, this_joint);
}
@@ -453,10 +454,10 @@ void mjCComposite::CopyIntoSkin(mjsSkin* skin) {
mjs_setFloat(skin->bindquat, bindquat.data(), bindquat.size());
mjs_setFloat(skin->texcoord, texcoord.data(), texcoord.size());
for (int i=0; i<vertid.size(); i++) {
for (int i=0; i < vertid.size(); i++) {
mjs_appendIntVec(skin->vertid, vertid[i].data(), vertid[i].size());
}
for (int i=0; i< vertweight.size(); i++) {
for (int i=0; i < vertweight.size(); i++) {
mjs_appendFloatVec(skin->vertweight, vertweight[i].data(), vertweight[i].size());
}
@@ -486,9 +487,9 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
skin->group = skingroup;
// populate mesh: two sides
for (int i=0; i<2; i++) {
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int i=0; i < 2; i++) {
for (int ix=0; ix < count[0]; ix++) {
for (int iy=0; iy < count[1]; iy++) {
// vertex
vert.push_back(0);
vert.push_back(0);
@@ -501,21 +502,21 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
}
// face
if (ix<count[0]-1 && iy<count[1]-1) {
if (ix < count[0]-1 && iy < count[1]-1) {
face.push_back(i*N + ix*count[1]+iy);
face.push_back(i*N + (ix+1)*count[1]+iy+(i==1));
face.push_back(i*N + (ix+1)*count[1]+iy+(i==0));
face.push_back(i*N + (ix+1)*count[1]+iy+(i == 1));
face.push_back(i*N + (ix+1)*count[1]+iy+(i == 0));
face.push_back(i*N + ix*count[1]+iy);
face.push_back(i*N + (ix+(i==0))*count[1]+iy+1);
face.push_back(i*N + (ix+(i==1))*count[1]+iy+1);
face.push_back(i*N + (ix+(i == 0))*count[1]+iy+1);
face.push_back(i*N + (ix+(i == 1))*count[1]+iy+1);
}
}
}
}
// add thin triangles: X direction, iy = 0
for (int ix=0; ix<count[0]-1; ix++) {
for (int ix=0; ix < count[0]-1; ix++) {
face.push_back(ix*count[1]);
face.push_back(N + (ix+1)*count[1]);
face.push_back((ix+1)*count[1]);
@@ -526,7 +527,7 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
}
// add thin triangles: X direction, iy = count[1]-1
for (int ix=0; ix<count[0]-1; ix++) {
for (int ix=0; ix < count[0]-1; ix++) {
face.push_back(ix*count[1] + count[1]-1);
face.push_back((ix+1)*count[1] + count[1]-1);
face.push_back(N + (ix+1)*count[1] + count[1]-1);
@@ -537,7 +538,7 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
}
// add thin triangles: Y direction, ix = 0
for (int iy=0; iy<count[1]-1; iy++) {
for (int iy=0; iy < count[1]-1; iy++) {
face.push_back(iy);
face.push_back(iy+1);
face.push_back(N + iy+1);
@@ -548,7 +549,7 @@ void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
}
// add thin triangles: Y direction, ix = count[0]-1
for (int iy=0; iy<count[1]-1; iy++) {
for (int iy=0; iy < count[1]-1; iy++) {
face.push_back(iy + (count[0]-1)*count[1]);
face.push_back(N + iy+1 + (count[0]-1)*count[1]);
face.push_back(iy+1 + (count[0]-1)*count[1]);
@@ -573,28 +574,28 @@ void mjCComposite::MakeCableBones(mjCModel* model, mjsSkin* skin) {
int N = count[0]*count[1];
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int ix=0; ix < count[0]; ix++) {
for (int iy=0; iy < count[1]; iy++) {
// body name
if (ix==0) {
if (ix == 0) {
mju::sprintf_arr(this_body, "%sB_first", prefix.c_str());
} else if (ix>=count[0]-2) {
} else if (ix >= count[0]-2) {
mju::sprintf_arr(this_body, "%sB_last", prefix.c_str());
} else {
mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
}
// bind pose
if (iy==0) {
if (iy == 0) {
mjs_appendString(skin->bodyname, this_body);
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(-def[0].spec.geom->size[1]);
bindpos.push_back(0);
bindquat.push_back(1); bindquat.push_back(0);
bindquat.push_back(0); bindquat.push_back(0);
} else {
mjs_appendString(skin->bodyname, this_body);
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(def[0].spec.geom->size[1]);
bindpos.push_back(0);
bindquat.push_back(1); bindquat.push_back(0);
@@ -612,30 +613,30 @@ void mjCComposite::MakeCableBones(mjCModel* model, mjsSkin* skin) {
void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) {
// populate bones
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int ix=0; ix < count[0]; ix++) {
for (int iy=0; iy < count[1]; iy++) {
char txt[100];
// body name
if (ix==0) {
if (ix == 0) {
mju::sprintf_arr(txt, "%sB_first", prefix.c_str());
} else if (ix>=count[0]-2) {
} else if (ix >= count[0]-2) {
mju::sprintf_arr(txt, "%sB_last", prefix.c_str());
} else {
mju::sprintf_arr(txt, "%sB_%d", prefix.c_str(), ix);
}
// bind pose
if (iy==0) {
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
if (iy == 0) {
bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(-def[0].Geom().spec.size[1]);
bindpos.push_back(0);
} else if (iy==2) {
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
} else if (iy == 2) {
bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(def[0].Geom().spec.size[1]);
bindpos.push_back(0);
} else {
bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back((ix == count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
bindpos.push_back(0);
bindpos.push_back(0);
}
@@ -903,8 +904,8 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
// XY matrix
const mjtNum step = 1.0/(1+skinsubgrid);
int rxy = 0;
for (int sx=0; sx<=1+skinsubgrid; sx++) {
for (int sy=0; sy<=1+skinsubgrid; sy++) {
for (int sx=0; sx <= 1+skinsubgrid; sx++) {
for (int sy=0; sy <= 1+skinsubgrid; sy++) {
// compute x, y
mjtNum x = sx*step;
mjtNum y = sy*step;
@@ -939,15 +940,15 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
mju_mulMatMat(XY_W, XY, subW, N, 16, 16);
// Weight matrices
for (int dx=0; dx<3; dx++) {
for (int dy=0; dy<3; dy++) {
for (int dx=0; dx < 3; dx++) {
for (int dy=0; dy < 3; dy++) {
// make dense D
mju_zero(D, 16*16);
int cnt = 0;
int r = 0, c;
while (r<16) {
while (r < 16) {
// scan row
while ((c = mju_round(Dp[dx][dy][cnt]))!=-1) {
while ((c = mju_round(Dp[dx][dy][cnt])) != -1) {
D[r*16+c] = Dp[dx][dy][cnt+1];
cnt +=2;
}
@@ -977,9 +978,9 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
int C0 = count[0] + (count[0]-1)*skinsubgrid;
int C1 = count[1] + (count[1]-1)*skinsubgrid;
int NN = C0*C1;
for (int i=0; i<2; i++) {
for (int ix=0; ix<C0; ix++) {
for (int iy=0; iy<C1; iy++) {
for (int i=0; i < 2; i++) {
for (int ix=0; ix < C0; ix++) {
for (int iy=0; iy < C1; iy++) {
// vertex
vert.push_back(ix*S);
vert.push_back(iy*S);
@@ -992,21 +993,21 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
}
// face
if (ix<C0-1 && iy<C1-1) {
if (ix < C0-1 && iy < C1-1) {
face.push_back(i*NN + ix*C1+iy);
face.push_back(i*NN + (ix+1)*C1+iy+(i==1));
face.push_back(i*NN + (ix+1)*C1+iy+(i==0));
face.push_back(i*NN + (ix+1)*C1+iy+(i == 1));
face.push_back(i*NN + (ix+1)*C1+iy+(i == 0));
face.push_back(i*NN + ix*C1+iy);
face.push_back(i*NN + (ix+(i==0))*C1+iy+1);
face.push_back(i*NN + (ix+(i==1))*C1+iy+1);
face.push_back(i*NN + (ix+(i == 0))*C1+iy+1);
face.push_back(i*NN + (ix+(i == 1))*C1+iy+1);
}
}
}
}
// add thin triangles: X direction, iy = 0
for (int ix=0; ix<C0-1; ix++) {
for (int ix=0; ix < C0-1; ix++) {
face.push_back(ix*C1);
face.push_back(NN + (ix+1)*C1);
face.push_back((ix+1)*C1);
@@ -1017,7 +1018,7 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
}
// add thin triangles: X direction, iy = C1-1
for (int ix=0; ix<C0-1; ix++) {
for (int ix=0; ix < C0-1; ix++) {
face.push_back(ix*C1 + C1-1);
face.push_back((ix+1)*C1 + C1-1);
face.push_back(NN + (ix+1)*C1 + C1-1);
@@ -1028,7 +1029,7 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
}
// add thin triangles: Y direction, ix = 0
for (int iy=0; iy<C1-1; iy++) {
for (int iy=0; iy < C1-1; iy++) {
face.push_back(iy);
face.push_back(iy+1);
face.push_back(NN + iy+1);
@@ -1039,7 +1040,7 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
}
// add thin triangles: Y direction, ix = C0-1
for (int iy=0; iy<C1-1; iy++) {
for (int iy=0; iy < C1-1; iy++) {
face.push_back(iy + (C0-1)*C1);
face.push_back(NN + iy+1 + (C0-1)*C1);
face.push_back(iy+1 + (C0-1)*C1);
@@ -1052,24 +1053,24 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
MakeCableBonesSubgrid(model, skin);
// bind vertices to bones: one big square at a time
for (int ix=0; ix<count[0]-1; ix++) {
for (int iy=0; iy<count[1]-1; iy++) {
for (int ix=0; ix < count[0]-1; ix++) {
for (int iy=0; iy < count[1]-1; iy++) {
// determine d for Weight indexing
int d = 3 * (ix==0 ? 0 : (ix==count[0]-2 ? 2 : 1)) +
(iy==0 ? 0 : (iy==count[1]-2 ? 2 : 1));
int d = 3 * (ix == 0 ? 0 : (ix == count[0]-2 ? 2 : 1)) +
(iy == 0 ? 0 : (iy == count[1]-2 ? 2 : 1));
// precompute 16 bone indices for big square
int boneid[16];
int cnt = 0;
for (int dx=-1; dx<3; dx++) {
for (int dy=-1; dy<3; dy++) {
for (int dx=-1; dx < 3; dx++) {
for (int dy=-1; dy < 3; dy++) {
boneid[cnt++] = (ix+dx)*count[1] + (iy+dy);
}
}
// process subgrid, top-rigth owns last index
for (int dx=0; dx<1+skinsubgrid+(ix==count[0]-2); dx++) {
for (int dy=0; dy<1+skinsubgrid+(iy==count[1]-2); dy++) {
for (int dx=0; dx < 1+skinsubgrid+(ix == count[0]-2); dx++) {
for (int dy=0; dy < 1+skinsubgrid+(iy == count[1]-2); dy++) {
// recover vertex id
int vid = (ix*(1+skinsubgrid)+dx)*C1 + iy*(1+skinsubgrid)+dy;
@@ -1077,7 +1078,7 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
int n = dx*(2+skinsubgrid) + dy;
// add vertex to 16 bones
for (int bi=0; bi<16; bi++) {
for (int bi=0; bi < 16; bi++) {
mjtNum w = Weight[d*N*16 + n*16 + bi];
if (w) {
vertid[boneid[bi]].push_back(vid);
src/user/user_flexcomp.cc
+39 -38
View File
@@ -26,6 +26,7 @@
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjplugin.h>
#include "cc/array_safety.h"
#include "engine/engine_crossplatform.h"
@@ -133,12 +134,12 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
// check scale
if (scale[0]<mjMINVAL || scale[1]<mjMINVAL || scale[2]<mjMINVAL) {
if (scale[0] < mjMINVAL || scale[1] < mjMINVAL || scale[2] < mjMINVAL) {
return comperr(error, "Scale must be larger than mjMINVAL", error_sz);
}
// check mass and inertia
if (mass<mjMINVAL || inertiabox<mjMINVAL) {
if (mass < mjMINVAL || inertiabox < mjMINVAL) {
return comperr(error, "Mass and inertiabox must be larger than mjMINVAL", error_sz);
}
@@ -151,36 +152,36 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
// type-specific constructor: populate point and element, possibly set dim
bool res;
switch (type) {
case mjFCOMPTYPE_GRID:
case mjFCOMPTYPE_CIRCLE:
res = MakeGrid(error, error_sz);
break;
case mjFCOMPTYPE_GRID:
case mjFCOMPTYPE_CIRCLE:
res = MakeGrid(error, error_sz);
break;
case mjFCOMPTYPE_BOX:
case mjFCOMPTYPE_CYLINDER:
case mjFCOMPTYPE_ELLIPSOID:
res = MakeBox(error, error_sz);
break;
case mjFCOMPTYPE_BOX:
case mjFCOMPTYPE_CYLINDER:
case mjFCOMPTYPE_ELLIPSOID:
res = MakeBox(error, error_sz);
break;
case mjFCOMPTYPE_SQUARE:
case mjFCOMPTYPE_DISC:
res = MakeSquare(error, error_sz);
break;
case mjFCOMPTYPE_SQUARE:
case mjFCOMPTYPE_DISC:
res = MakeSquare(error, error_sz);
break;
case mjFCOMPTYPE_MESH:
res = MakeMesh(model, error, error_sz);
break;
case mjFCOMPTYPE_MESH:
res = MakeMesh(model, error, error_sz);
break;
case mjFCOMPTYPE_GMSH:
res = MakeGMSH(model, error, error_sz);
break;
case mjFCOMPTYPE_GMSH:
res = MakeGMSH(model, error, error_sz);
break;
case mjFCOMPTYPE_DIRECT:
res = true;
break;
case mjFCOMPTYPE_DIRECT:
res = true;
break;
default:
return comperr(error, "Unknown flexcomp type", error_sz);
default:
return comperr(error, "Unknown flexcomp type", error_sz);
}
if (!res) {
return false;
@@ -329,7 +330,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
}
}
else if (dflex->dim==3) {
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++) {
@@ -779,14 +780,14 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) {
};
// box
if (type==mjFCOMPTYPE_BOX) {
if (type == mjFCOMPTYPE_BOX) {
pos[0] *= size[0];
pos[1] *= size[1];
pos[2] *= size[2];
}
// cylinder
else if (type==mjFCOMPTYPE_CYLINDER) {
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;
@@ -795,7 +796,7 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) {
}
// ellipsoid
else if (type==mjFCOMPTYPE_ELLIPSOID) {
else if (type == mjFCOMPTYPE_ELLIPSOID) {
mjuu_normvec(pos, 3);
pos[0] *= size[0];
pos[1] *= size[1];
@@ -816,7 +817,7 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) {
}
// do projection
if (type==mjFCOMPTYPE_DISC) {
if (type == mjFCOMPTYPE_DISC) {
double size[2] = {
0.5*spacing[0]*(count[0]-1),
0.5*spacing[1]*(count[1]-1),
@@ -892,7 +893,7 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
for (int ix=0; ix < count[0]; ix++) {
for (int iz=0; iz < count[2]; iz++) {
// add point
if (iz>0 && iz < count[2]-1) {
if (iz > 0 && iz < count[2]-1) {
BoxProject(pos, ix, iy, iz);
point.push_back(pos[0]);
point.push_back(pos[1]);
@@ -1137,7 +1138,7 @@ bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) {
// load GMSH format 4.1
void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
int nodebegin, int elemend, int elembegin){
int nodebegin, int elemend, int elembegin){
// header size
constexpr int kGmsh41HeaderSize = 52;
// base for node tags, to be subtracted from element data
@@ -1555,7 +1556,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
numNodeTags = 4;
}
if (numNodeTags < 1 || numNodeTags >4) {
if (numNodeTags < 1 || numNodeTags > 4) {
throw mjCError(NULL, "Invalid number of node tags");
}
@@ -1564,7 +1565,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
// read elements, discard all tags
element.reserve(numNodeTags*numElements);
for (size_t i=0; i<numElements; i++) {
for (size_t i=0; i < numElements; i++) {
int nodeTag = 0, physicalEntityTag = 0, elementModelEntityTag = 0;
if (i != 0) {
ss >> tag >> elementType >> numTags;
@@ -1643,7 +1644,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
numNodeTags = 4;
}
if (numNodeTags < 1 || numNodeTags >4) {
if (numNodeTags < 1 || numNodeTags > 4) {
throw mjCError(NULL, "Invalid number of node tags");
}
@@ -1680,7 +1681,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
}
// read first element
for (int k =0; k<numNodeTags; k++) {
for (int k =0; k < numNodeTags; k++) {
ReadFromBuffer(&nodeTag, elementsBuffer + componentSize*(6+k));
if (nodeTag > numNodes || nodeTag < 1) {
throw mjCError(NULL, "Invalid node tag");
@@ -1719,7 +1720,7 @@ void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
}
// read every other element
for (int i=0; i<numElements-1; i++) {
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);
src/user/user_mesh.cc
+193 -193
View File
@@ -282,8 +282,8 @@ mjCMesh::~mjCMesh() {
void mjCMesh::LoadSDF() {
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for mesh '%s', (id = %d)",
name.c_str(), id);
this, "neither 'plugin' nor 'instance' is specified for mesh '%s', (id = %d)",
name.c_str(), id);
}
if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) {
@@ -409,23 +409,23 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) {
// calculate estimated size of mesh
std::size_t size = sizeof(mjCMesh)
+ (sizeof(double) * vert_.size())
+ (sizeof(float) * normal_.size())
+ (sizeof(float) * texcoord_.size())
+ (sizeof(int) * face_.size())
+ (sizeof(int) * facenormal_.size())
+ (sizeof(int) * facetexcoord_.size())
+ (sizeof(int) * 2 * halfedge_.size())
+ (sizeof(int) * szgraph_)
+ (sizeof(int) * npolygonvert())
+ (sizeof(double) * polygon_normals_.size())
+ (sizeof(int) * npolygonmap())
+ (sizeof(double) * 18)
+ (sizeof(int) * ncenter)
+ tree_.Size()
+ (sizeof(double) * face_aabb_.size());
+ (sizeof(double) * vert_.size())
+ (sizeof(float) * normal_.size())
+ (sizeof(float) * texcoord_.size())
+ (sizeof(int) * face_.size())
+ (sizeof(int) * facenormal_.size())
+ (sizeof(int) * facetexcoord_.size())
+ (sizeof(int) * 2 * halfedge_.size())
+ (sizeof(int) * szgraph_)
+ (sizeof(int) * npolygonvert())
+ (sizeof(double) * polygon_normals_.size())
+ (sizeof(int) * npolygonmap())
+ (sizeof(double) * 18)
+ (sizeof(int) * ncenter)
+ tree_.Size()
+ (sizeof(double) * face_aabb_.size());
std::shared_ptr<const void> cached_data(mesh, +[](const void* data) {
std::shared_ptr<const void> cached_data(mesh, +[] (const void* data) {
const mjCMesh* mesh = static_cast<const mjCMesh*>(data);
delete mesh;
});
@@ -445,8 +445,8 @@ struct VertexKey {
std::size_t operator()(const VertexKey& vertex) const {
// combine all three hash values into a single hash value
return ((std::hash<float>()(vertex.v[0])
^ (std::hash<float>()(vertex.v[1]) << 1)) >> 1)
^ (std::hash<float>()(vertex.v[2]) << 1);
^ (std::hash<float>()(vertex.v[1]) << 1)) >> 1)
^ (std::hash<float>()(vertex.v[2]) << 1);
}
};
@@ -541,7 +541,7 @@ bool mjCMesh::IsMSH(std::string_view filename, std::string_view ct) {
return asset_type == "model/vnd.mujoco.msh";
}
bool mjCMesh::IsObj() const{
bool mjCMesh::IsObj() const {
return content_type_ == "model/obj";
}
@@ -641,7 +641,7 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
} else if (normal_.empty()) {
normal_ = spec_normal_;
}
if (!texcoord_.empty() && !spec_texcoord_.empty()) {
if (!texcoord_.empty() && !spec_texcoord_.empty()) {
throw mjCError(this, "repeated texcoord specification");
} else if (texcoord_.empty()) {
texcoord_ = spec_texcoord_;
@@ -835,29 +835,29 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
// get inertia box type (shell or volume)
double* boxsz = GetInertiaBoxPtr();
switch (geom->type) {
case mjGEOM_SPHERE:
geom->size[0] = (boxsz[0] + boxsz[1] + boxsz[2])/3;
break;
case mjGEOM_SPHERE:
geom->size[0] = (boxsz[0] + boxsz[1] + boxsz[2])/3;
break;
case mjGEOM_CAPSULE:
geom->size[0] = (boxsz[0] + boxsz[1])/2;
geom->size[1] = max(0.0, boxsz[2] - geom->size[0]/2);
break;
case mjGEOM_CAPSULE:
geom->size[0] = (boxsz[0] + boxsz[1])/2;
geom->size[1] = max(0.0, boxsz[2] - geom->size[0]/2);
break;
case mjGEOM_CYLINDER:
geom->size[0] = (boxsz[0] + boxsz[1])/2;
geom->size[1] = boxsz[2];
break;
case mjGEOM_CYLINDER:
geom->size[0] = (boxsz[0] + boxsz[1])/2;
geom->size[1] = boxsz[2];
break;
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = boxsz[0];
geom->size[1] = boxsz[1];
geom->size[2] = boxsz[2];
break;
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = boxsz[0];
geom->size[1] = boxsz[1];
geom->size[2] = boxsz[2];
break;
default:
throw mjCError(this, "invalid geom type in fitting mesh %s", name.c_str());
default:
throw mjCError(this, "invalid geom type in fitting mesh %s", name.c_str());
}
}
@@ -873,58 +873,58 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
// compute depending on type
switch (geom->type) {
case mjGEOM_SPHERE:
// find maximum distance
geom->size[0] = 0;
for (int i=0; i < nvert(); i++) {
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
double dst = mjuu_dist3(v, cen);
geom->size[0] = max(geom->size[0], dst);
}
break;
case mjGEOM_SPHERE:
// find maximum distance
geom->size[0] = 0;
for (int i=0; i < nvert(); i++) {
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
double dst = mjuu_dist3(v, cen);
geom->size[0] = max(geom->size[0], dst);
}
break;
case mjGEOM_CAPSULE:
case mjGEOM_CYLINDER:
// find maximum distance in XY, separately in Z
geom->size[0] = 0;
geom->size[1] = 0;
for (int i=0; i < nvert(); i++) {
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
(v[1]-cen[1])*(v[1]-cen[1]));
geom->size[0] = max(geom->size[0], dst);
// proceed with z: valid for cylinder
double dst2 = abs(v[2]-cen[2]);
geom->size[1] = max(geom->size[1], dst2);
}
// special handling of capsule: consider curved cap
if (geom->type==mjGEOM_CAPSULE) {
case mjGEOM_CAPSULE:
case mjGEOM_CYLINDER:
// find maximum distance in XY, separately in Z
geom->size[0] = 0;
geom->size[1] = 0;
for (int i=0; i < nvert(); i++) {
// get distance in XY and Z
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
(v[1]-cen[1])*(v[1]-cen[1]));
geom->size[0] = max(geom->size[0], dst);
// proceed with z: valid for cylinder
double dst2 = abs(v[2]-cen[2]);
// get spherical elevation at horizontal distance dst
double h = geom->size[0] * sin(acos(dst/geom->size[0]));
geom->size[1] = max(geom->size[1], dst2-h);
geom->size[1] = max(geom->size[1], dst2);
}
}
break;
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = aamm_[3] - cen[0];
geom->size[1] = aamm_[4] - cen[1];
geom->size[2] = aamm_[5] - cen[2];
break;
// special handling of capsule: consider curved cap
if (geom->type == mjGEOM_CAPSULE) {
geom->size[1] = 0;
for (int i=0; i < nvert(); i++) {
// get distance in XY and Z
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
(v[1]-cen[1])*(v[1]-cen[1]));
double dst2 = abs(v[2]-cen[2]);
default:
throw mjCError(this, "invalid fittype in mesh %s", name.c_str());
// get spherical elevation at horizontal distance dst
double h = geom->size[0] * sin(acos(dst/geom->size[0]));
geom->size[1] = max(geom->size[1], dst2-h);
}
}
break;
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = aamm_[3] - cen[0];
geom->size[1] = aamm_[4] - cen[1];
geom->size[2] = aamm_[5] - cen[2];
break;
default:
throw mjCError(this, "invalid fittype in mesh %s", name.c_str());
}
}
@@ -1186,7 +1186,7 @@ void mjCMesh::LoadMSH(mjResource* resource, bool remove_repeated) {
}
// make sure header is present
if (buffer_sz<4*sizeof(int)) {
if (buffer_sz < 4*sizeof(int)) {
throw mjCError(this, "missing header in MSH file '%s'", resource->name);
}
@@ -1626,13 +1626,13 @@ double mjCMesh::ComputeInertia(double inert[6], const double CoM[3]) const {
int C = (inertia == mjMESH_INERTIA_SHELL) ? 12 : 20;
for (int j = 0; j < 6; j++) {
P[j] += volume /
C * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
C * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}
@@ -1669,7 +1669,7 @@ void mjCMesh::Rotate(double quat[4]) {
const double nrm[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]};
double res[3];
mjuu_mulvecmat(res, nrm, mat);
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
normal_[3*i+j] = (float) res[j];
}
}
@@ -1784,7 +1784,7 @@ void mjCMesh::MakeGraph() {
FORALLvertices {
// point id of this vertex, check
int pid = qh_pointid(qh, vertex->point);
if (pid<0 || pid>=nvert()) {
if (pid < 0 || pid >= nvert()) {
ok = 0;
break;
}
@@ -1802,29 +1802,29 @@ void mjCMesh::MakeGraph() {
// point id of face vertex, check
int pid1 = qh_pointid(qh, vertex1->point);
if (pid1<0 || pid1>=nvert()) {
if (pid1 < 0 || pid1 >= nvert()) {
ok = 0;
break;
}
// if different from vertex id, try to insert
if (pid!=pid1) {
if (pid != pid1) {
// check for previous record
int j;
for (j=start; j<adr; j++)
if (pid1==edge_localid[j]) {
for (j=start; j < adr; j++)
if (pid1 == edge_localid[j]) {
break;
}
// not found: insert
if (j>=adr) {
if (j >= adr) {
edge_localid[adr++] = pid1;
}
}
}
// make sure we have triangle: SHOULD NOT OCCUR
if (cnt!=3) {
if (cnt != 3) {
mju_error("Qhull did not return triangle");
}
}
@@ -1835,7 +1835,7 @@ void mjCMesh::MakeGraph() {
}
// size check: SHOULD NOT OCCUR
if (adr!=numvert+3*numface) {
if (adr != numvert+3*numface) {
mju_error("Wrong size in convex hull graph");
}
@@ -1852,7 +1852,7 @@ void mjCMesh::MakeGraph() {
// copy triangle data
FOREACHsetelement_(vertexT, facet->vertices, vertex1) {
// make sure we have triangle: SHOULD NOT OCCUR
if (ii>=3) {
if (ii >= 3) {
mju_error("Qhull did not return triangle");
}
@@ -1877,18 +1877,18 @@ void mjCMesh::MakeGraph() {
// replace global ids with local ids in edge data
for (int i=0; i < numvert+3*numface; i++) {
if (edge_localid[i]>=0) {
if (edge_localid[i] >= 0) {
// search vert_globalid for match
int adr;
for (adr=0; adr<numvert; adr++) {
if (vert_globalid[adr]==edge_localid[i]) {
for (adr=0; adr < numvert; adr++) {
if (vert_globalid[adr] == edge_localid[i]) {
edge_localid[i] = adr;
break;
}
}
// make sure we found a match: SHOULD NOT OCCUR
if (adr>=numvert) {
if (adr >= numvert) {
mju_error("Vertex id not found in convex hull");
}
}
@@ -1952,13 +1952,13 @@ void mjCMesh::MakeNormal() {
for (int i=0; i < nface(); i++) {
// get vertex ids
int vertid[3];
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
vertid[j] = face_[3*i+j];
}
// get triangle edges
double vec01[3], vec02[3];
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j];
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j];
}
@@ -1969,8 +1969,8 @@ void mjCMesh::MakeNormal() {
double area = mjuu_normvec(nrm, 3);
// add normal to each vertex with weight = area
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
for (int j=0; j < 3; j++) {
for (int k=0; k < 3; k++) {
normal_[3*vertid[j]+k] += nrm[k]*area;
}
facenormal_[3*i+j] = vertid[j];
@@ -1987,13 +1987,13 @@ void mjCMesh::MakeNormal() {
for (int i=0; i < nface(); i++) {
// get vertex ids
int vertid[3];
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
vertid[j] = face_[3*i+j];
}
// get triangle edges
double vec01[3], vec02[3];
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j];
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j];
}
@@ -2004,14 +2004,14 @@ void mjCMesh::MakeNormal() {
double area = mjuu_normvec(nrm, 3);
// compare to vertex normal, subtract contribution if dot product too small
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
// normalized vertex normal
double vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]};
mjuu_normvec(vnrm, 3);
// dot too small: remove
if (mjuu_dot3(nrm, vnrm)<0.8) {
for (int k=0; k<3; k++) {
if (mjuu_dot3(nrm, vnrm) < 0.8) {
for (int k=0; k < 3; k++) {
nremove[3*vertid[j]+k] += nrm[k]*area;
}
}
@@ -2033,12 +2033,12 @@ void mjCMesh::MakeNormal() {
normal_[3*i+2]*normal_[3*i+2]);
// divide by length
if (len>mjMINVAL)
for (int j=0; j<3; j++) {
if (len > mjMINVAL)
for (int j=0; j < 3; j++) {
normal_[3*i+j] /= len;
} else {
normal_[3*i] = normal_[3*i+1] = 0;
normal_[3*i+2] = 1;
}else {
normal_[3*i] = normal_[3*i+1] = 0;
normal_[3*i+2] = 1;
}
}
}
@@ -2061,7 +2061,7 @@ void mjCMesh::MakeCenter() {
// get triangle edges
double a[3], b[3];
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
a[j] = vert_[3*vertid[0]+j] - vert_[3*vertid[2]+j];
b[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[2]+j];
}
@@ -2093,12 +2093,12 @@ void mjCMesh::MakeCenter() {
// compute the normals of the polygons
void mjCMesh::MakePolygonNormals() {
for (int i = 0; i < polygons_.size(); ++i) {
double n[3];
mjuu_makenormal(n, &vert_[3*polygons_[i][0]], &vert_[3*polygons_[i][1]],
&vert_[3*polygons_[i][2]]);
polygon_normals_[3*i + 0] = n[0];
polygon_normals_[3*i + 1] = n[1];
polygon_normals_[3*i + 2] = n[2];
double n[3];
mjuu_makenormal(n, &vert_[3*polygons_[i][0]], &vert_[3*polygons_[i][1]],
&vert_[3*polygons_[i][2]]);
polygon_normals_[3*i + 0] = n[0];
polygon_normals_[3*i + 1] = n[1];
polygon_normals_[3*i + 2] = n[2];
}
}
@@ -2265,8 +2265,8 @@ void MeshPolygon::InsertFace(int v1, int v2, int v3) {
// return the traverse vertices of the polygon; there may be multiple paths if the polygon is
// not connected
std::vector<std::vector<int>> MeshPolygon::Paths() const {
std::vector<std::vector<int>> paths;
std::vector<std::vector<int> > MeshPolygon::Paths() const {
std::vector<std::vector<int> > paths;
// shortcut if polygon is just a triangular face
if (edges_.size() == 3) {
return {{edges_[0].first, edges_[1].first, edges_[2].first}};
@@ -2304,7 +2304,7 @@ std::vector<std::vector<int>> MeshPolygon::Paths() const {
}
path.push_back(next);
break;
}
}
}
// back at start
@@ -2357,7 +2357,7 @@ void mjCMesh::MakePolygons() {
}
for (const auto& polygon : polygons) {
std::vector<std::vector<int>> paths = polygon.Paths();
std::vector<std::vector<int> > paths = polygon.Paths();
// separate the polygons if they were grouped together
for (const auto& path : paths) {
@@ -2595,7 +2595,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
if (vert_.size()%3) {
throw mjCError(this, "Vertex data must be multiple of 3");
}
if (!texcoord_.empty() && texcoord_.size()!=2*vert_.size()/3) {
if (!texcoord_.empty() && texcoord_.size() != 2*vert_.size()/3) {
throw mjCError(this, "Vertex and texcoord data incompatible size");
}
if (face_.size()%3) {
@@ -2604,16 +2604,16 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// check bone sizes
size_t nbone = bodyname_.size();
if (bindpos_.size()!=3*nbone) {
if (bindpos_.size() != 3*nbone) {
throw mjCError(this, "Unexpected bindpos size in skin");
}
if (bindquat_.size()!=4*nbone) {
if (bindquat_.size() != 4*nbone) {
throw mjCError(this, "Unexpected bindquat size in skin");
}
if (vertid_.size()!=nbone) {
if (vertid_.size() != nbone) {
throw mjCError(this, "Unexpected vertid size in skin");
}
if (vertweight_.size()!=nbone) {
if (vertweight_.size() != nbone) {
throw mjCError(this, "Unexpected vertweight size in skin");
}
@@ -2638,15 +2638,15 @@ void mjCSkin::Compile(const mjVFS* vfs) {
for (int i=0; i < nbone; i++) {
// make sure bone has vertices and sizes match
size_t nbv = vertid_[i].size();
if (vertweight_[i].size()!=nbv || nbv==0) {
if (vertweight_[i].size() != nbv || nbv == 0) {
throw mjCError(this, "vertid and vertweight must have same non-zero size in skin");
}
// accumulate weights in global array
for (int j=0; j<nbv; j++) {
for (int j=0; j < nbv; j++) {
// get index and check range
int jj = vertid_[i][j];
if (jj<0 || jj>=nvert) {
if (jj < 0 || jj >= nvert) {
throw mjCError(this, "vertid %d out of range in skin", NULL, jj);
}
@@ -2657,14 +2657,14 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// check coverage
for (int i=0; i < nvert; i++) {
if (vw[i]<=mjMINVAL) {
if (vw[i] <= mjMINVAL) {
throw mjCError(this, "vertex %d must have positive total weight in skin", NULL, i);
}
}
// normalize vertex weights
for (int i=0; i < nbone; i++) {
for (int j=0; j<vertid_[i].size(); j++) {
for (int j=0; j < vertid_[i].size(); j++) {
vertweight_[i][j] /= vw[vertid_[i][j]];
}
}
@@ -2700,7 +2700,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
}
// make sure header is present
if (buffer_sz<16) {
if (buffer_sz < 16) {
throw mjCError(this, "missing header in SKN file '%s'", resource->name);
}
@@ -2711,7 +2711,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
int nbone = ((int*)buffer)[3];
// negative sizes not allowed
if (nvert<0 || ntexcoord<0 || nface<0 || nbone<0) {
if (nvert < 0 || ntexcoord < 0 || nface < 0 || nbone < 0) {
throw mjCError(this, "negative size in header of SKN file '%s'", resource->name);
}
@@ -2779,7 +2779,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
cnt += 1;
// check for negative
if (vcount<1) {
if (vcount < 1) {
throw mjCError(this, "vertex count must be positive in SKN file '%s', bone %d",
resource->name, i);
}
@@ -2814,10 +2814,10 @@ void mjCSkin::LoadSKN(mjResource* resource) {
// hash function for std::pair
struct PairHash
{
template <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
template <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
};
// simplex connectivity
@@ -3122,7 +3122,7 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
// compute stiffness matrix
std::vector<std::array<double, 3>> F(n);
std::vector<std::array<double, 3> > F(n);
double la = E * nu / (1 + nu) / (1 - 2 * nu);
double mu = E / (2 * (1 + nu));
@@ -3316,22 +3316,22 @@ void mjCFlex::Compile(const mjVFS* vfs) {
interpolated = !nodebody_.empty();
// set nelem; check sizes
if (dim<1 || dim>3) {
throw mjCError(this, "dim must be 1, 2 or 3");
if (dim < 1 || dim > 3) {
throw mjCError(this, "dim must be 1, 2 or 3");
}
if (elem_.empty()) {
throw mjCError(this, "elem is empty");
throw mjCError(this, "elem is empty");
}
if (elem_.size() % (dim+1)) {
throw mjCError(this, "elem size must be multiple of (dim+1)");
throw mjCError(this, "elem size must be multiple of (dim+1)");
}
if (vertbody_.empty() && !interpolated) {
throw mjCError(this, "vertbody and nodebody are both empty");
throw mjCError(this, "vertbody and nodebody are both empty");
}
if (vert_.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3");
throw mjCError(this, "vert size must be a multiple of 3");
}
if (edgestiffness>0 && dim>1) {
if (edgestiffness > 0 && dim > 1) {
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
}
if (interpolated && selfcollide != mjFLEXSELF_NONE) {
@@ -3349,29 +3349,29 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
else {
nvert = (int)vert_.size()/3;
if (vertbody_.size()==1) {
if (vertbody_.size() == 1) {
rigid = true;
}
}
if (nvert<dim+1) {
if (nvert < dim+1) {
throw mjCError(this, "not enough vertices");
}
// set nnode
nnode = (int)nodebody_.size();
if (nnode && nnode!=8) {
if (nnode && nnode != 8) {
throw mjCError(this, "number of nodes must be 2^dim, it is %d", "", nnode);
}
// check elem vertex ids
for (const auto& elem : elem_) {
if (elem<0 || elem>=nvert) {
if (elem < 0 || elem >= nvert) {
throw mjCError(this, "elem vertex id out of range");
}
}
// check texcoord
if (!texcoord_.empty() && texcoord_.size()!=2*nvert && elemtexcoord_.empty()) {
if (!texcoord_.empty() && texcoord_.size() != 2*nvert && elemtexcoord_.empty()) {
throw mjCError(this, "two texture coordinates per vertex expected");
}
@@ -3393,15 +3393,15 @@ void mjCFlex::Compile(const mjVFS* vfs) {
ResolveReferences(model);
// process elements
for (int e=0; e<(int)elem_.size()/(dim+1); e++) {
for (int e=0; e < (int)elem_.size()/(dim+1); e++) {
// make sorted copy of element
std::vector<int> el;
el.assign(elem_.begin()+e*(dim+1), elem_.begin()+(e+1)*(dim+1));
std::sort(el.begin(), el.end());
// check for repeated vertices
for (int k=0; k<dim; k++) {
if (el[k]==el[k+1]) {
for (int k=0; k < dim; k++) {
if (el[k] == el[k+1]) {
throw mjCError(this, "repeated vertex in element");
}
}
@@ -3411,7 +3411,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (!rigid && !interpolated) {
rigid = true;
for (unsigned i=1; i < vertbodyid.size(); i++) {
if (vertbodyid[i]!=vertbodyid[0]) {
if (vertbodyid[i] != vertbodyid[0]) {
rigid = false;
break;
}
@@ -3422,7 +3422,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (!centered && !interpolated) {
centered = true;
for (const auto& vert : vert_) {
if (vert!=0) {
if (vert != 0) {
centered = false;
break;
}
@@ -3432,7 +3432,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (!centered && interpolated) {
centered = true;
for (const auto& node : node_) {
if (node!=0) {
if (node != 0) {
centered = false;
break;
}
@@ -3477,8 +3477,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// reorder tetrahedra so right-handed face orientation is outside
// faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
if (dim==3) {
for (int e=0; e<nelem; e++) {
if (dim == 3) {
for (int e=0; e < nelem; e++) {
const int* edata = elem_.data() + e*(dim+1);
double* v0 = vertxpos.data() + 3*edata[0];
double* v1 = vertxpos.data() + 3*edata[1];
@@ -3491,7 +3491,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
// detect wrong orientation
double nrm[3];
mjuu_crossvec(nrm, v01, v02);
if (mjuu_dot3(nrm, v03)>0) {
if (mjuu_dot3(nrm, v03) > 0) {
// flip orientation
int tmp = elem_[e*(dim+1)+1];
elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2];
@@ -3513,7 +3513,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
auto pair = std::pair(
min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]),
max(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]])
);
);
// if edge is already present in the vector only store its index
auto [it, inserted] = edge_indices.insert({pair, nedge});
@@ -3547,11 +3547,11 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (interpolated) {
continue;
}
if (dim==2) {
if (dim == 2) {
ComputeStiffness<Stencil2D>(stiffness, vertxpos,
elem_.data() + (dim + 1) * t, t, young,
poisson, thickness);
} else if (dim==3) {
} else if (dim == 3) {
ComputeStiffness<Stencil3D>(stiffness, vertxpos,
elem_.data() + (dim + 1) * t, t, young,
poisson);
@@ -3570,7 +3570,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
if (model->Bodies()[vbodyid]->plugin.element) {
mjCPlugin* plugin_instance =
static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.element);
static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.element);
if (damping > 0) {
plugin_instance->config_attribs["damping"] = std::to_string(damping);
}
@@ -3613,11 +3613,11 @@ void mjCFlex::CreateBVH() {
tree.AllocateBoundingVolumes(nelem);
// construct element bounding boxes, add to hierarchy
for (int e=0; e<nelem; e++) {
for (int e=0; e < nelem; e++) {
const int* edata = elem_.data() + e*(dim+1);
// skip inactive in 3D
if (dim==3 && elemlayer[e]>=activelayers) {
if (dim == 3 && elemlayer[e] >= activelayers) {
continue;
}
@@ -3626,7 +3626,7 @@ void mjCFlex::CreateBVH() {
mjuu_copyvec(xmin, vertxpos.data() + 3*edata[0], 3);
mjuu_copyvec(xmax, vertxpos.data() + 3*edata[0], 3);
for (int i=1; i <= dim; i++) {
for (int j=0; j<3; j++) {
for (int j=0; j < 3; j++) {
xmin[j] = std::min(xmin[j], vertxpos[3*edata[i]+j]);
xmax[j] = std::max(xmax[j], vertxpos[3*edata[i]+j]);
}
@@ -3655,13 +3655,13 @@ void mjCFlex::CreateBVH() {
// create shells and element-vertex collision pairs
void mjCFlex::CreateShellPair(void) {
std::vector<std::vector<int>> fragspec(nelem*(dim+1)); // [sorted frag vertices, elem, original frag vertices]
std::vector<std::vector<int>> connectspec; // [elem1, elem2, common sorted frag vertices]
std::vector<std::vector<int> > fragspec(nelem*(dim+1)); // [sorted frag vertices, elem, original frag vertices]
std::vector<std::vector<int> > connectspec; // [elem1, elem2, common sorted frag vertices]
std::vector<bool> border(nelem, false); // is element on the border
std::vector<bool> borderfrag(nelem*(dim+1), false); // is fragment on the border
// make fragspec
for (int e=0; e<nelem; e++) {
for (int e=0; e < nelem; e++) {
int n = e*(dim+1);
// element vertices in original (unsorted) order
@@ -3669,7 +3669,7 @@ void mjCFlex::CreateShellPair(void) {
el.assign(elem_.begin()+n, elem_.begin()+n+dim+1);
// line: 2 vertex fragments
if (dim==1) {
if (dim == 1) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
@@ -3680,7 +3680,7 @@ void mjCFlex::CreateShellPair(void) {
}
// triangle: 3 edge fragments
else if (dim==2) {
else if (dim == 2) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(e);
@@ -3737,8 +3737,8 @@ void mjCFlex::CreateShellPair(void) {
}
// sort first segment of each fragspec
if (dim>1) {
for (int n=0; n<nelem*(dim+1); n++) {
if (dim > 1) {
for (int n=0; n < nelem*(dim+1); n++) {
std::sort(fragspec[n].begin(), fragspec[n].begin()+dim);
}
}
@@ -3748,13 +3748,13 @@ void mjCFlex::CreateShellPair(void) {
// make border and connectspec, record borderfrag
int cnt = 1;
for (int n=1; n<nelem*(dim+1); n++) {
for (int n=1; n < nelem*(dim+1); n++) {
// extract frag vertices, without elem
std::vector<int> previous = {fragspec[n-1].begin(), fragspec[n-1].begin()+dim};
std::vector<int> current = {fragspec[n].begin(), fragspec[n].begin()+dim};
// same sequential fragments
if (previous==current) {
if (previous == current) {
// found pair of elements connected by common fragment
std::vector<int> connect;
connect.insert(connect.end(), fragspec[n-1][dim]);
@@ -3769,7 +3769,7 @@ void mjCFlex::CreateShellPair(void) {
// different sequential fragments
else {
// found border fragment
if (cnt==1) {
if (cnt == 1) {
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
}
@@ -3780,7 +3780,7 @@ void mjCFlex::CreateShellPair(void) {
}
// last fragment is border
if (cnt==1) {
if (cnt == 1) {
int n = nelem*(dim+1);
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
@@ -3795,12 +3795,12 @@ void mjCFlex::CreateShellPair(void) {
}
// compute elemlayer (distance from border) via value iteration in 3D
if (dim<3) {
if (dim < 3) {
elemlayer = std::vector<int> (nelem, 0);
}
else {
elemlayer = std::vector<int> (nelem, nelem+1); // init with greater than max value
for (int e=0; e<nelem; e++) {
for (int e=0; e < nelem; e++) {
if (border[e]) {
elemlayer[e] = 0; // set border elements to 0
}
@@ -3814,10 +3814,10 @@ void mjCFlex::CreateShellPair(void) {
for (const auto& connect : connectspec) {
int e1 = connect[0]; // get element pair for this edge
int e2 = connect[1];
if (elemlayer[e1]>elemlayer[e2]+1) {
elemlayer[e1] = elemlayer[e2]+1; // better value found for e1: update
if (elemlayer[e1] > elemlayer[e2]+1) {
elemlayer[e1] = elemlayer[e2]+1; // better value found for e1: update
change = true;
} else if (elemlayer[e2]>elemlayer[e1]+1) {
} else if (elemlayer[e2] > elemlayer[e1]+1) {
elemlayer[e2] = elemlayer[e1]+1; // better value found for e2: update
change = true;
}
@@ -3826,7 +3826,7 @@ void mjCFlex::CreateShellPair(void) {
}
// create evpairs in 1D and 2D
if (dim<3) {
if (dim < 3) {
// process connected element pairs containing a border element
for (const auto& connect : connectspec) {
if (border[connect[0]] || border[connect[1]]) {
src/user/user_model.cc
+323 -314
View File
File diff suppressed because it is too large Load Diff
src/user/user_objects.cc
+858 -844
View File
File diff suppressed because it is too large Load Diff
src/user/user_resource.cc
+1 -1
View File
@@ -204,7 +204,7 @@ void mju_closeResource(mjResource* resource) {
if (provider) {
if (provider->close) provider->close(resource);
} else {
FileClose(resource); // clear OS filesystem if present
FileClose(resource); // clear OS filesystem if present
}
// free resource
src/user/user_util.cc
+21 -19
View File
@@ -52,11 +52,11 @@ 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) {
if (g1 < 0 || g2 < 0 || g1 >= n || g2 >= n) {
return -1;
}
if (g1>g2) {
if (g1 > g2) {
int tmp = g1;
g1 = g2;
g2 = tmp;
@@ -103,21 +103,21 @@ 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++) {
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++) {
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++) {
for (int i=0; i < n; i++) {
dest[i] = 0;
}
}
@@ -137,7 +137,7 @@ double mjuu_dist3(const double* a, const double* b) {
// 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++) {
for (int i=0; i < n; i++) {
res += std::abs(a[i]-b[i]);
}
@@ -149,7 +149,7 @@ 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++) {
for (int i=0; i < n; i++) {
nrm += vec[i]*vec[i];
}
if (nrm < mjEPS) {
@@ -160,7 +160,7 @@ double mjuu_normvec(double* vec, const int n) {
// don't normalize if nrm is within mjEPS of 1
if (std::abs(nrm - 1) > mjEPS) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
vec[i] /= nrm;
}
}
@@ -173,7 +173,7 @@ 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++) {
for (int i=0; i < n; i++) {
nrm += vec[i]*vec[i];
}
if (nrm < mjEPS) {
@@ -184,7 +184,7 @@ float mjuu_normvec(float* vec, const int n) {
// don't normalize if nrm is within mjEPS of 1
if (std::abs(nrm - 1) > mjEPS) {
for (int i=0; i<n; i++) {
for (int i=0; i < n; i++) {
vec[i] /= nrm;
}
}
@@ -390,7 +390,7 @@ 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);
if (s<1E-10) {
if (s < 1E-10) {
quat[1] = 1;
quat[2] = quat[3] = 0;
}
@@ -407,7 +407,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];
@@ -415,7 +415,7 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl
}
// q1 largest
else if (mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2]) {
else if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
quat[1] = 0.5 * sqrt(1 + mat[0][0] - mat[1][1] - mat[2][2]);
quat[0] = 0.25 * (mat[1][2] - mat[2][1]) / quat[1];
quat[2] = 0.25 * (mat[1][0] + mat[0][1]) / quat[1];
@@ -423,7 +423,7 @@ void mjuu_frame2quat(double* quat, const double* x, const double* y, const doubl
}
// q2 largest
else if (mat[1][1]>mat[2][2]) {
else if (mat[1][1] > mat[2][2]) {
quat[2] = 0.5 * sqrt(1 - mat[0][0] + mat[1][1] - mat[2][2]);
quat[0] = 0.25 * (mat[2][0] - mat[0][2]) / quat[2];
quat[1] = 0.25 * (mat[1][0] + mat[0][1]) / quat[2];
@@ -612,7 +612,7 @@ void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]) {
// 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) {
const double tprv[3], const double tnxt[3], int first) {
double tangent[3], binormal[3];
// normalize tangent
@@ -764,7 +764,7 @@ std::string mjuu_strippath(std::string filename) {
size_t start = filename.find_last_of("/\\");
// no path found: return original
if (start==std::string::npos) {
if (start == std::string::npos) {
return filename;
}
@@ -791,7 +791,7 @@ const char* mjuu_fullInertia(double quat[4], double inertia[3], const double ful
mjuu_eig3(eigval, eigvec, quattmp, full);
// check mimimal eigenvalue
if (eigval[2]<mjEPS) {
if (eigval[2] < mjEPS) {
return "inertia must have positive eigenvalues";
}
@@ -847,7 +847,7 @@ bool mjuu_isabspath(std::string path) {
// check first char
const char* str = path.c_str();
if (str[0]=='\\' || str[0]=='/') {
if (str[0] == '\\' || str[0] == '/') {
return true;
}
@@ -1102,7 +1102,9 @@ FilePath FilePath::StripPath() const {
std::string FilePath::StrLower() const {
std::string str = path_;
std::transform(str.begin(), str.end(), str.begin(),
[](unsigned char c) { return std::tolower(c); });
[](unsigned char c) {
return std::tolower(c);
});
return str;
}
src/user/user_vfs.cc
+1 -1
View File
@@ -273,6 +273,6 @@ void mj_deleteVFS(mjVFS* vfs) {
const mjpResourceProvider* GetVfsResourceProvider() {
static mjpResourceProvider provider
= { nullptr, &Open, &Read, &Close, &GetDir, &Modified, nullptr };
= { nullptr, &Open, &Read, &Close, &GetDir, &Modified, nullptr };
return &provider;
}