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

Declare loop variables inside for loop in C++ files.

Browse Source 
PiperOrigin-RevId: 518852339
Change-Id: I0174d873d89e48f11b816d337728f213c2798248
This commit is contained in:
Nimrod Gileadi
2023-03-23 07:21:46 -07:00
committed by Copybara-Service
parent 145fe7f354
commit 0e7e299bf6
11 changed files with 375 additions and 447 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simulate/simulate.cc
+37 -49
View File
@@ -139,8 +139,6 @@ const char help_title[] =
// init profiler figures
void profilerinit(mj::Simulate* sim) {
int i, n;
// set figures to default
mjv_defaultFigure(&sim->figconstraint);
mjv_defaultFigure(&sim->figcost);
@@ -225,8 +223,8 @@ void profilerinit(mj::Simulate* sim) {
sim->figtimer.range[1][1] = 0.4f;
// init x axis on history figures (do not show yet)
for (n=0; n<6; n++)
for (i=0; i<mjMAXLINEPNT; i++) {
for (int n=0; n<6; n++)
for (int i=0; i<mjMAXLINEPNT; i++) {
sim->figtimer.linedata[n][2*i] = -i;
sim->figsize.linedata[n][2*i] = -i;
}
@@ -234,11 +232,9 @@ void profilerinit(mj::Simulate* sim) {
// update profiler figures
void profilerupdate(mj::Simulate* sim) {
int i, n;
// update constraint figure
sim->figconstraint.linepnt[0] = mjMIN(mjMIN(sim->d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (i=1; i<5; i++) {
for (int i=1; i<5; i++) {
sim->figconstraint.linepnt[i] = sim->figconstraint.linepnt[0];
}
if (sim->m->opt.solver==mjSOL_PGS) {
@@ -248,7 +244,7 @@ void profilerupdate(mj::Simulate* sim) {
if (sim->m->opt.solver==mjSOL_CG) {
sim->figconstraint.linepnt[4] = 0;
}
for (i=0; i<sim->figconstraint.linepnt[0]; i++) {
for (int i=0; i<sim->figconstraint.linepnt[0]; i++) {
// x
sim->figconstraint.linedata[0][2*i] = i;
sim->figconstraint.linedata[1][2*i] = i;
@@ -266,7 +262,7 @@ void profilerupdate(mj::Simulate* sim) {
// update cost figure
sim->figcost.linepnt[0] = mjMIN(mjMIN(sim->d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (i=1; i<3; i++) {
for (int i=1; i<3; i++) {
sim->figcost.linepnt[i] = sim->figcost.linepnt[0];
}
if (sim->m->opt.solver==mjSOL_PGS) {
@@ -274,7 +270,7 @@ void profilerupdate(mj::Simulate* sim) {
sim->figcost.linepnt[2] = 0;
}
for (i=0; i<sim->figcost.linepnt[0]; i++) {
for (int i=0; i<sim->figcost.linepnt[0]; i++) {
// x
sim->figcost.linedata[0][2*i] = i;
sim->figcost.linedata[1][2*i] = i;
@@ -306,9 +302,9 @@ void profilerupdate(mj::Simulate* sim) {
// update figtimer
int pnt = mjMIN(201, sim->figtimer.linepnt[0]+1);
for (n=0; n<5; n++) {
for (int n=0; n<5; n++) {
// shift data
for (i=pnt-1; i>0; i--) {
for (int i=pnt-1; i>0; i--) {
sim->figtimer.linedata[n][2*i+1] = sim->figtimer.linedata[n][2*i-1];
}
@@ -329,9 +325,9 @@ void profilerupdate(mj::Simulate* sim) {
// update figsize
pnt = mjMIN(201, sim->figsize.linepnt[0]+1);
for (n=0; n<6; n++) {
for (int n=0; n<6; n++) {
// shift data
for (i=pnt-1; i>0; i--) {
for (int i=pnt-1; i>0; i--) {
sim->figsize.linedata[n][2*i+1] = sim->figsize.linedata[n][2*i-1];
}
@@ -542,7 +538,6 @@ void watch(mj::Simulate* sim) {
// make physics section of UI
void makephysics(mj::Simulate* sim, int oldstate) {
int i;
mjOption& opt = sim->m->opt;
mjuiDef defPhysics[] = {
@@ -591,13 +586,13 @@ void makephysics(mj::Simulate* sim, int oldstate) {
{mjITEM_CHECKINT, "", 2, nullptr, ""},
{mjITEM_END}
};
for (i=0; i<mjNDISABLE; i++) {
for (int i=0; i<mjNDISABLE; i++) {
mju::strcpy_arr(defFlag[0].name, mjDISABLESTRING[i]);
defFlag[0].pdata = sim->disable + i;
mjui_add(&sim->ui0, defFlag);
}
mjui_add(&sim->ui0, defEnableFlags);
for (i=0; i<mjNENABLE; i++) {
for (int i=0; i<mjNENABLE; i++) {
mju::strcpy_arr(defFlag[0].name, mjENABLESTRING[i]);
defFlag[0].pdata = sim->enable + i;
mjui_add(&sim->ui0, defFlag);
@@ -611,8 +606,6 @@ void makephysics(mj::Simulate* sim, int oldstate) {
// make rendering section of UI
void makerendering(mj::Simulate* sim, int oldstate) {
int i, j;
mjuiDef defRendering[] = {
{
mjITEM_SECTION,
@@ -665,7 +658,7 @@ void makerendering(mj::Simulate* sim, int oldstate) {
};
// add model cameras, up to UI limit
for (i=0; i<mjMIN(sim->m->ncam, mjMAXUIMULTI-2); i++) {
for (int i=0; i<mjMIN(sim->m->ncam, mjMAXUIMULTI-2); i++) {
// prepare name
char camname[mjMAXUITEXT] = "\n";
if (sim->m->names[sim->m->name_camadr[i]]) {
@@ -691,10 +684,10 @@ void makerendering(mj::Simulate* sim, int oldstate) {
{mjITEM_CHECKBYTE, "", 2, nullptr, ""},
{mjITEM_END}
};
for (i=0; i<mjNVISFLAG; i++) {
for (int i=0; i<mjNVISFLAG; i++) {
// set name, remove "&"
mju::strcpy_arr(defFlag[0].name, mjVISSTRING[i][0]);
for (j=0; j<strlen(mjVISSTRING[i][0]); j++)
for (int j=0; j<strlen(mjVISSTRING[i][0]); j++)
if (mjVISSTRING[i][0][j]=='&') {
mju_strncpy(
defFlag[0].name+j, mjVISSTRING[i][0]+j+1, mju::sizeof_arr(defFlag[0].name)-j);
@@ -718,7 +711,7 @@ void makerendering(mj::Simulate* sim, int oldstate) {
mjui_add(&sim->ui0, defTree);
mjui_add(&sim->ui0, defOpenGL);
for (i=0; i<mjNRNDFLAG; i++) {
for (int i=0; i<mjNRNDFLAG; i++) {
mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]);
if (mjRNDSTRING[i][2][0]) {
mju::sprintf_arr(defFlag[0].other, " %s", mjRNDSTRING[i][2]);
@@ -788,7 +781,6 @@ void makegroup(mj::Simulate* sim, int oldstate) {
// make joint section of UI
void makejoint(mj::Simulate* sim, int oldstate) {
int i;
mjuiDef defJoint[] = {
{mjITEM_SECTION, "Joint", oldstate, nullptr, "AJ"},
@@ -805,7 +797,7 @@ void makejoint(mj::Simulate* sim, int oldstate) {
// add scalar joints, exit if UI limit reached
int itemcnt = 0;
for (i=0; i<sim->m->njnt && itemcnt<mjMAXUIITEM; i++)
for (int i=0; i<sim->m->njnt && itemcnt<mjMAXUIITEM; i++)
if ((sim->m->jnt_type[i]==mjJNT_HINGE || sim->m->jnt_type[i]==mjJNT_SLIDE)) {
// skip if joint group is disabled
if (!sim->vopt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->m->jnt_group[i]))]) {
@@ -838,8 +830,6 @@ void makejoint(mj::Simulate* sim, int oldstate) {
// make control section of UI
void makecontrol(mj::Simulate* sim, int oldstate) {
int i;
mjuiDef defControl[] = {
{mjITEM_SECTION, "Control", oldstate, nullptr, "AC"},
{mjITEM_BUTTON, "Clear all", 2},
@@ -856,7 +846,7 @@ void makecontrol(mj::Simulate* sim, int oldstate) {
// add controls, exit if UI limit reached (Clear button already added)
int itemcnt = 1;
for (i=0; i<sim->m->nu && itemcnt<mjMAXUIITEM; i++) {
for (int i=0; i<sim->m->nu && itemcnt<mjMAXUIITEM; i++) {
// skip if actuator group is disabled
if (!sim->vopt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->m->actuator_group[i]))]) {
continue;
@@ -886,11 +876,9 @@ void makecontrol(mj::Simulate* sim, int oldstate) {
// make model-dependent UI sections
void makesections(mj::Simulate* sim) {
int i;
// get section open-close state, UI 0
int oldstate0[NSECT0];
for (i=0; i<NSECT0; i++) {
for (int i=0; i<NSECT0; i++) {
oldstate0[i] = 0;
if (sim->ui0.nsect>i) {
oldstate0[i] = sim->ui0.sect[i].state;
@@ -899,7 +887,7 @@ void makesections(mj::Simulate* sim) {
// get section open-close state, UI 1
int oldstate1[NSECT1];
for (i=0; i<NSECT1; i++) {
for (int i=0; i<NSECT1; i++) {
oldstate1[i] = 0;
if (sim->ui1.nsect>i) {
oldstate1[i] = sim->ui1.sect[i].state;
@@ -985,13 +973,11 @@ void copycamera(mj::Simulate* sim) {
// update UI 0 when MuJoCo structures change (except for joint sliders)
void updatesettings(mj::Simulate* sim) {
int i;
// physics flags
for (i=0; i<mjNDISABLE; i++) {
for (int i=0; i<mjNDISABLE; i++) {
sim->disable[i] = ((sim->m->opt.disableflags & (1<<i)) !=0);
}
for (i=0; i<mjNENABLE; i++) {
for (int i=0; i<mjNENABLE; i++) {
sim->enable[i] = ((sim->m->opt.enableflags & (1<<i)) !=0);
}
@@ -1094,7 +1080,6 @@ void uiEvent(mjuiState* state) {
mj::Simulate* sim = static_cast<mj::Simulate*>(state->userdata);
mjModel* m = sim->m;
mjData* d = sim->d;
int i;
char err[200];
// call UI 0 if event is directed to it
@@ -1202,22 +1187,25 @@ void uiEvent(mjuiState* state) {
case 5: // Adjust key
case 6: // Load key
i = sim->key;
{
int i = sim->key;
d->time = m->key_time[i];
mju_copy(d->qpos, m->key_qpos+i*m->nq, m->nq);
mju_copy(d->qvel, m->key_qvel+i*m->nv, m->nv);
mju_copy(d->act, m->key_act+i*m->na, m->na);
mju_copy(d->mocap_pos, m->key_mpos+i*3*m->nmocap, 3*m->nmocap);
mju_copy(d->mocap_quat, m->key_mquat+i*4*m->nmocap, 4*m->nmocap);
mju_copy(d->ctrl, m->key_ctrl+i*m->nu, m->nu);
mju_copy(d->qpos, m->key_qpos + i * m->nq, m->nq);
mju_copy(d->qvel, m->key_qvel + i * m->nv, m->nv);
mju_copy(d->act, m->key_act + i * m->na, m->na);
mju_copy(d->mocap_pos, m->key_mpos + i * 3 * m->nmocap, 3 * m->nmocap);
mju_copy(d->mocap_quat, m->key_mquat + i * 4 * m->nmocap,
4 * m->nmocap);
mju_copy(d->ctrl, m->key_ctrl + i * m->nu, m->nu);
mj_forward(m, d);
profilerupdate(sim);
sensorupdate(sim);
updatesettings(sim);
break;
} break;
case 7: // Save key
i = sim->key;
{
int i = sim->key;
m->key_time[i] = d->time;
mju_copy(m->key_qpos+i*m->nq, d->qpos, m->nq);
mju_copy(m->key_qvel+i*m->nv, d->qvel, m->nv);
@@ -1225,7 +1213,7 @@ void uiEvent(mjuiState* state) {
mju_copy(m->key_mpos+i*3*m->nmocap, d->mocap_pos, 3*m->nmocap);
mju_copy(m->key_mquat+i*4*m->nmocap, d->mocap_quat, 4*m->nmocap);
mju_copy(m->key_ctrl+i*m->nu, d->ctrl, m->nu);
break;
} break;
}
}
@@ -1233,14 +1221,14 @@ void uiEvent(mjuiState* state) {
else if (it && it->sectionid==SECT_PHYSICS) {
// update disable flags in mjOption
m->opt.disableflags = 0;
for (i=0; i<mjNDISABLE; i++)
for (int i=0; i<mjNDISABLE; i++)
if (sim->disable[i]) {
m->opt.disableflags |= (1<<i);
}
// update enable flags in mjOption
m->opt.enableflags = 0;
for (i=0; i<mjNENABLE; i++)
for (int i=0; i<mjNENABLE; i++)
if (sim->enable[i]) {
m->opt.enableflags |= (1<<i);
}
src/user/user_composite.cc
+12 -18
View File
@@ -122,11 +122,9 @@ bool mjCComposite::AddDefaultJoint(char* error, int error_sz) {
// set defaults, after reading top-level info and skin
void mjCComposite::SetDefault(void) {
int i;
// determine dimensionality
int tmpdim = 0;
for (i=0; i<3; i++) {
for (int i=0; i<3; i++) {
if (count[i]>1) {
tmpdim++;
}
@@ -149,7 +147,7 @@ void mjCComposite::SetDefault(void) {
type==mjCOMPTYPE_LOOP ||
type==mjCOMPTYPE_CABLE ||
(type==mjCOMPTYPE_GRID && tmpdim==1)) {
for (i=0; i<mjNCOMPKINDS; i++) {
for (int i=0; i<mjNCOMPKINDS; i++) {
def[i].geom.group = 0;
def[i].tendon.group = 0;
}
@@ -196,7 +194,7 @@ void mjCComposite::SetDefault(void) {
AdjustSoft(solrefsmooth, solimpsmooth, 1);
// soft fix everywhere
for (i=0; i<mjNCOMPKINDS; i++) {
for (int i=0; i<mjNCOMPKINDS; i++) {
AdjustSoft(def[i].equality.solref, def[i].equality.solimp, 1);
}
@@ -1173,7 +1171,6 @@ void mjCComposite::BoxProject(double* pos) {
// make 3d box, ellipsoid or cylinder
bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz) {
int ix, iy, iz;
char txt[100];
// check dim
@@ -1199,9 +1196,9 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro
ten->name = txt;
// create bodies, geoms and joints: outside shell only
for (ix=0; ix<count[0]; ix++) {
for (iy=0; iy<count[1]; iy++) {
for (iz=0; iz<count[2]; iz++) {
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
if (ix==0 || ix==count[0]-1 ||
iy==0 || iy==count[1]-1 ||
iz==0 || iz==count[2]-1) {
@@ -1297,11 +1294,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro
// add shear tendons to 2D
void mjCComposite::MakeShear(mjCModel* model) {
int ix, iy;
char txt[100], txt1[100], txt2[100];
for (ix=0; ix<count[0]-1; ix++) {
for (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++) {
// recover site names
mju::sprintf_arr(txt1, "%sS%d_%d", prefix.c_str(), ix, iy);
mju::sprintf_arr(txt2, "%sS%d_%d", prefix.c_str(), ix+1, iy+1);
@@ -2188,12 +2184,11 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
// make one face of 3D skin, box
void mjCComposite::MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side,
int& vcnt, const char* format) {
int i0, i1;
char txt[100];
// loop over bodies/vertices of specified face
for (i0=0; i0<c0; i0++) {
for (i1=0; i1<c1; i1++) {
for (int i0=0; i0<c0; i0++) {
for (int i1=0; i1<c1; i1++) {
// vertex
skin->vert.push_back(0);
skin->vert.push_back(0);
@@ -2248,12 +2243,11 @@ void mjCComposite::MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side,
// make one face of 3D skin, smooth
void mjCComposite::MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side,
const std::map<string, int>& vmap, const char* format) {
int i0, i1;
char txt00[100], txt01[100], txt10[100], txt11[100];
// loop over bodies/vertices of specified face
for (i0=0; i0<c0; i0++) {
for (i1=0; i1<c1; i1++) {
for (int i0=0; i0<c0; i0++) {
for (int i1=0; i1<c1; i1++) {
// body names
mju::sprintf_arr(txt00, format, prefix.c_str(), i0, i1);
mju::sprintf_arr(txt01, format, prefix.c_str(), i0, i1+1);
src/user/user_mesh.cc
+61 -69
View File
@@ -387,8 +387,6 @@ double* mjCMesh::GetQuatPtr(mjtMeshType type) {
// set geom size to match mesh
void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
int i;
// copy mesh pos into meshpos
mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3);
@@ -438,7 +436,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
case mjGEOM_SPHERE:
// find maximum distance
geom->size[0] = 0;
for (i=0; i<nvert; i++) {
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] = mjMAX(geom->size[0], dst);
@@ -450,7 +448,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
// find maximum distance in XY, separately in Z
geom->size[0] = 0;
geom->size[1] = 0;
for (i=0; i<nvert; i++) {
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]));
@@ -464,7 +462,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
// special handling of capsule: consider curved cap
if (geom->type==mjGEOM_CAPSULE) {
geom->size[1] = 0;
for (i=0; i<nvert; i++) {
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]) +
@@ -515,13 +513,12 @@ quicksortfunc(vertcompare, context, el1, el2) {
// remove repeated vertices
void mjCMesh::RemoveRepeated() {
int i, j;
int repeated = 0;
// allocate sort and redirection indices, set to identity
auto index = std::unique_ptr<int[]>(new int[nvert]);
auto redirect = std::unique_ptr<int[]>(new int[nvert]);
for (i=0; i < nvert; i++) {
for (int i=0; i < nvert; i++) {
index[i] = redirect[i] = i;
}
@@ -529,7 +526,7 @@ void mjCMesh::RemoveRepeated() {
mjQUICKSORT(index.get(), nvert, sizeof(int), vertcompare, vert);
// find repeated vertices, set redirect
for (i=1; i < nvert; i++) {
for (int i=1; i < nvert; i++) {
if (vert[3*index[i]] == vert[3*index[i-1]] &&
vert[3*index[i]+1] == vert[3*index[i-1]+1] &&
vert[3*index[i]+2] == vert[3*index[i-1]+2]) {
@@ -541,8 +538,8 @@ void mjCMesh::RemoveRepeated() {
// compress vertices, change face data
if (repeated) {
// track redirections until non-redirected vertex, set
for (i=0; i<nvert; i++) {
j = i;
for (int i=0; i<nvert; i++) {
int j = i;
while (redirect[j]!=j) {
j = redirect[j];
}
@@ -550,8 +547,8 @@ void mjCMesh::RemoveRepeated() {
}
// find good vertices, compress, reuse index to save compressed position
j = 0;
for (i=0; i<nvert; i++) {
int j = 0;
for (int i=0; i<nvert; i++) {
if (redirect[i]==i) {
index[i] = j;
memcpy(vert+3*j, vert+3*i, 3*sizeof(float));
@@ -562,7 +559,7 @@ void mjCMesh::RemoveRepeated() {
}
// recompute face data to reflect compressed vertices
for (i=0; i<3*nface; i++) {
for (int i=0; i<3*nface; i++) {
face[i] = index[redirect[face[i]]];
// sanity check, SHOULD NOT OCCUR
@@ -908,9 +905,8 @@ void mjCMesh::LoadMSH(const mjVFS* vfs) {
// rearange face data if left-handed scaling
if (nface && !righthand) {
int i, tmp;
for (i=0; i<nface; i++) {
tmp = face[3*i+1];
for (int i=0; i<nface; i++) {
int tmp = face[3*i+1];
face[3*i+1] = face[3*i+2];
face[3*i+2] = tmp;
}
@@ -932,7 +928,6 @@ void mjCMesh::Process() {
double area = 0;
double inert[6] = {0, 0, 0, 0, 0, 0};
int i, j;
double nrm[3];
double cen[3];
@@ -943,7 +938,7 @@ void mjCMesh::Process() {
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
// process vertices
for (i=0; i<nvert; i++) {
for (int i=0; i<nvert; i++) {
vert[3*i] -= rp[0];
vert[3*i+1] -= rp[1];
vert[3*i+2] -= rp[2];
@@ -959,7 +954,7 @@ void mjCMesh::Process() {
mju_quat2Mat(mat, quat);
// process vertices
for (i=0; i<nvert; i++) {
for (int i=0; i<nvert; i++) {
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
mju_rotVecMatT(p1, p0, mat);
vert[3*i] = (float) p1[0];
@@ -968,7 +963,7 @@ void mjCMesh::Process() {
}
// process normals
for (i=0; i<nnormal; i++) {
for (int i=0; i<nnormal; i++) {
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
mju_rotVecMatT(n1, n0, mat);
normal[3*i] = (float) n1[0];
@@ -979,13 +974,13 @@ void mjCMesh::Process() {
// scale
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
for (i=0; i<nvert; i++) {
for (int i=0; i<nvert; i++) {
vert[3*i] *= scale[0];
vert[3*i+1] *= scale[1];
vert[3*i+2] *= scale[2];
}
for (i=0; i<nnormal; i++) {
for (int i=0; i<nnormal; i++) {
normal[3*i] *= scale[0];
normal[3*i+1] *= scale[1];
normal[3*i+2] *= scale[2];
@@ -993,7 +988,7 @@ void mjCMesh::Process() {
}
// normalize normals
for (i=0; i<nnormal; i++) {
for (int i=0; i<nnormal; i++) {
// compute length
float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
@@ -1011,9 +1006,9 @@ void mjCMesh::Process() {
}
// find centroid of faces
for (i=0; i<nface; i++) {
for (int i=0; i<nface; i++) {
// check vertex indices
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
}
@@ -1023,7 +1018,7 @@ void mjCMesh::Process() {
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
// accumulate
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
facecen[j] += a*cen[j];
}
area += a;
@@ -1036,14 +1031,14 @@ void mjCMesh::Process() {
}
// finalize centroid of faces
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
facecen[j] /= area;
}
}
// compute CoM and volume from pyramid volumes
GetVolumeRef(type) = 0;
for (i=0; i<nface; i++) {
for (int i=0; i<nface; i++) {
// get area, normal and center
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
@@ -1058,7 +1053,7 @@ void mjCMesh::Process() {
// add pyramid com
GetVolumeRef(type) += vol;
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
}
}
@@ -1070,15 +1065,15 @@ void mjCMesh::Process() {
}
// finalize CoM, save as mesh center
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
CoM[j] /= GetVolumeRef(type);
}
mjuu_copyvec(GetPosPtr(type), CoM, 3);
// re-center mesh at CoM
if (type==mjVOLUME_MESH) {
for (i=0; i<nvert; i++) {
for (j=0; j<3; j++) {
for (int i=0; i<nvert; i++) {
for (int j=0; j<3; j++) {
vert[3*i+j] -= CoM[j];
}
}
@@ -1088,7 +1083,7 @@ void mjCMesh::Process() {
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
double P[6] = {0, 0, 0, 0, 0, 0};
GetVolumeRef(type) = 0;
for (i=0; i<nface; i++) {
for (int i=0; i<nface; i++) {
float* D = vert+3*face[3*i];
float* E = vert+3*face[3*i+1];
float* F = vert+3*face[3*i+2];
@@ -1104,7 +1099,7 @@ void mjCMesh::Process() {
// apply formula, accumulate
GetVolumeRef(type) += vol;
for (j=0; j<6; j++) {
for (int j=0; j<6; j++) {
P[j] += def->geom.density*vol /
(type==mjSHELL_MESH ? 12 : 20) * (
2*(D[k[j][0]] * D[k[j][1]] +
@@ -1153,7 +1148,7 @@ void mjCMesh::Process() {
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
// copy quat
for (j=0; j<4; j++) {
for (int j=0; j<4; j++) {
GetQuatPtr(type)[j] = type == mjVOLUME_MESH ? quattmp[j] : GetQuatPtr(mjVOLUME_MESH)[j];
}
@@ -1162,12 +1157,12 @@ void mjCMesh::Process() {
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
for (i=0; i<nvert; i++) {
for (int i=0; i<nvert; i++) {
// vertices
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
double res[3];
mjuu_mulvecmat(res, vec, mat);
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
vert[3*i+j] = (float) res[j];
// axis-aligned bounding box
@@ -1175,12 +1170,12 @@ void mjCMesh::Process() {
aabb[j+3] = mjMAX(aabb[j+3], res[j]);
}
}
for (i=0; i<nnormal; i++) {
for (int i=0; i<nnormal; i++) {
// normals
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
double res[3];
mjuu_mulvecmat(res, nrm, mat);
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
normal[3*i+j] = (float) res[j];
}
}
@@ -1225,7 +1220,7 @@ double& mjCMesh::GetVolumeRef(mjtMeshType type) {
// make graph describing convex hull
void mjCMesh::MakeGraph(void) {
int i, adr, ok, curlong, totlong, exitcode;
int adr, ok, curlong, totlong, exitcode;
double* data;
facetT* facet, **facetp;
vertexT* vertex, *vertex1, **vertex1p;
@@ -1241,7 +1236,7 @@ void mjCMesh::MakeGraph(void) {
if (!data) {
throw mjCError(this, "could not allocate data for qhull");
}
for (i=0; i<3*nvert; i++) {
for (int i=0; i<3*nvert; i++) {
data[i] = (double)vert[i];
}
@@ -1282,7 +1277,7 @@ void mjCMesh::MakeGraph(void) {
int* face_globalid = graph + 2 + 3*numvert + 3*numface;
// fill in graph data
i = adr = 0;
int i = adr = 0;
ok = 1;
FORALLvertices {
// point id of this vertex, check
@@ -1380,9 +1375,10 @@ void mjCMesh::MakeGraph(void) {
}
// replace global ids with local ids in edge data
for (i=0; i<numvert+3*numface; i++) {
for (int i=0; i<numvert+3*numface; i++) {
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]) {
edge_localid[i] = adr;
@@ -1428,10 +1424,9 @@ void mjCMesh::CopyGraph(void) {
face = (int*) mju_malloc(3*nface*sizeof(int));
// copy faces
int i, j;
for (i=0; i<nface; i++) {
for (int i=0; i<nface; i++) {
// address in graph
j = 2 + 3*numvert + 3*nface + 3*i;
int j = 2 + 3*numvert + 3*nface + 3*i;
// copy
face[3*i] = graph[j];
@@ -1444,8 +1439,6 @@ void mjCMesh::CopyGraph(void) {
// compute vertex normals
void mjCMesh::MakeNormal(void) {
int i, j, k;
// only if normal data is missing
if (normal) {
return;
@@ -1462,16 +1455,16 @@ void mjCMesh::MakeNormal(void) {
}
// loop over faces, accumulate vertex normals
for (i=0; i<nface; i++) {
for (int i=0; i<nface; i++) {
// get vertex ids
int vertid[3];
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
vertid[j] = face[3*i+j];
}
// get triangle edges
mjtNum vec01[3], vec02[3];
for (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];
}
@@ -1482,8 +1475,8 @@ void mjCMesh::MakeNormal(void) {
mjtNum area = mju_normalize3(nrm);
// add normal to each vertex with weight = area
for (j=0; j<3; j++) {
for (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];
@@ -1497,16 +1490,16 @@ void mjCMesh::MakeNormal(void) {
memset(nremove, 0, 3*nnormal*sizeof(float));
// remove contributions from faces at large angles with vertex normal
for (i=0; i<nface; i++) {
for (int i=0; i<nface; i++) {
// get vertex ids
int vertid[3];
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
vertid[j] = face[3*i+j];
}
// get triangle edges
mjtNum vec01[3], vec02[3];
for (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];
}
@@ -1517,14 +1510,14 @@ void mjCMesh::MakeNormal(void) {
mjtNum area = mju_normalize3(nrm);
// compare to vertex normal, subtract contribution if dot product too small
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
// normalized vertex normal
mjtNum vnrm[3] = {normal[3*vertid[j]], normal[3*vertid[j]+1], normal[3*vertid[j]+2]};
mju_normalize3(vnrm);
// dot too small: remove
if (mju_dot3(nrm, vnrm)<0.8) {
for (k=0; k<3; k++) {
for (int k=0; k<3; k++) {
nremove[3*vertid[j]+k] += nrm[k]*area;
}
}
@@ -1532,14 +1525,14 @@ void mjCMesh::MakeNormal(void) {
}
// apply removal, free nremove
for (i=0; i<3*nnormal; i++) {
for (int i=0; i<3*nnormal; i++) {
normal[i] -= nremove[i];
}
mju_free(nremove);
}
// normalize normals
for (i=0; i<nnormal; i++) {
for (int i=0; i<nnormal; i++) {
// compute length
float len = sqrtf(normal[3*i]*normal[3*i] +
normal[3*i+1]*normal[3*i+1] +
@@ -1547,7 +1540,7 @@ void mjCMesh::MakeNormal(void) {
// divide by length
if (len>mjMINVAL)
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
normal[3*i+j] /= len;
} else {
normal[3*i] = normal[3*i+1] = 0;
@@ -1608,7 +1601,6 @@ mjCSkin::~mjCSkin() {
// compiler
void mjCSkin::Compile(const mjVFS* vfs) {
size_t i, j;
// load file
if (!file.empty()) {
@@ -1678,7 +1670,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// resolve body names
bodyid.resize(nbone);
for (i=0; i<nbone; i++) {
for (int i=0; i<nbone; i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname[i]);
if (!pbody) {
throw mjCError(this, "unknown body '%s' in skin", bodyname[i].c_str());
@@ -1701,7 +1693,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
fill(vw.begin(), vw.end(), 0.0f);
// accumulate vertex weights from all bones
for (i=0; i<nbone; i++) {
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) {
@@ -1709,7 +1701,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
}
// accumulate weights in global array
for (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) {
@@ -1722,21 +1714,21 @@ void mjCSkin::Compile(const mjVFS* vfs) {
}
// check coverage
for (i=0; i<nvert; i++) {
for (int i=0; i<nvert; i++) {
if (vw[i]<=mjMINVAL) {
throw mjCError(this, "vertex %d must have positive total weight in skin", NULL, i);
}
}
// normalize vertex weights
for (i=0; i<nbone; i++) {
for (j=0; j<vertid[i].size(); j++) {
for (int i=0; i<nbone; i++) {
for (int j=0; j<vertid[i].size(); j++) {
vertweight[i][j] /= vw[vertid[i][j]];
}
}
// normalize bindquat
for (i=0; i<nbone; i++) {
for (int i=0; i<nbone; i++) {
mjtNum quat[4] = {
(mjtNum)bindquat[4*i],
(mjtNum)bindquat[4*i+1],
src/user/user_model.cc
+150 -168
View File
@@ -181,29 +181,27 @@ mjCModel::mjCModel() {
// destructor
mjCModel::~mjCModel() {
unsigned int i;
// delete kinematic tree and all objects allocated in it
delete bodies[0];
// delete objects allocated in mjCModel
for (i=0; i<meshes.size(); i++) delete meshes[i];
for (i=0; i<skins.size(); i++) delete skins[i];
for (i=0; i<hfields.size(); i++) delete hfields[i];
for (i=0; i<textures.size(); i++) delete textures[i];
for (i=0; i<materials.size(); i++) delete materials[i];
for (i=0; i<pairs.size(); i++) delete pairs[i];
for (i=0; i<excludes.size(); i++) delete excludes[i];
for (i=0; i<equalities.size(); i++) delete equalities[i];
for (i=0; i<tendons.size(); i++) delete tendons[i]; // also deletes wraps
for (i=0; i<actuators.size(); i++) delete actuators[i];
for (i=0; i<sensors.size(); i++) delete sensors[i];
for (i=0; i<numerics.size(); i++) delete numerics[i];
for (i=0; i<texts.size(); i++) delete texts[i];
for (i=0; i<tuples.size(); i++) delete tuples[i];
for (i=0; i<keys.size(); i++) delete keys[i];
for (i=0; i<plugins.size(); i++) delete plugins[i];
for (i=0; i<defaults.size(); i++) delete defaults[i];
for (int i=0; i<meshes.size(); i++) delete meshes[i];
for (int i=0; i<skins.size(); i++) delete skins[i];
for (int i=0; i<hfields.size(); i++) delete hfields[i];
for (int i=0; i<textures.size(); i++) delete textures[i];
for (int i=0; i<materials.size(); i++) delete materials[i];
for (int i=0; i<pairs.size(); i++) delete pairs[i];
for (int i=0; i<excludes.size(); i++) delete excludes[i];
for (int i=0; i<equalities.size(); i++) delete equalities[i];
for (int i=0; i<tendons.size(); i++) delete tendons[i]; // also deletes wraps
for (int i=0; i<actuators.size(); i++) delete actuators[i];
for (int i=0; i<sensors.size(); i++) delete sensors[i];
for (int i=0; i<numerics.size(); i++) delete numerics[i];
for (int i=0; i<texts.size(); i++) delete texts[i];
for (int i=0; i<tuples.size(); i++) delete tuples[i];
for (int i=0; i<keys.size(); i++) delete keys[i];
for (int i=0; i<plugins.size(); i++) delete plugins[i];
for (int i=0; i<defaults.size(); i++) delete defaults[i];
// clear pointer lists created in model construction
meshes.clear();
@@ -705,32 +703,28 @@ bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) {
// make lists of objects in tree: bodies, geoms, joints, sites, cameras, lights
void mjCModel::MakeLists(mjCBody* body) {
unsigned int i;
// add this body if not world
if (body!=bodies[0]) {
bodies.push_back(body);
}
// add body's geoms, joints, sites, cameras, lights
for (i=0; i<body->geoms.size(); i++) geoms.push_back(body->geoms[i]);
for (i=0; i<body->joints.size(); i++) joints.push_back(body->joints[i]);
for (i=0; i<body->sites.size(); i++) sites.push_back(body->sites[i]);
for (i=0; i<body->cameras.size(); i++) cameras.push_back(body->cameras[i]);
for (i=0; i<body->lights.size(); i++) lights.push_back(body->lights[i]);
for (int i=0; i<body->geoms.size(); i++) geoms.push_back(body->geoms[i]);
for (int i=0; i<body->joints.size(); i++) joints.push_back(body->joints[i]);
for (int i=0; i<body->sites.size(); i++) sites.push_back(body->sites[i]);
for (int i=0; i<body->cameras.size(); i++) cameras.push_back(body->cameras[i]);
for (int i=0; i<body->lights.size(); i++) lights.push_back(body->lights[i]);
// recursive call to all child bodies
for (i=0; i<body->bodies.size(); i++) MakeLists(body->bodies[i]);
for (int i=0; i<body->bodies.size(); i++) MakeLists(body->bodies[i]);
}
// index assets
void mjCModel::IndexAssets(void) {
unsigned int i;
// assets referenced in geoms
for (i=0; i<geoms.size(); i++) {
for (int i=0; i<geoms.size(); i++) {
mjCGeom* pgeom = geoms[i];
// find material by name
@@ -765,7 +759,7 @@ void mjCModel::IndexAssets(void) {
}
// assets referenced in skins
for (i=0; i<skins.size(); i++) {
for (int i=0; i<skins.size(); i++) {
mjCSkin* pskin = skins[i];
// find material by name
@@ -780,7 +774,7 @@ void mjCModel::IndexAssets(void) {
}
// materials referenced in sites
for (i=0; i<sites.size(); i++) {
for (int i=0; i<sites.size(); i++) {
mjCSite* psite = sites[i];
// find material by name
@@ -795,7 +789,7 @@ void mjCModel::IndexAssets(void) {
}
// materials referenced in tendons
for (i=0; i<tendons.size(); i++) {
for (int i=0; i<tendons.size(); i++) {
mjCTendon* pten = tendons[i];
// find material by name
@@ -810,7 +804,7 @@ void mjCModel::IndexAssets(void) {
}
// textures referenced in materials
for (i=0; i<materials.size(); i++) {
for (int i=0; i<materials.size(); i++) {
mjCMaterial* pmat = materials[i];
// find texture by name
@@ -830,10 +824,9 @@ void mjCModel::IndexAssets(void) {
// if asset name is missing, set to filename
void mjCModel::SetDefaultNames(void) {
string stripped;
unsigned int i;
// meshes
for (i=0; i<meshes.size(); i++) {
for (int i=0; i<meshes.size(); i++) {
if (meshes[i]->name.empty()) {
stripped = mjuu_strippath(meshes[i]->file);
meshes[i]->name = mjuu_stripext(stripped);
@@ -846,7 +839,7 @@ void mjCModel::SetDefaultNames(void) {
}
// skins
for (i=0; i<skins.size(); i++) {
for (int i=0; i<skins.size(); i++) {
if (skins[i]->name.empty()) {
stripped = mjuu_strippath(skins[i]->file);
skins[i]->name = mjuu_stripext(stripped);
@@ -854,7 +847,7 @@ void mjCModel::SetDefaultNames(void) {
}
// hfields
for (i=0; i<hfields.size(); i++) {
for (int i=0; i<hfields.size(); i++) {
if (hfields[i]->name.empty()) {
stripped = mjuu_strippath(hfields[i]->file);
hfields[i]->name = mjuu_stripext(stripped);
@@ -867,7 +860,7 @@ void mjCModel::SetDefaultNames(void) {
}
// textures
for (i=0; i<textures.size(); i++) {
for (int i=0; i<textures.size(); i++) {
if (textures[i]->name.empty()) {
stripped = mjuu_strippath(textures[i]->file);
textures[i]->name = mjuu_stripext(stripped);
@@ -880,7 +873,7 @@ void mjCModel::SetDefaultNames(void) {
}
// materials: name check only
for (i=0; i<materials.size(); i++) {
for (int i=0; i<materials.size(); i++) {
if (materials[i]->name.empty()) {
throw mjCError(materials[i], "empty name in material");
}
@@ -895,8 +888,6 @@ const int nVEL[4] = {6, 3, 1, 1};
// set array sizes
void mjCModel::SetSizes(void) {
int i, j;
// set from object list sizes
nbody = (int)bodies.size();
njnt = (int)joints.size();
@@ -921,13 +912,13 @@ void mjCModel::SetSizes(void) {
nplugin = (int)plugins.size();
// nq, nv
for (i=0; i<njnt; i++) {
for (int i=0; i<njnt; i++) {
nq += nPOS[joints[i]->type];
nv += nVEL[joints[i]->type];
}
// nu, na
for (i=0; i<(int)actuators.size(); i++) {
for (int i=0; i<(int)actuators.size(); i++) {
if (actuators[i]->dyntype == mjDYN_NONE) {
nu++;
} else {
@@ -937,12 +928,12 @@ void mjCModel::SetSizes(void) {
}
// nbvh
for (i=0; i<nbody; i++) {
for (int i=0; i<nbody; i++) {
nbvh += bodies[i]->nbvh;
}
// nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph
for (i=0; i<nmesh; i++) {
for (int i=0; i<nmesh; i++) {
nmeshvert += meshes[i]->nvert;
nmeshnormal += meshes[i]->nnormal;
nmeshface += meshes[i]->nface;
@@ -951,67 +942,67 @@ void mjCModel::SetSizes(void) {
}
// nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert
for (i=0; i<nskin; i++) {
for (int i=0; i<nskin; i++) {
nskinvert += skins[i]->vert.size()/3;
nskintexvert += skins[i]->texcoord.size()/2;
nskinface += skins[i]->face.size()/3;
nskinbone += skins[i]->bodyid.size();
for (j=0; j<skins[i]->bodyid.size(); j++) {
for (int j=0; j<skins[i]->bodyid.size(); j++) {
nskinbonevert += skins[i]->vertid[j].size();
}
}
// nhfielddata
for (i=0; i<nhfield; i++) nhfielddata += hfields[i]->nrow * hfields[i]->ncol;
for (int i=0; i<nhfield; i++) nhfielddata += hfields[i]->nrow * hfields[i]->ncol;
// ntexdata
for (i=0; i<ntex; i++) ntexdata += 3 * textures[i]->width * textures[i]->height;
for (int i=0; i<ntex; i++) ntexdata += 3 * textures[i]->width * textures[i]->height;
// nwrap
for (i=0; i<ntendon; i++) nwrap += (int)tendons[i]->path.size();
for (int i=0; i<ntendon; i++) nwrap += (int)tendons[i]->path.size();
// nsensordata
for (i=0; i<nsensor; i++) nsensordata += sensors[i]->dim;
for (int i=0; i<nsensor; i++) nsensordata += sensors[i]->dim;
// nnumericdata
for (i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size;
for (int i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size;
// ntextdata
for (i=0; i<ntext; i++) ntextdata += (int)texts[i]->data.size() + 1;
for (int i=0; i<ntext; i++) ntextdata += (int)texts[i]->data.size() + 1;
// ntupledata
for (i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype.size();
for (int i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype.size();
// npluginattr
for (i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
for (int i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
// nnames
nnames = (int)modelname.size() + 1;
for (i=0; i<nbody; i++) nnames += (int)bodies[i]->name.length() + 1;
for (i=0; i<njnt; i++) nnames += (int)joints[i]->name.length() + 1;
for (i=0; i<ngeom; i++) nnames += (int)geoms[i]->name.length() + 1;
for (i=0; i<nsite; i++) nnames += (int)sites[i]->name.length() + 1;
for (i=0; i<ncam; i++) nnames += (int)cameras[i]->name.length() + 1;
for (i=0; i<nlight; i++) nnames += (int)lights[i]->name.length() + 1;
for (i=0; i<nmesh; i++) nnames += (int)meshes[i]->name.length() + 1;
for (i=0; i<nskin; i++) nnames += (int)skins[i]->name.length() + 1;
for (i=0; i<nhfield; i++) nnames += (int)hfields[i]->name.length() + 1;
for (i=0; i<ntex; i++) nnames += (int)textures[i]->name.length() + 1;
for (i=0; i<nmat; i++) nnames += (int)materials[i]->name.length() + 1;
for (i=0; i<npair; i++) nnames += (int)pairs[i]->name.length() + 1;
for (i=0; i<nexclude; i++) nnames += (int)excludes[i]->name.length() + 1;
for (i=0; i<neq; i++) nnames += (int)equalities[i]->name.length() + 1;
for (i=0; i<ntendon; i++) nnames += (int)tendons[i]->name.length() + 1;
for (i=0; i<nu; i++) nnames += (int)actuators[i]->name.length() + 1;
for (i=0; i<nsensor; i++) nnames += (int)sensors[i]->name.length() + 1;
for (i=0; i<nnumeric; i++) nnames += (int)numerics[i]->name.length() + 1;
for (i=0; i<ntext; i++) nnames += (int)texts[i]->name.length() + 1;
for (i=0; i<ntuple; i++) nnames += (int)tuples[i]->name.length() + 1;
for (i=0; i<nkey; i++) nnames += (int)keys[i]->name.length() + 1;
for (i=0; i<nplugin; i++) nnames += (int)plugins[i]->name.length() + 1;
for (int i=0; i<nbody; i++) nnames += (int)bodies[i]->name.length() + 1;
for (int i=0; i<njnt; i++) nnames += (int)joints[i]->name.length() + 1;
for (int i=0; i<ngeom; i++) nnames += (int)geoms[i]->name.length() + 1;
for (int i=0; i<nsite; i++) nnames += (int)sites[i]->name.length() + 1;
for (int i=0; i<ncam; i++) nnames += (int)cameras[i]->name.length() + 1;
for (int i=0; i<nlight; i++) nnames += (int)lights[i]->name.length() + 1;
for (int i=0; i<nmesh; i++) nnames += (int)meshes[i]->name.length() + 1;
for (int i=0; i<nskin; i++) nnames += (int)skins[i]->name.length() + 1;
for (int i=0; i<nhfield; i++) nnames += (int)hfields[i]->name.length() + 1;
for (int i=0; i<ntex; i++) nnames += (int)textures[i]->name.length() + 1;
for (int i=0; i<nmat; i++) nnames += (int)materials[i]->name.length() + 1;
for (int i=0; i<npair; i++) nnames += (int)pairs[i]->name.length() + 1;
for (int i=0; i<nexclude; i++) nnames += (int)excludes[i]->name.length() + 1;
for (int i=0; i<neq; i++) nnames += (int)equalities[i]->name.length() + 1;
for (int i=0; i<ntendon; i++) nnames += (int)tendons[i]->name.length() + 1;
for (int i=0; i<nu; i++) nnames += (int)actuators[i]->name.length() + 1;
for (int i=0; i<nsensor; i++) nnames += (int)sensors[i]->name.length() + 1;
for (int i=0; i<nnumeric; i++) nnames += (int)numerics[i]->name.length() + 1;
for (int i=0; i<ntext; i++) nnames += (int)texts[i]->name.length() + 1;
for (int i=0; i<ntuple; i++) nnames += (int)tuples[i]->name.length() + 1;
for (int i=0; i<nkey; i++) nnames += (int)keys[i]->name.length() + 1;
for (int i=0; i<nplugin; i++) nnames += (int)plugins[i]->name.length() + 1;
// nemax
for (i=0; i<neq; i++)
for (int i=0; i<neq; i++)
if (equalities[i]->type==mjEQ_CONNECT) {
nemax += 3;
} else if (equalities[i]->type==mjEQ_WELD) {
@@ -1092,8 +1083,6 @@ void* LRfunc(void* arg) {
// compute actuator lengthrange
void mjCModel::LengthRange(mjModel* m, mjData* data) {
int i;
// save options and modify
mjOption saveopt = m->opt;
m->opt.disableflags = mjDSBL_FRICTIONLOSS | mjDSBL_CONTACT | mjDSBL_PASSIVE |
@@ -1107,7 +1096,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
// count actuators that need computation
int cnt = 0;
for (i=0; i<m->nu; i++) {
for (int i=0; i<m->nu; i++) {
// skip depending on mode and type
int ismuscle = (m->actuator_gaintype[i]==mjGAIN_MUSCLE ||
m->actuator_biastype[i]==mjBIAS_MUSCLE);
@@ -1132,7 +1121,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
// single thread
if (!usethread || cnt<2 || nthread<2) {
char err[200];
for (i=0; i<m->nu; i++) {
for (int i=0; i<m->nu; i++) {
if (!mj_setLengthRange(m, data, i, &LRopt, err, 200)) {
throw mjCError(0, err);
}
@@ -1144,7 +1133,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
// allocate mjData for each thread
char err[16][200];
mjData* pdata[16] = {data};
for (i=1; i<nthread; i++) {
for (int i=1; i<nthread; i++) {
pdata[i] = mj_makeData(m);
}
@@ -1156,7 +1145,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
// prepare thread function arguments, clear errors
LRThreadArg arg[16];
for (i=0; i<nthread; i++) {
for (int i=0; i<nthread; i++) {
LRThreadArg temp = {m, pdata[i], i*num, num, &LRopt, err[i], 200};
arg[i] = temp;
err[i][0] = 0;
@@ -1166,12 +1155,12 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
#if defined(_WIN32) || defined(__APPLE__)
// launch threads
thread th[16];
for (i=0; i<nthread; i++) {
for (int i=0; i<nthread; i++) {
th[i] = thread(LRfunc, arg+i);
}
// wait for threads to finish
for (i=0; i<nthread; i++) {
for (int i=0; i<nthread; i++) {
th[i].join();
}
@@ -1179,23 +1168,23 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
#else
// launch threads
pthread_t th[16];
for (i=0; i<nthread; i++) {
for (int i=0; i<nthread; i++) {
pthread_create(th+i, NULL, LRfunc, arg+i);
}
// wait for threads to finish
for (i=0; i<nthread; i++) {
for (int i=0; i<nthread; i++) {
pthread_join(th[i], NULL);
}
#endif
// free mjData allocated here
for (i=1; i<nthread; i++) {
for (int i=1; i<nthread; i++) {
mj_deleteData(pdata[i]);
}
// report first error
for (i=0; i<nthread; i++) {
for (int i=0; i<nthread; i++) {
if (err[i][0]) {
throw mjCError(0, err[i]);
}
@@ -1326,14 +1315,13 @@ void mjCModel::CopyNames(mjModel* m) {
// copy objects inside kinematic tree
void mjCModel::CopyTree(mjModel* m) {
int i, j, j1;
int jntadr = 0; // addresses in global arrays
int dofadr = 0;
int qposadr = 0;
int bvh_adr = 0;
// main loop over bodies
for (i=0; i<nbody; i++) {
for (int i=0; i<nbody; i++) {
// get body and parent pointers
mjCBody* pb = bodies[i];
mjCBody* par = bodies[pb->parentid];
@@ -1370,7 +1358,7 @@ void mjCModel::CopyTree(mjModel* m) {
// count free joints
int cntfree = 0;
for (j=0; j<(int)pb->joints.size(); j++) {
for (int j=0; j<(int)pb->joints.size(); j++) {
cntfree += (pb->joints[j]->type == mjJNT_FREE);
}
@@ -1396,7 +1384,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->body_sameframe[i] = IsNullPose(m->body_ipos+3*i, m->body_iquat+4*i);
// init simple: sameframe, and (self-root, or parent is fixed child of world)
j = m->body_parentid[i];
int j = m->body_parentid[i];
m->body_simple[i] = (m->body_sameframe[i] &&
(m->body_rootid[i]==i ||
(m->body_parentid[j]==0 &&
@@ -1409,7 +1397,7 @@ void mjCModel::CopyTree(mjModel* m) {
// loop over joints for this body
int rotfound = 0;
for (j=0; j<(int)pb->joints.size(); j++) {
for (int j=0; j<(int)pb->joints.size(); j++) {
// get pointer and id
mjCJoint* pj = pb->joints[j];
int jid = pj->id;
@@ -1470,7 +1458,7 @@ void mjCModel::CopyTree(mjModel* m) {
}
// set dof fields for this joint
for (j1=0; j1<nVEL[pj->type]; j1++) {
for (int j1=0; j1<nVEL[pj->type]; j1++) {
// set attributes
m->dof_bodyid[dofadr] = pb->id;
m->dof_jntid[dofadr] = jid;
@@ -1494,7 +1482,7 @@ void mjCModel::CopyTree(mjModel* m) {
}
// loop over geoms for this body
for (j=0; j<(int)pb->geoms.size(); j++) {
for (int j=0; j<(int)pb->geoms.size(); j++) {
// get pointer and id
mjCGeom* pg = pb->geoms[j];
int gid = pg->id;
@@ -1549,7 +1537,7 @@ void mjCModel::CopyTree(mjModel* m) {
}
// loop over sites for this body
for (j=0; j<(int)pb->sites.size(); j++) {
for (int j=0; j<(int)pb->sites.size(); j++) {
// get pointer and id
mjCSite* ps = pb->sites[j];
int sid = ps->id;
@@ -1582,7 +1570,7 @@ void mjCModel::CopyTree(mjModel* m) {
}
// loop over cameras for this body
for (j=0; j<(int)pb->cameras.size(); j++) {
for (int j=0; j<(int)pb->cameras.size(); j++) {
// get pointer and id
mjCCamera* pc = pb->cameras[j];
int cid = pc->id;
@@ -1599,7 +1587,7 @@ void mjCModel::CopyTree(mjModel* m) {
}
// loop over lights for this body
for (j=0; j<(int)pb->lights.size(); j++) {
for (int j=0; j<(int)pb->lights.size(); j++) {
// get pointer and id
mjCLight* pl = pb->lights[j];
int lid = pl->id;
@@ -1629,12 +1617,12 @@ void mjCModel::CopyTree(mjModel* m) {
// compute nM and dof_Madr
nM = 0;
for (i=0; i<nv; i++) {
for (int i=0; i<nv; i++) {
// set address of this dof
m->dof_Madr[i] = nM;
// count ancestor dofs including self
j = i;
int j = i;
while (j>=0) {
nM++;
j = m->dof_parentid[j];
@@ -1647,7 +1635,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->nD = nD;
// compute subtreedofs in backward pass over bodies
for (i = nbody - 1; i > 0; i--) {
for (int i = nbody - 1; i > 0; i--) {
// add body dofs to self count
bodies[i]->subtreedofs += bodies[i]->dofnum;
@@ -1662,12 +1650,12 @@ void mjCModel::CopyTree(mjModel* m) {
// compute nB
nB = 0;
for (i = 0; i < nbody; i++) {
for (int i = 0; i < nbody; i++) {
// add subtree dofs (including self)
nB += bodies[i]->subtreedofs;
// add dofs in ancestor bodies
j = bodies[i]->parentid;
int j = bodies[i]->parentid;
while (j > 0) {
nB += bodies[j]->dofnum;
j = bodies[j]->parentid;
@@ -1677,7 +1665,7 @@ void mjCModel::CopyTree(mjModel* m) {
// set dof_simplenum
int scnt = 0;
for (i=nv-1; i>=0; i--) {
for (int i=nv-1; i>=0; i--) {
// dof in simple body
if (m->body_simple[m->dof_bodyid[i]]) {
scnt++;
@@ -1696,7 +1684,7 @@ void mjCModel::CopyTree(mjModel* m) {
// copy objects outside kinematic tree
void mjCModel::CopyObjects(mjModel* m) {
int i, j, adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
int bonevert_adr, graph_adr, data_adr;
// sizes outside call to mj_makeModel
@@ -1712,7 +1700,7 @@ void mjCModel::CopyObjects(mjModel* m) {
texcoord_adr = 0;
face_adr = 0;
graph_adr = 0;
for (i=0; i<nmesh; i++) {
for (int i=0; i<nmesh; i++) {
// get pointer
mjCMesh* pme = meshes[i];
@@ -1756,7 +1744,7 @@ void mjCModel::CopyObjects(mjModel* m) {
texcoord_adr = 0;
bone_adr = 0;
bonevert_adr = 0;
for (i=0; i<nskin; i++) {
for (int i=0; i<nskin; i++) {
// get pointer
mjCSkin* psk = skins[i];
@@ -1789,7 +1777,7 @@ void mjCModel::CopyObjects(mjModel* m) {
psk->bodyid.size()*sizeof(int));
// copy per-bone vertex data, advance vertex counter
for (j=0; j<m->skin_bonenum[i]; j++) {
for (int j=0; j<m->skin_bonenum[i]; j++) {
// set fields
m->skin_bonevertadr[bone_adr+j] = bonevert_adr;
m->skin_bonevertnum[bone_adr+j] = (int)psk->vertid[j].size();
@@ -1813,7 +1801,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// hfields
data_adr = 0;
for (i=0; i<nhfield; i++) {
for (int i=0; i<nhfield; i++) {
// get pointer
mjCHField* phf = hfields[i];
@@ -1832,7 +1820,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// textures
data_adr = 0;
for (i=0; i<ntex; i++) {
for (int i=0; i<ntex; i++) {
// get pointer
mjCTexture* ptex = textures[i];
@@ -1850,7 +1838,7 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// materials
for (i=0; i<nmat; i++) {
for (int i=0; i<nmat; i++) {
// get pointer
mjCMaterial* pmat = materials[i];
@@ -1866,7 +1854,7 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// geom pairs to include
for (i=0; i<npair; i++) {
for (int i=0; i<npair; i++) {
m->pair_dim[i] = pairs[i]->condim;
m->pair_geom1[i] = pairs[i]->geom1;
m->pair_geom2[i] = pairs[i]->geom2;
@@ -1879,12 +1867,12 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// body pairs to exclude
for (i=0; i<nexclude; i++) {
for (int i=0; i<nexclude; i++) {
m->exclude_signature[i] = excludes[i]->signature;
}
// equality constraints
for (i=0; i<neq; i++) {
for (int i=0; i<neq; i++) {
// get pointer
mjCEquality* peq = equalities[i];
@@ -1900,7 +1888,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// tendons and wraps
adr = 0;
for (i=0; i<ntendon; i++) {
for (int i=0; i<ntendon; i++) {
// get pointer
mjCTendon* pte = tendons[i];
@@ -1927,7 +1915,7 @@ void mjCModel::CopyObjects(mjModel* m) {
copyvec(m->tendon_rgba+4*i, pte->rgba, 4);
// set wraps
for (j=0; j<(int)pte->path.size(); j++) {
for (int j=0; j<(int)pte->path.size(); j++) {
m->wrap_type[adr+j] = pte->path[j]->type;
m->wrap_objid[adr+j] = pte->path[j]->objid;
m->wrap_prm[adr+j] = (mjtNum)pte->path[j]->prm;
@@ -1942,7 +1930,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// actuators
adr = 0;
for (i=0; i<nu; i++) {
for (int i=0; i<nu; i++) {
// get pointer
mjCActuator* pac = actuators[i];
@@ -1974,7 +1962,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// sensors
adr = 0;
for (i=0; i<nsensor; i++) {
for (int i=0; i<nsensor; i++) {
// get pointer
mjCSensor* psen = sensors[i];
@@ -1998,17 +1986,17 @@ void mjCModel::CopyObjects(mjModel* m) {
// numeric fields
adr = 0;
for (i=0; i<nnumeric; i++) {
for (int i=0; i<nnumeric; i++) {
// get pointer
mjCNumeric* pcu = numerics[i];
// set fields
m->numeric_adr[i] = adr;
m->numeric_size[i] = pcu->size;
for (j=0; j<(int)pcu->data.size(); j++) {
for (int j=0; j<(int)pcu->data.size(); j++) {
m->numeric_data[adr+j] = (mjtNum)pcu->data[j];
}
for (j=(int)pcu->data.size(); j<(int)pcu->size; j++) {
for (int j=(int)pcu->data.size(); j<(int)pcu->size; j++) {
m->numeric_data[adr+j] = 0;
}
@@ -2018,7 +2006,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// text fields
adr = 0;
for (i=0; i<ntext; i++) {
for (int i=0; i<ntext; i++) {
// get pointer
mjCText* pte = texts[i];
@@ -2033,14 +2021,14 @@ void mjCModel::CopyObjects(mjModel* m) {
// tuple fields
adr = 0;
for (i=0; i<ntuple; i++) {
for (int i=0; i<ntuple; i++) {
// get pointer
mjCTuple* ptu = tuples[i];
// set fields
m->tuple_adr[i] = adr;
m->tuple_size[i] = (int)ptu->objtype.size();
for (j=0; j<m->tuple_size[i]; j++) {
for (int j=0; j<m->tuple_size[i]; j++) {
m->tuple_objtype[adr+j] = (int)ptu->objtype[j];
m->tuple_objid[adr+j] = ptu->objid[j];
m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm[j];
@@ -2051,7 +2039,7 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// copy keyframe data
for (i=0; i<nkey; i++) {
for (int i=0; i<nkey; i++) {
// copy data
m->key_time[i] = (mjtNum)keys[i]->time;
copyvec(m->key_qpos+i*nq, keys[i]->qpos.data(), nq);
@@ -2065,14 +2053,14 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// normalize quaternions in m->key_qpos
for (j=0; j<m->njnt; j++) {
for (int j=0; j<m->njnt; j++) {
if (m->jnt_type[j]==mjJNT_BALL || m->jnt_type[j]==mjJNT_FREE) {
mju_normalize4(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE));
}
}
// normalize quaternions in m->key_mquat
for (j=0; j<nmocap; j++) {
for (int j=0; j<nmocap; j++) {
mju_normalize4(m->key_mquat+i*4*nmocap+4*j);
}
@@ -2103,35 +2091,33 @@ static void changeframe(double childpos[3], double childquat[4],
// reindex elements during fuse
void mjCModel::FuseReindex(mjCBody* body) {
size_t i;
// set parentid and weldid of children
for (i=0; i<body->bodies.size(); i++) {
for (int i=0; i<body->bodies.size(); i++) {
body->bodies[i]->parentid = body->id;
body->bodies[i]->weldid = (!body->bodies[i]->joints.empty() ?
body->bodies[i]->id : body->weldid);
}
// joints
for (i=0; i<body->joints.size(); i++) {
for (int i=0; i<body->joints.size(); i++) {
body->joints[i]->id = (int)joints.size();
joints.push_back(body->joints[i]);
}
// geoms
for (i=0; i<body->geoms.size(); i++) {
for (int i=0; i<body->geoms.size(); i++) {
body->geoms[i]->id = (int)geoms.size();
geoms.push_back(body->geoms[i]);
}
// sites
for (i=0; i<body->sites.size(); i++) {
for (int i=0; i<body->sites.size(); i++) {
body->sites[i]->id = (int)sites.size();
sites.push_back(body->sites[i]);
}
// process children recursively
for (i=0; i<body->bodies.size(); i++) {
for (int i=0; i<body->bodies.size(); i++) {
FuseReindex(body->bodies[i]);
}
}
@@ -2140,8 +2126,6 @@ void mjCModel::FuseReindex(mjCBody* body) {
// fuse static bodies with their parent
void mjCModel::FuseStatic(void) {
int i, j, k;
// skip if model has potential to reference elements with changed ids
if (!skins.empty() ||
!pairs.empty() ||
@@ -2157,7 +2141,7 @@ void mjCModel::FuseStatic(void) {
}
// process fusable bodies
for (i=1; i<bodies.size(); i++) {
for (int i=1; i<bodies.size(); i++) {
// get body and parent
mjCBody* body = bodies[i];
mjCBody* par = bodies[body->parentid];
@@ -2195,7 +2179,7 @@ void mjCModel::FuseStatic(void) {
// compute total mass
par->mass = 0;
mjuu_setvec(par->locipos, 0, 0, 0);
for (j=0; j<2; j++) {
for (int j=0; j<2; j++) {
par->mass += mass[j];
par->locipos[0] += mass[j]*ipos[j][0];
par->locipos[1] += mass[j]*ipos[j][1];
@@ -2219,7 +2203,7 @@ void mjCModel::FuseStatic(void) {
// add inertias
double toti[6] = {0, 0, 0, 0, 0, 0};
for (j=0; j<2; j++) {
for (int j=0; j<2; j++) {
double inertA[6], inertB[6];
double dpos[3] = {
ipos[j][0] - par->locipos[0],
@@ -2229,7 +2213,7 @@ void mjCModel::FuseStatic(void) {
mjuu_globalinertia(inertA, inertia[j], iquat[j]);
mjuu_offcenter(inertB, mass[j], dpos);
for (k=0; k<6; k++) {
for (int k=0; k<6; k++) {
toti[k] += inertA[k] + inertB[k];
}
}
@@ -2247,7 +2231,7 @@ void mjCModel::FuseStatic(void) {
//------------- replace body with its children in parent body list
// change frames of child bodies
for (j=0; j<body->bodies.size(); j++)
for (int j=0; j<body->bodies.size(); j++)
changeframe(body->bodies[j]->locpos, body->bodies[j]->locquat,
body->locpos, body->locquat);
@@ -2280,7 +2264,7 @@ void mjCModel::FuseStatic(void) {
//------------- assign geoms and sites to parent, change frames
// geoms
for (j=0; j<body->geoms.size(); j++) {
for (int j=0; j<body->geoms.size(); j++) {
// assign
body->geoms[j]->body = par;
par->geoms.push_back(body->geoms[j]);
@@ -2290,7 +2274,7 @@ void mjCModel::FuseStatic(void) {
}
// sites
for (j=0; j<body->sites.size(); j++) {
for (int j=0; j<body->sites.size(); j++) {
// assign
body->sites[j]->body = par;
par->sites.push_back(body->sites[j]);
@@ -2321,7 +2305,7 @@ void mjCModel::FuseStatic(void) {
//------------- re-index bodies, joints, geoms, sites
// body ids
for (j=0; j<bodies.size(); j++) {
for (int j=0; j<bodies.size(); j++) {
bodies[j]->id = j;
}
@@ -2872,8 +2856,6 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// get numeric data back from mjModel
bool mjCModel::CopyBack(const mjModel* m) {
int i, j;
// check for null pointer
if (!m) {
errInfo = mjCError(0, "mjModel pointer is null in CopyBack");
@@ -2905,7 +2887,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
visual = m->vis;
// qpos0, qpos_spring
for (i=0; i<njnt; i++) {
for (int i=0; i<njnt; i++) {
switch (joints[i]->type) {
case mjJNT_FREE:
copyvec(bodies[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3);
@@ -2927,7 +2909,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
// body
mjCBody* pb;
for (i=0; i<nbody; i++) {
for (int i=0; i<nbody; i++) {
pb = bodies[i];
copyvec(pb->locpos, m->body_pos+3*i, 3);
@@ -2944,7 +2926,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
// joint and dof
mjCJoint* pj;
for (i=0; i<njnt; i++) {
for (int i=0; i<njnt; i++) {
pj = joints[i];
// joint data
@@ -2961,7 +2943,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// dof data
j = m->jnt_dofadr[i];
int j = m->jnt_dofadr[i];
copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
pj->armature = (double)m->dof_armature[j];
@@ -2971,7 +2953,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
// geom
mjCGeom* pg;
for (i=0; i<ngeom; i++) {
for (int i=0; i<ngeom; i++) {
pg = geoms[i];
copyvec(pg->size, m->geom_size+3*i, 3);
@@ -2991,7 +2973,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// sites
for (i=0; i<nsite; i++) {
for (int i=0; i<nsite; i++) {
copyvec(sites[i]->size, m->site_size + 3 * i, 3);
copyvec(sites[i]->locpos, m->site_pos+3*i, 3);
copyvec(sites[i]->locquat, m->site_quat+4*i, 4);
@@ -3003,7 +2985,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// cameras
for (i=0; i<ncam; i++) {
for (int i=0; i<ncam; i++) {
copyvec(cameras[i]->pos, m->cam_pos+3*i, 3);
copyvec(cameras[i]->quat, m->cam_quat+4*i, 4);
cameras[i]->fovy = (double)m->cam_fovy[i];
@@ -3015,7 +2997,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// lights
for (i=0; i<nlight; i++) {
for (int i=0; i<nlight; i++) {
copyvec(lights[i]->pos, m->light_pos+3*i, 3);
copyvec(lights[i]->dir, m->light_dir+3*i, 3);
copyvec(lights[i]->attenuation, m->light_attenuation+3*i, 3);
@@ -3027,7 +3009,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// materials
for (i=0; i<nmat; i++) {
for (int i=0; i<nmat; i++) {
copyvec(materials[i]->texrepeat, m->mat_texrepeat+2*i, 2);
materials[i]->emission = m->mat_emission[i];
materials[i]->specular = m->mat_specular[i];
@@ -3037,7 +3019,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// pairs
for (i=0; i<npair; i++) {
for (int i=0; i<npair; i++) {
copyvec(pairs[i]->solref, m->pair_solref+mjNREF*i, mjNREF);
copyvec(pairs[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP);
pairs[i]->margin = (double)m->pair_margin[i];
@@ -3046,14 +3028,14 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// equality constraints
for (i=0; i<neq; i++) {
for (int i=0; i<neq; i++) {
copyvec(equalities[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA);
copyvec(equalities[i]->solref, m->eq_solref+mjNREF*i, mjNREF);
copyvec(equalities[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP);
}
// tendons
for (i=0; i<ntendon; i++) {
for (int i=0; i<ntendon; i++) {
copyvec(tendons[i]->range, m->tendon_range+2*i, 2);
copyvec(tendons[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
copyvec(tendons[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
@@ -3073,7 +3055,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
// actuators
mjCActuator* pa;
for (i=0; i<nu; i++) {
for (int i=0; i<nu; i++) {
pa = actuators[i];
copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN);
@@ -3092,7 +3074,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// sensors
for (i=0; i<nsensor; i++) {
for (int i=0; i<nsensor; i++) {
sensors[i]->cutoff = (double)m->sensor_cutoff[i];
sensors[i]->noise = (double)m->sensor_noise[i];
@@ -3102,21 +3084,21 @@ bool mjCModel::CopyBack(const mjModel* m) {
}
// numeric data
for (i=0; i<nnumeric; i++) {
for (j=0; j<m->numeric_size[i]; j++) {
for (int i=0; i<nnumeric; i++) {
for (int j=0; j<m->numeric_size[i]; j++) {
numerics[i]->data[j] = (double)m->numeric_data[m->numeric_adr[i]+j];
}
}
// tuple data
for (i=0; i<ntuple; i++) {
for (j=0; j<m->tuple_size[i]; j++) {
for (int i=0; i<ntuple; i++) {
for (int j=0; j<m->tuple_size[i]; j++) {
tuples[i]->objprm[j] = (double)m->tuple_objprm[m->tuple_adr[i]+j];
}
}
// keyframes
for (i=0; i<m->nkey; i++) {
for (int i=0; i<m->nkey; i++) {
mjCKey* pk = keys[i];
pk->time = (double)m->key_time[i];
src/user/user_objects.cc
+67 -82
View File
@@ -356,15 +356,13 @@ mjCBody::mjCBody(mjCModel* _model) {
// destructor
mjCBody::~mjCBody() {
unsigned int i;
// delete objects allocated here
for (i=0; i<bodies.size(); i++) delete bodies[i];
for (i=0; i<geoms.size(); i++) delete geoms[i];
for (i=0; i<joints.size(); i++) delete joints[i];
for (i=0; i<sites.size(); i++) delete sites[i];
for (i=0; i<cameras.size(); i++) delete cameras[i];
for (i=0; i<lights.size(); i++) delete lights[i];
for (int i=0; i<bodies.size(); i++) delete bodies[i];
for (int i=0; i<geoms.size(); i++) delete geoms[i];
for (int i=0; i<joints.size(); i++) delete joints[i];
for (int i=0; i<sites.size(); i++) delete sites[i];
for (int i=0; i<cameras.size(); i++) delete cameras[i];
for (int i=0; i<lights.size(); i++) delete lights[i];
bodies.clear();
geoms.clear();
@@ -576,14 +574,14 @@ mjCBase* mjCBody::FindObject(mjtObj type, string _name, bool recursive) {
// compute geom inertial frame: ipos, iquat, mass, inertia
void mjCBody::GeomFrame(void) {
int i, sz;
int sz;
double com[3] = {0, 0, 0};
double toti[6] = {0, 0, 0, 0, 0, 0};
vector<mjCGeom*> sel;
// select geoms based on group
sel.clear();
for (i=0; i<geoms.size(); i++) {
for (int i=0; i<geoms.size(); i++) {
if (geoms[i]->group>=model->inertiagrouprange[0] &&
geoms[i]->group<=model->inertiagrouprange[1]) {
sel.push_back(geoms[i]);
@@ -603,7 +601,7 @@ void mjCBody::GeomFrame(void) {
else if (sz>1) {
// compute total mass and center of mass
mass = 0;
for (i=0; i<sz; i++) {
for (int i=0; i<sz; i++) {
mass += sel[i]->mass;
com[0] += sel[i]->mass * sel[i]->pos[0];
com[1] += sel[i]->mass * sel[i]->pos[1];
@@ -621,7 +619,7 @@ void mjCBody::GeomFrame(void) {
ipos[2] = com[2]/mass;
// add geom inertias
for (i=0; i<sz; i++) {
for (int i=0; i<sz; i++) {
double inert0[6], inert1[6];
double dpos[3] = {
sel[i]->pos[0] - ipos[0],
@@ -820,8 +818,6 @@ int mjCBody::MakeBVH(std::vector<mjCGeom *>& elements, int lev) {
// compiler
void mjCBody::Compile(void) {
unsigned int i;
// resize userdata
if (userdata.size() > model->nuser_body) {
throw mjCError(this, "user has more values than nuser_body in body '%s' (id = %d)",
@@ -839,7 +835,7 @@ void mjCBody::Compile(void) {
mjuu_normvec(iquat, 4);
// set parentid and weldid of children
for (i=0; i<bodies.size(); i++) {
for (int i=0; i<bodies.size(); i++) {
bodies[i]->parentid = id;
bodies[i]->weldid = (!bodies[i]->joints.empty() ? bodies[i]->id : weldid);
}
@@ -857,7 +853,7 @@ void mjCBody::Compile(void) {
}
// compile all geoms, phase 1
for (i=0; i<geoms.size(); i++) {
for (int i=0; i<geoms.size(); i++) {
geoms[i]->inferinertia = id>0 &&
(!explicitinertial || model->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) &&
geoms[i]->group >= model->inertiagrouprange[0] &&
@@ -924,7 +920,7 @@ void mjCBody::Compile(void) {
}
// make local frames of geoms
for (i=0; i<geoms.size(); i++) {
for (int i=0; i<geoms.size(); i++) {
MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat);
}
@@ -935,7 +931,7 @@ void mjCBody::Compile(void) {
// compile all joints, count dofs
dofnum = 0;
for (i=0; i<joints.size(); i++) {
for (int i=0; i<joints.size(); i++) {
dofnum += joints[i]->Compile();
}
@@ -946,7 +942,7 @@ void mjCBody::Compile(void) {
// check for rotation dof after ball joint
bool hasball = false;
for (i=0; i<joints.size(); i++) {
for (int i=0; i<joints.size(); i++) {
if ((joints[i]->type==mjJNT_BALL || joints[i]->type==mjJNT_HINGE) && hasball) {
throw mjCError(this, "ball followed by rotation in body '%s'", name.c_str());
}
@@ -962,13 +958,13 @@ void mjCBody::Compile(void) {
}
// compile all sites
for (i=0; i<sites.size(); i++) sites[i]->Compile();
for (int i=0; i<sites.size(); i++) sites[i]->Compile();
// compile all cameras
for (i=0; i<cameras.size(); i++) cameras[i]->Compile();
for (int i=0; i<cameras.size(); i++) cameras[i]->Compile();
// compile all lights
for (i=0; i<lights.size(); i++) lights[i]->Compile();
for (int i=0; i<lights.size(); i++) lights[i]->Compile();
// plugin
if (is_plugin) {
@@ -2217,11 +2213,10 @@ mjCTexture::~mjCTexture() {
// insert random dots
static void randomdot(unsigned char* rgb, const double* markrgb,
int width, int height, double probability) {
int r, c, j;
for (r=0; r<height; r++) {
for (c=0; c<width; c++) {
for (int r=0; r<height; r++) {
for (int c=0; c<width; c++) {
if (rand()<probability*RAND_MAX) {
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
rgb[3*(r*width+c)+j] = (mjtByte)(255*markrgb[j]);
}
}
@@ -2251,25 +2246,23 @@ static void interp(unsigned char* rgb, const double* rgb1, const double* rgb2, d
// make checker pattern for one side
static void checker(unsigned char* rgb, const unsigned char* RGB1, const unsigned char* RGB2,
int width, int height) {
int r, c;
for (r=0; r<height/2; r++) {
for (c=0; c<width/2; c++) {
for (int r=0; r<height/2; r++) {
for (int c=0; c<width/2; c++) {
memcpy(rgb+3*(r*width+c), RGB1, 3);
}
}
for (r=height/2; r<height; r++) {
for (c=width/2; c<width; c++) {
for (int r=height/2; r<height; r++) {
for (int c=width/2; c<width; c++) {
memcpy(rgb+3*(r*width+c), RGB1, 3);
}
}
for (r=0; r<height/2; r++) {
for (c=width/2; c<width; c++) {
for (int r=0; r<height/2; r++) {
for (int c=width/2; c<width; c++) {
memcpy(rgb+3*(r*width+c), RGB2, 3);
}
}
for (r=height/2; r<height; r++) {
for (c=0; c<width/2; c++) {
for (int r=height/2; r<height; r++) {
for (int c=0; c<width/2; c++) {
memcpy(rgb+3*(r*width+c), RGB2, 3);
}
}
@@ -2280,10 +2273,8 @@ static void checker(unsigned char* rgb, const unsigned char* RGB1, const unsigne
// make builtin: 2D
void mjCTexture::Builtin2D(void) {
unsigned char RGB1[3], RGB2[3], RGBm[3];
int r, c, j;
// convert fixed colors
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
RGB1[j] = (mjtByte)(255*rgb1[j]);
RGB2[j] = (mjtByte)(255*rgb2[j]);
RGBm[j] = (mjtByte)(255*markrgb[j]);
@@ -2293,8 +2284,8 @@ void mjCTexture::Builtin2D(void) {
// gradient
if (builtin==mjBUILTIN_GRADIENT) {
for (r=0; r<height; r++) {
for (c=0; c<width; c++) {
for (int r=0; r<height; r++) {
for (int c=0; c<width; c++) {
// compute normalized coordinates and radius
double x = 2*c/((double)(width-1)) - 1;
double y = 1 - 2*r/((double)(height-1));
@@ -2313,8 +2304,8 @@ void mjCTexture::Builtin2D(void) {
// flat
else if (builtin==mjBUILTIN_FLAT) {
for (r=0; r<height; r++) {
for (c=0; c<width; c++) {
for (int r=0; r<height; r++) {
for (int c=0; c<width; c++) {
memcpy(rgb+3*(r*width+c), RGB1, 3);
}
}
@@ -2324,11 +2315,11 @@ void mjCTexture::Builtin2D(void) {
// edge
if (mark==mjMARK_EDGE) {
for (r=0; r<height; r++) {
for (int r=0; r<height; r++) {
memcpy(rgb+3*(r*width+0), RGBm, 3);
memcpy(rgb+3*(r*width+width-1), RGBm, 3);
}
for (c=0; c<width; c++) {
for (int c=0; c<width; c++) {
memcpy(rgb+3*(0*width+c), RGBm, 3);
memcpy(rgb+3*((height-1)*width+c), RGBm, 3);
}
@@ -2336,10 +2327,10 @@ void mjCTexture::Builtin2D(void) {
// cross
else if (mark==mjMARK_CROSS) {
for (r=0; r<height; r++) {
for (int r=0; r<height; r++) {
memcpy(rgb+3*(r*width+width/2), RGBm, 3);
}
for (c=0; c<width; c++) {
for (int c=0; c<width; c++) {
memcpy(rgb+3*(height/2*width+c), RGBm, 3);
}
}
@@ -2355,10 +2346,9 @@ void mjCTexture::Builtin2D(void) {
// make builtin: Cube
void mjCTexture::BuiltinCube(void) {
unsigned char RGB1[3], RGB2[3], RGBm[3], RGBi[3];
int r, c, j;
// convert fixed colors
for (j=0; j<3; j++) {
for (int j=0; j<3; j++) {
RGB1[j] = (mjtByte)(255*rgb1[j]);
RGB2[j] = (mjtByte)(255*rgb2[j]);
RGBm[j] = (mjtByte)(255*markrgb[j]);
@@ -2368,8 +2358,8 @@ void mjCTexture::BuiltinCube(void) {
// gradient
if (builtin==mjBUILTIN_GRADIENT) {
for (r=0; r<width; r++) {
for (c=0; c<width; c++) {
for (int r=0; r<width; r++) {
for (int c=0; c<width; c++) {
// compute normalized pixel coordinates
double x = 2*c/((double)(width-1)) - 1;
double y = 1 - 2*r/((double)(width-1));
@@ -2404,8 +2394,8 @@ void mjCTexture::BuiltinCube(void) {
// flat
else if (builtin==mjBUILTIN_FLAT) {
for (r=0; r<width; r++) {
for (c=0; c<width; c++) {
for (int r=0; r<width; r++) {
for (int c=0; c<width; c++) {
// set sides and up
memcpy(rgb+0*3*width*width+3*(r*width+c), RGB1, 3);
memcpy(rgb+1*3*width*width+3*(r*width+c), RGB1, 3);
@@ -2423,12 +2413,12 @@ void mjCTexture::BuiltinCube(void) {
// edge
if (mark==mjMARK_EDGE) {
for (j=0; j<6; j++) {
for (r=0; r<width; r++) {
for (int j=0; j<6; j++) {
for (int r=0; r<width; r++) {
memcpy(rgb+j*3*width*width+3*(r*width+0), RGBm, 3);
memcpy(rgb+j*3*width*width+3*(r*width+width-1), RGBm, 3);
}
for (c=0; c<width; c++) {
for (int c=0; c<width; c++) {
memcpy(rgb+j*3*width*width+3*(0*width+c), RGBm, 3);
memcpy(rgb+j*3*width*width+3*((width-1)*width+c), RGBm, 3);
}
@@ -2437,11 +2427,11 @@ void mjCTexture::BuiltinCube(void) {
// cross
else if (mark==mjMARK_CROSS) {
for (j=0; j<6; j++) {
for (r=0; r<width; r++) {
for (int j=0; j<6; j++) {
for (int r=0; r<width; r++) {
memcpy(rgb+j*3*width*width+3*(r*width+width/2), RGBm, 3);
}
for (c=0; c<width; c++) {
for (int c=0; c<width; c++) {
memcpy(rgb+j*3*width*width+3*(width/2*width+c), RGBm, 3);
}
}
@@ -2637,8 +2627,6 @@ void mjCTexture::Load2D(string filename, const mjVFS* vfs) {
// load cube or skybox from single file (repeated or grid)
void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
int i, j, k, s;
// check gridsize
if (gridsize[0]<1 || gridsize[1]<1 || gridsize[0]*gridsize[1]>12) {
throw mjCError(this,
@@ -2685,9 +2673,9 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
int loaded[6] = {0, 0, 0, 0, 0, 0};
// process grid
for (k=0; k<gridsize[0]*gridsize[1]; k++) {
for (int k=0; k<gridsize[0]*gridsize[1]; k++) {
// decode face symbol
i = -1;
int i = -1;
if (gridlayout[k]=='R') {
i = 0;
} else if (gridlayout[k]=='L') {
@@ -2709,7 +2697,7 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
// extract sub-image
int rstart = width*(k/gridsize[1]);
int cstart = width*(k%gridsize[1]);
for (j=0; j<width; j++) {
for (int j=0; j<width; j++) {
memcpy(rgb+i*3*width*width+j*3*width, image.data()+(j+rstart)*3*w+3*cstart, 3*width);
}
@@ -2719,11 +2707,11 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
}
// set undefined faces to rgb1
for (i=0; i<6; i++) {
for (int i=0; i<6; i++) {
if (!loaded[i]) {
for (k=0; k<width; k++) {
for (s=0; s<width; s++) {
for (j=0; j<3; j++) {
for (int k=0; k<width; k++) {
for (int s=0; s<width; s++) {
for (int j=0; j<3; j++) {
rgb[i*3*width*width + 3*(k*width+s) + j] = (mjtByte)(255*rgb1[j]);
}
}
@@ -2739,13 +2727,11 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
// load cube or skybox from separate file
void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
int i, j, k, s;
// keep track of which faces were defined
int loaded[6] = {0, 0, 0, 0, 0, 0};
// process nonempty files
for (i=0; i<6; i++) {
for (int i=0; i<6; i++) {
if (!cubefiles[i].empty()) {
// remove path from file if necessary
if (model->strippath) {
@@ -2796,11 +2782,11 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
}
// set undefined faces to rgb1
for (i=0; i<6; i++) {
for (int i=0; i<6; i++) {
if (!loaded[i]) {
for (k=0; k<width; k++) {
for (s=0; s<width; s++) {
for (j=0; j<3; j++) {
for (int k=0; k<width; k++) {
for (int s=0; s<width; s++) {
for (int j=0; j<3; j++) {
rgb[i*3*width*width + 3*(k*width+s) + j] = (mjtByte)(255*rgb1[j]);
}
}
@@ -4419,12 +4405,11 @@ mjCKey::~mjCKey() {
// compiler
void mjCKey::Compile(const mjModel* m) {
int i;
// qpos: allocate or check size
if (qpos.empty()) {
qpos.resize(m->nq);
for (i=0; i<m->nq; i++) {
for (int i=0; i<m->nq; i++) {
qpos[i] = (double)m->qpos0[i];
}
} else if (qpos.size()!=m->nq) {
@@ -4434,7 +4419,7 @@ void mjCKey::Compile(const mjModel* m) {
// qvel: allocate or check size
if (qvel.empty()) {
qvel.resize(m->nv);
for (i=0; i<m->nv; i++) {
for (int i=0; i<m->nv; i++) {
qvel[i] = 0;
}
} else if (qvel.size()!=m->nv) {
@@ -4444,7 +4429,7 @@ void mjCKey::Compile(const mjModel* m) {
// act: allocate or check size
if (act.empty()) {
act.resize(m->na);
for (i=0; i<m->na; i++) {
for (int i=0; i<m->na; i++) {
act[i] = 0;
}
} else if (act.size()!=m->na) {
@@ -4455,7 +4440,7 @@ void mjCKey::Compile(const mjModel* m) {
if (mpos.empty()) {
mpos.resize(3*m->nmocap);
if (m->nmocap) {
for (i=0; i<m->nbody; i++) {
for (int i=0; i<m->nbody; i++) {
if (m->body_mocapid[i]>=0) {
int mocapid = m->body_mocapid[i];
mpos[3*mocapid] = m->body_pos[3*i];
@@ -4472,7 +4457,7 @@ void mjCKey::Compile(const mjModel* m) {
if (mquat.empty()) {
mquat.resize(4*m->nmocap);
if (m->nmocap) {
for (i=0; i<m->nbody; i++) {
for (int i=0; i<m->nbody; i++) {
if (m->body_mocapid[i]>=0) {
int mocapid = m->body_mocapid[i];
mquat[4*mocapid] = m->body_quat[4*i];
@@ -4489,7 +4474,7 @@ void mjCKey::Compile(const mjModel* m) {
// ctrl: allocate or check size
if (ctrl.empty()) {
ctrl.resize(m->nu);
for (i=0; i<m->nu; i++) {
for (int i=0; i<m->nu; i++) {
ctrl[i] = 0;
}
} else if (ctrl.size()!=m->nu) {
src/user/user_util.cc
+2 -3
View File
@@ -141,9 +141,8 @@ double mjuu_L1(const double* a, const double* b, int n) {
// normalize vector to unit length, return previous length
double mjuu_normvec(double* vec, const int n) {
double nrm = 0;
int i;
for (i=0; i<n; i++) {
for (int i=0; i<n; i++) {
nrm += vec[i]*vec[i];
}
if (nrm < mjEPS) {
@@ -151,7 +150,7 @@ double mjuu_normvec(double* vec, const int n) {
}
nrm = sqrt(nrm);
for (i=0; i<n; i++) {
for (int i=0; i<n; i++) {
vec[i] /= nrm;
}
src/xml/xml_native_reader.cc
+18 -20
View File
@@ -730,8 +730,6 @@ void mjXReader::PrintSchema(std::stringstream& str, bool html, bool pad) {
// main entry point for XML parser
// mjCModel is allocated here; caller is responsible for deallocation
void mjXReader::Parse(XMLElement* root) {
XMLElement *section;
// check schema
if (!schema.GetError().empty()) {
throw mjXError(0, "XML Schema Construction Error: %s\n",
@@ -757,86 +755,86 @@ void mjXReader::Parse(XMLElement* root) {
//------------------- parse MuJoCo sections embedded in all XML formats
for (section = root->FirstChildElement("compiler"); section;
for (XMLElement* section = root->FirstChildElement("compiler"); section;
section = section->NextSiblingElement("compiler")) {
Compiler(section, model);
}
for (section = root->FirstChildElement("option"); section;
for (XMLElement* section = root->FirstChildElement("option"); section;
section = section->NextSiblingElement("option")) {
Option(section, &model->option);
}
for (section = root->FirstChildElement("size"); section;
for (XMLElement* section = root->FirstChildElement("size"); section;
section = section->NextSiblingElement("size")) {
Size(section, model);
}
//------------------ parse MJCF-specific sections
for (section = root->FirstChildElement("visual"); section;
for (XMLElement* section = root->FirstChildElement("visual"); section;
section = section->NextSiblingElement("visual")) {
Visual(section);
}
for (section = root->FirstChildElement("statistic"); section;
for (XMLElement* section = root->FirstChildElement("statistic"); section;
section = section->NextSiblingElement("statistic")) {
Statistic(section);
}
readingdefaults = true;
for (section = root->FirstChildElement("default"); section;
for (XMLElement* section = root->FirstChildElement("default"); section;
section = section->NextSiblingElement("default")) {
Default(section, -1);
}
readingdefaults = false;
for (section = root->FirstChildElement("extension"); section;
for (XMLElement* section = root->FirstChildElement("extension"); section;
section = section->NextSiblingElement("extension")) {
Extension(section);
}
for (section = root->FirstChildElement("custom"); section;
for (XMLElement* section = root->FirstChildElement("custom"); section;
section = section->NextSiblingElement("custom")) {
Custom(section);
}
for (section = root->FirstChildElement("asset"); section;
for (XMLElement* section = root->FirstChildElement("asset"); section;
section = section->NextSiblingElement("asset")) {
Asset(section);
}
for (section = root->FirstChildElement("worldbody"); section;
for (XMLElement* section = root->FirstChildElement("worldbody"); section;
section = section->NextSiblingElement("worldbody")) {
Body(section, model->GetWorld());
}
for (section = root->FirstChildElement("contact"); section;
for (XMLElement* section = root->FirstChildElement("contact"); section;
section = section->NextSiblingElement("contact")) {
Contact(section);
}
for (section = root->FirstChildElement("equality"); section;
for (XMLElement* section = root->FirstChildElement("equality"); section;
section = section->NextSiblingElement("equality")) {
Equality(section);
}
for (section = root->FirstChildElement("tendon"); section;
for (XMLElement* section = root->FirstChildElement("tendon"); section;
section = section->NextSiblingElement("tendon")) {
Tendon(section);
}
for (section = root->FirstChildElement("actuator"); section;
for (XMLElement* section = root->FirstChildElement("actuator"); section;
section = section->NextSiblingElement("actuator")) {
Actuator(section);
}
for (section = root->FirstChildElement("sensor"); section;
for (XMLElement* section = root->FirstChildElement("sensor"); section;
section = section->NextSiblingElement("sensor")) {
Sensor(section);
}
for (section = root->FirstChildElement("keyframe"); section;
for (XMLElement* section = root->FirstChildElement("keyframe"); section;
section = section->NextSiblingElement("keyframe")) {
Keyframe(section);
}
@@ -1605,7 +1603,6 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) {
// actuator element parser
void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
int n;
string text, type;
double diameter;
@@ -1672,6 +1669,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
// explicit attributes
if (type=="general") {
// explicit attributes
int n;
if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) {
pact->dyntype = (mjtDyn)n;
}
@@ -1811,7 +1809,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
ReadAttr(elem, "fvmax", 1, pact->gainprm+8, text);
// biasprm = gainprm
for (n=0; n<9; n++) {
for (int n=0; n<9; n++) {
pact->biasprm[n] = pact->gainprm[n];
}
src/xml/xml_native_writer.cc
+17 -21
View File
@@ -1135,7 +1135,6 @@ void mjXWriter::Extension(XMLElement* root) {
// custom section
void mjXWriter::Custom(XMLElement* root) {
XMLElement* elem;
int i, j;
// get sizes, skip section if empty
int nnum = model->NumObjects(mjOBJ_NUMERIC);
@@ -1151,7 +1150,7 @@ void mjXWriter::Custom(XMLElement* root) {
XMLElement* section = InsertEnd(root, "custom");
// write all numerics
for (i=0; i<nnum; i++) {
for (int i=0; i<nnum; i++) {
mjCNumeric* ptr = (mjCNumeric*)model->GetObject(mjOBJ_NUMERIC, i);
elem = InsertEnd(section, "numeric");
WriteAttrTxt(elem, "name", ptr->name);
@@ -1160,7 +1159,7 @@ void mjXWriter::Custom(XMLElement* root) {
}
// write all texts
for (i=0; i<ntxt; i++) {
for (int i=0; i<ntxt; i++) {
mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i);
elem = InsertEnd(section, "text");
WriteAttrTxt(elem, "name", ptr->name);
@@ -1168,13 +1167,13 @@ void mjXWriter::Custom(XMLElement* root) {
}
// write all tuples
for (i=0; i<ntup; i++) {
for (int i=0; i<ntup; i++) {
mjCTuple* ptr = (mjCTuple*)model->GetObject(mjOBJ_TUPLE, i);
elem = InsertEnd(section, "tuple");
WriteAttrTxt(elem, "name", ptr->name);
// write objects in tuple
for (j=0; j<(int)ptr->objtype.size(); j++) {
for (int j=0; j<(int)ptr->objtype.size(); j++) {
XMLElement* obj = InsertEnd(elem, "element");
WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype[j]));
WriteAttrTxt(obj, "objname", ptr->objname[j].c_str());
@@ -1191,7 +1190,6 @@ void mjXWriter::Custom(XMLElement* root) {
// asset section
void mjXWriter::Asset(XMLElement* root) {
XMLElement* elem;
int i;
// get sizes
int ntex = model->NumObjects(mjOBJ_TEXTURE);
@@ -1210,7 +1208,7 @@ void mjXWriter::Asset(XMLElement* root) {
// write textures
mjCTexture deftex(0);
for (i=0; i<ntex; i++) {
for (int i=0; i<ntex; i++) {
// create element
mjCTexture* ptex = (mjCTexture*)model->GetObject(mjOBJ_TEXTURE, i);
elem = InsertEnd(section, "texture");
@@ -1260,7 +1258,7 @@ void mjXWriter::Asset(XMLElement* root) {
}
// write materials
for (i=0; i<nmat; i++) {
for (int i=0; i<nmat; i++) {
// create element and write
mjCMaterial* pmat = (mjCMaterial*)model->GetObject(mjOBJ_MATERIAL, i);
elem = InsertEnd(section, "material");
@@ -1268,7 +1266,7 @@ void mjXWriter::Asset(XMLElement* root) {
}
// write meshes
for (i=0; i<nmesh; i++) {
for (int i=0; i<nmesh; i++) {
// create element and write
mjCMesh* pmesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i);
elem = InsertEnd(section, "mesh");
@@ -1276,7 +1274,7 @@ void mjXWriter::Asset(XMLElement* root) {
}
// write skins
for (i=0; i<nskin; i++) {
for (int i=0; i<nskin; i++) {
// create element and write
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
elem = InsertEnd(section, "skin");
@@ -1284,7 +1282,7 @@ void mjXWriter::Asset(XMLElement* root) {
}
// write hfields
for (i=0; i<nhfield; i++) {
for (int i=0; i<nhfield; i++) {
// create element
mjCHField* phf = (mjCHField*)model->GetObject(mjOBJ_HFIELD, i);
elem = InsertEnd(section, "hfield");
@@ -1306,7 +1304,6 @@ void mjXWriter::Asset(XMLElement* root) {
// recursive body writer
void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
double unitq[4] = {1, 0, 0, 0};
unsigned int i;
if (!body) {
throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR
@@ -1340,27 +1337,27 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
}
// write joints
for (i=0; i<body->joints.size(); i++) {
for (int i=0; i<body->joints.size(); i++) {
OneJoint(InsertEnd(elem, "joint"), body->joints[i], body->joints[i]->def);
}
// write geoms
for (i=0; i<body->geoms.size(); i++) {
for (int i=0; i<body->geoms.size(); i++) {
OneGeom(InsertEnd(elem, "geom"), body->geoms[i], body->geoms[i]->def);
}
// write sites
for (i=0; i<body->sites.size(); i++) {
for (int i=0; i<body->sites.size(); i++) {
OneSite(InsertEnd(elem, "site"), body->sites[i], body->sites[i]->def);
}
// write cameras
for (i=0; i<body->cameras.size(); i++) {
for (int i=0; i<body->cameras.size(); i++) {
OneCamera(InsertEnd(elem, "camera"), body->cameras[i], body->cameras[i]->def);
}
// write lights
for (i=0; i<body->lights.size(); i++) {
for (int i=0; i<body->lights.size(); i++) {
OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def);
}
@@ -1387,7 +1384,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
}
// write child bodies recursively
for (i=0; i<body->bodies.size(); i++) {
for (int i=0; i<body->bodies.size(); i++) {
Body(InsertEnd(elem, "body"), body->bodies[i]);
}
}
@@ -1397,7 +1394,6 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
// collision section
void mjXWriter::Contact(XMLElement* root) {
XMLElement* elem;
int i;
// get number of pairs of each type
int npair = model->NumObjects(mjOBJ_PAIR);
@@ -1412,7 +1408,7 @@ void mjXWriter::Contact(XMLElement* root) {
XMLElement* section = InsertEnd(root, "contact");
// write all geom pairs
for (i=0; i<npair; i++) {
for (int i=0; i<npair; i++) {
// create element and write
mjCPair* ppair = (mjCPair*)model->GetObject(mjOBJ_PAIR, i);
elem = InsertEnd(section, "pair");
@@ -1420,7 +1416,7 @@ void mjXWriter::Contact(XMLElement* root) {
}
// write all exclude pairs
for (i=0; i<nexclude; i++) {
for (int i=0; i<nexclude; i++) {
// create element
mjCBodyPair* pexclude = (mjCBodyPair*)model->GetObject(mjOBJ_EXCLUDE, i);
elem = InsertEnd(section, "exclude");
src/xml/xml_urdf.cc
+2 -2
View File
@@ -74,7 +74,7 @@ void mjXURDF::Clear(void) {
void mjXURDF::Parse(XMLElement* root) {
std::string name, text;
XMLElement *elem, *temp;
int id_parent, id_child, i;
int id_parent, id_child;
// set compiler defaults suitable for URDF
model->strippath = true;
@@ -158,7 +158,7 @@ void mjXURDF::Parse(XMLElement* root) {
}
// find all top-level bodies, call recursive tree constructor
for (i=0; i<(int)urName.size(); i++) {
for (int i=0; i<(int)urName.size(); i++) {
if (urParent[i] < 0) {
AddToTree(i);
}
src/xml/xml_util.cc
+8 -13
View File
@@ -282,8 +282,6 @@ static int _max(int a, int b) {
// print schema as text
void mjXSchema::Print(std::stringstream& str, int level) {
int i;
// replace body with (world)body
string name1 = (name=="body" ? "(world)body" : name);
@@ -297,7 +295,7 @@ void mjXSchema::Print(std::stringstream& str, int level) {
// attributes
int cnt = _max(baselen, 30);
for (i=0; i<(int)attr.size(); i++) {
for (int i=0; i<(int)attr.size(); i++) {
if (cnt>60) {
str << "\n";
printspace(str, (cnt = _max(30, baselen)), " ");
@@ -310,7 +308,7 @@ void mjXSchema::Print(std::stringstream& str, int level) {
str << "\n";
// children
for (i=0; i<(int)child.size(); i++) {
for (int i=0; i<(int)child.size(); i++) {
child[i]->Print(str, level+1);
}
}
@@ -319,8 +317,6 @@ void mjXSchema::Print(std::stringstream& str, int level) {
// print schema as HTML table
void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) {
int i;
// replace body with (world)body
string name1 = (name=="body" ? "(world)body" : name);
@@ -350,7 +346,7 @@ void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) {
// attributes
str << "\t<td class=\"at\">";
if (!attr.empty()) {
for (i=0; i<(int)attr.size(); i++) {
for (int i=0; i<(int)attr.size(); i++) {
str << attr[i] << " ";
}
} else {
@@ -359,7 +355,7 @@ void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) {
str << "</td>\n</tr>\n";
// children
for (i=0; i<(int)child.size(); i++) {
for (int i=0; i<(int)child.size(); i++) {
child[i]->PrintHTML(str, level+1, pad);
}
@@ -394,7 +390,6 @@ bool mjXSchema::NameMatch(XMLElement* elem, int level) {
// validator
XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
int i;
bool missing;
char msg[100];
XMLElement *bad, *sub;
@@ -414,7 +409,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
const XMLAttribute* attribute = elem->FirstAttribute();
while (attribute) {
missing = true;
for (i=0; i<(int)attr.size(); i++) {
for (int i=0; i<(int)attr.size(); i++) {
if (attr[i]==attribute->Name()) {
missing = false;
break;
@@ -445,7 +440,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
}
// clear reference counts
for (i=0; i<(int)child.size(); i++) {
for (int i=0; i<(int)child.size(); i++) {
child[i]->refcnt = 0;
}
@@ -454,7 +449,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
while (sub) {
// find in child array, update refcnt
missing = true;
for (i=0; i<(int)child.size(); i++) {
for (int i=0; i<(int)child.size(); i++) {
if (child[i]->NameMatch(sub, level+1)) {
// check sub-tree
if ((bad = child[i]->Check(sub, level+1))) {
@@ -481,7 +476,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
// enforce sub-element types
msg[0] = 0;
for (i=0; i<(int)child.size(); i++) {
for (int i=0; i<(int)child.size(); i++) {
switch (child[i]->type) {
case '!':
if (child[i]->refcnt != 1)
test/engine/engine_util_solve_test.cc
+1 -2
View File
@@ -92,8 +92,7 @@ bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
// compare to nudged solution
mju_copy(res_nudge, res, n);
int i;
for (i=0; i < n; i++) {
for (int i=0; i < n; i++) {
// nudge down
res_nudge[i] = res[i] - eps;
if (lower) {